diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 1c82e7e6..6c0d739d 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -29,6 +29,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.independent.launch.xml b/ouster-ros/launch/record.independent.launch.xml index 16a234d8..7c03f590 100644 --- a/ouster-ros/launch/record.independent.launch.xml +++ b/ouster-ros/launch/record.independent.launch.xml @@ -74,7 +74,6 @@ - diff --git a/ouster-ros/launch/replay.independent.launch.xml b/ouster-ros/launch/replay.independent.launch.xml index 1038a738..787ffac6 100644 --- a/ouster-ros/launch/replay.independent.launch.xml +++ b/ouster-ros/launch/replay.independent.launch.xml @@ -51,7 +51,6 @@ - diff --git a/ouster-ros/launch/rviz.launch.py b/ouster-ros/launch/rviz.launch.py index 2d753728..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=["0", "0", "0", "0", "0", "0", "os_sensor", "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=["0", "0", "0", "0", "0", "0", "os_sensor", "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 82197418..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 26772709..77af0335 100644 --- a/ouster-ros/launch/sensor_mtp.launch.xml +++ b/ouster-ros/launch/sensor_mtp.launch.xml @@ -87,7 +87,6 @@ - diff --git a/ouster-ros/package.xml b/ouster-ros/package.xml index 9935bb60..0d52fe14 100644 --- a/ouster-ros/package.xml +++ b/ouster-ros/package.xml @@ -2,7 +2,7 @@ ouster_ros - 0.8.5 + 0.8.6 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 0a1b7d5d..3bdcb241 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 @@ -95,6 +95,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) { @@ -104,6 +105,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 declare_parameters() { declare_parameter("sensor_frame"); declare_parameter("lidar_frame"); @@ -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;