From 58bbc3bc5d4e677f055c25464251efe631bcf62b Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Tue, 24 Mar 2026 00:47:43 +0000 Subject: [PATCH 1/4] make node use argparse --- hydra_ros/app/csv_to_tf | 86 +++++++++++++++++++++++++---------------- 1 file changed, 52 insertions(+), 34 deletions(-) diff --git a/hydra_ros/app/csv_to_tf b/hydra_ros/app/csv_to_tf index 6f8954a..594a0a3 100755 --- a/hydra_ros/app/csv_to_tf +++ b/hydra_ros/app/csv_to_tf @@ -1,6 +1,7 @@ #!/usr/bin/env python3 """Node that broadcasts a CSV as a tf.""" +import argparse import pathlib import sys @@ -46,44 +47,26 @@ def _parse_file(trajectory_file, parent, child, skip_first=True, order=None): return transforms -class CsvToTf(rclpy.node.Node): - def __init__(self): +class CsvToTfNode(rclpy.node.Node): + """Node that uses a CSV to simulate a live TF.""" + + def __init__( + self, transforms, offset=0.0, lookahead=0.0, stale=0.5, poll_period=0.01 + ): + """Initialize the node.""" super().__init__("csv_to_tf") + self._idx = 0 + self._transforms = transforms self._broadcaster = tf2_ros.TransformBroadcaster(self) - offset_s = self._get_param("offset_s", 0.0).double_value - self._offset = rclpy.duration.Duration(seconds=offset_s) - stale_threshold_s = self._get_param("stale_threshold_s", 0.5).double_value - self._stale_threshold = rclpy.duration.Duration(seconds=stale_threshold_s) - lookahead_s = self._get_param("lookahead_s", 0.1).double_value - self._lookahead = rclpy.duration.Duration(seconds=lookahead_s) + self._offset = rclpy.duration.Duration(seconds=offset) + self._stale_threshold = rclpy.duration.Duration(seconds=stale) + self._lookahead = rclpy.duration.Duration(seconds=lookahead) self.get_logger().info( f"offset: {self._offset}, lookahead: {self._lookahead}, stale: {self._stale_threshold}" ) - trajectory_file = self._get_param("trajectory_file", "").string_value - if trajectory_file == "": - self.get_logger().fatal("File required!") - sys.exit(1) - - trajectory_file = pathlib.Path(trajectory_file).expanduser().absolute() - if not trajectory_file.exists(): - self.get_logger().fatal( - f"Trajectory file {trajectory_file} does not exist!" - ) - sys.exit(1) - - parent_frame = self._get_param("parent_frame", "odom").string_value - child_frame = self._get_param("child_frame", "base_link").string_value - self._transforms = _parse_file(trajectory_file, parent_frame, child_frame) - self._idx = 0 - self.get_logger().info(f"Publishing {parent_frame}_T_{child_frame}") - - poll_period_s = self._get_param("poll_period_s", 0.01).double_value - self._timer = self.create_timer(poll_period_s, self._callback) - - def _get_param(self, name, default=None): - return self.declare_parameter(name, default).get_parameter_value() + self._timer = self.create_timer(poll_period, self._callback) def _curr_tf_is_stale(self, ros_stamp): # we want to reject any transforms that are more than stale_threshold_s than the current stamp @@ -137,13 +120,48 @@ class CsvToTf(rclpy.node.Node): def main(args=None): """Do stuff.""" - rclpy.init(args=args) + parser = argparse.ArgumentParser( + "Node that publishes parent_T_child from a CSV file" + ) + parser.add_argument("filepath") + parser.add_argument( + "--offset", default=0.0, help="Amount of time to offset stamps by" + ) + parser.add_argument( + "--lookahead", default=0.0, help="Amount of lead time to publish TF by" + ) + parser.add_argument( + "--stale", default=0.5, help="Amount of time before a TF is stale" + ) + parser.add_argument( + "--poll_period", default=0.01, help="Amount of time before a TF is stale" + ) + parser.add_argument("--parent", default="odom", help="frame ID for parent frame") + parser.add_argument("--child", default="base_link", help="frame ID for child frame") + args, rest = parser.parse_known_args() + + filepath = pathlib.Path(args.filepath).expanduser().absolute() + if not filepath.exists(): + sys.exit(f"Trajectory file {filepath} does not exist!") + + transforms = _parse_file(filepath, args.parent, args.child) + + rclpy.init(args=rest) try: - node = CsvToTf() + node = CsvToTfNode( + transforms, + offset=args.offset, + lookahead=args.lookahead, + stale=args.stale, + poll_period=args.poll_period, + ) + node.get_logger().info(f"Publishing {args.parent}_T_{args.child}") rclpy.spin(node) node.destroy_node() + except KeyboardInterrupt: + pass finally: - rclpy.utilities.try_shutdown() + rclpy.try_shutdown() if __name__ == "__main__": From 3d48773f9d1f2f4cf1f2f631ec185a58b7561a84 Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Tue, 24 Mar 2026 16:20:40 +0000 Subject: [PATCH 2/4] add more csv options to arguments --- hydra_ros/app/csv_to_tf | 44 +++++++++++++++++++++++++++++++---------- 1 file changed, 34 insertions(+), 10 deletions(-) diff --git a/hydra_ros/app/csv_to_tf b/hydra_ros/app/csv_to_tf index 594a0a3..cb3d656 100755 --- a/hydra_ros/app/csv_to_tf +++ b/hydra_ros/app/csv_to_tf @@ -2,6 +2,7 @@ """Node that broadcasts a CSV as a tf.""" import argparse +import json import pathlib import sys @@ -14,9 +15,18 @@ import tf2_ros DEFAULT_ORDER = ["x", "y", "z", "qw", "qx", "qy", "qz"] -def _parse_file(trajectory_file, parent, child, skip_first=True, order=None): +def _parse_file( + trajectory_file, parent, child, skip_first=True, order=None, origin=None +): if order is None: order = DEFAULT_ORDER + else: + order = json.loads(order) + + if origin: + origin = json.loads(origin) + else: + origin = [0.0, 0.0, 0.0] transforms = [] have_first_line = False @@ -29,19 +39,19 @@ def _parse_file(trajectory_file, parent, child, skip_first=True, order=None): values = [x.strip() for x in line.split(",")] time_ns = int(values[0]) - pose = [float(x) for x in values[1:]] + pose = {n: float(x) for n, x in zip(order, values[1:])} t = geometry_msgs.msg.TransformStamped() t.header.stamp = rclpy.time.Time(nanoseconds=time_ns).to_msg() t.header.frame_id = parent t.child_frame_id = child - t.transform.translation.x = pose[0] - t.transform.translation.y = pose[1] - t.transform.translation.z = pose[2] - t.transform.rotation.w = pose[3] - t.transform.rotation.x = pose[4] - t.transform.rotation.y = pose[5] - t.transform.rotation.z = pose[6] + t.transform.translation.x = pose["x"] - origin[0] + t.transform.translation.y = pose["y"] - origin[1] + t.transform.translation.z = pose["z"] - origin[2] + t.transform.rotation.w = pose["qw"] + t.transform.rotation.x = pose["qx"] + t.transform.rotation.y = pose["qy"] + t.transform.rotation.z = pose["qz"] transforms.append(t) return transforms @@ -138,13 +148,27 @@ def main(args=None): ) parser.add_argument("--parent", default="odom", help="frame ID for parent frame") parser.add_argument("--child", default="base_link", help="frame ID for child frame") + parser.add_argument( + "--origin", default=None, help="origin coordinates to use in json" + ) + parser.add_argument("--column_order", default=None, help="column names in json") + parser.add_argument( + "--parse_first", action="store_true", help="parse first CSV line" + ) args, rest = parser.parse_known_args() filepath = pathlib.Path(args.filepath).expanduser().absolute() if not filepath.exists(): sys.exit(f"Trajectory file {filepath} does not exist!") - transforms = _parse_file(filepath, args.parent, args.child) + transforms = _parse_file( + filepath, + args.parent, + args.child, + origin=args.origin, + order=args.column_order, + skip_first=not args.parse_first, + ) rclpy.init(args=rest) try: From 3b356674e25e021a67b1b7b4273a83e22f743597 Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Wed, 25 Mar 2026 18:35:51 +0000 Subject: [PATCH 3/4] flip back to using some params --- hydra_ros/app/csv_to_tf | 96 ++++++++++++++--------------------------- 1 file changed, 33 insertions(+), 63 deletions(-) diff --git a/hydra_ros/app/csv_to_tf b/hydra_ros/app/csv_to_tf index cb3d656..feec19f 100755 --- a/hydra_ros/app/csv_to_tf +++ b/hydra_ros/app/csv_to_tf @@ -15,22 +15,13 @@ import tf2_ros DEFAULT_ORDER = ["x", "y", "z", "qw", "qx", "qy", "qz"] -def _parse_file( - trajectory_file, parent, child, skip_first=True, order=None, origin=None -): - if order is None: - order = DEFAULT_ORDER - else: - order = json.loads(order) - - if origin: - origin = json.loads(origin) - else: - origin = [0.0, 0.0, 0.0] +def _parse_file(filepath, parent, child, skip_first=True, order=None, origin=None): + order = DEFAULT_ORDER if order is None else json.loads(order) + origin = json.loads(origin) if origin else [0.0, 0.0, 0.0] transforms = [] have_first_line = False - with open(trajectory_file, "r") as fin: + with open(filepath, "r") as fin: for line in fin: if not have_first_line: have_first_line = True @@ -60,23 +51,27 @@ def _parse_file( class CsvToTfNode(rclpy.node.Node): """Node that uses a CSV to simulate a live TF.""" - def __init__( - self, transforms, offset=0.0, lookahead=0.0, stale=0.5, poll_period=0.01 - ): + def __init__(self, transforms): """Initialize the node.""" super().__init__("csv_to_tf") self._idx = 0 self._transforms = transforms self._broadcaster = tf2_ros.TransformBroadcaster(self) - self._offset = rclpy.duration.Duration(seconds=offset) - self._stale_threshold = rclpy.duration.Duration(seconds=stale) - self._lookahead = rclpy.duration.Duration(seconds=lookahead) - self.get_logger().info( - f"offset: {self._offset}, lookahead: {self._lookahead}, stale: {self._stale_threshold}" - ) + self._offset = self._duration_param("offset_s", 0.0) + self._lookahead = self._duration_param("lookahead_s", 0.0) + self._stale = self._duration_param("stale_threshold_s", 0.5) + period = self._duration_param("poll_period_s", 0.001) + self._timer = self.create_timer(period, self._callback) - self._timer = self.create_timer(poll_period, self._callback) + self.get_logger().info(f"Polling clock every {period}") + self.get_logger().info(f"Offsetting transforms by {self._offset}") + self.get_logger().info(f"Using lookahead of {self._lookahead}") + self.get_logger().info(f"Discarding TFs more than {self._stale} behind clock") + + def _duration_param(self, name, default): + val = self._declare_parameter(name, default).get_parameter_value().double_value + return rclpy.durationDuration(seconds=val) def _curr_tf_is_stale(self, ros_stamp): # we want to reject any transforms that are more than stale_threshold_s than the current stamp @@ -84,7 +79,7 @@ class CsvToTfNode(rclpy.node.Node): if curr_stamp is None: return False - return ros_stamp - curr_stamp > self._stale_threshold + return ros_stamp - curr_stamp > self._stale def _get_curr_tf_stamp(self): if self._idx >= len(self._transforms): @@ -95,6 +90,7 @@ class CsvToTfNode(rclpy.node.Node): def _callback(self): stamp = self.get_clock().now() + stamp_ns = stamp.nanoseconds prev_idx = self._idx while self._curr_tf_is_stale(stamp): @@ -102,9 +98,7 @@ class CsvToTfNode(rclpy.node.Node): num_dropped = self._idx - prev_idx if num_dropped > 0: - self.get_logger().warn( - f"Dropped {num_dropped} TFs older than {stamp.nanoseconds} [ns]" - ) + self.get_logger().warn(f"Dropped {num_dropped} older than {stamp_ns} [ns]") if self._idx >= len(self._transforms): self.get_logger().info("Finished publishing transforms") @@ -112,16 +106,16 @@ class CsvToTfNode(rclpy.node.Node): return next_stamp = self._get_curr_tf_stamp() - diff_str = f"diff: {(next_stamp - stamp).nanoseconds} [ns]" - self.get_logger().debug( - f"waiting for time {next_stamp.nanoseconds} [ns] ({diff_str})" - ) - # if the next TF timestamp is ahead of the current ROS time by more than the lookahead, - # don't publish anything - if next_stamp - stamp > self._lookahead: + next_stamp_ns = next_stamp.nanoseconds + diff = next_stamp - stamp + diff_str = f"diff: {diff.nanoseconds} [ns]" + self.get_logger().debug(f"waiting for time {next_stamp_ns} [ns] ({diff_str})") + if diff > self._lookahead: + # if the next TF timestamp is ahead of the current ROS time by more than the lookahead, + # don't publish anything return - self.get_logger().debug(f"sending {next_stamp.nanoseconds} [ns]") + self.get_logger().debug(f"sending {next_stamp_ns} [ns]") msg = self._transforms[self._idx] msg.header.stamp = next_stamp.to_msg() # reset header stamp self._broadcaster.sendTransform(msg) @@ -130,31 +124,13 @@ class CsvToTfNode(rclpy.node.Node): def main(args=None): """Do stuff.""" - parser = argparse.ArgumentParser( - "Node that publishes parent_T_child from a CSV file" - ) + parser = argparse.ArgumentParser("Node publishing parent_T_child from CSV") parser.add_argument("filepath") - parser.add_argument( - "--offset", default=0.0, help="Amount of time to offset stamps by" - ) - parser.add_argument( - "--lookahead", default=0.0, help="Amount of lead time to publish TF by" - ) - parser.add_argument( - "--stale", default=0.5, help="Amount of time before a TF is stale" - ) - parser.add_argument( - "--poll_period", default=0.01, help="Amount of time before a TF is stale" - ) parser.add_argument("--parent", default="odom", help="frame ID for parent frame") parser.add_argument("--child", default="base_link", help="frame ID for child frame") - parser.add_argument( - "--origin", default=None, help="origin coordinates to use in json" - ) + parser.add_argument("--origin", default=None, help="origin to use in json") parser.add_argument("--column_order", default=None, help="column names in json") - parser.add_argument( - "--parse_first", action="store_true", help="parse first CSV line" - ) + parser.add_argument("--parse_first", action="store_true", help="parse first line") args, rest = parser.parse_known_args() filepath = pathlib.Path(args.filepath).expanduser().absolute() @@ -172,13 +148,7 @@ def main(args=None): rclpy.init(args=rest) try: - node = CsvToTfNode( - transforms, - offset=args.offset, - lookahead=args.lookahead, - stale=args.stale, - poll_period=args.poll_period, - ) + node = CsvToTfNode(transforms) node.get_logger().info(f"Publishing {args.parent}_T_{args.child}") rclpy.spin(node) node.destroy_node() From b4388be272a39bb7eb046c10c35165b72a4d4d36 Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Thu, 26 Mar 2026 01:41:00 +0000 Subject: [PATCH 4/4] drop csv node for ianvs impl --- hydra_ros/CMakeLists.txt | 3 +- hydra_ros/app/csv_to_tf | 162 --------------------------------------- 2 files changed, 1 insertion(+), 164 deletions(-) delete mode 100755 hydra_ros/app/csv_to_tf diff --git a/hydra_ros/CMakeLists.txt b/hydra_ros/CMakeLists.txt index 0c5b70d..458f3bc 100644 --- a/hydra_ros/CMakeLists.txt +++ b/hydra_ros/CMakeLists.txt @@ -114,8 +114,7 @@ install( ) install(TARGETS hydra_ros_node RUNTIME DESTINATION lib/${PROJECT_NAME}) install( - PROGRAMS app/csv_to_tf - app/dsg_file_publisher + PROGRAMS app/dsg_file_publisher app/dsg_republisher app/odom_to_tf app/noisy_tf_publisher diff --git a/hydra_ros/app/csv_to_tf b/hydra_ros/app/csv_to_tf deleted file mode 100755 index feec19f..0000000 --- a/hydra_ros/app/csv_to_tf +++ /dev/null @@ -1,162 +0,0 @@ -#!/usr/bin/env python3 -"""Node that broadcasts a CSV as a tf.""" - -import argparse -import json -import pathlib -import sys - -import geometry_msgs.msg -import rclpy -import rclpy.node -import rclpy.utilities -import tf2_ros - -DEFAULT_ORDER = ["x", "y", "z", "qw", "qx", "qy", "qz"] - - -def _parse_file(filepath, parent, child, skip_first=True, order=None, origin=None): - order = DEFAULT_ORDER if order is None else json.loads(order) - origin = json.loads(origin) if origin else [0.0, 0.0, 0.0] - - transforms = [] - have_first_line = False - with open(filepath, "r") as fin: - for line in fin: - if not have_first_line: - have_first_line = True - if skip_first: - continue - - values = [x.strip() for x in line.split(",")] - time_ns = int(values[0]) - pose = {n: float(x) for n, x in zip(order, values[1:])} - - t = geometry_msgs.msg.TransformStamped() - t.header.stamp = rclpy.time.Time(nanoseconds=time_ns).to_msg() - t.header.frame_id = parent - t.child_frame_id = child - t.transform.translation.x = pose["x"] - origin[0] - t.transform.translation.y = pose["y"] - origin[1] - t.transform.translation.z = pose["z"] - origin[2] - t.transform.rotation.w = pose["qw"] - t.transform.rotation.x = pose["qx"] - t.transform.rotation.y = pose["qy"] - t.transform.rotation.z = pose["qz"] - transforms.append(t) - - return transforms - - -class CsvToTfNode(rclpy.node.Node): - """Node that uses a CSV to simulate a live TF.""" - - def __init__(self, transforms): - """Initialize the node.""" - super().__init__("csv_to_tf") - self._idx = 0 - self._transforms = transforms - self._broadcaster = tf2_ros.TransformBroadcaster(self) - - self._offset = self._duration_param("offset_s", 0.0) - self._lookahead = self._duration_param("lookahead_s", 0.0) - self._stale = self._duration_param("stale_threshold_s", 0.5) - period = self._duration_param("poll_period_s", 0.001) - self._timer = self.create_timer(period, self._callback) - - self.get_logger().info(f"Polling clock every {period}") - self.get_logger().info(f"Offsetting transforms by {self._offset}") - self.get_logger().info(f"Using lookahead of {self._lookahead}") - self.get_logger().info(f"Discarding TFs more than {self._stale} behind clock") - - def _duration_param(self, name, default): - val = self._declare_parameter(name, default).get_parameter_value().double_value - return rclpy.durationDuration(seconds=val) - - def _curr_tf_is_stale(self, ros_stamp): - # we want to reject any transforms that are more than stale_threshold_s than the current stamp - curr_stamp = self._get_curr_tf_stamp() - if curr_stamp is None: - return False - - return ros_stamp - curr_stamp > self._stale - - def _get_curr_tf_stamp(self): - if self._idx >= len(self._transforms): - return None - - stamp = rclpy.time.Time.from_msg(self._transforms[self._idx].header.stamp) - return stamp + self._offset - - def _callback(self): - stamp = self.get_clock().now() - stamp_ns = stamp.nanoseconds - - prev_idx = self._idx - while self._curr_tf_is_stale(stamp): - self._idx += 1 - - num_dropped = self._idx - prev_idx - if num_dropped > 0: - self.get_logger().warn(f"Dropped {num_dropped} older than {stamp_ns} [ns]") - - if self._idx >= len(self._transforms): - self.get_logger().info("Finished publishing transforms") - self._timer.cancel() - return - - next_stamp = self._get_curr_tf_stamp() - next_stamp_ns = next_stamp.nanoseconds - diff = next_stamp - stamp - diff_str = f"diff: {diff.nanoseconds} [ns]" - self.get_logger().debug(f"waiting for time {next_stamp_ns} [ns] ({diff_str})") - if diff > self._lookahead: - # if the next TF timestamp is ahead of the current ROS time by more than the lookahead, - # don't publish anything - return - - self.get_logger().debug(f"sending {next_stamp_ns} [ns]") - msg = self._transforms[self._idx] - msg.header.stamp = next_stamp.to_msg() # reset header stamp - self._broadcaster.sendTransform(msg) - self._idx += 1 - - -def main(args=None): - """Do stuff.""" - parser = argparse.ArgumentParser("Node publishing parent_T_child from CSV") - parser.add_argument("filepath") - parser.add_argument("--parent", default="odom", help="frame ID for parent frame") - parser.add_argument("--child", default="base_link", help="frame ID for child frame") - parser.add_argument("--origin", default=None, help="origin to use in json") - parser.add_argument("--column_order", default=None, help="column names in json") - parser.add_argument("--parse_first", action="store_true", help="parse first line") - args, rest = parser.parse_known_args() - - filepath = pathlib.Path(args.filepath).expanduser().absolute() - if not filepath.exists(): - sys.exit(f"Trajectory file {filepath} does not exist!") - - transforms = _parse_file( - filepath, - args.parent, - args.child, - origin=args.origin, - order=args.column_order, - skip_first=not args.parse_first, - ) - - rclpy.init(args=rest) - try: - node = CsvToTfNode(transforms) - node.get_logger().info(f"Publishing {args.parent}_T_{args.child}") - rclpy.spin(node) - node.destroy_node() - except KeyboardInterrupt: - pass - finally: - rclpy.try_shutdown() - - -if __name__ == "__main__": - main()