From 0ed1cb8d503bdd23b914376bc7f0f2f4b54d9f1a Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Mon, 28 Oct 2024 16:33:19 -0700 Subject: [PATCH 1/5] Remap metadata topic + add loop to pcap + add play delay & play rate --- launch/replay.launch | 11 ++++++++--- launch/replay_pcap.launch | 11 +++++++++-- src/os_pcap_nodelet.cpp | 16 ++++++++++++---- 3 files changed, 29 insertions(+), 9 deletions(-) diff --git a/launch/replay.launch b/launch/replay.launch index 6bd3e5bc..0d2ca632 100644 --- a/launch/replay.launch +++ b/launch/replay.launch @@ -3,9 +3,13 @@ + + + + @@ -111,9 +115,10 @@ - + args="--clock $(arg bag_file) $(arg _loop) --rate $(arg play_rate)"/> diff --git a/launch/replay_pcap.launch b/launch/replay_pcap.launch index f6e957e4..bcf681d2 100644 --- a/launch/replay_pcap.launch +++ b/launch/replay_pcap.launch @@ -1,8 +1,13 @@ - + + + + + @@ -71,10 +76,12 @@ + + diff --git a/src/os_pcap_nodelet.cpp b/src/os_pcap_nodelet.cpp index b2cdab49..8b7c486e 100644 --- a/src/os_pcap_nodelet.cpp +++ b/src/os_pcap_nodelet.cpp @@ -35,6 +35,10 @@ class OusterPcap : public OusterSensorNodeletBase { virtual void onInit() override { auto meta_file = get_meta_file(); auto pcap_file = get_pcap_file(); + loop = getPrivateNodeHandle().param("loop", false); + progress_update_freq = getPrivateNodeHandle().param("progress_update_freq", 1.0); + if (progress_update_freq < 0.001) + progress_update_freq = 0.001; create_metadata_pub(); load_metadata_from_file(meta_file); allocate_buffers(); @@ -104,10 +108,12 @@ class OusterPcap : public OusterSensorNodeletBase { packet_read_active = true; packet_read_thread = std::make_unique([this]() { auto& pf = sensor::get_format(info); - while (packet_read_active) { + do { read_packets(*pcap, pf); - } + pcap->reset(); + } while (loop); NODELET_DEBUG("packet_read_thread done."); + ros::shutdown(); }); } @@ -125,9 +131,9 @@ class OusterPcap : public OusterSensorNodeletBase { auto file_start = packet_info.timestamp; auto last_update = file_start; using namespace std::chrono_literals; - const auto UPDATE_PERIOD = duration_cast(1s / 3); + const auto UPDATE_PERIOD = duration_cast(1s / progress_update_freq); - while (ros::ok() && payload_size) { + while (ros::ok() && packet_read_active && payload_size) { auto start = high_resolution_clock::now(); if (packet_info.dst_port == info.config.udp_port_imu) { std::memcpy(imu_packet.buf.data(), pcap.current_data(), @@ -167,6 +173,8 @@ class OusterPcap : public OusterSensorNodeletBase { PacketMsg imu_packet; ros::Publisher lidar_packet_pub; ros::Publisher imu_packet_pub; + bool loop; + double progress_update_freq; std::atomic packet_read_active = {false}; std::unique_ptr packet_read_thread; From 60ed5431084d606fb4a0a1dc44b3666182845a15 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Mon, 28 Oct 2024 16:41:17 -0700 Subject: [PATCH 2/5] Set default delay amount to zero --- launch/replay.launch | 2 +- launch/replay_pcap.launch | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/launch/replay.launch b/launch/replay.launch index 0d2ca632..c1e50c2d 100644 --- a/launch/replay.launch +++ b/launch/replay.launch @@ -3,7 +3,7 @@ - + diff --git a/launch/replay_pcap.launch b/launch/replay_pcap.launch index bcf681d2..2c754cd0 100644 --- a/launch/replay_pcap.launch +++ b/launch/replay_pcap.launch @@ -4,7 +4,7 @@ - + From 7a2387974353363cd31e43452d333e101965c285 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Tue, 29 Oct 2024 16:42:10 -0700 Subject: [PATCH 3/5] Update CHANGELOG --- CHANGELOG.rst | 1 + launch/replay_pcap.launch | 2 +- src/os_pcap_nodelet.cpp | 2 +- 3 files changed, 3 insertions(+), 2 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index d3f1295e..51dd6477 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -6,6 +6,7 @@ Changelog [unreleased] ============ * [BUGFIX]: correctly align timestamps to the generated point cloud +* Added support to enable **loop** for pcap replay + other replay config ouster_ros v0.13.0 diff --git a/launch/replay_pcap.launch b/launch/replay_pcap.launch index 2c754cd0..abe4fee7 100644 --- a/launch/replay_pcap.launch +++ b/launch/replay_pcap.launch @@ -6,7 +6,7 @@ + doc="playback progress update frequency per second"/> diff --git a/src/os_pcap_nodelet.cpp b/src/os_pcap_nodelet.cpp index 8b7c486e..54b173cc 100644 --- a/src/os_pcap_nodelet.cpp +++ b/src/os_pcap_nodelet.cpp @@ -111,7 +111,7 @@ class OusterPcap : public OusterSensorNodeletBase { do { read_packets(*pcap, pf); pcap->reset(); - } while (loop); + } while (ros::ok() && packet_read_active && loop); NODELET_DEBUG("packet_read_thread done."); ros::shutdown(); }); From 83af8d4e676650546d3f686d1cb6413e20539897 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Wed, 30 Oct 2024 08:49:46 -0700 Subject: [PATCH 4/5] Add a parameter to enable/disable broadcast of sensor transfroms --- CHANGELOG.rst | 2 ++ launch/common.launch | 5 +++++ launch/driver.launch | 5 +++++ launch/record.launch | 5 +++++ launch/replay.launch | 5 +++++ launch/replay_pcap.launch | 5 +++++ launch/sensor.launch | 5 +++++ launch/sensor_mtp.launch | 5 +++++ package.xml | 2 +- src/os_cloud_nodelet.cpp | 30 ++++++++++++++++-------------- src/os_driver_nodelet.cpp | 4 +++- src/os_transforms_broadcaster.h | 4 ++++ 12 files changed, 61 insertions(+), 16 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 51dd6477..57a28d01 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -7,6 +7,8 @@ Changelog ============ * [BUGFIX]: correctly align timestamps to the generated point cloud * Added support to enable **loop** for pcap replay + other replay config +* Added a new launch file parameter ``pub_static_tf`` that allows uers to turn off the broadcast + of sensor TF transforms. ouster_ros v0.13.0 diff --git a/launch/common.launch b/launch/common.launch index 8ab62420..e3347b81 100644 --- a/launch/common.launch +++ b/launch/common.launch @@ -12,6 +12,10 @@ + @@ -56,6 +60,7 @@ + diff --git a/launch/driver.launch b/launch/driver.launch index ae62eaef..ee8ce955 100644 --- a/launch/driver.launch +++ b/launch/driver.launch @@ -46,6 +46,10 @@ doc="which frame to be used when publishing PointCloud2 or LaserScan messages. Choose between the value of sensor_frame or lidar_frame, leaving this value empty would set lidar_frame to be the frame used when publishing these messages."/> + @@ -119,6 +123,7 @@ + diff --git a/launch/record.launch b/launch/record.launch index edd74a2d..d898e030 100644 --- a/launch/record.launch +++ b/launch/record.launch @@ -44,6 +44,10 @@ doc="which frame to be used when publishing PointCloud2 or LaserScan messages. Choose between the value of sensor_frame or lidar_frame, leaving this value empty would set lidar_frame to be the frame used when publishing these messages."/> + @@ -123,6 +127,7 @@ + + @@ -98,6 +102,7 @@ + + @@ -94,6 +98,7 @@ + + @@ -140,6 +144,7 @@ + diff --git a/launch/sensor_mtp.launch b/launch/sensor_mtp.launch index 142165ac..b200ca19 100644 --- a/launch/sensor_mtp.launch +++ b/launch/sensor_mtp.launch @@ -51,6 +51,10 @@ doc="which frame to be used when publishing PointCloud2 or LaserScan messages. Choose between the value of sensor_frame or lidar_frame, leaving this value empty would set lidar_frame to be the frame used when publishing these messages."/> + @@ -146,6 +150,7 @@ + diff --git a/package.xml b/package.xml index 5d3715d9..e09a1602 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ ouster_ros - 0.13.1 + 0.13.2 Ouster ROS driver ouster developers BSD diff --git a/src/os_cloud_nodelet.cpp b/src/os_cloud_nodelet.cpp index 189a5bb1..d3c81c09 100644 --- a/src/os_cloud_nodelet.cpp +++ b/src/os_cloud_nodelet.cpp @@ -76,20 +76,22 @@ class OusterCloud : public nodelet::Nodelet { dynamic_transforms_rate = 1.0; } - if (!dynamic_transforms) { - NODELET_INFO("OusterCloud: using static transforms broadcast"); - tf_bcast.broadcast_transforms(info); - } else { - NODELET_INFO_STREAM( - "OusterCloud: dynamic transforms broadcast enabled with " - "broadcast rate of: " - << dynamic_transforms_rate << " Hz"); - timer_.stop(); - timer_ = getNodeHandle().createTimer( - ros::Duration(1.0 / dynamic_transforms_rate), - [this, info](const ros::TimerEvent&) { - tf_bcast.broadcast_transforms(info, last_msg_ts); - }); + if (tf_bcast.publish_static_tf()) { + if (!dynamic_transforms) { + NODELET_INFO("OusterCloud: using static transforms broadcast"); + tf_bcast.broadcast_transforms(info); + } else { + NODELET_INFO_STREAM( + "OusterCloud: dynamic transforms broadcast enabled with " + "broadcast rate of: " + << dynamic_transforms_rate << " Hz"); + timer_.stop(); + timer_ = getNodeHandle().createTimer( + ros::Duration(1.0 / dynamic_transforms_rate), + [this, info](const ros::TimerEvent&) { + tf_bcast.broadcast_transforms(info, last_msg_ts); + }); + } } create_handlers(info); diff --git a/src/os_driver_nodelet.cpp b/src/os_driver_nodelet.cpp index 13129a9e..34984915 100644 --- a/src/os_driver_nodelet.cpp +++ b/src/os_driver_nodelet.cpp @@ -62,7 +62,9 @@ class OusterDriver : public OusterSensor { // for OusterDriver we are going to always assume static broadcast // at least for now tf_bcast.parse_parameters(getPrivateNodeHandle()); - tf_bcast.broadcast_transforms(info); + if (tf_bcast.publish_static_tf()) { + tf_bcast.broadcast_transforms(info); + } create_handlers(); } diff --git a/src/os_transforms_broadcaster.h b/src/os_transforms_broadcaster.h index c4b6b215..4bd4bdb5 100644 --- a/src/os_transforms_broadcaster.h +++ b/src/os_transforms_broadcaster.h @@ -32,6 +32,7 @@ class OusterTransformsBroadcaster { lidar_frame = pnh.param("lidar_frame", std::string{"os_lidar"}); imu_frame = pnh.param("imu_frame", std::string{"os_imu"}); point_cloud_frame = pnh.param("point_cloud_frame", std::string{}); + pub_static_tf = pnh.param("pub_static_tf", true); if (!is_arg_set(sensor_frame) || !is_arg_set(lidar_frame) || !is_arg_set(imu_frame)) { @@ -100,6 +101,8 @@ class OusterTransformsBroadcaster { return point_cloud_frame == sensor_frame; } + bool publish_static_tf() const { return pub_static_tf; } + private: std::string node_name; tf2_ros::StaticTransformBroadcaster static_tf_bcast; @@ -108,6 +111,7 @@ class OusterTransformsBroadcaster { std::string lidar_frame; std::string sensor_frame; std::string point_cloud_frame; + bool pub_static_tf; }; } // namespace ouster_ros From 1d7d7eec80cb5f350319ae27abb4dcc84d610be7 Mon Sep 17 00:00:00 2001 From: Ussama Naal Date: Wed, 30 Oct 2024 08:53:10 -0700 Subject: [PATCH 5/5] CHANGELOG typo --- CHANGELOG.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 57a28d01..90239a4b 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -7,7 +7,7 @@ Changelog ============ * [BUGFIX]: correctly align timestamps to the generated point cloud * Added support to enable **loop** for pcap replay + other replay config -* Added a new launch file parameter ``pub_static_tf`` that allows uers to turn off the broadcast +* Added a new launch file parameter ``pub_static_tf`` that allows users to turn off the broadcast of sensor TF transforms.