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;