diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 7b6fdbdf..72735d2b 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -26,6 +26,7 @@ ouster_ros(2) launch parameters. * bugfix: fixed an issue that prevents running multiple instances of the sensor and cloud components in the same process. +* switch to using static transform publisher for the ros2 driver. ouster_client ------------- diff --git a/ouster-ros/launch/record.composite.launch.xml b/ouster-ros/launch/record.composite.launch.xml index a27d47f1..d81f1173 100644 --- a/ouster-ros/launch/record.composite.launch.xml +++ b/ouster-ros/launch/record.composite.launch.xml @@ -81,7 +81,6 @@ - diff --git a/ouster-ros/launch/replay.composite.launch.xml b/ouster-ros/launch/replay.composite.launch.xml index 5789b620..cbca5733 100644 --- a/ouster-ros/launch/replay.composite.launch.xml +++ b/ouster-ros/launch/replay.composite.launch.xml @@ -53,7 +53,6 @@ - diff --git a/ouster-ros/launch/rviz.launch.py b/ouster-ros/launch/rviz.launch.py index 0cb08eec..bf8ca655 100644 --- a/ouster-ros/launch/rviz.launch.py +++ b/ouster-ros/launch/rviz.launch.py @@ -24,30 +24,6 @@ def generate_launch_description(): ouster_ns_arg = DeclareLaunchArgument( 'ouster_ns', default_value='ouster') - enable_static_tf = LaunchConfiguration('_enable_static_tf_publishers') - enable_static_tf_arg = DeclareLaunchArgument( - '_enable_static_tf_publishers', default_value='false') - - # NOTE: the two static tf publishers are rather a workaround to let rviz2 - # get going and not complain while waiting for the actual sensor frames - # to be published that is when running rviz2 using a parent launch file - # TODO: need to be able to propagate the modified frame names from the - # parameters file to RVIZ launch py. - sensor_imu_tf = Node( - package="tf2_ros", - executable="static_transform_publisher", - name="stp_sensor_imu", - namespace=ouster_ns, - condition=IfCondition(enable_static_tf), - arguments=["--frame-id", "os_sensor", "--child-frame-id", "os_imu"]) - sensor_ldr_tf = Node( - package="tf2_ros", - executable="static_transform_publisher", - name="stp_sensor_lidar", - namespace=ouster_ns, - condition=IfCondition(enable_static_tf), - arguments=["--frame-id", "os_sensor", "--child-frame-id", "os_lidar"]) - rviz_node = Node( package='rviz2', namespace=ouster_ns, @@ -59,8 +35,5 @@ def generate_launch_description(): return LaunchDescription([ ouster_ns_arg, rviz_config_arg, - enable_static_tf_arg, - sensor_imu_tf, - sensor_ldr_tf, rviz_node ]) diff --git a/ouster-ros/launch/rviz.launch.xml b/ouster-ros/launch/rviz.launch.xml index b8ce09ce..54b490b7 100644 --- a/ouster-ros/launch/rviz.launch.xml +++ b/ouster-ros/launch/rviz.launch.xml @@ -4,8 +4,6 @@ description="Override the default namespace of all ouster nodes"/> - @@ -15,15 +13,6 @@ - - - \ No newline at end of file diff --git a/ouster-ros/launch/sensor.composite.launch.py b/ouster-ros/launch/sensor.composite.launch.py index a6e00e7f..0db299ac 100644 --- a/ouster-ros/launch/sensor.composite.launch.py +++ b/ouster-ros/launch/sensor.composite.launch.py @@ -77,8 +77,7 @@ def generate_launch_description(): Path(ouster_ros_pkg_dir) / 'launch' / 'rviz.launch.py' rviz_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource([str(rviz_launch_file_path)]), - condition=IfCondition(rviz_enable), - launch_arguments={'_enable_static_tf_publishers': 'true'}.items() + condition=IfCondition(rviz_enable) ) # HACK: to configure and activate the the sensor since state transition diff --git a/ouster-ros/launch/sensor.composite.launch.xml b/ouster-ros/launch/sensor.composite.launch.xml index 208f7082..717cabd0 100644 --- a/ouster-ros/launch/sensor.composite.launch.xml +++ b/ouster-ros/launch/sensor.composite.launch.xml @@ -79,7 +79,6 @@ - diff --git a/ouster-ros/launch/sensor.independent.launch.py b/ouster-ros/launch/sensor.independent.launch.py index 632a619a..8ef67d4f 100644 --- a/ouster-ros/launch/sensor.independent.launch.py +++ b/ouster-ros/launch/sensor.independent.launch.py @@ -105,8 +105,7 @@ def generate_launch_description(): Path(ouster_ros_pkg_dir) / 'launch' / 'rviz.launch.py' rviz_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource([str(rviz_launch_file_path)]), - condition=IfCondition(rviz_enable), - launch_arguments={'_enable_static_tf_publishers': 'true'}.items() + condition=IfCondition(rviz_enable) ) return launch.LaunchDescription([ diff --git a/ouster-ros/launch/sensor.independent.launch.xml b/ouster-ros/launch/sensor.independent.launch.xml index 62a1f110..1bb6f464 100644 --- a/ouster-ros/launch/sensor.independent.launch.xml +++ b/ouster-ros/launch/sensor.independent.launch.xml @@ -77,7 +77,6 @@ - diff --git a/ouster-ros/launch/sensor_mtp.launch.xml b/ouster-ros/launch/sensor_mtp.launch.xml index 5d7885c4..169720b7 100644 --- a/ouster-ros/launch/sensor_mtp.launch.xml +++ b/ouster-ros/launch/sensor_mtp.launch.xml @@ -85,7 +85,6 @@ - diff --git a/ouster-ros/package.xml b/ouster-ros/package.xml index e79b8980..dd589cf4 100644 --- a/ouster-ros/package.xml +++ b/ouster-ros/package.xml @@ -2,7 +2,7 @@ ouster_ros - 0.8.3 + 0.8.4 Ouster ROS2 driver ouster developers BSD diff --git a/ouster-ros/src/os_cloud_node.cpp b/ouster-ros/src/os_cloud_node.cpp index f4267857..ad3f71b0 100644 --- a/ouster-ros/src/os_cloud_node.cpp +++ b/ouster-ros/src/os_cloud_node.cpp @@ -14,7 +14,7 @@ #include #include -#include +#include #include #include @@ -118,6 +118,7 @@ class OusterCloud : public OusterProcessingNodeBase { RCLCPP_INFO(get_logger(), "OusterCloud: retrieved new sensor metadata!"); info = sensor::parse_metadata(metadata_msg->data); + send_static_transforms(); n_returns = get_n_returns(); create_lidarscan_objects(); compute_scan_ts = [this](const auto& ts_v) { @@ -127,6 +128,13 @@ class OusterCloud : public OusterProcessingNodeBase { create_subscriptions(); } + void send_static_transforms() { + tf_bcast.sendTransform(ouster_ros::transform_to_tf_msg( + info.lidar_to_sensor_transform, sensor_frame, lidar_frame, now())); + tf_bcast.sendTransform(ouster_ros::transform_to_tf_msg( + info.imu_to_sensor_transform, sensor_frame, imu_frame, now())); + } + void create_lidarscan_objects() { // The ouster_ros drive currently only uses single precision when it // produces the point cloud. So it isn't of a benefit to compute point @@ -195,9 +203,6 @@ class OusterCloud : public OusterProcessingNodeBase { pc_msg.header.frame_id = sensor_frame; lidar_pubs[i]->publish(pc_msg); } - - tf_bcast.sendTransform(ouster_ros::transform_to_tf_msg( - info.lidar_to_sensor_transform, sensor_frame, lidar_frame, msg_ts)); } uint64_t impute_value(int last_scan_last_nonzero_idx, @@ -316,8 +321,6 @@ class OusterCloud : public OusterProcessingNodeBase { auto imu_msg = ouster_ros::packet_to_imu_msg(*packet, msg_ts, imu_frame, pf); imu_pub->publish(imu_msg); - tf_bcast.sendTransform(ouster_ros::transform_to_tf_msg( - info.imu_to_sensor_transform, sensor_frame, imu_frame, msg_ts)); }; static inline rclcpp::Time to_ros_time(uint64_t ts) { @@ -346,7 +349,7 @@ class OusterCloud : public OusterProcessingNodeBase { std::string imu_frame; std::string lidar_frame; - tf2_ros::TransformBroadcaster tf_bcast; + tf2_ros::StaticTransformBroadcaster tf_bcast; bool use_ros_time;