diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 75029997..b2e2fafb 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -19,6 +19,9 @@ ouster_ros(2) * add frame_id to image topics * fixed a potential issue of time values within generated point clouds that could result in a value overflow +* added a new ``/ouster/metadata`` topic that is consumed by os_cloud and os_image nodes and save it + to the bag file on record +* make specifying metadata file optional during record and replay modes as of package version 8.1 ouster_client ------------- diff --git a/README.md b/README.md index 2143eeb1..4ac7ba1e 100644 --- a/README.md +++ b/README.md @@ -129,18 +129,25 @@ ros2 launch ouster_ros sensor.launch.xml \ ``` #### Recording Mode +> Note +> As of package version 8.1, specifiying metadata file is optional since the introduction of the +> metadata topic ```bash ros2 launch ouster_ros record.launch.xml \ sensor_hostname:= \ - metadata:= \ - bag_file:= + bag_file:= \ + metadata:= # optional ``` #### Replay Mode +> Note +> As of package version 8.1, specifiying metadata file is optional if the bag file being replayed +> already contains the metadata topic + ```bash ros2 launch ouster_ros replay.launch.xml \ - metadata:= \ - bag_file:= + bag_file:= \ + metadata:= # optional if bag file has /metadata topic ``` #### Multicast Mode (experimental) diff --git a/ouster-ros/CMakeLists.txt b/ouster-ros/CMakeLists.txt index e24829ca..d7f70c4a 100644 --- a/ouster-ros/CMakeLists.txt +++ b/ouster-ros/CMakeLists.txt @@ -11,6 +11,7 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(rclcpp_lifecycle REQUIRED) +find_package(std_msgs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(ouster_msgs REQUIRED) @@ -101,6 +102,7 @@ function(create_ros2_component class_loader rclcpp_components rclcpp_lifecycle + std_msgs sensor_msgs geometry_msgs ${additonal_dependencies} diff --git a/ouster-ros/config/metadata-qos-override.yaml b/ouster-ros/config/metadata-qos-override.yaml new file mode 100644 index 00000000..c11e521f --- /dev/null +++ b/ouster-ros/config/metadata-qos-override.yaml @@ -0,0 +1,5 @@ +/ouster/metadata: + history: keep_last + depth: 1 + reliability: reliable + durability: transient_local diff --git a/ouster-ros/include/ouster_ros/os_processing_node_base.h b/ouster-ros/include/ouster_ros/os_processing_node_base.h index 1a0cbeef..66237edd 100644 --- a/ouster-ros/include/ouster_ros/os_processing_node_base.h +++ b/ouster-ros/include/ouster_ros/os_processing_node_base.h @@ -11,8 +11,7 @@ #include #include - -#include "ouster_srvs/srv/get_metadata.hpp" +#include namespace ouster_ros { @@ -22,24 +21,19 @@ class OusterProcessingNodeBase : public rclcpp::Node { const rclcpp::NodeOptions& options) : rclcpp::Node(name, options) {} - protected: - bool spin_till_attempts_exahused(const std::string& log_msg, - std::function lambda); - - std::string get_metadata(); + void create_metadata_subscriber( + std::function + on_sensor_metadata); - int get_n_returns(); + int get_n_returns() const; - std::string topic_for_return(std::string base, int idx) { + static std::string topic_for_return(std::string base, int idx) { if (idx == 0) return base; return base + std::to_string(idx + 1); } protected: - // TODO: Add as node parameters? - static constexpr auto wait_time_per_attempt = std::chrono::seconds(10); - static constexpr auto total_attempts = 10; - + rclcpp::Subscription::SharedPtr metadata_sub; ouster::sensor::sensor_info info; }; diff --git a/ouster-ros/include/ouster_ros/os_ros.h b/ouster-ros/include/ouster_ros/os_ros.h index 48d933cb..7f17cd0d 100644 --- a/ouster-ros/include/ouster_ros/os_ros.h +++ b/ouster-ros/include/ouster_ros/os_ros.h @@ -84,7 +84,8 @@ sensor_msgs::msg::Imu packet_to_imu_msg(const ouster_msgs::msg::PacketMsg& pm, [[deprecated("use the 2nd version of scan_to_cloud_f")]] void scan_to_cloud_f( ouster::PointsF& points, const ouster::PointsF& lut_direction, const ouster::PointsF& lut_offset, std::chrono::nanoseconds scan_ts, - const ouster::LidarScan& lidar_scan, ouster_ros::Cloud& cloud, int return_index); + const ouster::LidarScan& lidar_scan, ouster_ros::Cloud& cloud, + int return_index); /** * Populate a PCL point cloud from a LidarScan. @@ -101,8 +102,8 @@ sensor_msgs::msg::Imu packet_to_imu_msg(const ouster_msgs::msg::PacketMsg& pm, void scan_to_cloud_f(ouster::PointsF& points, const ouster::PointsF& lut_direction, const ouster::PointsF& lut_offset, uint64_t scan_ts, - const ouster::LidarScan& lidar_scan, ouster_ros::Cloud& cloud, - int return_index); + const ouster::LidarScan& lidar_scan, + ouster_ros::Cloud& cloud, int return_index); /** * Serialize a PCL point cloud to a ROS message 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 fd175adb..8ec78e07 100644 --- a/ouster-ros/include/ouster_ros/os_sensor_node_base.h +++ b/ouster-ros/include/ouster_ros/os_sensor_node_base.h @@ -11,6 +11,7 @@ #include #include +#include #include "ouster_srvs/srv/get_metadata.hpp" @@ -23,18 +24,23 @@ class OusterSensorNodeBase : public rclcpp_lifecycle::LifecycleNode { : rclcpp_lifecycle::LifecycleNode(name, options) {} protected: - bool is_arg_set(const std::string& arg) { + bool is_arg_set(const std::string& arg) const { return arg.find_first_not_of(' ') != std::string::npos; } void create_get_metadata_service(); + void create_metadata_publisher(); + + void publish_metadata(); + void display_lidar_info(const ouster::sensor::sensor_info& info); protected: ouster::sensor::sensor_info info; rclcpp::Service::SharedPtr get_metadata_srv; std::string cached_metadata; + rclcpp::Publisher::SharedPtr metadata_pub; }; } // namespace ouster_ros \ No newline at end of file diff --git a/ouster-ros/launch/record.composite.launch.xml b/ouster-ros/launch/record.composite.launch.xml index 9d6fe01d..d219d093 100644 --- a/ouster-ros/launch/record.composite.launch.xml +++ b/ouster-ros/launch/record.composite.launch.xml @@ -49,6 +49,8 @@ + + @@ -83,9 +85,14 @@ + /$(var ouster_ns)/imu_packets + /$(var ouster_ns)/lidar_packets + /$(var ouster_ns)/metadata"/> + cmd="ros2 bag record + /$(var ouster_ns)/imu_packets + /$(var ouster_ns)/lidar_packets + /$(var ouster_ns)/metadata"/> diff --git a/ouster-ros/launch/replay.composite.launch.xml b/ouster-ros/launch/replay.composite.launch.xml index 84a60a20..2941ba30 100644 --- a/ouster-ros/launch/replay.composite.launch.xml +++ b/ouster-ros/launch/replay.composite.launch.xml @@ -10,7 +10,7 @@ TIME_FROM_PTP_1588, TIME_FROM_ROS_TIME }"/> - @@ -20,12 +20,14 @@ description="optional rviz config file"/> + + + + + - - - @@ -36,9 +38,11 @@ - - @@ -51,6 +55,7 @@ + cmd="ros2 bag play $(var bag_file) --clock + --qos-profile-overrides-path $(find-pkg-share ouster_ros)/config/metadata-qos-override.yaml"/> diff --git a/ouster-ros/package.xml b/ouster-ros/package.xml index c0a715b1..e6855890 100644 --- a/ouster-ros/package.xml +++ b/ouster-ros/package.xml @@ -2,7 +2,7 @@ ouster_ros - 0.8.0 + 0.8.1 Ouster ROS2 driver ouster developers BSD @@ -13,6 +13,7 @@ rclcpp rclcpp_components rclcpp_lifecycle + std_msgs sensor_msgs geometry_msgs tf2_ros diff --git a/ouster-ros/src/os_cloud_node.cpp b/ouster-ros/src/os_cloud_node.cpp index 2d1040e1..d4a451c4 100644 --- a/ouster-ros/src/os_cloud_node.cpp +++ b/ouster-ros/src/os_cloud_node.cpp @@ -29,7 +29,6 @@ namespace sensor = ouster::sensor; using ouster_msgs::msg::PacketMsg; -using ouster_srvs::srv::GetMetadata; namespace { @@ -86,15 +85,9 @@ class OusterCloud : public OusterProcessingNodeBase { void on_init() { declare_parameters(); parse_parameters(); - auto metadata = get_metadata(); - info = sensor::parse_metadata(metadata); - n_returns = get_n_returns(); - create_lidarscan_objects(); - compute_scan_ts = [this](const auto& ts_v) { - return compute_scan_ts_0(ts_v); - }; - create_publishers(); - create_subscriptions(); + create_metadata_subscriber( + [this](const auto& msg) { metadata_handler(msg); }); + RCLCPP_INFO(get_logger(), "OusterCloud: node initialized!"); } void declare_parameters() { @@ -120,6 +113,20 @@ class OusterCloud : public OusterProcessingNodeBase { return one_sec_in_ns / (scan_width * scan_frequency); } + void metadata_handler( + const std_msgs::msg::String::ConstSharedPtr& metadata_msg) { + RCLCPP_INFO(get_logger(), + "OusterCloud: retrieved new sensor metadata!"); + info = sensor::parse_metadata(metadata_msg->data); + n_returns = get_n_returns(); + create_lidarscan_objects(); + compute_scan_ts = [this](const auto& ts_v) { + return compute_scan_ts_0(ts_v); + }; + create_publishers(); + create_subscriptions(); + } + void create_lidarscan_objects() { // The ouster_ros drive currently only uses single precision when it // produces the point cloud. So it isn't of a benefit to compute point diff --git a/ouster-ros/src/os_image_node.cpp b/ouster-ros/src/os_image_node.cpp index 522d2d77..4289adf8 100644 --- a/ouster-ros/src/os_image_node.cpp +++ b/ouster-ros/src/os_image_node.cpp @@ -36,7 +36,6 @@ namespace sensor = ouster::sensor; namespace viz = ouster::viz; -using ouster_srvs::srv::GetMetadata; using pixel_type = uint16_t; const size_t pixel_value_max = std::numeric_limits::max(); @@ -53,8 +52,15 @@ class OusterImage : public OusterProcessingNodeBase { private: void on_init() { - auto metadata = get_metadata(); - info = sensor::parse_metadata(metadata); + create_metadata_subscriber( + [this](const auto& msg) { metadata_handler(msg); }); + RCLCPP_INFO(get_logger(), "OusterImage: node initialized!"); + } + + void metadata_handler(const std_msgs::msg::String::ConstPtr& metadata_msg) { + RCLCPP_INFO(get_logger(), + "OusterImage: retrieved new sensor metadata!"); + info = sensor::parse_metadata(metadata_msg->data); create_cloud_object(); const int n_returns = get_n_returns(); create_publishers(n_returns); diff --git a/ouster-ros/src/os_processing_node_base.cpp b/ouster-ros/src/os_processing_node_base.cpp index 4024620b..a066980d 100644 --- a/ouster-ros/src/os_processing_node_base.cpp +++ b/ouster-ros/src/os_processing_node_base.cpp @@ -8,60 +8,21 @@ #include "ouster_ros/os_processing_node_base.h" -using namespace std::chrono_literals; using ouster::sensor::UDPProfileLidar; -using ouster_srvs::srv::GetMetadata; -using rclcpp::FutureReturnCode; namespace ouster_ros { -bool OusterProcessingNodeBase::spin_till_attempts_exahused( - const std::string& log_msg, std::function lambda) { - int remaining_attempts = total_attempts; - bool done; - do { - RCLCPP_INFO_STREAM(get_logger(), - log_msg << "; attempt no: " - << total_attempts - remaining_attempts + 1 - << "/" << total_attempts); - done = lambda(); - } while (rclcpp::ok() && !done && --remaining_attempts > 0); - return done; +void OusterProcessingNodeBase::create_metadata_subscriber( + std::function + on_sensor_metadata) { + auto latching_qos = rclcpp::QoS(rclcpp::KeepLast(1)); + latching_qos.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE); + latching_qos.durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + metadata_sub = create_subscription( + "metadata", latching_qos, on_sensor_metadata); } -std::string OusterProcessingNodeBase::get_metadata() { - auto client = create_client("get_metadata"); - if (!spin_till_attempts_exahused( - "contacting get_metadata service", [this, &client]() -> bool { - return client->wait_for_service(wait_time_per_attempt); - })) { - auto error_msg = "get_metadata service is unavailable"; - RCLCPP_ERROR_STREAM(get_logger(), error_msg); - throw std::runtime_error(error_msg); - } - auto request = std::make_shared(); - auto result = client->async_send_request(request); - - rclcpp::FutureReturnCode return_code; - spin_till_attempts_exahused( - "waiting for get_metadata service to respond", - [this, &return_code, &result]() -> bool { - return_code = rclcpp::spin_until_future_complete( - get_node_base_interface(), result, wait_time_per_attempt); - return return_code != FutureReturnCode::TIMEOUT; - }); - - if (return_code != FutureReturnCode::SUCCESS) { - auto error_msg = "get_metadata service timed out or interrupted"; - RCLCPP_ERROR_STREAM(get_logger(), error_msg); - throw std::runtime_error(error_msg); - } - - RCLCPP_INFO(get_logger(), "retrieved sensor metadata!"); - return result.get()->metadata; -} - -int OusterProcessingNodeBase::get_n_returns() { +int OusterProcessingNodeBase::get_n_returns() const { return info.format.udp_profile_lidar == UDPProfileLidar::PROFILE_RNG19_RFL8_SIG16_NIR16_DUAL ? 2 diff --git a/ouster-ros/src/os_replay_node.cpp b/ouster-ros/src/os_replay_node.cpp index 831d8942..c13b1b24 100644 --- a/ouster-ros/src/os_replay_node.cpp +++ b/ouster-ros/src/os_replay_node.cpp @@ -30,7 +30,9 @@ class OusterReplay : public OusterSensorNodeBase { try { auto meta_file = parse_parameters(); - populate_metadata_from_json(meta_file); + create_metadata_publisher(); + load_metadata_from_file(meta_file); + publish_metadata(); create_get_metadata_service(); RCLCPP_INFO(get_logger(), "Running in replay mode"); } catch (const std::exception& ex) { @@ -110,7 +112,7 @@ class OusterReplay : public OusterSensorNodeBase { return meta_file; } - void populate_metadata_from_json(const std::string& meta_file) { + void load_metadata_from_file(const std::string& meta_file) { try { std::ifstream in_file(meta_file); std::stringstream buffer; @@ -129,7 +131,6 @@ class OusterReplay : public OusterSensorNodeBase { void cleanup() { get_metadata_srv.reset(); } - }; } // namespace ouster_ros diff --git a/ouster-ros/src/os_sensor_node.cpp b/ouster-ros/src/os_sensor_node.cpp index 08de1be1..ffe3c716 100644 --- a/ouster-ros/src/os_sensor_node.cpp +++ b/ouster-ros/src/os_sensor_node.cpp @@ -63,7 +63,9 @@ class OusterSensor : public OusterSensorNodeBase { sensor_client = create_sensor_client(sensor_hostname, config); if (!sensor_client) return LifecycleNodeInterface::CallbackReturn::FAILURE; + create_metadata_publisher(); update_config_and_metadata(*sensor_client); + publish_metadata(); save_metadata(); create_reset_service(); create_get_metadata_service(); @@ -340,6 +342,7 @@ class OusterSensor : public OusterSensorNodeBase { connection_loop_timer->cancel(); reset_last_init_id = init_id_reset; update_config_and_metadata(*sensor_client); + publish_metadata(); save_metadata(); auto request_transitions = std::vector{ lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE, diff --git a/ouster-ros/src/os_sensor_node_base.cpp b/ouster-ros/src/os_sensor_node_base.cpp index faee0154..729a28f4 100644 --- a/ouster-ros/src/os_sensor_node_base.cpp +++ b/ouster-ros/src/os_sensor_node_base.cpp @@ -28,6 +28,20 @@ void OusterSensorNodeBase::create_get_metadata_service() { RCLCPP_INFO(get_logger(), "get_metadata service created"); } +void OusterSensorNodeBase::create_metadata_publisher() { + auto latching_qos = rclcpp::QoS(rclcpp::KeepLast(1)); + latching_qos.reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE); + latching_qos.durability(RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL); + metadata_pub = + create_publisher("metadata", latching_qos); +} + +void OusterSensorNodeBase::publish_metadata() { + std_msgs::msg::String metadata_msg; + metadata_msg.data = cached_metadata; + metadata_pub->publish(metadata_msg); +} + void OusterSensorNodeBase::display_lidar_info(const sensor::sensor_info& info) { auto lidar_profile = info.format.udp_profile_lidar; RCLCPP_INFO_STREAM(