diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 135d4bed..143dd7f9 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -5,6 +5,10 @@ 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 braodcast + of sensor TF transforms. + ouster_ros v0.13.2 ================== diff --git a/ouster-ros/config/driver_params.yaml b/ouster-ros/config/driver_params.yaml index 8f6f5b40..05a0c217 100644 --- a/ouster-ros/config/driver_params.yaml +++ b/ouster-ros/config/driver_params.yaml @@ -56,7 +56,11 @@ ouster/os_driver: # point_cloud_frame[optional]: which frame of reference to use when # generating PointCloud2 or LaserScan messages, select between the values of # lidar_frame and sensor_frame. - point_cloud_frame: os_lidar + point_cloud_frame: os_lidar + # pub_static_tf[optional]: when this flag is set to True, the driver will + # broadcast the TF transforms for the imu/sensor/lidar frames. Prevent the + # driver from broadcasting TF transforms by setting this parameter to False. + pub_static_tf: true # proc_mask[optional]: use any combination of the 4 flags IMG, PCL, IMU and # SCAN to enable or disable their respective messages. proc_mask: IMU|PCL|SCAN|IMG|RAW diff --git a/ouster-ros/config/os_sensor_cloud_image_params.yaml b/ouster-ros/config/os_sensor_cloud_image_params.yaml index 52296c1d..593053f8 100644 --- a/ouster-ros/config/os_sensor_cloud_image_params.yaml +++ b/ouster-ros/config/os_sensor_cloud_image_params.yaml @@ -27,6 +27,7 @@ ouster/os_cloud: lidar_frame: os_lidar imu_frame: os_imu point_cloud_frame: os_lidar + pub_static_tf: true timestamp_mode: '' # this value needs to match os_sensor/timestamp_mode ptp_utc_tai_offset: -37.0 # UTC/TAI offset in seconds to apply when using TIME_FROM_PTP_1588 proc_mask: IMU|PCL|SCAN # pick IMU, PCL, SCAN or any combination of the three options diff --git a/ouster-ros/include/ouster_ros/os_sensor_node_base.h b/ouster-ros/include/ouster_ros/os_sensor_node_base.h index cb303a48..2bafe696 100644 --- a/ouster-ros/include/ouster_ros/os_sensor_node_base.h +++ b/ouster-ros/include/ouster_ros/os_sensor_node_base.h @@ -7,21 +7,23 @@ * */ -#include +#include #include #include +#include #include #include "ouster_sensor_msgs/srv/get_metadata.hpp" +#include + namespace ouster_ros { class OusterSensorNodeBase : public rclcpp_lifecycle::LifecycleNode { protected: explicit OusterSensorNodeBase(const std::string& name, - const rclcpp::NodeOptions& options) - : rclcpp_lifecycle::LifecycleNode(name, options) {} + const rclcpp::NodeOptions& options); protected: bool is_arg_set(const std::string& arg) const { @@ -41,7 +43,20 @@ class OusterSensorNodeBase : public rclcpp_lifecycle::LifecycleNode { static bool write_text_to_file(const std::string& file_path, const std::string& text); + static std::string transition_id_to_string(uint8_t transition_id); + + template + bool change_state(std::uint8_t transition_id, CallbackT callback, + CallbackT_Args... callback_args, + std::chrono::seconds time_out = std::chrono::seconds{3}); + + void execute_transitions_sequence(std::vector transitions_sequence, + size_t at); + + protected: + std::shared_ptr> change_state_client; + ouster::sensor::sensor_info info; rclcpp::Service::SharedPtr get_metadata_srv; std::string cached_metadata; diff --git a/ouster-ros/launch/record.composite.launch.xml b/ouster-ros/launch/record.composite.launch.xml index 0908ada5..d89d3c1f 100644 --- a/ouster-ros/launch/record.composite.launch.xml +++ b/ouster-ros/launch/record.composite.launch.xml @@ -55,6 +55,10 @@ description="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."/> + @@ -125,6 +129,7 @@ + diff --git a/ouster-ros/launch/replay.composite.launch.xml b/ouster-ros/launch/replay.composite.launch.xml index a78da49a..2d0a99a5 100644 --- a/ouster-ros/launch/replay.composite.launch.xml +++ b/ouster-ros/launch/replay.composite.launch.xml @@ -3,6 +3,9 @@ + + + @@ -37,6 +40,10 @@ description="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."/> + @@ -77,12 +84,14 @@ + + @@ -95,6 +104,7 @@ + @@ -119,9 +129,9 @@ diff --git a/ouster-ros/launch/replay_pcap.launch.xml b/ouster-ros/launch/replay_pcap.launch.xml index bb758771..9c5a7f3b 100644 --- a/ouster-ros/launch/replay_pcap.launch.xml +++ b/ouster-ros/launch/replay_pcap.launch.xml @@ -1,8 +1,13 @@ - + + + + + + @@ -65,9 +74,12 @@ - + + + @@ -76,6 +88,7 @@ + @@ -94,15 +107,6 @@ - - - - diff --git a/ouster-ros/launch/sensor.composite.launch.xml b/ouster-ros/launch/sensor.composite.launch.xml index 8d1c40f6..7ffae269 100644 --- a/ouster-ros/launch/sensor.composite.launch.xml +++ b/ouster-ros/launch/sensor.composite.launch.xml @@ -53,6 +53,10 @@ description="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/ouster-ros/launch/sensor.independent.launch.xml b/ouster-ros/launch/sensor.independent.launch.xml index 21cbeac1..41625bdb 100644 --- a/ouster-ros/launch/sensor.independent.launch.xml +++ b/ouster-ros/launch/sensor.independent.launch.xml @@ -53,6 +53,10 @@ description="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."/> + @@ -122,6 +126,7 @@ + diff --git a/ouster-ros/launch/sensor_mtp.launch.xml b/ouster-ros/launch/sensor_mtp.launch.xml index ab721f82..387aec86 100644 --- a/ouster-ros/launch/sensor_mtp.launch.xml +++ b/ouster-ros/launch/sensor_mtp.launch.xml @@ -61,6 +61,10 @@ description="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."/> + @@ -117,6 +121,7 @@ + diff --git a/ouster-ros/package.xml b/ouster-ros/package.xml index a2a1bd15..825707e9 100644 --- a/ouster-ros/package.xml +++ b/ouster-ros/package.xml @@ -2,7 +2,7 @@ ouster_ros - 0.13.3 + 0.13.4 Ouster ROS2 driver ouster developers BSD diff --git a/ouster-ros/src/os_cloud_node.cpp b/ouster-ros/src/os_cloud_node.cpp index b28ddc72..1732e781 100644 --- a/ouster-ros/src/os_cloud_node.cpp +++ b/ouster-ros/src/os_cloud_node.cpp @@ -72,7 +72,9 @@ class OusterCloud : public OusterProcessingNodeBase { RCLCPP_INFO(get_logger(), "OusterCloud: retrieved new sensor metadata!"); info = sensor::parse_metadata(metadata_msg->data); - tf_bcast.broadcast_transforms(info); + if (tf_bcast.publish_static_tf()) { + tf_bcast.broadcast_transforms(info); + } create_publishers_subscriptions(info); } diff --git a/ouster-ros/src/os_driver_node.cpp b/ouster-ros/src/os_driver_node.cpp index 0a198804..8ad3b99a 100644 --- a/ouster-ros/src/os_driver_node.cpp +++ b/ouster-ros/src/os_driver_node.cpp @@ -53,7 +53,9 @@ class OusterDriver : public OusterSensor { virtual void on_metadata_updated(const sensor::sensor_info& info) override { OusterSensor::on_metadata_updated(info); - tf_bcast.broadcast_transforms(info); + if (tf_bcast.publish_static_tf()) { + tf_bcast.broadcast_transforms(info); + } } virtual void create_publishers() override { diff --git a/ouster-ros/src/os_pcap_node.cpp b/ouster-ros/src/os_pcap_node.cpp index 0b458c1c..2c4adb84 100644 --- a/ouster-ros/src/os_pcap_node.cpp +++ b/ouster-ros/src/os_pcap_node.cpp @@ -18,16 +18,18 @@ #include #include +#include +#include #include "ouster_sensor_msgs/msg/packet_msg.h" #include "ouster_ros/os_sensor_node_base.h" #include "ouster_ros/visibility_control.h" #include +using namespace std::chrono; +using namespace std::chrono_literals; namespace sensor = ouster::sensor; using ouster::sensor_utils::PcapReader; -using namespace std::chrono; - using ouster_sensor_msgs::msg::PacketMsg; namespace ouster_ros { @@ -36,8 +38,19 @@ class OusterPcap : public OusterSensorNodeBase { public: OUSTER_ROS_PUBLIC explicit OusterPcap(const rclcpp::NodeOptions& options) - : OusterSensorNodeBase("os_pcap", options) { + : OusterSensorNodeBase("os_pcap", options) + { declare_parameters(); + bool auto_start = get_parameter("auto_start").as_bool(); + + if (auto_start) { + RCLCPP_INFO(get_logger(), "auto start requested"); + auto request_transitions = std::vector{ + lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE, + lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE}; + execute_transitions_sequence(request_transitions, 0); + RCLCPP_INFO(get_logger(), "auto start initiated"); + } } ~OusterPcap() override { @@ -52,6 +65,10 @@ class OusterPcap : public OusterSensorNodeBase { try { auto meta_file = get_meta_file(); auto pcap_file = get_pcap_file(); + loop = get_parameter("loop").as_bool(); + progress_update_freq = get_parameter("progress_update_freq").as_double(); + if (progress_update_freq < 0.001) + progress_update_freq = 0.001; create_metadata_pub(); load_metadata_from_file(meta_file); open_pcap(pcap_file); @@ -131,8 +148,11 @@ class OusterPcap : public OusterSensorNodeBase { } void declare_parameters() { + declare_parameter("auto_start", true); declare_parameter("metadata"); declare_parameter("pcap_file"); + declare_parameter("loop", false); + declare_parameter("progress_update_freq", 1.0); declare_parameter("use_system_default_qos", false); } @@ -195,11 +215,13 @@ class OusterPcap : public OusterSensorNodeBase { 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(rclcpp::ok() && packet_read_active && loop); RCLCPP_DEBUG(get_logger(), "packet_read_thread done."); + rclcpp::shutdown(); }); } @@ -236,10 +258,9 @@ class OusterPcap : public OusterSensorNodeBase { auto packet_info = pcap.current_info(); auto file_start = packet_info.timestamp; auto last_update = file_start; - using namespace std::chrono_literals; - const auto UPDATE_PERIOD = duration_cast(1s); + const auto UPDATE_PERIOD = duration_cast(1s / progress_update_freq); - while (rclcpp::ok() && payload_size) { + while (rclcpp::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(), @@ -278,7 +299,8 @@ class OusterPcap : public OusterSensorNodeBase { ouster_sensor_msgs::msg::PacketMsg imu_packet; rclcpp::Publisher::SharedPtr lidar_packet_pub; rclcpp::Publisher::SharedPtr imu_packet_pub; - + bool loop; + double progress_update_freq; 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 9426aaff..ed7e86f0 100644 --- a/ouster-ros/src/os_sensor_node.cpp +++ b/ouster-ros/src/os_sensor_node.cpp @@ -22,7 +22,6 @@ using ouster_sensor_msgs::srv::SetConfig; using std::to_string; using namespace std::chrono_literals; -using namespace std::string_literals; using sensor::LidarPacket; using sensor::ImuPacket; @@ -31,9 +30,7 @@ namespace ouster_ros { OusterSensor::OusterSensor(const std::string& name, const rclcpp::NodeOptions& options) - : OusterSensorNodeBase(name, options), - change_state_client{ - create_client(get_name() + "/change_state"s)} { + : OusterSensorNodeBase(name, options) { declare_parameters(); staged_config = parse_config_from_ros_parameters(); attempt_reconnect = get_parameter("attempt_reconnect").as_bool(); @@ -292,75 +289,6 @@ void OusterSensor::save_metadata() { } } -std::string OusterSensor::transition_id_to_string(uint8_t transition_id) { - switch (transition_id) { - case lifecycle_msgs::msg::Transition::TRANSITION_CREATE: - return "create"s; - case lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE: - return "configure"s; - case lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP: - return "cleanup"s; - case lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE: - return "activate"; - case lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE: - return "deactivate"s; - case lifecycle_msgs::msg::Transition::TRANSITION_DESTROY: - return "destroy"s; - default: - return "unknown"s; - } -} - -template -bool OusterSensor::change_state(std::uint8_t transition_id, CallbackT callback, - CallbackT_Args... callback_args, - std::chrono::seconds time_out) { - if (!change_state_client->wait_for_service(time_out)) { - RCLCPP_ERROR_STREAM( - get_logger(), "Service " << change_state_client->get_service_name() - << "is not available."); - return false; - } - - auto request = std::make_shared(); - request->transition.id = transition_id; - // send an async request to perform the transition - change_state_client->async_send_request( - request, [this, callback, - callback_args...](rclcpp::Client::SharedFuture ff) { - callback(callback_args..., ff.get()->success); - }); - return true; -} - -void OusterSensor::execute_transitions_sequence( - std::vector transitions_sequence, size_t at) { - assert(at < transitions_sequence.size() && - "at index exceeds the number of transitions"); - auto transition_id = transitions_sequence[at]; - RCLCPP_DEBUG_STREAM( - get_logger(), "transition: [" << transition_id_to_string(transition_id) - << "] started"); - change_state(transition_id, [this, transitions_sequence, at](bool success) { - if (success) { - RCLCPP_DEBUG_STREAM( - get_logger(), - "transition: [" << transition_id_to_string(transitions_sequence[at]) - << "] completed"); - if (at < transitions_sequence.size() - 1) { - execute_transitions_sequence(transitions_sequence, at + 1); - } else { - RCLCPP_DEBUG_STREAM(get_logger(), "transitions sequence completed"); - } - } else { - RCLCPP_DEBUG_STREAM( - get_logger(), - "transition: [" << transition_id_to_string(transitions_sequence[at]) - << "] failed"); - } - }); -} - // param init_id_reset is overriden to true when force_reinit is true void OusterSensor::reset_sensor(bool force_reinit, bool init_id_reset) { if (!sensor_connection_active) { diff --git a/ouster-ros/src/os_sensor_node.h b/ouster-ros/src/os_sensor_node.h index fc2049cf..9db67b89 100644 --- a/ouster-ros/src/os_sensor_node.h +++ b/ouster-ros/src/os_sensor_node.h @@ -18,8 +18,6 @@ #include #include -#include -#include #include "ouster_sensor_msgs/msg/packet_msg.hpp" #include "ouster_sensor_msgs/srv/get_config.hpp" #include "ouster_sensor_msgs/srv/set_config.hpp" @@ -28,7 +26,6 @@ namespace sensor = ouster::sensor; -using lifecycle_msgs::srv::ChangeState; using rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface; namespace ouster_ros { @@ -77,15 +74,6 @@ class OusterSensor : public OusterSensorNodeBase { void save_metadata(); - static std::string transition_id_to_string(uint8_t transition_id); - template - bool change_state(std::uint8_t transition_id, CallbackT callback, - CallbackT_Args... callback_args, - std::chrono::seconds time_out = std::chrono::seconds{3}); - - void execute_transitions_sequence(std::vector transitions_sequence, - size_t at); - // param init_id_reset is overriden to true when force_reinit is true void reset_sensor(bool force_reinit, bool init_id_reset = false); @@ -152,7 +140,6 @@ class OusterSensor : public OusterSensorNodeBase { rclcpp::Service::SharedPtr reset_srv; rclcpp::Service::SharedPtr get_config_srv; rclcpp::Service::SharedPtr set_config_srv; - std::shared_ptr> change_state_client; std::atomic sensor_connection_active = {false}; std::unique_ptr sensor_connection_thread; diff --git a/ouster-ros/src/os_sensor_node_base.cpp b/ouster-ros/src/os_sensor_node_base.cpp index a0dadedd..817d3001 100644 --- a/ouster-ros/src/os_sensor_node_base.cpp +++ b/ouster-ros/src/os_sensor_node_base.cpp @@ -7,17 +7,25 @@ */ #include "ouster_ros/os_sensor_node_base.h" - #include - #include +#include +using namespace std::string_literals; +using lifecycle_msgs::srv::ChangeState; namespace sensor = ouster::sensor; using ouster_sensor_msgs::srv::GetMetadata; using sensor::UDPProfileLidar; namespace ouster_ros { +OusterSensorNodeBase::OusterSensorNodeBase( + const std::string& name, const rclcpp::NodeOptions& options) +: rclcpp_lifecycle::LifecycleNode(name, options), + change_state_client{create_client(name + "/change_state"s)} { +} + + void OusterSensorNodeBase::create_get_metadata_service() { get_metadata_srv = create_service( "get_metadata", @@ -74,4 +82,73 @@ bool OusterSensorNodeBase::write_text_to_file(const std::string& file_path, return true; } +std::string OusterSensorNodeBase::transition_id_to_string(uint8_t transition_id) { + switch (transition_id) { + case lifecycle_msgs::msg::Transition::TRANSITION_CREATE: + return "create"s; + case lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE: + return "configure"s; + case lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP: + return "cleanup"s; + case lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE: + return "activate"s; + case lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE: + return "deactivate"s; + case lifecycle_msgs::msg::Transition::TRANSITION_DESTROY: + return "destroy"s; + default: + return "unknown"s; + } +} + +template +bool OusterSensorNodeBase::change_state(std::uint8_t transition_id, CallbackT callback, + CallbackT_Args... callback_args, + std::chrono::seconds time_out) { + if (!change_state_client->wait_for_service(time_out)) { + RCLCPP_ERROR_STREAM( + get_logger(), "Service " << change_state_client->get_service_name() + << "is not available."); + return false; + } + + auto request = std::make_shared(); + request->transition.id = transition_id; + // send an async request to perform the transition + change_state_client->async_send_request( + request, [this, callback, + callback_args...](rclcpp::Client::SharedFuture ff) { + callback(callback_args..., ff.get()->success); + }); + return true; +} + +void OusterSensorNodeBase::execute_transitions_sequence( + std::vector transitions_sequence, size_t at) { + assert(at < transitions_sequence.size() && + "at index exceeds the number of transitions"); + auto transition_id = transitions_sequence[at]; + RCLCPP_DEBUG_STREAM( + get_logger(), "transition: [" << transition_id_to_string(transition_id) + << "] started"); + change_state(transition_id, [this, transitions_sequence, at](bool success) { + if (success) { + RCLCPP_DEBUG_STREAM( + get_logger(), + "transition: [" << transition_id_to_string(transitions_sequence[at]) + << "] completed"); + if (at < transitions_sequence.size() - 1) { + execute_transitions_sequence(transitions_sequence, at + 1); + } else { + RCLCPP_DEBUG_STREAM(get_logger(), "transitions sequence completed"); + } + } else { + RCLCPP_DEBUG_STREAM( + get_logger(), + "transition: [" << transition_id_to_string(transitions_sequence[at]) + << "] failed"); + } + }); +} + } // namespace ouster_ros diff --git a/ouster-ros/src/os_static_transforms_broadcaster.h b/ouster-ros/src/os_static_transforms_broadcaster.h index 760b5cda..911df513 100644 --- a/ouster-ros/src/os_static_transforms_broadcaster.h +++ b/ouster-ros/src/os_static_transforms_broadcaster.h @@ -23,6 +23,7 @@ class OusterStaticTransformsBroadcaster { node->declare_parameter("lidar_frame", "os_lidar"); node->declare_parameter("imu_frame", "os_imu"); node->declare_parameter("point_cloud_frame", ""); + node->declare_parameter("pub_static_tf", true); } void parse_parameters() { @@ -31,6 +32,7 @@ class OusterStaticTransformsBroadcaster { imu_frame = node->get_parameter("imu_frame").as_string(); point_cloud_frame = node->get_parameter("point_cloud_frame").as_string(); + pub_static_tf = node->get_parameter("pub_static_tf").as_bool(); // validate point_cloud_frame if (point_cloud_frame.empty()) { @@ -65,6 +67,7 @@ class OusterStaticTransformsBroadcaster { bool apply_lidar_to_sensor_transform() const { return point_cloud_frame == sensor_frame; } + bool publish_static_tf() const { return pub_static_tf; } private: NodeT* node; @@ -73,6 +76,7 @@ class OusterStaticTransformsBroadcaster { std::string lidar_frame; std::string sensor_frame; std::string point_cloud_frame; + bool pub_static_tf; }; } // namespace ouster_ros