Skip to content

Commit

Permalink
fix: Use the stable packet data copy for callback processing in Ouste…
Browse files Browse the repository at this point in the history
…rDriver as well.

* Utilizing the PacketMsg data structure to have the pointed-to buffer data transferred immediately once received and use the stable copy for further processing, instead of a reference to potentially mutable underlying data.
  • Loading branch information
Imaniac230 committed Mar 6, 2024
1 parent ed06e57 commit 455aff2
Show file tree
Hide file tree
Showing 3 changed files with 22 additions and 10 deletions.
8 changes: 4 additions & 4 deletions ouster-ros/src/os_driver_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -186,13 +186,13 @@ class OusterDriver : public OusterSensor {
static_cast<int64_t>(ptp_utc_tai_offset * 1e+9));
}

virtual void on_lidar_packet_msg(const uint8_t* raw_lidar_packet) override {
if (lidar_packet_handler) lidar_packet_handler(raw_lidar_packet);
void process_lidar_packet() override {
if (lidar_packet_handler) lidar_packet_handler(lidar_packet.buf.data());
}

virtual void on_imu_packet_msg(const uint8_t* raw_imu_packet) override {
void process_imu_packet() override {
if (imu_packet_handler)
imu_pub->publish(imu_packet_handler(raw_imu_packet));
imu_pub->publish(imu_packet_handler(imu_packet.buf.data()));
}

virtual void cleanup() override {
Expand Down
10 changes: 8 additions & 2 deletions ouster-ros/src/os_sensor_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -840,7 +840,7 @@ void OusterSensor::on_lidar_packet_msg(const uint8_t* raw_lidar_packet) {
// now we are focusing on optimizing the code for OusterDriver
std::memcpy(lidar_packet.buf.data(), raw_lidar_packet,
lidar_packet.buf.size());
lidar_packet_pub->publish(lidar_packet);
process_lidar_packet();
}

void OusterSensor::on_imu_packet_msg(const uint8_t* raw_imu_packet) {
Expand All @@ -849,9 +849,15 @@ void OusterSensor::on_imu_packet_msg(const uint8_t* raw_imu_packet) {
// OusterSensor has its own RingBuffer of PacketMsg but for
// now we are focusing on optimizing the code for OusterDriver
std::memcpy(imu_packet.buf.data(), raw_imu_packet, imu_packet.buf.size());
imu_packet_pub->publish(imu_packet);
process_imu_packet();
}

void OusterSensor::process_lidar_packet() {
lidar_packet_pub->publish(lidar_packet);
}

void OusterSensor::process_imu_packet() { imu_packet_pub->publish(imu_packet); }

} // namespace ouster_ros

#include <rclcpp_components/register_node_macro.hpp>
Expand Down
14 changes: 10 additions & 4 deletions ouster-ros/src/os_sensor_node.h
Original file line number Diff line number Diff line change
Expand Up @@ -61,9 +61,9 @@ class OusterSensor : public OusterSensorNodeBase {

virtual void create_publishers();

virtual void on_lidar_packet_msg(const uint8_t* raw_lidar_packet);
virtual void process_lidar_packet();

virtual void on_imu_packet_msg(const uint8_t* raw_imu_packet);
virtual void process_imu_packet();

virtual void cleanup();

Expand Down Expand Up @@ -141,15 +141,21 @@ class OusterSensor : public OusterSensorNodeBase {

void stop_packet_processing_threads();

void on_lidar_packet_msg(const uint8_t* raw_lidar_packet);

void on_imu_packet_msg(const uint8_t* raw_imu_packet);

protected:
ouster_sensor_msgs::msg::PacketMsg lidar_packet;
ouster_sensor_msgs::msg::PacketMsg imu_packet;

private:
std::string sensor_hostname;
std::string staged_config;
std::string active_config;
std::string mtp_dest;
bool mtp_main;
std::shared_ptr<sensor::client> sensor_client;
ouster_sensor_msgs::msg::PacketMsg lidar_packet;
ouster_sensor_msgs::msg::PacketMsg imu_packet;
rclcpp_lifecycle::LifecyclePublisher<ouster_sensor_msgs::msg::PacketMsg>::SharedPtr
lidar_packet_pub;
rclcpp_lifecycle::LifecyclePublisher<ouster_sensor_msgs::msg::PacketMsg>::SharedPtr
Expand Down

0 comments on commit 455aff2

Please sign in to comment.