diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 97c5e72c..f0282402 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -25,6 +25,8 @@ ouster_ros(2) * make specifying metadata file optional during record and replay modes as of package version 8.1 * bugfix: activate metadata topic explicitly * bugfix: remove ``--clock`` option when playing bag files in ros-foxy. +* replace ``tf_prefix`` from ``os_cloud`` with ``sensor_frame``, ``lidar_frame`` and ``imu_frame`` + launch parameters. ouster_client ------------- diff --git a/ouster-ros/config/parameters.yaml b/ouster-ros/config/parameters.yaml index 44189e15..7789016d 100644 --- a/ouster-ros/config/parameters.yaml +++ b/ouster-ros/config/parameters.yaml @@ -12,5 +12,7 @@ ouster/os_sensor: imu_port: 0 ouster/os_cloud: ros__parameters: - tf_prefix: '' + sensor_frame: 'os_sensor' + lidar_frame: 'os_lidar' + imu_frame: 'os_imu' timestamp_mode: '' diff --git a/ouster-ros/launch/replay.independent.launch.xml b/ouster-ros/launch/replay.independent.launch.xml index 96ad1185..1038a738 100644 --- a/ouster-ros/launch/replay.independent.launch.xml +++ b/ouster-ros/launch/replay.independent.launch.xml @@ -18,7 +18,10 @@ description="whether to run a rviz"/> - + + + + @@ -28,8 +31,10 @@ - - + + + + @@ -47,6 +52,9 @@ + + + diff --git a/ouster-ros/launch/rviz.launch.py b/ouster-ros/launch/rviz.launch.py index 56e0e25e..2d753728 100644 --- a/ouster-ros/launch/rviz.launch.py +++ b/ouster-ros/launch/rviz.launch.py @@ -31,6 +31,8 @@ def generate_launch_description(): # 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", diff --git a/ouster-ros/launch/rviz.launch.xml b/ouster-ros/launch/rviz.launch.xml index 995faf5e..82197418 100644 --- a/ouster-ros/launch/rviz.launch.xml +++ b/ouster-ros/launch/rviz.launch.xml @@ -7,6 +7,10 @@ + + + + + args="0 0 0 0 0 0 $(var sensor_frame) $(var imu_frame)"/> + args="0 0 0 0 0 0 $(var sensor_frame) $(var lidar_frame)"/> \ No newline at end of file diff --git a/ouster-ros/launch/sensor.composite.launch.xml b/ouster-ros/launch/sensor.composite.launch.xml index cd2965c1..208f7082 100644 --- a/ouster-ros/launch/sensor.composite.launch.xml +++ b/ouster-ros/launch/sensor.composite.launch.xml @@ -39,7 +39,10 @@ description="whether to run a rviz"/> - + + + + @@ -57,7 +60,9 @@ - + + + @@ -75,6 +80,9 @@ + + + diff --git a/ouster-ros/launch/sensor.independent.launch.xml b/ouster-ros/launch/sensor.independent.launch.xml index a40c09f7..62a1f110 100644 --- a/ouster-ros/launch/sensor.independent.launch.xml +++ b/ouster-ros/launch/sensor.independent.launch.xml @@ -39,7 +39,10 @@ description="whether to run a rviz"/> - + + + + @@ -56,7 +59,9 @@ - + + + @@ -73,6 +78,9 @@ + + + diff --git a/ouster-ros/launch/sensor_mtp.launch.xml b/ouster-ros/launch/sensor_mtp.launch.xml index 7fe115f7..26772709 100644 --- a/ouster-ros/launch/sensor_mtp.launch.xml +++ b/ouster-ros/launch/sensor_mtp.launch.xml @@ -47,7 +47,10 @@ description="whether to run a rviz"/> - + + + + @@ -65,7 +68,9 @@ - + + + @@ -83,6 +88,9 @@ + + + diff --git a/ouster-ros/package.xml b/ouster-ros/package.xml index 17e5385c..3c61e6e8 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 6e4f369b..3b9b34ff 100644 --- a/ouster-ros/src/os_cloud_node.cpp +++ b/ouster-ros/src/os_cloud_node.cpp @@ -105,17 +105,17 @@ class OusterCloud : public OusterProcessingNodeBase { } void declare_parameters() { - declare_parameter("tf_prefix"); + declare_parameter("sensor_frame"); + declare_parameter("lidar_frame"); + declare_parameter("imu_frame"); declare_parameter("timestamp_mode"); } void parse_parameters() { - auto tf_prefix = get_parameter("tf_prefix").as_string(); - if (is_arg_set(tf_prefix) && tf_prefix.back() != '/') - tf_prefix.append("/"); - sensor_frame = tf_prefix + "os_sensor"; - imu_frame = tf_prefix + "os_imu"; - lidar_frame = tf_prefix + "os_lidar"; + sensor_frame = get_parameter("sensor_frame").as_string(); + lidar_frame = get_parameter("lidar_frame").as_string(); + imu_frame = get_parameter("imu_frame").as_string(); + auto timestamp_mode_arg = get_parameter("timestamp_mode").as_string(); use_ros_time = timestamp_mode_arg == "TIME_FROM_ROS_TIME"; }