diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp index eef3ad2c869..0c636536b34 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/bt_action_server_impl.hpp @@ -135,7 +135,7 @@ bool BtActionServer::on_configure() node, "robot_base_frame", rclcpp::ParameterValue(std::string("base_link"))); nav2_util::declare_parameter_if_not_declared( node, "transform_tolerance", rclcpp::ParameterValue(0.1)); - rclcpp::copy_all_parameter_values(node, client_node_); + nav2_util::copy_all_parameters(node, client_node_); // set the timeout in seconds for the action server to discard goal handles if not finished double action_server_result_timeout; diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt index 723f81f0f9b..97dade4efc6 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) @@ -40,6 +41,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 @@ -66,6 +68,7 @@ set(dependencies rclcpp_lifecycle sensor_msgs std_msgs + vision_msgs std_srvs tf2 tf2_geometry_msgs @@ -88,6 +91,7 @@ add_library(layers SHARED plugins/voxel_layer.cpp plugins/range_sensor_layer.cpp plugins/denoise_layer.cpp + plugins/semantic_segmentation_layer.cpp ) add_library(${PROJECT_NAME}::layers ALIAS layers) ament_target_dependencies(layers diff --git a/nav2_costmap_2d/costmap_plugins.xml b/nav2_costmap_2d/costmap_plugins.xml index 6748cd5fcae..ccc9766e691 100644 --- a/nav2_costmap_2d/costmap_plugins.xml +++ b/nav2_costmap_2d/costmap_plugins.xml @@ -18,6 +18,9 @@ Filters noise-induced freestanding obstacles or small obstacles groups + + A plugin for semantic segmentation data produced by RGBD cameras + diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.hpp index 2ff834a41e7..57d2b6a1196 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/observation_buffer.hpp @@ -62,6 +62,7 @@ class ObservationBuffer /** * @brief Constructs an observation buffer * @param topic_name The topic of the observations, used as an identifier for error and warning messages + * @param source_name The name of the source as declared in the parameters, used to change source specific params * @param observation_keep_time Defines the persistence of observations 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 @@ -78,6 +79,7 @@ class ObservationBuffer ObservationBuffer( const nav2_util::LifecycleNode::WeakPtr & parent, std::string topic_name, + std::string source_name, double observation_keep_time, double expected_update_rate, double min_obstacle_height, double max_obstacle_height, double obstacle_max_range, @@ -132,6 +134,22 @@ class ObservationBuffer */ void resetLastUpdated(); + /** + * @brief Set max obstacle distance + */ + void setMaxObstacleDistance(double max_obstacle_distance) + { + obstacle_max_range_ = max_obstacle_distance; + } + + /** + * @brief Get source name + */ + std::string getSourceName() + { + return source_name_; + } + private: /** * @brief Removes any stale observations from the buffer list @@ -148,6 +166,7 @@ class ObservationBuffer std::string sensor_frame_; std::list observation_list_; std::string topic_name_; + std::string source_name_; double min_obstacle_height_, max_obstacle_height_; std::recursive_mutex lock_; ///< @brief A lock for accessing data in callbacks safely double obstacle_max_range_, obstacle_min_range_, raytrace_max_range_, raytrace_min_range_; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp index 4b8409f4678..6b1a9723444 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/obstacle_layer.hpp @@ -253,6 +253,7 @@ class ObstacleLayer : public CostmapLayer bool rolling_window_; bool was_reset_; nav2_costmap_2d::CombinationMethod combination_method_; + std::string _topics_string; }; } // namespace nav2_costmap_2d 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..07281ce7314 --- /dev/null +++ b/nav2_costmap_2d/include/nav2_costmap_2d/segmentation_buffer.hpp @@ -0,0 +1,628 @@ +/********************************************************************* + * + * 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_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" + +/** + * @brief Represents the parameters associated with the cost calculation for a given class + */ +struct CostHeuristicParams +{ + uint8_t base_cost, max_cost, mark_confidence; + int samples_to_max_cost; +}; + +/** + * @brief Represents a 2D grid index with equality comparison. Supports negative indexes + */ +struct TileIndex { + int x, y; + + bool operator==(const TileIndex& other) const { + return x == other.x && y == other.y; + } +}; + +namespace std { + /** + * @brief Custom hash function for TileIndex to enable its use as a key in unordered_map. + */ + template<> + struct hash { + size_t operator()(const TileIndex& coord) const { + // Compute individual hash values for two integers + // and combine them using bitwise XOR + // and bit shifting: + return std::hash()(coord.x) ^ (std::hash()(coord.y) << 1); + } + }; +} + + +/** + * @brief Represents the world coordinates of a tile. + */ +struct TileWorldXY +{ + double x, y; +}; + +/** + * @brief Encapsulates the observation data for a tile, including class ID, cost, confidence, and timestamp. + */ +struct TileObservation { + using UniquePtr = std::unique_ptr; + + uint8_t class_id; + float confidence; + double timestamp; +}; + +/** + * @brief Manages temporal observations with a decay mechanism, maintaining a sum of confidences. + * Wraps a std::deque to store observations, allowing for efficient insertion and removal. + */ +class TemporalObservationQueue { +private: + + std::deque queue_; + float confidence_sum_ = 0.0f; + double decay_time_; + +public: + TemporalObservationQueue(){} + + /** + * @brief Adds an observation to the queue, resets the queue if class ID changes. + * @param tile_obs The observation to add. + */ + void push(TileObservation tile_obs) { + if(tile_obs.class_id != getClassId()) + { + std::deque emptyQueue; + std::swap(queue_, emptyQueue); + confidence_sum_ = 0.0; + } + confidence_sum_ += tile_obs.confidence; + queue_.push_back(tile_obs); + } + + /** + * @brief Removes the oldest observation from the queue. + */ + void pop() { + if (!queue_.empty()) { + confidence_sum_ -= queue_.front().confidence; + queue_.pop_front(); + } + } + + /** + * @brief Checks if the queue is empty. + * @return True if empty, false otherwise. + */ + bool empty() const {return queue_.empty();} + + /** + * @brief Gets the size of the queue. + * @return The number of observations in the queue. + */ + int size() const { return queue_.size(); } + + /** + * @brief Sets the decay time for observations. + * @param decay_time The decay time in seconds. + */ + void setDecayTime(float decay_time) + { + decay_time_ = decay_time; + } + + /** + * @brief Gets the current sum of confidence values of all observations. + * @return The sum of confidences. + */ + float getConfidenceSum() const { + return confidence_sum_; + } + + /** + * @brief Gets the class ID of the most recent observation. + * @return The class ID, or 0 if queue is empty. + */ + uint8_t getClassId() const + { + if(!queue_.empty()) + { + return queue_.back().class_id; + } + return 0; + } + + /** + * @brief Returns a copy of the deque object. Will have overhead + * due to the copy operation but avoids race conditions since + * the object in the class is not made editable by others + * @return The class cost, or 0 if queue is empty. + */ + std::deque getQueue() + { + return queue_; + } + + /** + * @brief Removes observations older than the decay time. + * @param current_time The current time for comparison. + */ + void purgeOld(double current_time) { + while (!queue_.empty()) { + double age = current_time - queue_.front().timestamp; + if (age > decay_time_) { + pop(); + } else { + break; + } + } + } +}; + +/** + * @brief Manages a map of tile observations, allowing for spatial and temporal querying. + * Utilizes an unordered_map to efficiently index observations by tile and supports locking for thread safety. + */ +class SegmentationTileMap { + private: + std::unordered_map tile_map_; + float resolution_; + float decay_time_; + std::recursive_mutex lock_; + + + public: + using SharedPtr = std::shared_ptr; + + // Define iterator types + using Iterator = typename std::unordered_map::iterator; + using ConstIterator = typename std::unordered_map::const_iterator; + + SegmentationTileMap(float resolution, float decay_time) : resolution_(resolution), decay_time_(decay_time) { + // 10k observations seemed to be a good estimate of the amount of data to be held for a decay time of ~5s + tile_map_.reserve(1e4); + } + SegmentationTileMap(){} + + // Return iterator to the beginning of the tile_map_ + Iterator begin() { return tile_map_.begin(); } + ConstIterator begin() const { return tile_map_.begin(); } + + // Return iterator to the end of the tile_map_ + Iterator end() { return tile_map_.end(); } + ConstIterator end() const { return tile_map_.end(); } + + /** + * @brief Locks the map for exclusive access. + */ + inline void lock() { lock_.lock(); } + + /** + * @brief Unlocks the map. + */ + inline void unlock() { lock_.unlock(); } + + /** + * @brief Returns the number of elements in the map. + * @return The size of the map. + */ + int size() + { + return tile_map_.size(); + } + + /** + * @brief Converts world coordinates to a TileIndex. + * @param x X coordinate in world space. + * @param y Y coordinate in world space. + * @return The corresponding TileIndex. + */ + TileIndex worldToIndex(double x, double y) const { + // Convert world coordinates to grid indices + int ix = static_cast(std::floor(x / resolution_)); + int iy = static_cast(std::floor(y / resolution_)); + return TileIndex{ix, iy}; + } + + /** + * @brief Converts a TileIndex to world coordinates. + * @param idx The index to convert. + * @return The world coordinates of the tile's center. + */ + TileWorldXY indexToWorld(int x, int y) const { + // Calculate the world coordinates of the center of the grid cell + double x_world = (static_cast(x) + 0.5) * resolution_; + double y_world = (static_cast(y) + 0.5) * resolution_; + return TileWorldXY{x_world, y_world}; + } + + /** + * @brief Adds an observation to the specified tile. + * @param obs The observation to add. + * @param idx The index of the tile. + */ + void pushObservation(TileObservation& obs, TileIndex& idx) + { + auto it = tile_map_.find(idx); + if (it != tile_map_.end()) { + // TileIndex exists, push the observation + it->second.push(obs); + } else { + // TileIndex does not exist, create a new TemporalObservationQueue with decay time + TemporalObservationQueue& queue = tile_map_[idx]; + queue.setDecayTime(decay_time_); + queue.push(obs); + } + } + + /** + * @brief Removes observations older than the decay time from all tiles. + * @param current_time The current time for comparison. + */ + void purgeOldObservations(double current_time) + { + std::vector tiles_to_remove; + for (auto& tile : tile_map_) + { + tile.second.purgeOld(current_time); + if(tile.second.empty()) + { + tiles_to_remove.emplace_back(tile.first); + } + } + if(tile_map_.size() > 0) + for (auto& tile : tiles_to_remove) + { + tile_map_.erase(tile); + } + } +}; + +/** + * @brief Struct for holding the relevant data of any observation. Includes + * its position, its confidence, the confidence sum of the tile and the + * class to which it belongs + */ +struct PointData { + float x, y, z; + float confidence, confidence_sum; + uint8_t class_id; +}; + +/** + * @brief Creates a PointCloud2 message that contains a visual representation of + * a temporal tile map. There's a "column" of points on each tile, each point represents + * a segmentation observation over that tile and they are all stacked together. Each observation + * Has a channel for the class, for the confidence, and the confidence sum of the observations + * over that tile + * @param tileMap The segmentation tile map + */ +sensor_msgs::msg::PointCloud2 visualizeTemporalTileMap(SegmentationTileMap& tileMap) { + sensor_msgs::msg::PointCloud2 cloud; + cloud.header.frame_id = "map"; // Set appropriate frame_id + cloud.header.stamp = rclcpp::Clock().now(); // Set current time as timestamp + + // Define fields for PointCloud2 + sensor_msgs::PointCloud2Modifier modifier(cloud); + modifier.setPointCloud2Fields(6, "x", 1, sensor_msgs::msg::PointField::FLOAT32, + "y", 1, sensor_msgs::msg::PointField::FLOAT32, + "z", 1, sensor_msgs::msg::PointField::FLOAT32, + "confidence", 1, sensor_msgs::msg::PointField::FLOAT32, + "confidence_sum", 1, sensor_msgs::msg::PointField::FLOAT32, + "class", 1, sensor_msgs::msg::PointField::UINT8); + + // Reserve space for points + std::vector points; + for (auto& tile : tileMap) { + TileIndex idx = tile.first; + TileWorldXY worldXY = tileMap.indexToWorld(idx.x, idx.y); + double z = 0.0; + for (auto& obs : tile.second.getQueue()) { + PointData point; + point.x = worldXY.x; + point.y = worldXY.y; + point.z = z; + point.confidence = obs.confidence; + point.confidence_sum = tile.second.getConfidenceSum() / tile.second.size(); + point.class_id = static_cast(obs.class_id); + points.push_back(point); + z += 0.02; // Increment Z by 0.02m for each observation + } + } + + // Set data in PointCloud2 + modifier.resize(points.size()); // Number of points + sensor_msgs::PointCloud2Iterator iter_x(cloud, "x"); + sensor_msgs::PointCloud2Iterator iter_y(cloud, "y"); + sensor_msgs::PointCloud2Iterator iter_z(cloud, "z"); + sensor_msgs::PointCloud2Iterator iter_confidence(cloud, "confidence"); + sensor_msgs::PointCloud2Iterator iter_confidence_sum(cloud, "confidence_sum"); + sensor_msgs::PointCloud2Iterator iter_class(cloud, "class"); + + for (const auto& point : points) { + *iter_x = point.x; + *iter_y = point.y; + *iter_z = point.z; + *iter_confidence = point.confidence; + *iter_confidence_sum = point.confidence_sum; + *iter_class = point.class_id; + ++iter_x; ++iter_y; ++iter_z; ++iter_confidence;++iter_confidence_sum; ++iter_class; + } + + return cloud; +} + +/** + * Manages segmentation class information, including mapping between class names and IDs, + * as well as managing the cost heuristic parameters associated with each class. + */ +class SegmentationCostMultimap { +public: + SegmentationCostMultimap(){} + /** + * Constructs the SegmentationCostMultimap. + * + * @param nameToIdMap A map from class names to class IDs. + * @param nameToCostMap A map from class names to CostHeuristicParams. + */ + SegmentationCostMultimap(const std::unordered_map& nameToIdMap, + const std::unordered_map& nameToCostMap) { + name_to_id_ = nameToIdMap; + for (const auto& pair : nameToIdMap) { + const auto& name = pair.first; + uint8_t id = pair.second; + CostHeuristicParams cost = nameToCostMap.at(name); + id_to_cost_[id] = cost; + } + } + + /** + * Updates the cost heuristic parameters associated with a class ID. + * + * @param id The class ID. + * @param cost The new CostHeuristicParams to associate with the class. + */ + void updateCostById(uint8_t id, const CostHeuristicParams& cost) { + id_to_cost_[id] = cost; + } + + /** + * Retrieves the cost heuristic parameters associated with a class ID. + * + * @param id The class ID. + * @return The CostHeuristicParams associated with the class. + */ + CostHeuristicParams getCostById(uint8_t id) const { + return id_to_cost_.at(id); + } + + /** + * Updates the cost heuristic parameters associated with a class name. + * + * @param name The class name. + * @param cost The new CostHeuristicParams to associate with the class. + */ + void updateCostByName(const std::string& name, const CostHeuristicParams& cost) { + uint8_t id = name_to_id_.at(name); + id_to_cost_[id] = cost; + } + + /** + * Retrieves the cost heuristic parameters associated with a class name. + * + * @param name The class name. + * @return The CostHeuristicParams associated with the class. + */ + CostHeuristicParams getCostByName(const std::string& name) const { + uint8_t id = name_to_id_.at(name); + return id_to_cost_.at(id); + } + + bool empty() + { + return name_to_id_.empty() || id_to_cost_.empty(); + } + +private: + std::unordered_map name_to_id_; // Maps class names to class IDs + std::unordered_map id_to_cost_; // Maps class IDs to CostHeuristicParams +}; + +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: + using SharedPtr = std::shared_ptr; + /** + * @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, double costmap_resolution, double tile_map_decay_time, bool visualize_tile_map = false); + + /** + * @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 gets the class map associated with the segmentations stored in the buffer + * @return the class map + */ + std::unordered_map getClassMap(); + + void createSegmentationCostMultimap(const vision_msgs::msg::LabelInfo& label_info); + + bool isClassIdCostMapEmpty() { return segmentation_cost_multimap_.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, CostHeuristicParams new_cost); + + SegmentationTileMap::SharedPtr getSegmentationTileMap() + { + return temporal_tile_map_; + } + + CostHeuristicParams getCostForClassId(uint8_t class_id) + { + return segmentation_cost_multimap_.getCostById(class_id); + } + + CostHeuristicParams getCostForClassName(std::string class_name) + { + return segmentation_cost_multimap_.getCostByName(class_name); + } + + 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_; + 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::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_; + + SegmentationCostMultimap segmentation_cost_multimap_; + + SegmentationTileMap::SharedPtr temporal_tile_map_; + + bool visualize_tile_map_; + rclcpp::Publisher::SharedPtr tile_map_pub_; +}; +} // 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..d04a9c84262 --- /dev/null +++ b/nav2_costmap_2d/include/nav2_costmap_2d/semantic_segmentation_layer.hpp @@ -0,0 +1,175 @@ +/********************************************************************* + * + * 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 +#include + +#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_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; } + + /** + * @brief Get the buffers and the tile maps the plugin stores. one for each source. Takes a vector of tile maps + * as reference and fills it inside the function + * @param segmentation_tile_maps the vector of tile maps to be filled by the function + * @return whether the tile maps could be retrieved and filled successfully + */ + bool getSegmentationTileMaps(std::vector>& segmentation_tile_maps); + + 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 syncSegmConfPointcloudCb(const std::shared_ptr& segmentation, + const std::shared_ptr& confidence, + 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>> + semantic_segmentation_confidence_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< + std::shared_ptr>> + segm_conf_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_ \ No newline at end of file diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index 77d020799bc..7ce21b875f5 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 diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index cccae4d3938..15405d8d0e5 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -73,9 +73,6 @@ void ObstacleLayer::onInitialize() bool track_unknown_space; double transform_tolerance; - // The topics that we'll subscribe to from the parameter server - std::string topics_string; - declareParameter("enabled", rclcpp::ParameterValue(true)); declareParameter("footprint_clearing_enabled", rclcpp::ParameterValue(true)); declareParameter("min_obstacle_height", rclcpp::ParameterValue(0.0)); @@ -94,7 +91,7 @@ void ObstacleLayer::onInitialize() node->get_parameter(name_ + "." + "max_obstacle_height", max_obstacle_height_); node->get_parameter("track_unknown_space", track_unknown_space); node->get_parameter("transform_tolerance", transform_tolerance); - node->get_parameter(name_ + "." + "observation_sources", topics_string); + node->get_parameter(name_ + "." + "observation_sources", _topics_string); int combination_method_param{}; node->get_parameter(name_ + "." + "combination_method", combination_method_param); @@ -108,7 +105,7 @@ void ObstacleLayer::onInitialize() RCLCPP_INFO( logger_, - "Subscribed to Topics: %s", topics_string.c_str()); + "Subscribed to Topics: %s", _topics_string.c_str()); rolling_window_ = layered_costmap_->isRolling(); @@ -128,7 +125,7 @@ void ObstacleLayer::onInitialize() sub_opt.callback_group = callback_group_; // now we need to split the topics based on whitespace which we can use a stringstream for - std::stringstream ss(topics_string); + std::stringstream ss(_topics_string); std::string source; while (ss >> source) { @@ -197,7 +194,7 @@ void ObstacleLayer::onInitialize() std::shared_ptr( new ObservationBuffer( - node, topic, observation_keep_time, expected_update_rate, + node, topic, source, observation_keep_time, expected_update_rate, min_obstacle_height, max_obstacle_height, obstacle_max_range, obstacle_min_range, raytrace_max_range, raytrace_min_range, *tf_, @@ -317,6 +314,22 @@ ObstacleLayer::dynamicParametersCallback( combination_method_ = combination_method_from_int(parameter.as_int()); } } + std::stringstream ss(_topics_string); + std::string source; + while (ss >> source) + { + if (param_type == ParameterType::PARAMETER_DOUBLE) { + if (param_name == name_ + "." + source + "." + "obstacle_max_range") { + for (auto & buffer : observation_buffers_) { + if (buffer->getSourceName() == source) { + buffer->lock(); + buffer->setMaxObstacleDistance(parameter.as_double()); + buffer->unlock(); + } + } + } + } + } } result.successful = true; 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..d31e72753e5 --- /dev/null +++ b/nav2_costmap_2d/plugins/semantic_segmentation_layer.cpp @@ -0,0 +1,534 @@ +/********************************************************************* + * + * 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, confidence_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, tile_map_decay_time; + bool track_unknown_space, visualize_tile_map; + + 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 + "." + "confidence_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)); + declareParameter(source + "." + "tile_map_decay_time", rclcpp::ParameterValue(5.0)); + declareParameter(source + "." + "visualize_tile_map", rclcpp::ParameterValue(false)); + + node->get_parameter(name_ + "." + source + "." + "segmentation_topic", segmentation_topic); + node->get_parameter(name_ + "." + source + "." + "confidence_topic", confidence_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); + node->get_parameter(name_ + "." + source + "." + "tile_map_decay_time", tile_map_decay_time); + node->get_parameter(name_ + "." + source + "." + "visualize_tile_map", visualize_tile_map); + 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; + declareParameter(source + "." + class_type + ".classes", rclcpp::ParameterValue(std::vector({}))); + declareParameter(source + "." + class_type + ".base_cost", rclcpp::ParameterValue(0)); + declareParameter(source + "." + class_type + ".max_cost", rclcpp::ParameterValue(0)); + declareParameter(source + "." + class_type + ".mark_confidence", rclcpp::ParameterValue(0)); + declareParameter(source + "." + class_type + ".samples_to_max_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; + } + CostHeuristicParams cost_params; + node->get_parameter(name_ + "." + source + "." + class_type + ".base_cost", cost_params.base_cost); + node->get_parameter(name_ + "." + source + "." + class_type + ".max_cost", cost_params.max_cost); + node->get_parameter(name_ + "." + source + "." + class_type + ".mark_confidence", cost_params.mark_confidence); + node->get_parameter(name_ + "." + source + "." + class_type + ".samples_to_max_cost", cost_params.samples_to_max_cost); + for (auto& class_id : classes_ids) + { + class_map.insert(std::pair(class_id, cost_params)); + } + } + + 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), getResolution(), tile_map_decay_time, visualize_tile_map); + + 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); + + 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); + + 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_subs_.push_back(pointcloud_tf_sub); + + if(!confidence_topic.empty()) + { + auto semantic_segmentation_confidence_sub = + std::make_shared>( + node, confidence_topic, custom_qos_profile, sub_opt); + semantic_segmentation_confidence_subs_.push_back(semantic_segmentation_confidence_sub); + auto segm_conf_pc_sync = + std::make_shared>( + *semantic_segmentation_subs_.back(), *semantic_segmentation_confidence_subs_.back(), *pointcloud_tf_subs_.back(), 1000); + segm_conf_pc_sync->registerCallback(std::bind(&SemanticSegmentationLayer::syncSegmConfPointcloudCb, this, + std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, segmentation_buffers_.back())); + segm_conf_pc_notifiers_.push_back(segm_conf_pc_sync); + RCLCPP_INFO(logger_, "Confidence is enabled for source %s", source.c_str()); + } + else + { + RCLCPP_WARN(logger_, "Confidence topic was empty for source %s, not using segmentation confidence in that source", source.c_str()); + 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> segmentation_tile_maps; + getSegmentationTileMaps(segmentation_tile_maps); + for (auto& tile_map : segmentation_tile_maps) + { + for(auto& tile: *tile_map.first) + { + TileWorldXY tile_world_coords = tile_map.first->indexToWorld(tile.first.x, tile.first.y); + // alias tile.second with a name for more readability + TemporalObservationQueue& obs_queue = tile.second; + unsigned int mx, my; + if (!worldToMap(tile_world_coords.x, tile_world_coords.y, mx, my)) + { + RCLCPP_DEBUG(logger_, "Computing map coords failed"); + continue; + } + unsigned int index = getIndex(mx, my); + CostHeuristicParams cost_params = tile_map.second->getCostForClassId(obs_queue.getClassId()); + if(obs_queue.size() >= cost_params.samples_to_max_cost && obs_queue.getConfidenceSum() / obs_queue.size() > cost_params.mark_confidence) + { + costmap_[index] = cost_params.max_cost; + } + else + { + costmap_[index] = cost_params.base_cost; + } + touch(tile_world_coords.x, tile_world_coords.y, min_x, min_y, max_x, max_y); + } + } + + current_ = true; +} + +// 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->createSegmentationCostMultimap(*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; + } + // if no confidence available, create a mask with all elements having max confidence + // in this case the plugin thresholding will only work with the number of observations + // accumulated in a given tile + sensor_msgs::msg::Image conf_mask = *segmentation; + std::fill(conf_mask.data.begin(), conf_mask.data.end(), 255); + buffer->lock(); + buffer->bufferSegmentation(*pointcloud, *segmentation, conf_mask); + buffer->unlock(); +} + +void SemanticSegmentationLayer::syncSegmConfPointcloudCb(const std::shared_ptr& segmentation, + const std::shared_ptr& confidence, + 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(); + buffer->bufferSegmentation(*pointcloud, *segmentation, *confidence); + buffer->unlock(); +} + +void SemanticSegmentationLayer::reset() +{ + resetMaps(); + current_ = false; + was_reset_ = true; +} + +bool SemanticSegmentationLayer::getSegmentationTileMaps( + std::vector>& segmentation_tile_maps) +{ + bool current = true; + // get the marking observations + for (unsigned int i = 0; i < segmentation_buffers_.size(); ++i) { + segmentation_buffers_[i]->lock(); + SegmentationTileMap::SharedPtr tile_map = segmentation_buffers_[i]->getSegmentationTileMap(); + segmentation_tile_maps.emplace_back(std::make_pair(tile_map, segmentation_buffers_[i])); + 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()); + } + } + } + } 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 + "." + "base_cost") { + CostHeuristicParams cost_params; + 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){ + cost_params = buffer->getCostForClassName(class_name); + cost_params.base_cost = parameter.as_int(); + buffer->updateClassMap(class_name, cost_params); + } + } + if (name == name_ + "." + source + "." + class_type + "." + "max_cost") { + CostHeuristicParams cost_params; + 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){ + cost_params = buffer->getCostForClassName(class_name); + cost_params.max_cost = parameter.as_int(); + buffer->updateClassMap(class_name, cost_params); + } + } + if (name == name_ + "." + source + "." + class_type + "." + "mark_confidence") { + CostHeuristicParams cost_params; + 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){ + cost_params = buffer->getCostForClassName(class_name); + cost_params.mark_confidence = parameter.as_int(); + buffer->updateClassMap(class_name, cost_params); + } + } + if (name == name_ + "." + source + "." + class_type + "." + "samples_to_max_cost") { + CostHeuristicParams cost_params; + 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){ + cost_params = buffer->getCostForClassName(class_name); + cost_params.samples_to_max_cost = parameter.as_int(); + buffer->updateClassMap(class_name, cost_params); + } + } + } + } + } + } + } + } + + 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) \ No newline at end of file diff --git a/nav2_costmap_2d/src/observation_buffer.cpp b/nav2_costmap_2d/src/observation_buffer.cpp index 8a06823df38..6f39dcc5220 100644 --- a/nav2_costmap_2d/src/observation_buffer.cpp +++ b/nav2_costmap_2d/src/observation_buffer.cpp @@ -51,6 +51,7 @@ namespace nav2_costmap_2d ObservationBuffer::ObservationBuffer( const nav2_util::LifecycleNode::WeakPtr & parent, std::string topic_name, + std::string source_name, double observation_keep_time, double expected_update_rate, double min_obstacle_height, double max_obstacle_height, double obstacle_max_range, @@ -65,6 +66,7 @@ ObservationBuffer::ObservationBuffer( global_frame_(global_frame), sensor_frame_(sensor_frame), topic_name_(topic_name), + source_name_(source_name), min_obstacle_height_(min_obstacle_height), max_obstacle_height_(max_obstacle_height), obstacle_max_range_(obstacle_max_range), obstacle_min_range_(obstacle_min_range), raytrace_max_range_(raytrace_max_range), raytrace_min_range_( diff --git a/nav2_costmap_2d/src/segmentation_buffer.cpp b/nav2_costmap_2d/src/segmentation_buffer.cpp new file mode 100644 index 00000000000..19843eb4310 --- /dev/null +++ b/nav2_costmap_2d/src/segmentation_buffer.cpp @@ -0,0 +1,238 @@ +/********************************************************************* + * + * 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 + *********************************************************************/ +#include "nav2_costmap_2d/segmentation_buffer.hpp" + +#include +#include +#include +#include +#include + +#include "sensor_msgs/point_cloud2_iterator.hpp" +#include "tf2/convert.h" +#include "rclcpp/rclcpp.hpp" +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, double costmap_resolution, double tile_map_decay_time, bool visualize_tile_map) + : 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(); + temporal_tile_map_ = std::make_shared(costmap_resolution, tile_map_decay_time); + visualize_tile_map_ = visualize_tile_map; + if(visualize_tile_map_) + { + tile_map_pub_ = node->create_publisher(buffer_source + "/tile_map",1); + } +} + +SegmentationBuffer::~SegmentationBuffer() {} + +void SegmentationBuffer::createSegmentationCostMultimap(const vision_msgs::msg::LabelInfo& label_info) +{ + std::unordered_map class_to_id_map; + for (const auto& semantic_class : label_info.class_map) + { + class_to_id_map[semantic_class.class_name] = semantic_class.class_id; + } + segmentation_cost_multimap_ = SegmentationCostMultimap(class_to_id_map, class_names_cost_map_); +} + +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; + // 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_); + + 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; + + 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"); + std::unordered_map best_observations_idxs; + double cloud_time_seconds = rclcpp::Time(cloud.header.stamp.sec, cloud.header.stamp.nanosec).seconds(); + + // 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))) + { + ++iter_x_global; + ++iter_y_global; + ++iter_z_global; + continue; + } + double sq_dist = + std::pow(*(iter_x_global) - global_origin.point.x, 2) + + std::pow(*(iter_y_global) - global_origin.point.y, 2) + + std::pow(*(iter_z_global) - global_origin.point.z, 2); + if (sq_dist >= sq_max_lookahead_distance_ || sq_dist <= sq_min_lookahead_distance_) + { + ++iter_x_global; + ++iter_y_global; + ++iter_z_global; + continue; + } + + TileIndex costmap_index = temporal_tile_map_->worldToIndex(*iter_x_global, *iter_y_global); + + // Find the observation with the greater confidence for each tile + auto it = best_observations_idxs.find(costmap_index); + // if an observation already exists in the index, compare its confidence to the current + // one and if its greater store this element as the new best confidence index for the tile + if (it != best_observations_idxs.end()) { + if(confidence.data[pixel_idx] > confidence.data[best_observations_idxs[costmap_index]]) + { + best_observations_idxs[costmap_index] = pixel_idx; + } + } + // if this is the first observation for the tile index just store its confidence as the best one + else + { + best_observations_idxs[costmap_index] = pixel_idx; + } + ++iter_x_global; + ++iter_y_global; + ++iter_z_global; + } + } + + // emplace the best observations in the mask into the tile map + temporal_tile_map_->lock(); + temporal_tile_map_->purgeOldObservations(cloud_time_seconds); + for (auto& idx : best_observations_idxs) + { + int img_idx_for_best_obs = idx.second; + TileIndex costmap_index = idx.first; + TileObservation best_obs{segmentation.data[img_idx_for_best_obs], static_cast(confidence.data[img_idx_for_best_obs]), cloud_time_seconds}; + temporal_tile_map_->pushObservation(best_obs, costmap_index); + } + temporal_tile_map_->unlock(); + + if(visualize_tile_map_) + { + sensor_msgs::msg::PointCloud2 tile_map_cloud = visualizeTemporalTileMap(*temporal_tile_map_); + tile_map_pub_->publish(tile_map_cloud); + } + + } catch (tf2::TransformException& ex) + { + 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(); +} + + +std::unordered_map SegmentationBuffer::getClassMap() +{ + return class_names_cost_map_; +} + + +void SegmentationBuffer::updateClassMap(std::string new_class, CostHeuristicParams new_cost) +{ + segmentation_cost_multimap_.updateCostByName(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 diff --git a/nav2_util/include/nav2_util/node_utils.hpp b/nav2_util/include/nav2_util/node_utils.hpp index 99570224add..a6a511a89c5 100644 --- a/nav2_util/include/nav2_util/node_utils.hpp +++ b/nav2_util/include/nav2_util/node_utils.hpp @@ -153,6 +153,25 @@ std::string get_plugin_type_param( return plugin_type; } +/** + * @brief A method to copy all parameters from one node (parent) to another (child). + * May throw parameter exceptions in error conditions + * @param parent Node to copy parameters from + * @param child Node to copy parameters to + */ +template +void copy_all_parameters(const NodeT1 & parent, const NodeT2 & child) +{ + using Parameters = std::vector; + std::vector param_names = parent->list_parameters({}, 0).names; + Parameters params = parent->get_parameters(param_names); + for (Parameters::const_iterator iter = params.begin(); iter != params.end(); ++iter) { + if (!child->has_parameter(iter->get_name())) { + child->declare_parameter(iter->get_name(), iter->get_parameter_value()); + } + } +} + /** * @brief Sets the caller thread to have a soft-realtime prioritization by * increasing the priority level of the host thread. diff --git a/nav2_util/test/test_node_utils.cpp b/nav2_util/test/test_node_utils.cpp index bf945276358..1ae944b46e4 100644 --- a/nav2_util/test/test_node_utils.cpp +++ b/nav2_util/test/test_node_utils.cpp @@ -94,3 +94,35 @@ TEST(GetPluginTypeParam, GetPluginTypeParam) ASSERT_EQ(get_plugin_type_param(node, "Foo"), "bar"); EXPECT_THROW(get_plugin_type_param(node, "Waldo"), std::runtime_error); } + +TEST(TestParamCopying, TestParamCopying) +{ + auto node1 = std::make_shared("test_node1"); + auto node2 = std::make_shared("test_node2"); + + // Tests for (1) multiple types, (2) recursion, (3) overriding values + node1->declare_parameter("Foo1", rclcpp::ParameterValue(std::string(("bar1")))); + node1->declare_parameter("Foo2", rclcpp::ParameterValue(0.123)); + node1->declare_parameter("Foo", rclcpp::ParameterValue(std::string(("bar")))); + node1->declare_parameter("Foo.bar", rclcpp::ParameterValue(std::string(("steve")))); + node2->declare_parameter("Foo", rclcpp::ParameterValue(std::string(("barz2")))); + + // Show Node2 is empty of Node1's parameters, but contains its own + EXPECT_FALSE(node2->has_parameter("Foo1")); + EXPECT_FALSE(node2->has_parameter("Foo2")); + EXPECT_FALSE(node2->has_parameter("Foo.bar")); + EXPECT_TRUE(node2->has_parameter("Foo")); + EXPECT_EQ(node2->get_parameter("Foo").as_string(), std::string("barz2")); + + nav2_util::copy_all_parameters(node1, node2); + + // Test new parameters exist, of expected value, and original param is not overridden + EXPECT_TRUE(node2->has_parameter("Foo1")); + EXPECT_EQ(node2->get_parameter("Foo1").as_string(), std::string("bar1")); + EXPECT_TRUE(node2->has_parameter("Foo2")); + EXPECT_EQ(node2->get_parameter("Foo2").as_double(), 0.123); + EXPECT_TRUE(node2->has_parameter("Foo.bar")); + EXPECT_EQ(node2->get_parameter("Foo.bar").as_string(), std::string("steve")); + EXPECT_TRUE(node2->has_parameter("Foo")); + EXPECT_EQ(node2->get_parameter("Foo").as_string(), std::string("barz2")); +} \ No newline at end of file