diff --git a/CHANGELOG.rst b/CHANGELOG.rst
index ecfb9fb4..21794558 100644
--- a/CHANGELOG.rst
+++ b/CHANGELOG.rst
@@ -7,6 +7,10 @@ Changelog
ouster_ros(1)
-------------
* breaking: publish PCL point clouds destaggered.
+* introduced a new launch file parameter ``ptp_utc_tai_offset`` which represent offset in seconds
+ to be applied to all ROS messages the driver generates when ``TIME_FROM_PTP_1588`` timestamp mode
+ is used.
+* fix: destagger columns timestamp when generating destaggered point clouds.
ouster_ros v0.10.0
diff --git a/Dockerfile b/Dockerfile
index 34811a05..f7fed546 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -29,8 +29,8 @@ RUN set -xe \
&& groupadd -o -g ${BUILD_GID} build \
&& useradd -o -u ${BUILD_UID} -d ${BUILD_HOME} -rm -s /bin/bash -g build build
-# Install build dependencies using rosdep
-COPY --chown=build:build package.xml $OUSTER_ROS_PATH/package.xml
+# Set up build environment
+COPY --chown=build:build . $OUSTER_ROS_PATH
RUN set -xe \
&& apt-get update \
@@ -38,9 +38,6 @@ RUN set -xe \
&& rosdep update --rosdistro=$ROS_DISTRO \
&& rosdep install -y --from-paths $OUSTER_ROS_PATH
-# Set up build environment
-COPY --chown=build:build . $OUSTER_ROS_PATH
-
USER build:build
WORKDIR ${BUILD_HOME}
diff --git a/README.md b/README.md
index 042dd3ef..9c97ffdc 100644
--- a/README.md
+++ b/README.md
@@ -2,7 +2,7 @@
[ROS1 (melodic/noetic)](https://github.com/ouster-lidar/ouster-ros/tree/master) |
[ROS2 (rolling/humble/iron)](https://github.com/ouster-lidar/ouster-ros/tree/ros2) |
-[ROS2 (foxy)](https://github.com/ouster-lidar/ouster-ros/tree/ros2-foxy)
+[ROS2 (galactic/foxy)](https://github.com/ouster-lidar/ouster-ros/tree/ros2-foxy)
@@ -10,7 +10,7 @@
|:-----------:|:------:|
| ROS1 (melodic/noetic) | [](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml)
| ROS2 (rolling/humble/iron) | [](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml)
-| ROS2 (foxy) | [](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml)
+| ROS2 (galactic/foxy) | [](https://github.com/ouster-lidar/ouster-ros/actions/workflows/docker-image.yml)
- [Overview](#overview)
- [Requirements](#requirements)
diff --git a/include/ouster_ros/os_ros.h b/include/ouster_ros/os_ros.h
index 8868a9c9..1eb70146 100644
--- a/include/ouster_ros/os_ros.h
+++ b/include/ouster_ros/os_ros.h
@@ -206,7 +206,20 @@ inline ouster::img_t get_or_fill_zero(sensor::ChanField field,
return result;
}
-ros::Time ts_to_ros_time(uint64_t ts);
+/**
+ * simple utility function that ensures we don't wrap around uint64_t due
+ * to a negative value being bigger than ts value in absolute terms.
+ * @remark method does not check upper boundary
+ */
+inline uint64_t ts_safe_offset_add(uint64_t ts, int64_t offset) {
+ return offset < 0 && ts < static_cast(std::abs(offset)) ? 0 : ts + offset;
+}
+
+inline ros::Time ts_to_ros_time(uint64_t ts) {
+ ros::Time t;
+ t.fromNSec(ts);
+ return t;
+}
} // namespace impl
diff --git a/launch/common.launch b/launch/common.launch
index 70c3bc9b..81af6c23 100644
--- a/launch/common.launch
+++ b/launch/common.launch
@@ -17,8 +17,9 @@
would set lidar_frame to be the frame used when publishing these messages."/>
-
+
+
+
@@ -47,6 +48,7 @@
value="$(arg dynamic_transforms_broadcast_rate)"/>
+
diff --git a/launch/driver.launch b/launch/driver.launch
index c3b4f633..7ecc5a82 100644
--- a/launch/driver.launch
+++ b/launch/driver.launch
@@ -25,6 +25,8 @@
TIME_FROM_PTP_1588,
TIME_FROM_ROS_TIME
}"/>
+
@@ -71,6 +73,8 @@
+
diff --git a/launch/record.launch b/launch/record.launch
index df445780..cfa39c3e 100644
--- a/launch/record.launch
+++ b/launch/record.launch
@@ -25,6 +25,8 @@
TIME_FROM_PTP_1588,
TIME_FROM_ROS_TIME
}"/>
+
@@ -89,6 +91,7 @@
+
+
@@ -64,6 +66,7 @@
+
+
+
@@ -92,6 +95,7 @@
+
diff --git a/launch/sensor_mtp.launch b/launch/sensor_mtp.launch
index 39545c15..5f481009 100644
--- a/launch/sensor_mtp.launch
+++ b/launch/sensor_mtp.launch
@@ -29,6 +29,9 @@
TIME_FROM_PTP_1588,
TIME_FROM_ROS_TIME
}"/>
+
+
@@ -98,6 +101,7 @@
+
diff --git a/package.xml b/package.xml
index b4fea8ca..32ed19b4 100644
--- a/package.xml
+++ b/package.xml
@@ -1,7 +1,7 @@
ouster_ros
- 0.9.1
+ 0.9.2
Ouster ROS driver
ouster developers
BSD
diff --git a/src/imu_packet_handler.h b/src/imu_packet_handler.h
index ffd89113..b8349ba9 100644
--- a/src/imu_packet_handler.h
+++ b/src/imu_packet_handler.h
@@ -24,20 +24,31 @@ class ImuPacketHandler {
public:
static HandlerType create_handler(const ouster::sensor::sensor_info& info,
const std::string& frame,
- bool use_ros_time) {
+ const std::string& timestamp_mode,
+ int64_t ptp_utc_tai_offset) {
const auto& pf = ouster::sensor::get_format(info);
using Timestamper = std::function;
- // clang-format off
- auto timestamper = use_ros_time ?
- Timestamper{[](const uint8_t* /*imu_buf*/) {
- return ros::Time::now(); }} :
- Timestamper{[pf](const uint8_t* imu_buf) {
- return impl::ts_to_ros_time(pf.imu_gyro_ts(imu_buf)); }};
- // clang-format on
+ Timestamper timestamper;
+ if (timestamp_mode == "TIME_FROM_ROS_TIME") {
+ timestamper = Timestamper{
+ [](const uint8_t* /*imu_buf*/) { return ros::Time::now(); }};
+ } else if (timestamp_mode == "TIME_FROM_PTP_1588") {
+ timestamper =
+ Timestamper{[pf, ptp_utc_tai_offset](const uint8_t* imu_buf) {
+ uint64_t ts = pf.imu_gyro_ts(imu_buf);
+ ts = impl::ts_safe_offset_add(ts, ptp_utc_tai_offset);
+ return impl::ts_to_ros_time(ts);
+ }};
+ } else {
+ timestamper = Timestamper{[pf](const uint8_t* imu_buf) {
+ return impl::ts_to_ros_time(pf.imu_gyro_ts(imu_buf));
+ }};
+ }
+
return [&pf, &frame, timestamper](const uint8_t* imu_buf) {
return packet_to_imu_msg(pf, timestamper(imu_buf), frame, imu_buf);
};
}
};
-} // namespace ouster_ros
\ No newline at end of file
+} // namespace ouster_ros
\ No newline at end of file
diff --git a/src/lidar_packet_handler.h b/src/lidar_packet_handler.h
index 9ee56005..7effc6ba 100644
--- a/src/lidar_packet_handler.h
+++ b/src/lidar_packet_handler.h
@@ -82,7 +82,6 @@ namespace sensor = ouster::sensor;
using LidarScanProcessor =
std::function;
-
class LidarPacketHandler {
using LidarPacketAccumlator = std::function;
@@ -93,7 +92,10 @@ class LidarPacketHandler {
public:
LidarPacketHandler(const ouster::sensor::sensor_info& info,
- bool use_ros_time) {
+ const std::vector& handlers,
+ const std::string& timestamp_mode,
+ int64_t ptp_utc_tai_offset)
+ : lidar_scan_handlers{handlers} {
// initialize lidar_scan processor and buffer
scan_batcher = std::make_unique(info);
lidar_scan = std::make_unique(
@@ -107,14 +109,23 @@ class LidarPacketHandler {
};
const sensor::packet_format& pf = sensor::get_format(info);
- lidar_packet_accumlator =
- use_ros_time
- ? LidarPacketAccumlator{[this, pf](const uint8_t* lidar_buf) {
- return lidar_handler_ros_time(pf, lidar_buf);
- }}
- : LidarPacketAccumlator{[this, pf](const uint8_t* lidar_buf) {
- return lidar_handler_sensor_time(pf, lidar_buf);
- }};
+ if (timestamp_mode == "TIME_FROM_ROS_TIME") {
+ lidar_packet_accumlator =
+ LidarPacketAccumlator{[this, pf](const uint8_t* lidar_buf) {
+ return lidar_handler_ros_time(pf, lidar_buf);
+ }};
+ } else if (timestamp_mode == "TIME_FROM_PTP_1588") {
+ lidar_packet_accumlator = LidarPacketAccumlator{
+ [this, pf, ptp_utc_tai_offset](const uint8_t* lidar_buf) {
+ return lidar_handler_sensor_time_ptp(pf, lidar_buf,
+ ptp_utc_tai_offset);
+ }};
+ } else {
+ lidar_packet_accumlator =
+ LidarPacketAccumlator{[this, pf](const uint8_t* lidar_buf) {
+ return lidar_handler_sensor_time(pf, lidar_buf);
+ }};
+ }
}
LidarPacketHandler(const LidarPacketHandler&) = delete;
@@ -129,12 +140,11 @@ class LidarPacketHandler {
public:
static HandlerType create_handler(
- const ouster::sensor::sensor_info& info, bool use_ros_time,
- const std::vector& handlers) {
- auto handler = std::make_shared(info, use_ros_time);
-
- handler->lidar_scan_handlers = handlers;
-
+ const ouster::sensor::sensor_info& info,
+ const std::vector& handlers,
+ const std::string& timestamp_mode, int64_t ptp_utc_tai_offset) {
+ auto handler = std::make_shared(
+ info, handlers, timestamp_mode, ptp_utc_tai_offset);
return [handler](const uint8_t* lidar_buf) {
if (handler->lidar_packet_accumlator(lidar_buf)) {
for (auto h : handler->lidar_scan_handlers) {
@@ -202,7 +212,6 @@ class LidarPacketHandler {
assert(idx != ts_v.data() + ts_v.size()); // should never happen
int curr_scan_first_nonzero_idx = idx - ts_v.data();
uint64_t curr_scan_first_nonzero_value = *idx;
-
uint64_t scan_ns = curr_scan_first_nonzero_idx == 0
? curr_scan_first_nonzero_value
: impute_value(last_scan_last_nonzero_idx,
@@ -210,7 +219,6 @@ class LidarPacketHandler {
curr_scan_first_nonzero_idx,
curr_scan_first_nonzero_value,
static_cast(ts_v.size()));
-
last_scan_last_nonzero_idx =
find_if_reverse(ts_v, [](uint64_t h) { return h != 0; });
assert(last_scan_last_nonzero_idx >= 0); // should never happen
@@ -242,6 +250,19 @@ class LidarPacketHandler {
return true;
}
+ bool lidar_handler_sensor_time_ptp(const sensor::packet_format&,
+ const uint8_t* lidar_buf,
+ int64_t ptp_utc_tai_offset) {
+ if (!(*scan_batcher)(lidar_buf, *lidar_scan)) return false;
+ auto ts_v = lidar_scan->timestamp();
+ for (int i = 0; i < ts_v.rows(); ++i)
+ ts_v[i] = impl::ts_safe_offset_add(ts_v[i], ptp_utc_tai_offset);
+ lidar_scan_estimated_ts = compute_scan_ts(ts_v);
+ lidar_scan_estimated_msg_ts =
+ impl::ts_to_ros_time(lidar_scan_estimated_ts);
+ return true;
+ }
+
bool lidar_handler_ros_time(const sensor::packet_format& pf,
const uint8_t* lidar_buf) {
auto packet_receive_time = ros::Time::now();
@@ -289,4 +310,4 @@ class LidarPacketHandler {
LidarPacketAccumlator lidar_packet_accumlator;
};
-} // namespace ouster_ros
\ No newline at end of file
+} // namespace ouster_ros
\ No newline at end of file
diff --git a/src/os_cloud_nodelet.cpp b/src/os_cloud_nodelet.cpp
index b6ea552d..75deb21f 100644
--- a/src/os_cloud_nodelet.cpp
+++ b/src/os_cloud_nodelet.cpp
@@ -93,15 +93,16 @@ class OusterCloud : public nodelet::Nodelet {
auto proc_mask = pnh.param("proc_mask", std::string{"IMU|PCL|SCAN"});
auto tokens = parse_tokens(proc_mask, '|');
- auto timestamp_mode_arg = pnh.param("timestamp_mode", std::string{});
- bool use_ros_time = timestamp_mode_arg == "TIME_FROM_ROS_TIME";
+ auto timestamp_mode = pnh.param("timestamp_mode", std::string{});
+ double ptp_utc_tai_offset = pnh.param("ptp_utc_tai_offset", -37.0);
auto& nh = getNodeHandle();
if (check_token(tokens, "IMU")) {
imu_pub = nh.advertise("imu", 100);
imu_packet_handler = ImuPacketHandler::create_handler(
- info, tf_bcast.imu_frame_id(), use_ros_time);
+ info, tf_bcast.imu_frame_id(), timestamp_mode,
+ static_cast(ptp_utc_tai_offset * 1e+9));
imu_packet_sub = nh.subscribe(
"imu_packets", 100, [this](const PacketMsg::ConstPtr msg) {
auto imu_msg = imu_packet_handler(msg->buf.data());
@@ -152,12 +153,8 @@ class OusterCloud : public nodelet::Nodelet {
}
processors.push_back(LaserScanProcessor::create(
- info,
- tf_bcast
- .point_cloud_frame_id(), // TODO: should we allow having a
- // different frame for the laser
- // scan than point cloud???
- scan_ring, [this](LaserScanProcessor::OutputType msgs) {
+ info, tf_bcast.lidar_frame_id(), scan_ring,
+ [this](LaserScanProcessor::OutputType msgs) {
for (size_t i = 0; i < msgs.size(); ++i) {
if (msgs[i]->header.stamp > last_msg_ts)
last_msg_ts = msgs[i]->header.stamp;
@@ -168,7 +165,8 @@ class OusterCloud : public nodelet::Nodelet {
if (check_token(tokens, "PCL") || check_token(tokens, "SCAN")) {
lidar_packet_handler = LidarPacketHandler::create_handler(
- info, use_ros_time, processors);
+ info, processors, timestamp_mode,
+ static_cast(ptp_utc_tai_offset * 1e+9));
lidar_packet_sub = nh.subscribe(
"lidar_packets", 100, [this](const PacketMsg::ConstPtr msg) {
lidar_packet_handler(msg->buf.data());
diff --git a/src/os_driver_nodelet.cpp b/src/os_driver_nodelet.cpp
index 65d15074..8c602cee 100644
--- a/src/os_driver_nodelet.cpp
+++ b/src/os_driver_nodelet.cpp
@@ -52,15 +52,16 @@ class OusterDriver : public OusterSensor {
pnh.param("proc_mask", std::string{"IMU|IMG|PCL|SCAN"});
auto tokens = parse_tokens(proc_mask, '|');
- auto timestamp_mode_arg = pnh.param("timestamp_mode", std::string{});
- bool use_ros_time = timestamp_mode_arg == "TIME_FROM_ROS_TIME";
+ auto timestamp_mode = pnh.param("timestamp_mode", std::string{});
+ double ptp_utc_tai_offset = pnh.param("ptp_utc_tai_offset", -37.0);
auto& nh = getNodeHandle();
if (check_token(tokens, "IMU")) {
imu_pub = nh.advertise("imu", 100);
imu_packet_handler = ImuPacketHandler::create_handler(
- info, tf_bcast.imu_frame_id(), use_ros_time);
+ info, tf_bcast.imu_frame_id(), timestamp_mode,
+ static_cast(ptp_utc_tai_offset * 1e+9));
}
int num_returns = get_n_returns(info);
@@ -102,12 +103,8 @@ class OusterDriver : public OusterSensor {
}
processors.push_back(LaserScanProcessor::create(
- info,
- tf_bcast
- .point_cloud_frame_id(), // TODO: should we allow having a
- // different frame for the laser
- // scan than point cloud???
- scan_ring, [this](LaserScanProcessor::OutputType msgs) {
+ info, tf_bcast.lidar_frame_id(), scan_ring,
+ [this](LaserScanProcessor::OutputType msgs) {
for (size_t i = 0; i < msgs.size(); ++i) {
scan_pubs[i].publish(*msgs[i]);
}
@@ -151,7 +148,8 @@ class OusterDriver : public OusterSensor {
if (check_token(tokens, "PCL") || check_token(tokens, "SCAN") ||
check_token(tokens, "IMG"))
lidar_packet_handler = LidarPacketHandler::create_handler(
- info, use_ros_time, processors);
+ info, processors, timestamp_mode,
+ static_cast(ptp_utc_tai_offset * 1e+9));
}
virtual void on_lidar_packet_msg(const uint8_t* raw_lidar_packet) override {
diff --git a/src/os_ros.cpp b/src/os_ros.cpp
index 10fe847a..63e88593 100644
--- a/src/os_ros.cpp
+++ b/src/os_ros.cpp
@@ -112,12 +112,6 @@ sensor::ChanField suitable_return(sensor::ChanField input_field, bool second) {
}
}
-ros::Time ts_to_ros_time(uint64_t ts) {
- ros::Time t;
- t.fromNSec(ts);
- return t;
-}
-
} // namespace impl
template & near_ir, const ouster::img_t& signal,
const std::vector& pixel_shift_by_row) {
auto timestamp = ls.timestamp();
-
const auto rg = range.data();
const auto rf = reflectivity.data();
const auto nr = near_ir.data();
@@ -216,11 +209,9 @@ void copy_scan_to_cloud_destaggered(
#endif
for (auto u = 0; u < ls.h; u++) {
for (auto v = 0; v < ls.w; v++) {
- const auto ts_src_idx = (v + ls.w - pixel_shift_by_row[u]) % ls.w;
- const auto col_ts = timestamp[ts_src_idx];
- const auto ts = col_ts > scan_ts ? col_ts - scan_ts : 0UL;
- const auto src_idx =
- u * ls.w + (v + ls.w - pixel_shift_by_row[u]) % ls.w;
+ const auto v_shift = (v + ls.w - pixel_shift_by_row[u]) % ls.w;
+ auto ts = timestamp[v_shift]; ts = ts > scan_ts ? ts - scan_ts : 0UL;
+ const auto src_idx = u * ls.w + v_shift;
const auto tgt_idx = u * ls.w + v;
const auto xyz = points.row(src_idx);
cloud.points[tgt_idx] = ouster_ros::Point{
diff --git a/src/os_sensor_nodelet_base.cpp b/src/os_sensor_nodelet_base.cpp
index b4681c62..6f4b1aec 100644
--- a/src/os_sensor_nodelet_base.cpp
+++ b/src/os_sensor_nodelet_base.cpp
@@ -6,13 +6,13 @@
* @brief implementation of OusterSensorNodeletBase interface
*/
-#include
-
#include "ouster_ros/os_sensor_nodelet_base.h"
#include
#include
+#include
+
#include "ouster_ros/GetMetadata.h"
namespace sensor = ouster::sensor;
diff --git a/src/os_transforms_broadcaster.h b/src/os_transforms_broadcaster.h
index 7213b553..ca6f12d2 100644
--- a/src/os_transforms_broadcaster.h
+++ b/src/os_transforms_broadcaster.h
@@ -89,6 +89,9 @@ class OusterTransformsBroadcaster {
}
const std::string& imu_frame_id() const { return imu_frame; }
+ const std::string& lidar_frame_id() const { return lidar_frame; }
+ const std::string& sensor_frame_id() const { return sensor_frame; }
+
const std::string& point_cloud_frame_id() const {
return point_cloud_frame;
}
@@ -100,9 +103,9 @@ class OusterTransformsBroadcaster {
std::string node_name;
tf2_ros::StaticTransformBroadcaster static_tf_bcast;
tf2_ros::TransformBroadcaster tf_bcast; // non-static
- std::string sensor_frame;
std::string imu_frame;
std::string lidar_frame;
+ std::string sensor_frame;
std::string point_cloud_frame;
};
diff --git a/src/point_cloud_processor.h b/src/point_cloud_processor.h
index 7990c31a..f8e0fa0f 100644
--- a/src/point_cloud_processor.h
+++ b/src/point_cloud_processor.h
@@ -62,9 +62,9 @@ class PointCloudProcessor {
void process(const ouster::LidarScan& lidar_scan, uint64_t scan_ts,
const ros::Time& msg_ts) {
for (int i = 0; i < static_cast(pc_msgs.size()); ++i) {
- scan_to_cloud_f_destaggered(cloud,
- points, lut_direction, lut_offset,
- scan_ts, lidar_scan, pixel_shift_by_row, i);
+ scan_to_cloud_f_destaggered(cloud, points, lut_direction,
+ lut_offset, scan_ts, lidar_scan,
+ pixel_shift_by_row, i);
pcl_toROSMsg(cloud, *pc_msgs[i]);
pc_msgs[i]->header.stamp = msg_ts;
pc_msgs[i]->header.frame_id = frame;
@@ -105,4 +105,4 @@ class PointCloudProcessor {
PostProcessingFn post_processing_fn;
};
-} // namespace ouster_ros
\ No newline at end of file
+} // namespace ouster_ros
\ No newline at end of file