diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt index f039cf5d42b..eeb9d61703e 100644 --- a/nav2_costmap_2d/CMakeLists.txt +++ b/nav2_costmap_2d/CMakeLists.txt @@ -17,6 +17,7 @@ find_package(rclcpp_lifecycle REQUIRED) find_package(rmw REQUIRED) find_package(sensor_msgs REQUIRED) find_package(std_msgs REQUIRED) +find_package(vision_msgs REQUIRED) find_package(std_srvs REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(tf2 REQUIRED) @@ -48,6 +49,7 @@ add_library(nav2_costmap_2d_core SHARED src/footprint.cpp src/costmap_layer.cpp src/observation_buffer.cpp + src/segmentation_buffer.cpp src/clear_costmap_service.cpp src/footprint_collision_checker.cpp plugins/costmap_filters/costmap_filter.cpp @@ -67,6 +69,7 @@ set(dependencies rclcpp_lifecycle sensor_msgs std_msgs + vision_msgs std_srvs tf2 tf2_geometry_msgs @@ -87,6 +90,7 @@ add_library(layers SHARED src/observation_buffer.cpp plugins/voxel_layer.cpp plugins/range_sensor_layer.cpp + plugins/semantic_segmentation_layer.cpp plugins/denoise_layer.cpp ) ament_target_dependencies(layers diff --git a/nav2_costmap_2d/costmap_plugins.xml b/nav2_costmap_2d/costmap_plugins.xml index 6748cd5fcae..afdde92303c 100644 --- a/nav2_costmap_2d/costmap_plugins.xml +++ b/nav2_costmap_2d/costmap_plugins.xml @@ -15,6 +15,9 @@ A range-sensor (sonar, IR) based obstacle layer for costmap_2d + + A plugin for semantic segmentation data produced by RGBD cameras + Filters noise-induced freestanding obstacles or small obstacles groups diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/segmentation.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/segmentation.hpp new file mode 100644 index 00000000000..fd653d9ad24 --- /dev/null +++ b/nav2_costmap_2d/include/nav2_costmap_2d/segmentation.hpp @@ -0,0 +1,123 @@ +/* + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * * Neither the name of the Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Authors: Pedro Gonzalez + */ + +#ifndef NAV2_COSTMAP_2D__SEGMENTATION_HPP_ +#define NAV2_COSTMAP_2D__SEGMENTATION_HPP_ + +#include +#include +#include + +namespace nav2_costmap_2d +{ + +/** + * @brief Stores an segmentation in terms of a point cloud and the origin of the source + * @note Tried to make members and constructor arguments const but the compiler would not accept the + * default assignment operator for vector insertion! + */ +class Segmentation +{ +public: + /** + * @brief Creates an empty segmentation + */ + Segmentation() + : cloud_(new sensor_msgs::msg::PointCloud2()) {} + /** + * @brief A destructor + */ + virtual ~Segmentation() {delete cloud_;} + + /** + * @brief Copy assignment operator + * @param obs The segmentation to copy + */ + Segmentation & operator=(const Segmentation & obs) + { + origin_ = obs.origin_; + cloud_ = new sensor_msgs::msg::PointCloud2(*(obs.cloud_)); + class_map_ = obs.class_map_; + + return *this; + } + + /** + * @brief Creates an segmentation from an origin point and a point cloud + * @param origin The origin point of the segmentation + * @param cloud The point cloud of the segmentation + * @param obstacle_max_range The range out to which an segmentation should be able to insert + * obstacles + * @param obstacle_min_range The range from which an segmentation should be able to insert + * obstacles + * @param raytrace_max_range The range out to which an segmentation should be able to clear via + * raytracing + * @param raytrace_min_range The range from which an segmentation should be able to clear via + * raytracing + */ + Segmentation( + geometry_msgs::msg::Point & origin, const sensor_msgs::msg::PointCloud2 & cloud, + std::unordered_map class_map) + : origin_(origin), cloud_(new sensor_msgs::msg::PointCloud2(cloud)), class_map_(class_map) + { + } + + /** + * @brief Copy constructor + * @param obs The segmentation to copy + */ + Segmentation(const Segmentation & obs) + : origin_(obs.origin_) + , cloud_(new sensor_msgs::msg::PointCloud2(*(obs.cloud_))) + , class_map_(obs.class_map_) + { + } + + /** + * @brief Creates an segmentation from a point cloud + * @param cloud The point cloud of the segmentation + * @param obstacle_max_range The range out to which an segmentation should be able to insert + * obstacles + * @param obstacle_min_range The range from which an segmentation should be able to insert + * obstacles + */ + Segmentation(const sensor_msgs::msg::PointCloud2 & cloud) + : cloud_(new sensor_msgs::msg::PointCloud2(cloud)) + { + } + + geometry_msgs::msg::Point origin_; + sensor_msgs::msg::PointCloud2 * cloud_; + std::unordered_map class_map_; // contains class_id->cost relation +}; + +} // namespace nav2_costmap_2d +#endif // NAV2_COSTMAP_2D__SEGMENTATION_HPP_ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/segmentation_buffer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/segmentation_buffer.hpp new file mode 100644 index 00000000000..fa3ebcf86e7 --- /dev/null +++ b/nav2_costmap_2d/include/nav2_costmap_2d/segmentation_buffer.hpp @@ -0,0 +1,185 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Eitan Marder-Eppstein + *********************************************************************/ +#ifndef NAV2_COSTMAP_2D__SEGMENTATION_BUFFER_HPP_ +#define NAV2_COSTMAP_2D__SEGMENTATION_BUFFER_HPP_ + +#include +#include +#include + +#include "nav2_costmap_2d/segmentation.hpp" +#include "nav2_util/lifecycle_node.hpp" +#include "rclcpp/time.hpp" +#include "sensor_msgs/msg/image.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_sensor_msgs/tf2_sensor_msgs.hpp" +#include "vision_msgs/msg/label_info.hpp" + +namespace nav2_costmap_2d +{ +/** + * @class SegmentationBuffer + * @brief Takes in point clouds from sensors, transforms them to the desired frame, and stores them + */ +class SegmentationBuffer +{ +public: + /** + * @brief Constructs an segmentation buffer + * @param topic_name The topic of the segmentations, used as an identifier for error and warning + * messages + * @param observation_keep_time Defines the persistence of segmentations in seconds, 0 means only + * keep the latest + * @param expected_update_rate How often this buffer is expected to be updated, 0 means there is + * no limit + * @param min_obstacle_height The minimum height of a hitpoint to be considered legal + * @param max_obstacle_height The minimum height of a hitpoint to be considered legal + * @param obstacle_max_range The range to which the sensor should be trusted for inserting + * obstacles + * @param obstacle_min_range The range from which the sensor should be trusted for inserting + * obstacles + * @param raytrace_max_range The range to which the sensor should be trusted for raytracing to + * clear out space + * @param raytrace_min_range The range from which the sensor should be trusted for raytracing to + * clear out space + * @param tf2_buffer A reference to a tf2 Buffer + * @param global_frame The frame to transform PointClouds into + * @param sensor_frame The frame of the origin of the sensor, can be left blank to be read from + * the messages + * @param tf_tolerance The amount of time to wait for a transform to be available when setting a + * new global frame + */ + SegmentationBuffer( + const nav2_util::LifecycleNode::WeakPtr & parent, std::string buffer_source, + std::vector class_types, + std::unordered_map class_names_cost_map, double observation_keep_time, + double expected_update_rate, double max_lookahead_distance, double min_lookahead_distance, + tf2_ros::Buffer & tf2_buffer, std::string global_frame, std::string sensor_frame, + tf2::Duration tf_tolerance); + + /** + * @brief Destructor... cleans up + */ + ~SegmentationBuffer(); + + /** + * @brief Transforms a PointCloud to the global frame and buffers it + * Note: The burden is on the user to make sure the transform is available... ie they should + * use a MessageNotifier + * @param cloud The cloud to be buffered + */ + void bufferSegmentation( + const sensor_msgs::msg::PointCloud2 & cloud, const sensor_msgs::msg::Image & segmentation, + const sensor_msgs::msg::Image & confidence); + + /** + * @brief Pushes copies of all current segmentations onto the end of the vector passed in + * @param segmentations The vector to be filled + */ + void getSegmentations(std::vector & segmentations); + + /** + * @brief gets the class map associated with the segmentations stored in the buffer + * @return the class map + */ + std::unordered_map getClassMap(); + + void createClassIdCostMap(const vision_msgs::msg::LabelInfo & label_info); + + bool isClassIdCostMapEmpty() {return class_ids_cost_map_.empty();} + + /** + * @brief Check if the segmentation buffer is being update at its expected rate + * @return True if it is being updated at the expected rate, false otherwise + */ + bool isCurrent() const; + + /** + * @brief Lock the segmentation buffer + */ + inline void lock() {lock_.lock();} + + /** + * @brief Lock the segmentation buffer + */ + inline void unlock() {lock_.unlock();} + + /** + * @brief Reset last updated timestamp + */ + void resetLastUpdated(); + + /** + * @brief Reset last updated timestamp + */ + std::string getBufferSource() {return buffer_source_;} + std::vector getClassTypes() {return class_types_;} + + void setMinObstacleDistance(double distance) {sq_min_lookahead_distance_ = pow(distance, 2);} + + void setMaxObstacleDistance(double distance) {sq_max_lookahead_distance_ = pow(distance, 2);} + + void updateClassMap(std::string new_class, uint8_t new_cost); + +private: + /** + * @brief Removes any stale segmentations from the buffer list + */ + void purgeStaleSegmentations(); + + rclcpp::Clock::SharedPtr clock_; + rclcpp::Logger logger_{rclcpp::get_logger("nav2_costmap_2d")}; + tf2_ros::Buffer & tf2_buffer_; + std::vector class_types_; + std::unordered_map class_names_cost_map_; + std::unordered_map class_ids_cost_map_; + const rclcpp::Duration observation_keep_time_; + const rclcpp::Duration expected_update_rate_; + rclcpp::Time last_updated_; + std::string global_frame_; + std::string sensor_frame_; + std::list segmentation_list_; + std::string buffer_source_; + std::recursive_mutex lock_; ///< @brief A lock for accessing data in callbacks safely + double sq_max_lookahead_distance_; + double sq_min_lookahead_distance_; + tf2::Duration tf_tolerance_; +}; +} // namespace nav2_costmap_2d +#endif // NAV2_COSTMAP_2D__SEGMENTATION_BUFFER_HPP_ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/semantic_segmentation_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/semantic_segmentation_layer.hpp new file mode 100644 index 00000000000..cefe4799d83 --- /dev/null +++ b/nav2_costmap_2d/include/nav2_costmap_2d/semantic_segmentation_layer.hpp @@ -0,0 +1,171 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * Copyright (c) 2020, Samsung R&D Institute Russia + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Pedro Gonzalez + + *********************************************************************/ +#ifndef SEMANTIC_SEGMENTATION_LAYER_HPP_ +#define SEMANTIC_SEGMENTATION_LAYER_HPP_ + +#include "rclcpp/rclcpp.hpp" + +#include "message_filters/subscriber.h" +#include "message_filters/time_synchronizer.h" +#include "nav2_costmap_2d/costmap_layer.hpp" +#include "nav2_costmap_2d/layer.hpp" +#include "nav2_costmap_2d/layered_costmap.hpp" +#include "nav2_costmap_2d/segmentation.hpp" +#include "nav2_costmap_2d/segmentation_buffer.hpp" +#include "nav2_util/node_utils.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/image.hpp" +#include "tf2_ros/message_filter.h" +#include "vision_msgs/msg/label_info.hpp" + +namespace nav2_costmap_2d +{ + +/** + * @class SemanticSegmentationLayer + * @brief Takes in semantic segmentation messages and aligned pointclouds to populate the 2D costmap + */ +class SemanticSegmentationLayer : public CostmapLayer +{ +public: + /** + * @brief A constructor + */ + SemanticSegmentationLayer(); + + /** + * @brief A destructor + */ + virtual ~SemanticSegmentationLayer() {} + + /** + * @brief Initialization process of layer on startup + */ + virtual void onInitialize(); + /** + * @brief Update the bounds of the master costmap by this layer's update dimensions + * @param robot_x X pose of robot + * @param robot_y Y pose of robot + * @param robot_yaw Robot orientation + * @param min_x X min map coord of the window to update + * @param min_y Y min map coord of the window to update + * @param max_x X max map coord of the window to update + * @param max_y Y max map coord of the window to update + */ + virtual void updateBounds( + double robot_x, double robot_y, double robot_yaw, double * min_x, double * min_y, + double * max_x, double * max_y); + /** + * @brief Update the costs in the master costmap in the window + * @param master_grid The master costmap grid to update + * @param min_x X min map coord of the window to update + * @param min_y Y min map coord of the window to update + * @param max_x X max map coord of the window to update + * @param max_y Y max map coord of the window to update + */ + virtual void updateCosts( + nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, + int max_i, int max_j); + + /** + * @brief Reset this costmap + */ + virtual void reset(); + + virtual void onFootprintChanged(); + + /** + * @brief If clearing operations should be processed on this layer or not + */ + virtual bool isClearable() {return true;} + + bool getSegmentations(std::vector & segmentations) const; + + rcl_interfaces::msg::SetParametersResult dynamicParametersCallback( + std::vector parameters); + +private: + void syncSegmPointcloudCb( + const std::shared_ptr & segmentation, + const std::shared_ptr & pointcloud, + const std::shared_ptr & buffer); + + void labelinfoCb( + const std::shared_ptr & label_info, + const std::shared_ptr & buffer); + + std::vector>> + semantic_segmentation_subs_; + std::vector< + std::shared_ptr>> + label_info_subs_; + std::vector< + std::shared_ptr>> + pointcloud_subs_; + std::vector< + std::shared_ptr>> + segm_pc_notifiers_; + std::vector>> + pointcloud_tf_subs_; + + // debug publishers + std::map>> proc_pointcloud_pubs_map_; + + std::vector> segmentation_buffers_; + + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; + + std::string global_frame_; + std::string topics_string_; + + std::map class_map_; + + bool rolling_window_; + bool was_reset_; + int combination_method_; +}; + +} // namespace nav2_costmap_2d + +#endif // SEMANTIC_SEGMENTATION_LAYER_HPP_ diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index 77d020799bc..fdcd5cf127e 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -32,6 +32,7 @@ rclcpp_lifecycle sensor_msgs std_msgs + vision_msgs std_srvs tf2 tf2_geometry_msgs @@ -51,4 +52,4 @@ ament_cmake - + \ No newline at end of file diff --git a/nav2_costmap_2d/plugins/semantic_segmentation_layer.cpp b/nav2_costmap_2d/plugins/semantic_segmentation_layer.cpp new file mode 100644 index 00000000000..d1bb6e31a99 --- /dev/null +++ b/nav2_costmap_2d/plugins/semantic_segmentation_layer.cpp @@ -0,0 +1,481 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * Copyright (c) 2020, Samsung R&D Institute Russia + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Eitan Marder-Eppstein + * David V. Lu!! + * Alexey Merzlyakov + * + * Reference tutorial: + * https://navigation.ros.org/tutorials/docs/writing_new_costmap2d_plugin.html + *********************************************************************/ +#include "nav2_costmap_2d/semantic_segmentation_layer.hpp" + +#include "nav2_costmap_2d/costmap_math.hpp" +#include "nav2_costmap_2d/footprint.hpp" +#include "rclcpp/parameter_events_filter.hpp" + +using nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE; +using nav2_costmap_2d::LETHAL_OBSTACLE; +using nav2_costmap_2d::NO_INFORMATION; + +namespace nav2_costmap_2d +{ + +SemanticSegmentationLayer::SemanticSegmentationLayer() {} + +// This method is called at the end of plugin initialization. +// It contains ROS parameter(s) declaration and initialization +// of need_recalculation_ variable. +void SemanticSegmentationLayer::onInitialize() +{ + current_ = true; + was_reset_ = false; + auto node = node_.lock(); + if (!node) { + throw std::runtime_error{"Failed to lock node"}; + } + std::string segmentation_topic, pointcloud_topic, labels_topic, sensor_frame; + std::vector class_types_string; + double max_obstacle_distance, min_obstacle_distance, observation_keep_time, transform_tolerance, + expected_update_rate; + bool track_unknown_space; + + declareParameter("enabled", rclcpp::ParameterValue(true)); + declareParameter("combination_method", rclcpp::ParameterValue(1)); + declareParameter("observation_sources", rclcpp::ParameterValue(std::string(""))); + declareParameter("publish_debug_topics", rclcpp::ParameterValue(false)); + + node->get_parameter(name_ + "." + "enabled", enabled_); + node->get_parameter(name_ + "." + "combination_method", combination_method_); + node->get_parameter("track_unknown_space", track_unknown_space); + node->get_parameter("transform_tolerance", transform_tolerance); + + global_frame_ = layered_costmap_->getGlobalFrameID(); + rolling_window_ = layered_costmap_->isRolling(); + + if (track_unknown_space) { + default_value_ = NO_INFORMATION; + } else { + default_value_ = FREE_SPACE; + } + + matchSize(); + + node->get_parameter(name_ + "." + "observation_sources", topics_string_); + + // now we need to split the topics based on whitespace which we can use a stringstream for + std::stringstream ss(topics_string_); + + std::string source; + + while (ss >> source) { + declareParameter(source + "." + "segmentation_topic", rclcpp::ParameterValue("")); + declareParameter(source + "." + "labels_topic", rclcpp::ParameterValue("")); + declareParameter(source + "." + "pointcloud_topic", rclcpp::ParameterValue("")); + declareParameter(source + "." + "observation_persistence", rclcpp::ParameterValue(0.0)); + declareParameter(source + "." + "sensor_frame", rclcpp::ParameterValue("")); + declareParameter(source + "." + "expected_update_rate", rclcpp::ParameterValue(0.0)); + declareParameter( + source + "." + "class_types", + rclcpp::ParameterValue(std::vector({}))); + declareParameter(source + "." + "max_obstacle_distance", rclcpp::ParameterValue(5.0)); + declareParameter(source + "." + "min_obstacle_distance", rclcpp::ParameterValue(0.3)); + + node->get_parameter(name_ + "." + source + "." + "segmentation_topic", segmentation_topic); + node->get_parameter(name_ + "." + source + "." + "labels_topic", labels_topic); + node->get_parameter(name_ + "." + source + "." + "pointcloud_topic", pointcloud_topic); + node->get_parameter( + name_ + "." + source + "." + "observation_persistence", + observation_keep_time); + node->get_parameter(name_ + "." + source + "." + "sensor_frame", sensor_frame); + node->get_parameter(name_ + "." + source + "." + "expected_update_rate", expected_update_rate); + node->get_parameter(name_ + "." + source + "." + "class_types", class_types_string); + node->get_parameter( + name_ + "." + source + "." + "max_obstacle_distance", + max_obstacle_distance); + node->get_parameter( + name_ + "." + source + "." + "min_obstacle_distance", + min_obstacle_distance); + if (class_types_string.empty()) { + RCLCPP_ERROR( + logger_, + "no class types defined for source %s. Segmentation plugin cannot work this way", + source.c_str()); + exit(-1); + } + + std::unordered_map class_map; + + for (auto & class_type : class_types_string) { + std::vector classes_ids; + uint8_t cost; + declareParameter( + source + "." + class_type + ".classes", + rclcpp::ParameterValue(std::vector({}))); + declareParameter(source + "." + class_type + ".cost", rclcpp::ParameterValue(0)); + node->get_parameter(name_ + "." + source + "." + class_type + ".classes", classes_ids); + if (classes_ids.empty()) { + RCLCPP_ERROR(logger_, "no classes defined on type %s", class_type.c_str()); + continue; + } + node->get_parameter(name_ + "." + source + "." + class_type + ".cost", cost); + for (auto & class_id : classes_ids) { + class_map.insert(std::pair(class_id, cost)); + } + } + + if (class_map.empty()) { + RCLCPP_ERROR( + logger_, + "No classes defined for source %s. Segmentation plugin cannot work this way", + source.c_str()); + exit(-1); + } + + //sensor data subscriptions + auto sub_opt = rclcpp::SubscriptionOptions(); + sub_opt.callback_group = callback_group_; + rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_sensor_data; + + // label info subscription + rclcpp::SubscriptionOptionsWithAllocator> tl_sub_opt; + tl_sub_opt.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable; + tl_sub_opt.callback_group = callback_group_; + rmw_qos_profile_t tl_qos = rmw_qos_profile_system_default; + tl_qos.history = RMW_QOS_POLICY_HISTORY_KEEP_ALL; + tl_qos.depth = 5; + tl_qos.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; + tl_qos.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL; + + auto segmentation_buffer = std::make_shared( + node, source, class_types_string, class_map, observation_keep_time, expected_update_rate, + max_obstacle_distance, + min_obstacle_distance, *tf_, global_frame_, sensor_frame, + tf2::durationFromSec(transform_tolerance)); + + segmentation_buffers_.push_back(segmentation_buffer); + + + auto semantic_segmentation_sub = + std::make_shared>( + node, segmentation_topic, custom_qos_profile, sub_opt); + semantic_segmentation_subs_.push_back(semantic_segmentation_sub); + // semantic_segmentation_sub->registerCallback([&](std::shared_ptr /*msg*/){ + // std::cout << "got sgm" << std::endl; + // }); + + auto label_info_sub = std::make_shared>( + node, labels_topic, tl_qos, tl_sub_opt); + label_info_sub->registerCallback( + std::bind( + &SemanticSegmentationLayer::labelinfoCb, this, + std::placeholders::_1, segmentation_buffers_.back())); + label_info_subs_.push_back(label_info_sub); + + auto pointcloud_sub = std::make_shared>( + node, pointcloud_topic, custom_qos_profile, sub_opt); + pointcloud_subs_.push_back(pointcloud_sub); + // pointcloud_sub->registerCallback([&](std::shared_ptr /*msg*/){ + // std::cout << "got pc" << std::endl; + // }); + + auto pointcloud_tf_sub = + std::make_shared>( + *pointcloud_subs_.back(), *tf_, global_frame_, 1000, node->get_node_logging_interface(), + node->get_node_clock_interface(), + tf2::durationFromSec(transform_tolerance)); + // pointcloud_tf_sub->registerCallback([&](std::shared_ptr /*msg*/){ + // std::cout << "got pc tf" << std::endl; + // }); + pointcloud_tf_subs_.push_back(pointcloud_tf_sub); + + auto segm_pc_sync = + std::make_shared>( + *semantic_segmentation_subs_.back(), *pointcloud_tf_subs_.back(), 1000); + segm_pc_sync->registerCallback( + std::bind( + &SemanticSegmentationLayer::syncSegmPointcloudCb, this, + std::placeholders::_1, std::placeholders::_2, segmentation_buffers_.back())); + + segm_pc_notifiers_.push_back(segm_pc_sync); + } + + dyn_params_handler_ = node->add_on_set_parameters_callback( + std::bind( + &SemanticSegmentationLayer::dynamicParametersCallback, + this, + std::placeholders::_1)); +} + +// The method is called to ask the plugin: which area of costmap it needs to update. +// Inside this method window bounds are re-calculated if need_recalculation_ is true +// and updated independently on its value. +void SemanticSegmentationLayer::updateBounds( + double robot_x, double robot_y, double /*robot_yaw*/, + double * min_x, double * min_y, double * max_x, + double * max_y) +{ + std::lock_guard guard(*getMutex()); + if (rolling_window_) { + updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2); + } + if (!enabled_) { + return; + } + std::vector segmentations; + getSegmentations(segmentations); + + + current_ = true; + + for (auto & segmentation : segmentations) { + sensor_msgs::PointCloud2ConstIterator iter_x(*segmentation.cloud_, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y(*segmentation.cloud_, "y"); + sensor_msgs::PointCloud2ConstIterator iter_class(*segmentation.cloud_, "class"); + // sensor_msgs::PointCloud2ConstIterator iter_confidence(*segmentation.cloud_, + // "confidence"); + for (size_t point = 0; point < segmentation.cloud_->height * segmentation.cloud_->width; + point++) + { + unsigned int mx, my; + if (!worldToMap(*(iter_x + point), *(iter_y + point), mx, my)) { + RCLCPP_DEBUG(logger_, "Computing map coords failed"); + continue; + } + unsigned int index = getIndex(mx, my); + uint8_t class_id = *(iter_class + point); + if (!segmentation.class_map_.count(class_id)) { + RCLCPP_DEBUG(logger_, "Cost for class id %i was not defined, skipping", class_id); + continue; + } + costmap_[index] = segmentation.class_map_[class_id]; + touch(*(iter_x + point), *(iter_y + point), min_x, min_y, max_x, max_y); + } + } +} + +// The method is called when footprint was changed. +// Here it just resets need_recalculation_ variable. +void SemanticSegmentationLayer::onFootprintChanged() +{ + RCLCPP_DEBUG( + rclcpp::get_logger("nav2_costmap_2d"), + "SemanticSegmentationLayer::onFootprintChanged(): num footprint points: %lu", + layered_costmap_->getFootprint().size()); +} + +// The method is called when costmap recalculation is required. +// It updates the costmap within its window bounds. +// Inside this method the costmap gradient is generated and is writing directly +// to the resulting costmap master_grid without any merging with previous layers. +void SemanticSegmentationLayer::updateCosts( + nav2_costmap_2d::Costmap2D & master_grid, int min_i, + int min_j, int max_i, int max_j) +{ + std::lock_guard guard(*getMutex()); + if (!enabled_) { + return; + } + + if (!current_ && was_reset_) { + was_reset_ = false; + current_ = true; + } + if (!costmap_) { + return; + } + // RCLCPP_INFO(logger_, "Updating costmap"); + switch (combination_method_) { + case 0: // Overwrite + updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j); + break; + case 1: // Maximum + updateWithMax(master_grid, min_i, min_j, max_i, max_j); + break; + default: // Nothing + break; + } +} + +void SemanticSegmentationLayer::labelinfoCb( + const std::shared_ptr & label_info, + const std::shared_ptr & buffer) +{ + buffer->createClassIdCostMap(*label_info); +} + +void SemanticSegmentationLayer::syncSegmPointcloudCb( + const std::shared_ptr & segmentation, + const std::shared_ptr & pointcloud, + const std::shared_ptr & buffer) +{ + if (segmentation->width * segmentation->height != pointcloud->width * pointcloud->height) { + RCLCPP_WARN( + logger_, + "Pointcloud and segmentation sizes are different, will not buffer message. " + "segmentation->width:%u, " + "segmentation->height:%u, pointcloud->width:%u, pointcloud->height:%u", + segmentation->width, segmentation->height, pointcloud->width, pointcloud->height); + return; + } + unsigned expected_array_size = segmentation->width * segmentation->height; + if (segmentation->data.size() < expected_array_size) { + RCLCPP_WARN( + logger_, + "segmentation arrays have wrong sizes: data->%lu, expected->%u. " + "Will not buffer message", + segmentation->data.size(), expected_array_size); + return; + } + if (buffer->isClassIdCostMapEmpty()) { + RCLCPP_WARN( + logger_, + "Class map is empty because a labelinfo message has not been received for topic %s. Will not buffer message", + buffer->getBufferSource().c_str()); + return; + } + buffer->lock(); + // we are passing the segmentation as confidence temporarily while we figure out a good use for getting + // confidence maps + buffer->bufferSegmentation(*pointcloud, *segmentation, *segmentation); + buffer->unlock(); +} + +void SemanticSegmentationLayer::reset() +{ + resetMaps(); + current_ = false; + was_reset_ = true; +} + +bool SemanticSegmentationLayer::getSegmentations( + std::vector & segmentations) const +{ + bool current = true; + // get the marking observations + for (unsigned int i = 0; i < segmentation_buffers_.size(); ++i) { + segmentation_buffers_[i]->lock(); + segmentation_buffers_[i]->getSegmentations(segmentations); + current = segmentation_buffers_[i]->isCurrent() && current; + segmentation_buffers_[i]->unlock(); + } + return current; +} + +rcl_interfaces::msg::SetParametersResult +SemanticSegmentationLayer::dynamicParametersCallback( + std::vector parameters) +{ + std::lock_guard guard(*getMutex()); + auto result = rcl_interfaces::msg::SetParametersResult(); + for (auto parameter : parameters) { + const auto & type = parameter.get_type(); + const auto & name = parameter.get_name(); + + if (type == rclcpp::ParameterType::PARAMETER_BOOL) { + if (name == name_ + "." + "enabled") { + enabled_ = parameter.as_bool(); + } + } + + std::stringstream ss(topics_string_); + std::string source; + while (ss >> source) { + if (type == rclcpp::ParameterType::PARAMETER_DOUBLE) { + if (name == name_ + "." + source + "." + "max_obstacle_distance") { + for (auto & buffer : segmentation_buffers_) { + if (buffer->getBufferSource() == source) { + buffer->setMaxObstacleDistance(parameter.as_double()); + } + } + } else if (name == name_ + "." + source + "." + "min_obstacle_distance") { + for (auto & buffer : segmentation_buffers_) { + if (buffer->getBufferSource() == source) { + buffer->setMinObstacleDistance(parameter.as_double()); + } + } + } + // allow to change which class ids belong to each type + } else if (type == rclcpp::ParameterType::PARAMETER_STRING_ARRAY) { + for (auto & buffer : segmentation_buffers_) { + if (buffer->getBufferSource() == source) { + for (auto & class_type : buffer->getClassTypes()) { + if (name == name_ + "." + source + "." + class_type + "." + "classes") { + int class_type_cost; + node_.lock()->get_parameter( + name_ + "." + source + "." + class_type + ".cost", + class_type_cost); + for (auto & class_name : parameter.as_string_array()) { + buffer->updateClassMap(class_name, class_type_cost); + } + } + } + } + } + } else if (type == rclcpp::ParameterType::PARAMETER_INTEGER) { + for (auto & buffer : segmentation_buffers_) { + if (buffer->getBufferSource() == source) { + for (auto & class_type : buffer->getClassTypes()) { + if (name == name_ + "." + source + "." + class_type + "." + "cost") { + std::vector class_names_for_type; + node_.lock()->get_parameter( + name_ + "." + source + "." + class_type + ".classes", + class_names_for_type); + for (auto & class_name : class_names_for_type) { + buffer->updateClassMap(class_name, parameter.as_int()); + } + } + } + } + } + } + } + } + + result.successful = true; + return result; +} + +} // namespace nav2_costmap_2d + +// This is the macro allowing a nav2_costmap_2d::SemanticSegmentationLayer class +// to be registered in order to be dynamically loadable of base type nav2_costmap_2d::Layer. +// Usually places in the end of cpp-file where the loadable class written. +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(nav2_costmap_2d::SemanticSegmentationLayer, nav2_costmap_2d::Layer) diff --git a/nav2_costmap_2d/src/segmentation_buffer.cpp b/nav2_costmap_2d/src/segmentation_buffer.cpp new file mode 100644 index 00000000000..a3c8f7a6c7d --- /dev/null +++ b/nav2_costmap_2d/src/segmentation_buffer.cpp @@ -0,0 +1,265 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, 2013, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Pedro Gonzalez + *********************************************************************/ +#include "nav2_costmap_2d/segmentation_buffer.hpp" + +#include +#include +#include +#include +#include + +#include "sensor_msgs/point_cloud2_iterator.hpp" +#include "tf2/convert.h" +using namespace std::chrono_literals; + +namespace nav2_costmap_2d +{ +SegmentationBuffer::SegmentationBuffer( + const nav2_util::LifecycleNode::WeakPtr & parent, + std::string buffer_source, std::vector class_types, std::unordered_map class_names_cost_map, double observation_keep_time, + double expected_update_rate, double max_lookahead_distance, + double min_lookahead_distance, tf2_ros::Buffer & tf2_buffer, + std::string global_frame, std::string sensor_frame, + tf2::Duration tf_tolerance) +: tf2_buffer_(tf2_buffer) + , class_types_(class_types) + , class_names_cost_map_(class_names_cost_map) + , observation_keep_time_(rclcpp::Duration::from_seconds(observation_keep_time)) + , expected_update_rate_(rclcpp::Duration::from_seconds(expected_update_rate)) + , global_frame_(global_frame) + , sensor_frame_(sensor_frame) + , buffer_source_(buffer_source) + , sq_max_lookahead_distance_(std::pow(max_lookahead_distance, 2)) + , sq_min_lookahead_distance_(std::pow(min_lookahead_distance, 2)) + , tf_tolerance_(tf_tolerance) +{ + auto node = parent.lock(); + clock_ = node->get_clock(); + logger_ = node->get_logger(); + last_updated_ = node->now(); +} + +SegmentationBuffer::~SegmentationBuffer() {} + +void SegmentationBuffer::createClassIdCostMap(const vision_msgs::msg::LabelInfo & label_info) +{ + for (const auto & semantic_class : label_info.class_map) { + class_ids_cost_map_[semantic_class.class_id] = class_names_cost_map_[semantic_class.class_name]; + } +} + +void SegmentationBuffer::bufferSegmentation( + const sensor_msgs::msg::PointCloud2 & cloud, + const sensor_msgs::msg::Image & segmentation, + const sensor_msgs::msg::Image & confidence) +{ + geometry_msgs::msg::PointStamped global_origin; + + // create a new segmentation on the list to be populated + segmentation_list_.push_front(Segmentation()); + + // check whether the origin frame has been set explicitly + // or whether we should get it from the cloud + std::string origin_frame = sensor_frame_ == "" ? cloud.header.frame_id : sensor_frame_; + + try { + // given these segmentations come from sensors... + // we'll need to store the origin pt of the sensor + geometry_msgs::msg::PointStamped local_origin; + local_origin.header.stamp = cloud.header.stamp; + local_origin.header.frame_id = origin_frame; + local_origin.point.x = 0; + local_origin.point.y = 0; + local_origin.point.z = 0; + tf2_buffer_.transform(local_origin, global_origin, global_frame_, tf_tolerance_); + tf2::convert(global_origin.point, segmentation_list_.front().origin_); + + sensor_msgs::msg::PointCloud2 global_frame_cloud; + + // transform the point cloud + tf2_buffer_.transform(cloud, global_frame_cloud, global_frame_, tf_tolerance_); + global_frame_cloud.header.stamp = cloud.header.stamp; + + // now we need to remove segmentations from the cloud that are below + // or above our height thresholds + sensor_msgs::msg::PointCloud2 & segmentation_cloud = *(segmentation_list_.front().cloud_); + segmentation_cloud.height = global_frame_cloud.height; + segmentation_cloud.width = global_frame_cloud.width; + segmentation_cloud.fields = global_frame_cloud.fields; + segmentation_cloud.is_bigendian = global_frame_cloud.is_bigendian; + segmentation_cloud.point_step = global_frame_cloud.point_step; + segmentation_cloud.row_step = global_frame_cloud.row_step; + segmentation_cloud.is_dense = global_frame_cloud.is_dense; + + unsigned int cloud_size = global_frame_cloud.height * global_frame_cloud.width; + sensor_msgs::PointCloud2Modifier modifier(segmentation_cloud); + + segmentation_cloud.point_step = + addPointField( + segmentation_cloud, "class", 1, sensor_msgs::msg::PointField::INT8, + segmentation_cloud.point_step); + segmentation_cloud.point_step = + addPointField( + segmentation_cloud, "confidence", 1, sensor_msgs::msg::PointField::INT8, + segmentation_cloud.point_step); + modifier.resize(cloud_size); + sensor_msgs::PointCloud2Iterator iter_class_obs(segmentation_cloud, "class"); + sensor_msgs::PointCloud2Iterator iter_confidence_obs(segmentation_cloud, "confidence"); + sensor_msgs::PointCloud2Iterator iter_x_obs(segmentation_cloud, "x"); + sensor_msgs::PointCloud2Iterator iter_y_obs(segmentation_cloud, "y"); + sensor_msgs::PointCloud2Iterator iter_z_obs(segmentation_cloud, "z"); + + sensor_msgs::PointCloud2ConstIterator iter_x_global(global_frame_cloud, "x"); + sensor_msgs::PointCloud2ConstIterator iter_y_global(global_frame_cloud, "y"); + sensor_msgs::PointCloud2ConstIterator iter_z_global(global_frame_cloud, "z"); + unsigned int point_count = 0; + + // copy over the points that are within our segmentation range + for (size_t v = 0; v < segmentation.height; v++) { + for (size_t u = 0; u < segmentation.width; u++) { + int pixel_idx = v * segmentation.width + u; + // remove invalid points + if (!std::isfinite(*(iter_z_global + pixel_idx))) { + continue; + } + double sq_dist = + std::pow(*(iter_x_global + pixel_idx) - segmentation_list_.front().origin_.x, 2) + + std::pow(*(iter_y_global + pixel_idx) - segmentation_list_.front().origin_.y, 2) + + std::pow(*(iter_z_global + pixel_idx) - segmentation_list_.front().origin_.z, 2); + if (sq_dist >= sq_max_lookahead_distance_ || sq_dist <= sq_min_lookahead_distance_) { + continue; + } + *(iter_class_obs + point_count) = segmentation.data[pixel_idx]; + *(iter_confidence_obs + point_count) = confidence.data[pixel_idx]; + *(iter_x_obs + point_count) = *(iter_x_global + pixel_idx); + *(iter_y_obs + point_count) = *(iter_y_global + pixel_idx); + *(iter_z_obs + point_count) = *(iter_z_global + pixel_idx); + point_count++; + } + } + + // resize the cloud for the number of legal points + modifier.resize(point_count); + segmentation_cloud.header.stamp = cloud.header.stamp; + segmentation_cloud.header.frame_id = global_frame_cloud.header.frame_id; + + segmentation_list_.front().class_map_ = class_ids_cost_map_; + } catch (tf2::TransformException & ex) { + // if an exception occurs, we need to remove the empty segmentation from the list + segmentation_list_.pop_front(); + RCLCPP_ERROR( + logger_, + "TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s", + sensor_frame_.c_str(), cloud.header.frame_id.c_str(), ex.what()); + return; + } + + // if the update was successful, we want to update the last updated time + last_updated_ = clock_->now(); + + // we'll also remove any stale segmentations from the list + purgeStaleSegmentations(); +} + +// returns a copy of the segmentations +void SegmentationBuffer::getSegmentations(std::vector & segmentations) +{ + // first... let's make sure that we don't have any stale segmentations + purgeStaleSegmentations(); + + // now we'll just copy the segmentations for the caller + std::list::iterator obs_it; + for (obs_it = segmentation_list_.begin(); obs_it != segmentation_list_.end(); ++obs_it) { + segmentations.push_back(*obs_it); + } + segmentation_list_.clear(); +} + +std::unordered_map SegmentationBuffer::getClassMap() +{ + return class_names_cost_map_; +} + +void SegmentationBuffer::purgeStaleSegmentations() +{ + if (!segmentation_list_.empty()) { + std::list::iterator obs_it = segmentation_list_.begin(); + // if we're keeping segmentations for no time... then we'll only keep one segmentation + if (observation_keep_time_ == rclcpp::Duration(0.0s)) { + segmentation_list_.erase(++obs_it, segmentation_list_.end()); + return; + } + + // otherwise... we'll have to loop through the segmentations to see which ones are stale + for (obs_it = segmentation_list_.begin(); obs_it != segmentation_list_.end(); ++obs_it) { + Segmentation & obs = *obs_it; + // check if the segmentation is out of date... and if it is, + // remove it and those that follow from the list + if ((clock_->now() - obs.cloud_->header.stamp) > observation_keep_time_) { + segmentation_list_.erase(obs_it, segmentation_list_.end()); + return; + } + } + } +} + +void SegmentationBuffer::updateClassMap(std::string new_class, uint8_t new_cost) +{ + class_names_cost_map_[new_class] = new_cost; +} + +bool SegmentationBuffer::isCurrent() const +{ + if (expected_update_rate_ == rclcpp::Duration(0.0s)) { + return true; + } + + bool current = (clock_->now() - last_updated_) <= expected_update_rate_; + if (!current) { + RCLCPP_WARN( + logger_, + "The %s segmentation buffer has not been updated for %.2f seconds, " + "and it should be updated every %.2f seconds.", + buffer_source_.c_str(), (clock_->now() - last_updated_).seconds(), + expected_update_rate_.seconds()); + } + return current; +} + +void SegmentationBuffer::resetLastUpdated() {last_updated_ = clock_->now();} +} // namespace nav2_costmap_2d