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 6f8954a..0000000 --- a/hydra_ros/app/csv_to_tf +++ /dev/null @@ -1,150 +0,0 @@ -#!/usr/bin/env python3 -"""Node that broadcasts a CSV as a tf.""" - -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(trajectory_file, parent, child, skip_first=True, order=None): - if order is None: - order = DEFAULT_ORDER - - transforms = [] - have_first_line = False - with open(trajectory_file, "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 = [float(x) for x in 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] - transforms.append(t) - - return transforms - - -class CsvToTf(rclpy.node.Node): - def __init__(self): - super().__init__("csv_to_tf") - 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.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() - - 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_threshold - - 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() - - 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} TFs older than {stamp.nanoseconds} [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() - 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: - return - - self.get_logger().debug(f"sending {next_stamp.nanoseconds} [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.""" - rclpy.init(args=args) - try: - node = CsvToTf() - rclpy.spin(node) - node.destroy_node() - finally: - rclpy.utilities.try_shutdown() - - -if __name__ == "__main__": - main()