diff --git a/CHANGELOG.rst b/CHANGELOG.rst
index d3f1295e..90239a4b 100644
--- a/CHANGELOG.rst
+++ b/CHANGELOG.rst
@@ -6,6 +6,9 @@ Changelog
[unreleased]
============
* [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 users 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 @@
+
+
+
+
+
@@ -35,6 +39,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."/>
+
@@ -94,6 +102,7 @@
+
-
+ 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..05b544c2 100644
--- a/launch/replay_pcap.launch
+++ b/launch/replay_pcap.launch
@@ -1,8 +1,13 @@
-
+
+
+
+
+
@@ -29,6 +34,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."/>
+
@@ -71,10 +80,12 @@
+
+
@@ -87,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_pcap_nodelet.cpp b/src/os_pcap_nodelet.cpp
index b2cdab49..54b173cc 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 (ros::ok() && packet_read_active && 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;
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