diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 505365dd..ea55f61b 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,9 +2,11 @@ Changelog ========= -ouster_ros v0.13.1 +ouster_ros v0.13.2 ================== * [BUGFIX]: Make sure to initialize the sensor with launch file parameters. +* [BUGFIX]: ``os_driver`` failed when RAW option is used. + ouster_ros v0.13.0 ================== diff --git a/ouster-ros/package.xml b/ouster-ros/package.xml index f29616b3..b00538b4 100644 --- a/ouster-ros/package.xml +++ b/ouster-ros/package.xml @@ -2,7 +2,7 @@ ouster_ros - 0.13.1 + 0.13.2 Ouster ROS2 driver ouster developers BSD diff --git a/ouster-ros/src/os_driver_node.cpp b/ouster-ros/src/os_driver_node.cpp index 7e6be202..0a198804 100644 --- a/ouster-ros/src/os_driver_node.cpp +++ b/ouster-ros/src/os_driver_node.cpp @@ -200,12 +200,14 @@ class OusterDriver : public OusterSensor { static_cast(ptp_utc_tai_offset * 1e+9)); publish_raw = impl::check_token(tokens, "RAW"); + if (publish_raw) + OusterSensor::create_publishers(); } virtual void on_lidar_packet_msg(const LidarPacket& lidar_packet) override { if (lidar_packet_handler) lidar_packet_handler(lidar_packet); - + if (publish_raw) OusterSensor::on_lidar_packet_msg(lidar_packet); } @@ -213,8 +215,8 @@ class OusterDriver : public OusterSensor { virtual void on_imu_packet_msg(const ImuPacket& imu_packet) override { if (imu_packet_handler) imu_pub->publish(imu_packet_handler(imu_packet)); - - if(publish_raw) + + if (publish_raw) OusterSensor::on_imu_packet_msg(imu_packet); } diff --git a/ouster-ros/src/os_pcap_node.cpp b/ouster-ros/src/os_pcap_node.cpp index f984a64a..0b458c1c 100644 --- a/ouster-ros/src/os_pcap_node.cpp +++ b/ouster-ros/src/os_pcap_node.cpp @@ -75,8 +75,6 @@ class OusterPcap : public OusterSensorNodeBase { RCLCPP_DEBUG(get_logger(), "on_activate() is called."); LifecycleNode::on_activate(state); create_publishers(); - if (imu_packet_pub) imu_packet_pub->on_activate(); - if (lidar_packet_pub) lidar_packet_pub->on_activate(); allocate_buffers(); start_packet_read_thread(); return LifecycleNodeInterface::CallbackReturn::SUCCESS; @@ -278,10 +276,8 @@ class OusterPcap : public OusterSensorNodeBase { std::shared_ptr pcap; ouster_sensor_msgs::msg::PacketMsg lidar_packet; ouster_sensor_msgs::msg::PacketMsg imu_packet; - rclcpp_lifecycle::LifecyclePublisher::SharedPtr - lidar_packet_pub; - rclcpp_lifecycle::LifecyclePublisher::SharedPtr - imu_packet_pub; + rclcpp::Publisher::SharedPtr lidar_packet_pub; + rclcpp::Publisher::SharedPtr imu_packet_pub; std::atomic packet_read_active = {false}; std::unique_ptr packet_read_thread; diff --git a/ouster-ros/src/os_sensor_node.cpp b/ouster-ros/src/os_sensor_node.cpp index fee03697..9426aaff 100644 --- a/ouster-ros/src/os_sensor_node.cpp +++ b/ouster-ros/src/os_sensor_node.cpp @@ -769,11 +769,10 @@ void OusterSensor::create_publishers() { void OusterSensor::allocate_buffers() { auto& pf = sensor::get_format(info); - - lidar_packet_msg.buf.resize(pf.lidar_packet_size); lidar_packet.buf.resize(pf.lidar_packet_size); + lidar_packet_msg.buf.resize(pf.lidar_packet_size); imu_packet.buf.resize(pf.imu_packet_size); - imu_packet.buf.resize(pf.imu_packet_size); + imu_packet_msg.buf.resize(pf.imu_packet_size); } bool OusterSensor::init_id_changed(const sensor::packet_format& pf,