From 1e2c78dd6e1a15317e4b6283cbe1799368f84aae Mon Sep 17 00:00:00 2001 From: pepisg Date: Fri, 18 Feb 2022 16:58:41 -0500 Subject: [PATCH 01/13] sketching semantic segmentation functionality --- nav2_costmap_2d/CMakeLists.txt | 9 + nav2_costmap_2d/costmap_plugins.xml | 3 + .../include/nav2_costmap_2d/ray_tracer.hpp | 104 +++++++ .../semantic_segmentation_layer.hpp | 101 +++++++ nav2_costmap_2d/package.xml | 2 + .../plugins/semantic_segmentation_layer.cpp | 266 ++++++++++++++++++ nav2_costmap_2d/src/ray_tracer.cpp | 122 ++++++++ 7 files changed, 607 insertions(+) create mode 100644 nav2_costmap_2d/include/nav2_costmap_2d/ray_tracer.hpp create mode 100644 nav2_costmap_2d/include/nav2_costmap_2d/semantic_segmentation_layer.hpp create mode 100644 nav2_costmap_2d/plugins/semantic_segmentation_layer.cpp create mode 100644 nav2_costmap_2d/src/ray_tracer.cpp diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt index 84f86617ab..427541c0cd 100644 --- a/nav2_costmap_2d/CMakeLists.txt +++ b/nav2_costmap_2d/CMakeLists.txt @@ -17,10 +17,12 @@ 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(tf2_geometry_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_sensor_msgs REQUIRED) +find_package(image_geometry REQUIRED) find_package(visualization_msgs REQUIRED) find_package(angles REQUIRED) @@ -47,11 +49,15 @@ add_library(nav2_costmap_2d_core SHARED src/footprint.cpp src/costmap_layer.cpp src/observation_buffer.cpp + src/ray_tracer.cpp src/clear_costmap_service.cpp src/footprint_collision_checker.cpp plugins/costmap_filters/costmap_filter.cpp ) +# prevent pluginlib from using boost +target_compile_definitions(nav2_costmap_2d_core PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + set(dependencies geometry_msgs laser_geometry @@ -66,10 +72,12 @@ set(dependencies rclcpp_lifecycle sensor_msgs std_msgs + vision_msgs tf2 tf2_geometry_msgs tf2_ros tf2_sensor_msgs + image_geometry visualization_msgs angles ) @@ -85,6 +93,7 @@ add_library(layers SHARED src/observation_buffer.cpp plugins/voxel_layer.cpp plugins/range_sensor_layer.cpp + plugins/semantic_segmentation_layer.cpp ) ament_target_dependencies(layers ${dependencies} diff --git a/nav2_costmap_2d/costmap_plugins.xml b/nav2_costmap_2d/costmap_plugins.xml index d7cb985493..f6e212031b 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 + + This is an example plugin which puts repeating costs gradients to costmap + diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/ray_tracer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/ray_tracer.hpp new file mode 100644 index 0000000000..d1cf9ded88 --- /dev/null +++ b/nav2_costmap_2d/include/nav2_costmap_2d/ray_tracer.hpp @@ -0,0 +1,104 @@ +#ifndef NAV2_COSTMAP_2D__RAY_TRACER_HPP_ +#define NAV2_COSTMAP_2D__RAY_TRACER_HPP_ + +#include +#include +#include + +#include "image_geometry/pinhole_camera_model.h" +#include "nav2_util/lifecycle_node.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2_ros/buffer.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "opencv2/core.hpp" +#include "sensor_msgs/msg/camera_info.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "message_filters/subscriber.h" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "geometry_msgs/msg/point.hpp" +#include "geometry_msgs/msg/point_stamped.hpp" +#include "tf2/convert.h" +#include "tf2_eigen/tf2_eigen.hpp" + + + +namespace nav2_costmap_2d +{ +/** + * @class RayTracer + * @brief Takes in camera calibration data and traces pixels over a given plane + */ + +class RayTracer +{ +public: + /** + * @brief Constructs an observation buffer + * @param camera_info_topic_name The topic in which the camera calibration is published + * @param aligned_pc2_topic_name The topic of the image aligned pointcloud2 + * @param use_pointcloud Whether to use the pointcloud or the camera calibration for ray tracing + * @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 + */ + RayTracer(); + + /** + * @brief Destructor... cleans up + */ + ~RayTracer(){}; + + void initialize( + rclcpp::Node::SharedPtr & parent_node, + std::string camera_info_topic_name, + std::string aligned_pc2_topic_name, + bool use_pointcloud, + tf2_ros::Buffer * tf2_buffer, + std::string global_frame, + std::string sensor_frame, + tf2::Duration tf_tolerance, + double max_trace_distance + ); + + // geometry_msgs::msg::Point rayTracePixel(cv::Point& pixel); + std::vector rayTracePixels(std::vector& pixels); + + cv::Point2d worldToImage(geometry_msgs::msg::PointStamped& point); + + geometry_msgs::msg::PointStamped imageToGroundPlane(cv::Point& pixel, std::string& target_frame); + + +private: + +geometry_msgs::msg::Point intersectWithGroundPlane(Eigen::Isometry3d camera_to_ground_tf, cv::Point3d cv_ray); + + void cameraInfoCb(sensor_msgs::msg::CameraInfo::SharedPtr msg); + + void pointCloudCb(sensor_msgs::msg::PointCloud2::SharedPtr msg); + + rclcpp::Node::SharedPtr parent_node_; + std::string global_frame_; + std::string sensor_frame_; + tf2_ros::Buffer * tf2_buffer_; + std::string camera_info_topic_name_; + std::string aligned_pc2_topic_name_; + tf2::Duration tf_tolerance_; + bool use_pointcloud_; + double max_trace_distance_; + + bool ready_to_raytrace_ = false; + + rclcpp::Subscription::SharedPtr camera_info_sub_; + rclcpp::Subscription::SharedPtr aligned_pc2_sub_; + + sensor_msgs::msg::PointCloud2 pointcloud_msg_; + image_geometry::PinholeCameraModel camera_model_ = image_geometry::PinholeCameraModel(); + +}; + +} // namespace nav2_costmap_2d + + + +#endif // NAV2_COSTMAP_2D__RAY_TRACER_HPP_ \ No newline at end of file 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 0000000000..a0562566fd --- /dev/null +++ b/nav2_costmap_2d/include/nav2_costmap_2d/semantic_segmentation_layer.hpp @@ -0,0 +1,101 @@ +/********************************************************************* + * + * 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 + *********************************************************************/ +#ifndef SEMANTIC_SEGMENTATION_LAYER_HPP_ +#define SEMANTIC_SEGMENTATION_LAYER_HPP_ + +#include "nav2_costmap_2d/layer.hpp" +#include "nav2_costmap_2d/layered_costmap.hpp" +#include "nav2_costmap_2d/ray_tracer.hpp" +#include "rclcpp/rclcpp.hpp" +#include "vision_msgs/msg/semantic_segmentation.hpp" +#include "opencv2/core.hpp" + +namespace nav2_costmap_2d { + +class SemanticSegmentationLayer : public nav2_costmap_2d::Layer +{ + public: + SemanticSegmentationLayer(); + + virtual void onInitialize(); + virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, + double* max_x, double* max_y); + virtual void updateCosts(nav2_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j); + + virtual void reset() { return; } + + virtual void onFootprintChanged(); + + virtual bool isClearable() { return false; } + + private: + void segmentationCb(vision_msgs::msg::SemanticSegmentation::SharedPtr msg); + + void updateCostmap(vision_msgs::msg::SemanticSegmentation& msg); + + void extractPolygons(vision_msgs::msg::SemanticSegmentation& msg); + + RayTracer tracer_; + double last_min_x_, last_min_y_, last_max_x_, last_max_y_; + rclcpp::Subscription::SharedPtr semantic_segmentation_sub_; + + vision_msgs::msg::SemanticSegmentation latest_segmentation_message; + geometry_msgs::msg::TransformStamped transform_at_message; + geometry_msgs::msg::PointStamped max_range_point_; + + std::string global_frame_; + std::string robot_base_frame_; + tf2::Duration transform_tolerance_; + + // Indicates that the entire gradient should be recalculated next time. + bool need_recalculation_; + + // Size of gradient in cells + int GRADIENT_SIZE = 20; + // Step of increasing cost per one cell in gradient + int GRADIENT_FACTOR = 10; +}; + +} // 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 8865f88b55..16f48d6ed7 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -32,10 +32,12 @@ rclcpp_lifecycle sensor_msgs std_msgs + vision_msgs tf2 tf2_geometry_msgs tf2_ros tf2_sensor_msgs + image_geometry visualization_msgs angles 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 0000000000..d65edfd1d0 --- /dev/null +++ b/nav2_costmap_2d/plugins/semantic_segmentation_layer.cpp @@ -0,0 +1,266 @@ +/********************************************************************* + * + * 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() + : last_min_x_(-std::numeric_limits::max()) + , last_min_y_(-std::numeric_limits::max()) + , last_max_x_(std::numeric_limits::max()) + , last_max_y_(std::numeric_limits::max()) +{ +} + +// 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() +{ + auto node = node_.lock(); + if (!node) + { + throw std::runtime_error{"Failed to lock node"}; + } + std::string segmentation_topic, camera_info_topic, pointcloud_topic; + declareParameter("enabled", rclcpp::ParameterValue(true)); + node->get_parameter(name_ + "." + "enabled", enabled_); + declareParameter("segmentation_topic", rclcpp::ParameterValue(true)); + node->get_parameter(name_ + "." + "segmentation_topic", segmentation_topic); + declareParameter("camera_info_topic", rclcpp::ParameterValue(true)); + node->get_parameter(name_ + "." + "camera_info_topic", camera_info_topic); + declareParameter("pointcloud_topic", rclcpp::ParameterValue(true)); + node->get_parameter(name_ + "." + "pointcloud_topic", pointcloud_topic); + + max_range_point_.point.x = 5.0; + max_range_point_.header.frame_id = robot_base_frame_; + + semantic_segmentation_sub_ = node->create_subscription(segmentation_topic, rclcpp::SensorDataQoS(), std::bind(&SemanticSegmentationLayer::segmentationCb, this, std::placeholders::_1)); + + tracer_.initialize(rclcpp_node_, camera_info_topic, pointcloud_topic, false, tf_, "base_link", "camera", tf2::Duration(0), 10.0); + + need_recalculation_ = false; + current_ = true; +} + +// 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) +{ + if (need_recalculation_) + { + last_min_x_ = *min_x; + last_min_y_ = *min_y; + last_max_x_ = *max_x; + last_max_y_ = *max_y; + // For some reason when I make these -::max() it does not + // work with Costmap2D::worldToMapEnforceBounds(), so I'm using + // -::max() instead. + *min_x = -std::numeric_limits::max(); + *min_y = -std::numeric_limits::max(); + *max_x = std::numeric_limits::max(); + *max_y = std::numeric_limits::max(); + need_recalculation_ = false; + } + else + { + double tmp_min_x = last_min_x_; + double tmp_min_y = last_min_y_; + double tmp_max_x = last_max_x_; + double tmp_max_y = last_max_y_; + last_min_x_ = *min_x; + last_min_y_ = *min_y; + last_max_x_ = *max_x; + last_max_y_ = *max_y; + *min_x = std::min(tmp_min_x, *min_x); + *min_y = std::min(tmp_min_y, *min_y); + *max_x = std::max(tmp_max_x, *max_x); + *max_y = std::max(tmp_max_y, *max_y); + } +} + +// The method is called when footprint was changed. +// Here it just resets need_recalculation_ variable. +void SemanticSegmentationLayer::onFootprintChanged() +{ + need_recalculation_ = true; + + 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) +{ + if (!enabled_) + { + return; + } + + // master_array - is a direct pointer to the resulting master_grid. + // master_grid - is a resulting costmap combined from all layers. + // By using this pointer all layers will be overwritten! + // To work with costmap layer and merge it with other costmap layers, + // please use costmap_ pointer instead (this is pointer to current + // costmap layer grid) and then call one of updates methods: + // - updateWithAddition() + // - updateWithMax() + // - updateWithOverwrite() + // - updateWithTrueOverwrite() + // In this case using master_array pointer is equal to modifying local costmap_ + // pointer and then calling updateWithTrueOverwrite(): + unsigned char* master_array = master_grid.getCharMap(); + unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY(); + + // {min_i, min_j} - {max_i, max_j} - are update-window coordinates. + // These variables are used to update the costmap only within this window + // avoiding the updates of whole area. + // + // Fixing window coordinates with map size if necessary. + min_i = std::max(0, min_i); + min_j = std::max(0, min_j); + max_i = std::min(static_cast(size_x), max_i); + max_j = std::min(static_cast(size_y), max_j); + + // Simply computing one-by-one cost per each cell + int gradient_index; + for (int j = min_j; j < max_j; j++) + { + // Reset gradient_index each time when reaching the end of re-calculated window + // by OY axis. + gradient_index = 0; + for (int i = min_i; i < max_i; i++) + { + int index = master_grid.getIndex(i, j); + // setting the gradient cost + unsigned char cost = (LETHAL_OBSTACLE - gradient_index * GRADIENT_FACTOR) % 255; + if (gradient_index <= GRADIENT_SIZE) + { + gradient_index++; + } + else + { + gradient_index = 0; + } + master_array[index] = cost; + } + } +} + +void SemanticSegmentationLayer::segmentationCb(vision_msgs::msg::SemanticSegmentation::SharedPtr msg) +{ + latest_segmentation_message = *msg; + // updateCostmap(latest_segmentation_message); + geometry_msgs::msg::PointStamped max_range_point_cam_frame; + if (!tf_->canTransform( + latest_segmentation_message.header.frame_id, robot_base_frame_, tf2_ros::fromMsg(latest_segmentation_message.header.stamp))) + { + RCLCPP_INFO( + logger_, "Range sensor layer can't transform from %s to %s", + global_frame_.c_str(), latest_segmentation_message.header.frame_id.c_str()); + return; + } + tf_->transform(max_range_point_, max_range_point_cam_frame, latest_segmentation_message.header.frame_id, transform_tolerance_); + tracer_.worldToImage(max_range_point_cam_frame) +} + +void SemanticSegmentationLayer::updateCostmap(vision_msgs::msg::SemanticSegmentation& msg) +{ + cv::Mat mask_image(msg.width, msg.height, CV_8U); + mask_image.setTo(msg.data); + std::vector> contours; + std::vector hierarchy; + std::map> bird_view_polygons; + cv::findContours(mask_image, contours, hierarchy, cv::RETR_TREE, cv::CHAIN_APPROX_SIMPLE); + std::map class_map; + for(auto& class_type : msg.class_map) + { + class_map[class_type.class_id] = class_type.class_name; + // cv::Mat + } + // for(auto& contour : contours) + // { + + // } + for(unsigned i = 0; i < msg.data.size(); i++){ + if(msg.data[i] == 0) + { + continue; + } + else if(msg.data[i] == 1) + { + int mx, my; + (void) mx; + (void) my; + // if(worldToMap(tracer_->Ray)) + } + } +} + +void SemanticSegmentationLayer::extractPolygons(vision_msgs::msg::SemanticSegmentation& msg) +{ + (void) msg; +} + +} // 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/ray_tracer.cpp b/nav2_costmap_2d/src/ray_tracer.cpp new file mode 100644 index 0000000000..ac446362d3 --- /dev/null +++ b/nav2_costmap_2d/src/ray_tracer.cpp @@ -0,0 +1,122 @@ +#include "nav2_costmap_2d/ray_tracer.hpp" + +namespace nav2_costmap_2d +{ +RayTracer::RayTracer() +{ +} + +void RayTracer::initialize( + rclcpp::Node::SharedPtr & parent_node, + std::string camera_info_topic_name, + std::string aligned_pc2_topic_name, + bool use_pointcloud, + tf2_ros::Buffer * tf2_buffer, + std::string global_frame, + std::string sensor_frame, + tf2::Duration tf_tolerance, + double max_trace_distance +) +{ + parent_node_ = parent_node; + global_frame_ = global_frame; + sensor_frame_ = sensor_frame; + tf2_buffer_ = tf2_buffer; + camera_info_topic_name_ = camera_info_topic_name; + aligned_pc2_topic_name_ = aligned_pc2_topic_name; + tf_tolerance_ = tf_tolerance; + use_pointcloud_ = use_pointcloud; + max_trace_distance_ = max_trace_distance; + if(use_pointcloud_) + { + aligned_pc2_sub_ = parent_node_->create_subscription(aligned_pc2_topic_name_, rclcpp::SensorDataQoS(), std::bind(&RayTracer::pointCloudCb, this, std::placeholders::_1)); + RCLCPP_INFO(parent_node_->get_logger(), "Subscribed to %s", aligned_pc2_topic_name_.c_str()); + } + else + { + camera_info_sub_ = parent_node_->create_subscription(camera_info_topic_name_, rclcpp::SensorDataQoS(), std::bind(&RayTracer::cameraInfoCb, this, std::placeholders::_1)); + RCLCPP_INFO(parent_node_->get_logger(), "Subscribed to %s", camera_info_topic_name_.c_str()); + } +} + +std::vector RayTracer::rayTracePixels(std::vector& pixels) +{ + std::vector ans = std::vector(); + if(!ready_to_raytrace_) + { + RCLCPP_WARN(parent_node_->get_logger(), "No messages received yet on the topic"); + return ans; + } + try + { + geometry_msgs::msg::TransformStamped sensor_to_global = tf2_buffer_->lookupTransform(sensor_frame_, global_frame_, rclcpp::Time(0)); + Eigen::Isometry3d eigen_transform = tf2::transformToEigen(sensor_to_global.transform); + for(auto& pixel : pixels) + { + cv::Point3d cv_ray = camera_model_.projectPixelTo3dRay(pixel); + geometry_msgs::msg::Point p = intersectWithGroundPlane(eigen_transform, cv_ray); + ans.push_back(p); + } + return ans; + } + catch(tf2::TransformException & ex) + { + RCLCPP_ERROR( + parent_node_->get_logger(), + "TF Exception that should never happen for sensor frame: %s, global_frame: %s, %s", + sensor_frame_.c_str(), + global_frame_.c_str(), ex.what()); + return ans; + } +} + +geometry_msgs::msg::Point RayTracer::intersectWithGroundPlane(Eigen::Isometry3d camera_to_ground_tf, cv::Point3d cv_ray) +{ + Eigen::Vector3d eigen_ray({cv_ray.x, cv_ray.y, cv_ray.z}); + Eigen::Vector3d camera_origin = camera_to_ground_tf.translation(); + Eigen::Vector3d ray_end = camera_to_ground_tf*eigen_ray; + Eigen::Vector3d ground_plane_origin({0.0, 0.0, 0.0}); + Eigen::Vector3d ground_plane_normal({0.0, 0.0, 1.0}); + Eigen::Vector3d u = ray_end - camera_origin; + Eigen::Vector3d w = camera_origin - ground_plane_origin; + double D = ground_plane_normal.dot(u); + double N = -ground_plane_normal.dot(w); + if(abs(D) < 0.0000001){ + return geometry_msgs::msg::Point(); + } + double sI = N / D; + Eigen::Vector3d intersection = camera_origin + sI * u; + return tf2::toMsg(intersection); +} + +cv::Point2d RayTracer::worldToImage(geometry_msgs::msg::PointStamped& point){ + if (point.header.frame_id != sensor_frame_) + { + RCLCPP_INFO( + parent_node_->get_logger(), "Point has to be on the %s frame but is in frame %s", + sensor_frame_.c_str(), point.header.frame_id.c_str()); + return cv::Point2d(); + } + cv::Point3d cv_point{point.point.x, point.point.y, point.point.z}; + cv::Point2d pixel = camera_model_.project3dToPixel(cv_point); + return pixel; +} + +void RayTracer::pointCloudCb(sensor_msgs::msg::PointCloud2::SharedPtr msg) +{ + pointcloud_msg_ = *msg; + ready_to_raytrace_ = true; +} + +void RayTracer::cameraInfoCb(sensor_msgs::msg::CameraInfo::SharedPtr msg) +{ + RCLCPP_INFO(parent_node_->get_logger(), "Received camera info message. Ready to raytrace"); + camera_model_.fromCameraInfo(*msg); + camera_info_sub_.reset(); + ready_to_raytrace_ = true; +} + + + + +} // namespace nav2_costmap_2d \ No newline at end of file From f315e1ff500ebac77c70853763302a2c5b303e24 Mon Sep 17 00:00:00 2001 From: pepisg Date: Mon, 21 Feb 2022 17:10:11 -0500 Subject: [PATCH 02/13] WIP --- nav2_costmap_2d/plugins/semantic_segmentation_layer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_costmap_2d/plugins/semantic_segmentation_layer.cpp b/nav2_costmap_2d/plugins/semantic_segmentation_layer.cpp index d65edfd1d0..4e13c266fd 100644 --- a/nav2_costmap_2d/plugins/semantic_segmentation_layer.cpp +++ b/nav2_costmap_2d/plugins/semantic_segmentation_layer.cpp @@ -216,7 +216,7 @@ void SemanticSegmentationLayer::segmentationCb(vision_msgs::msg::SemanticSegment return; } tf_->transform(max_range_point_, max_range_point_cam_frame, latest_segmentation_message.header.frame_id, transform_tolerance_); - tracer_.worldToImage(max_range_point_cam_frame) + // cv::Point2d point_on_max_range = tracer_.worldToImage(max_range_point_cam_frame); } void SemanticSegmentationLayer::updateCostmap(vision_msgs::msg::SemanticSegmentation& msg) From 22725472b69f56f1725f4d0145fa68e726ec7050 Mon Sep 17 00:00:00 2001 From: pepisg Date: Thu, 3 Mar 2022 16:53:18 -0500 Subject: [PATCH 03/13] MVP for semantic segmentation plugin. Tested on local_costmap --- nav2_costmap_2d/CMakeLists.txt | 2 +- .../{ray_tracer.hpp => ray_caster.hpp} | 20 +- .../semantic_segmentation_layer.hpp | 27 +-- .../plugins/semantic_segmentation_layer.cpp | 206 ++++++------------ nav2_costmap_2d/src/ray_caster.cpp | 129 +++++++++++ nav2_costmap_2d/src/ray_tracer.cpp | 122 ----------- 6 files changed, 220 insertions(+), 286 deletions(-) rename nav2_costmap_2d/include/nav2_costmap_2d/{ray_tracer.hpp => ray_caster.hpp} (83%) create mode 100644 nav2_costmap_2d/src/ray_caster.cpp delete mode 100644 nav2_costmap_2d/src/ray_tracer.cpp diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt index 427541c0cd..204610fdbf 100644 --- a/nav2_costmap_2d/CMakeLists.txt +++ b/nav2_costmap_2d/CMakeLists.txt @@ -49,7 +49,7 @@ add_library(nav2_costmap_2d_core SHARED src/footprint.cpp src/costmap_layer.cpp src/observation_buffer.cpp - src/ray_tracer.cpp + src/ray_caster.cpp src/clear_costmap_service.cpp src/footprint_collision_checker.cpp plugins/costmap_filters/costmap_filter.cpp diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/ray_tracer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/ray_caster.hpp similarity index 83% rename from nav2_costmap_2d/include/nav2_costmap_2d/ray_tracer.hpp rename to nav2_costmap_2d/include/nav2_costmap_2d/ray_caster.hpp index d1cf9ded88..79dd0a27dd 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/ray_tracer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/ray_caster.hpp @@ -25,11 +25,11 @@ namespace nav2_costmap_2d { /** - * @class RayTracer + * @class RayCaster * @brief Takes in camera calibration data and traces pixels over a given plane */ -class RayTracer +class RayCaster { public: /** @@ -42,12 +42,12 @@ class RayTracer * @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 */ - RayTracer(); + RayCaster(); /** * @brief Destructor... cleans up */ - ~RayTracer(){}; + ~RayCaster(){}; void initialize( rclcpp::Node::SharedPtr & parent_node, @@ -56,22 +56,18 @@ class RayTracer bool use_pointcloud, tf2_ros::Buffer * tf2_buffer, std::string global_frame, - std::string sensor_frame, tf2::Duration tf_tolerance, double max_trace_distance ); - // geometry_msgs::msg::Point rayTracePixel(cv::Point& pixel); - std::vector rayTracePixels(std::vector& pixels); + bool worldToImage(geometry_msgs::msg::PointStamped& point, cv::Point2d& pixel); - cv::Point2d worldToImage(geometry_msgs::msg::PointStamped& point); - - geometry_msgs::msg::PointStamped imageToGroundPlane(cv::Point& pixel, std::string& target_frame); + bool imageToGroundPlane(cv::Point2d& pixel, geometry_msgs::msg::PointStamped& point); private: -geometry_msgs::msg::Point intersectWithGroundPlane(Eigen::Isometry3d camera_to_ground_tf, cv::Point3d cv_ray); + geometry_msgs::msg::PointStamped rayToPointStamped(cv::Point3d& ray_end, std::string& frame_id); void cameraInfoCb(sensor_msgs::msg::CameraInfo::SharedPtr msg); @@ -95,6 +91,8 @@ geometry_msgs::msg::Point intersectWithGroundPlane(Eigen::Isometry3d camera_to_g sensor_msgs::msg::PointCloud2 pointcloud_msg_; image_geometry::PinholeCameraModel camera_model_ = image_geometry::PinholeCameraModel(); + geometry_msgs::msg::PointStamped camera_origin_; + }; } // namespace nav2_costmap_2d 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 index a0562566fd..1cb7c24c05 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/semantic_segmentation_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/semantic_segmentation_layer.hpp @@ -43,16 +43,19 @@ #ifndef SEMANTIC_SEGMENTATION_LAYER_HPP_ #define SEMANTIC_SEGMENTATION_LAYER_HPP_ +#include "rclcpp/rclcpp.hpp" + #include "nav2_costmap_2d/layer.hpp" +#include "nav2_costmap_2d/costmap_layer.hpp" #include "nav2_costmap_2d/layered_costmap.hpp" -#include "nav2_costmap_2d/ray_tracer.hpp" +#include "nav2_costmap_2d/ray_caster.hpp" #include "rclcpp/rclcpp.hpp" #include "vision_msgs/msg/semantic_segmentation.hpp" #include "opencv2/core.hpp" namespace nav2_costmap_2d { -class SemanticSegmentationLayer : public nav2_costmap_2d::Layer +class SemanticSegmentationLayer : public CostmapLayer { public: SemanticSegmentationLayer(); @@ -62,7 +65,7 @@ class SemanticSegmentationLayer : public nav2_costmap_2d::Layer double* max_x, double* max_y); virtual void updateCosts(nav2_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j); - virtual void reset() { return; } + virtual void reset(); virtual void onFootprintChanged(); @@ -71,29 +74,17 @@ class SemanticSegmentationLayer : public nav2_costmap_2d::Layer private: void segmentationCb(vision_msgs::msg::SemanticSegmentation::SharedPtr msg); - void updateCostmap(vision_msgs::msg::SemanticSegmentation& msg); - - void extractPolygons(vision_msgs::msg::SemanticSegmentation& msg); + RayCaster tracer_; - RayTracer tracer_; - double last_min_x_, last_min_y_, last_max_x_, last_max_y_; rclcpp::Subscription::SharedPtr semantic_segmentation_sub_; vision_msgs::msg::SemanticSegmentation latest_segmentation_message; - geometry_msgs::msg::TransformStamped transform_at_message; - geometry_msgs::msg::PointStamped max_range_point_; std::string global_frame_; - std::string robot_base_frame_; - tf2::Duration transform_tolerance_; - // Indicates that the entire gradient should be recalculated next time. - bool need_recalculation_; + bool rolling_window_; + bool was_reset_; - // Size of gradient in cells - int GRADIENT_SIZE = 20; - // Step of increasing cost per one cell in gradient - int GRADIENT_FACTOR = 10; }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/plugins/semantic_segmentation_layer.cpp b/nav2_costmap_2d/plugins/semantic_segmentation_layer.cpp index 4e13c266fd..b72ea1ac6a 100644 --- a/nav2_costmap_2d/plugins/semantic_segmentation_layer.cpp +++ b/nav2_costmap_2d/plugins/semantic_segmentation_layer.cpp @@ -53,10 +53,6 @@ using nav2_costmap_2d::NO_INFORMATION; namespace nav2_costmap_2d { SemanticSegmentationLayer::SemanticSegmentationLayer() - : last_min_x_(-std::numeric_limits::max()) - , last_min_y_(-std::numeric_limits::max()) - , last_max_x_(std::numeric_limits::max()) - , last_max_y_(std::numeric_limits::max()) { } @@ -65,75 +61,93 @@ SemanticSegmentationLayer::SemanticSegmentationLayer() // 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, camera_info_topic, pointcloud_topic; + bool use_pointcloud; + double max_lookahead_distance; + declareParameter("enabled", rclcpp::ParameterValue(true)); node->get_parameter(name_ + "." + "enabled", enabled_); - declareParameter("segmentation_topic", rclcpp::ParameterValue(true)); + declareParameter("use_pointcloud", rclcpp::ParameterValue(false)); + node->get_parameter(name_ + "." + "use_pointcloud", use_pointcloud); + declareParameter("max_lookahead_distance", rclcpp::ParameterValue(3.0)); + node->get_parameter(name_ + "." + "max_lookahead_distance", max_lookahead_distance); + declareParameter("segmentation_topic", rclcpp::ParameterValue("")); node->get_parameter(name_ + "." + "segmentation_topic", segmentation_topic); - declareParameter("camera_info_topic", rclcpp::ParameterValue(true)); + declareParameter("camera_info_topic", rclcpp::ParameterValue("")); node->get_parameter(name_ + "." + "camera_info_topic", camera_info_topic); - declareParameter("pointcloud_topic", rclcpp::ParameterValue(true)); + declareParameter("pointcloud_topic", rclcpp::ParameterValue("")); node->get_parameter(name_ + "." + "pointcloud_topic", pointcloud_topic); - max_range_point_.point.x = 5.0; - max_range_point_.header.frame_id = robot_base_frame_; + global_frame_ = layered_costmap_->getGlobalFrameID(); + rolling_window_ = layered_costmap_->isRolling(); + + matchSize(); semantic_segmentation_sub_ = node->create_subscription(segmentation_topic, rclcpp::SensorDataQoS(), std::bind(&SemanticSegmentationLayer::segmentationCb, this, std::placeholders::_1)); - tracer_.initialize(rclcpp_node_, camera_info_topic, pointcloud_topic, false, tf_, "base_link", "camera", tf2::Duration(0), 10.0); + tracer_.initialize(rclcpp_node_, camera_info_topic, pointcloud_topic, use_pointcloud, tf_, global_frame_, tf2::Duration(0), max_lookahead_distance); - need_recalculation_ = false; - current_ = true; } // 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*/, +void SemanticSegmentationLayer::updateBounds(double robot_x, double robot_y, double /*robot_yaw*/, double* min_x, double* min_y, double* max_x, double* max_y) { - if (need_recalculation_) - { - last_min_x_ = *min_x; - last_min_y_ = *min_y; - last_max_x_ = *max_x; - last_max_y_ = *max_y; - // For some reason when I make these -::max() it does not - // work with Costmap2D::worldToMapEnforceBounds(), so I'm using - // -::max() instead. - *min_x = -std::numeric_limits::max(); - *min_y = -std::numeric_limits::max(); - *max_x = std::numeric_limits::max(); - *max_y = std::numeric_limits::max(); - need_recalculation_ = false; + if (rolling_window_) { + updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2); } - else + if (!enabled_) { + current_ = true; + return; + } + vision_msgs::msg::SemanticSegmentation current_segmentation = latest_segmentation_message; + int touched_cells = 0; + for(size_t v = 0; v < current_segmentation.height; v=v+5) { - double tmp_min_x = last_min_x_; - double tmp_min_y = last_min_y_; - double tmp_max_x = last_max_x_; - double tmp_max_y = last_max_y_; - last_min_x_ = *min_x; - last_min_y_ = *min_y; - last_max_x_ = *max_x; - last_max_y_ = *max_y; - *min_x = std::min(tmp_min_x, *min_x); - *min_y = std::min(tmp_min_y, *min_y); - *max_x = std::max(tmp_max_x, *max_x); - *max_y = std::max(tmp_max_y, *max_y); + for(size_t u = 0; u < current_segmentation.width; u=u+5){ + uint8_t pixel = current_segmentation.data[v*current_segmentation.width+u]; + cv::Point2d pixel_idx; + pixel_idx.x = u; + pixel_idx.y = v; + geometry_msgs::msg::PointStamped world_point; + if(!tracer_.imageToGroundPlane(pixel_idx, world_point)) + { + RCLCPP_DEBUG(logger_, "Could not raycast"); + continue; + } + unsigned int mx, my; + if(!worldToMap(world_point.point.x, world_point.point.y, mx, my)) + { + RCLCPP_DEBUG(logger_, "Computing map coords failed"); + continue; + } + unsigned int index = getIndex(mx, my); + if(pixel!= 1){ + costmap_[index] = 100; + }else{ + costmap_[index] = 0; + } + touch(world_point.point.x, world_point.point.y, min_x, min_y, max_x, max_y); + touched_cells++; + } } + // RCLCPP_INFO(logger_, "touched %i cells", touched_cells); + } // The method is called when footprint was changed. // Here it just resets need_recalculation_ variable. void SemanticSegmentationLayer::onFootprintChanged() { - need_recalculation_ = true; RCLCPP_DEBUG(rclcpp::get_logger("nav2_costmap_2d"), "SemanticSegmentationLayer::onFootprintChanged(): num footprint points: %lu", @@ -152,109 +166,33 @@ void SemanticSegmentationLayer::updateCosts(nav2_costmap_2d::Costmap2D& master_g return; } - // master_array - is a direct pointer to the resulting master_grid. - // master_grid - is a resulting costmap combined from all layers. - // By using this pointer all layers will be overwritten! - // To work with costmap layer and merge it with other costmap layers, - // please use costmap_ pointer instead (this is pointer to current - // costmap layer grid) and then call one of updates methods: - // - updateWithAddition() - // - updateWithMax() - // - updateWithOverwrite() - // - updateWithTrueOverwrite() - // In this case using master_array pointer is equal to modifying local costmap_ - // pointer and then calling updateWithTrueOverwrite(): - unsigned char* master_array = master_grid.getCharMap(); - unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY(); - - // {min_i, min_j} - {max_i, max_j} - are update-window coordinates. - // These variables are used to update the costmap only within this window - // avoiding the updates of whole area. - // - // Fixing window coordinates with map size if necessary. - min_i = std::max(0, min_i); - min_j = std::max(0, min_j); - max_i = std::min(static_cast(size_x), max_i); - max_j = std::min(static_cast(size_y), max_j); - - // Simply computing one-by-one cost per each cell - int gradient_index; - for (int j = min_j; j < max_j; j++) - { - // Reset gradient_index each time when reaching the end of re-calculated window - // by OY axis. - gradient_index = 0; - for (int i = min_i; i < max_i; i++) - { - int index = master_grid.getIndex(i, j); - // setting the gradient cost - unsigned char cost = (LETHAL_OBSTACLE - gradient_index * GRADIENT_FACTOR) % 255; - if (gradient_index <= GRADIENT_SIZE) - { - gradient_index++; - } - else - { - gradient_index = 0; - } - master_array[index] = cost; - } + if (!current_ && was_reset_) { + was_reset_ = false; + current_ = true; } -} - -void SemanticSegmentationLayer::segmentationCb(vision_msgs::msg::SemanticSegmentation::SharedPtr msg) -{ - latest_segmentation_message = *msg; - // updateCostmap(latest_segmentation_message); - geometry_msgs::msg::PointStamped max_range_point_cam_frame; - if (!tf_->canTransform( - latest_segmentation_message.header.frame_id, robot_base_frame_, tf2_ros::fromMsg(latest_segmentation_message.header.stamp))) + // (void) max_j; + // (void) master_grid; + // (void) min_i; + // (void) max_i; + // (void) min_j; + if(!costmap_) { - RCLCPP_INFO( - logger_, "Range sensor layer can't transform from %s to %s", - global_frame_.c_str(), latest_segmentation_message.header.frame_id.c_str()); return; } - tf_->transform(max_range_point_, max_range_point_cam_frame, latest_segmentation_message.header.frame_id, transform_tolerance_); - // cv::Point2d point_on_max_range = tracer_.worldToImage(max_range_point_cam_frame); + // RCLCPP_INFO(logger_, "Updating costmap"); + updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j); } -void SemanticSegmentationLayer::updateCostmap(vision_msgs::msg::SemanticSegmentation& msg) +void SemanticSegmentationLayer::segmentationCb(vision_msgs::msg::SemanticSegmentation::SharedPtr msg) { - cv::Mat mask_image(msg.width, msg.height, CV_8U); - mask_image.setTo(msg.data); - std::vector> contours; - std::vector hierarchy; - std::map> bird_view_polygons; - cv::findContours(mask_image, contours, hierarchy, cv::RETR_TREE, cv::CHAIN_APPROX_SIMPLE); - std::map class_map; - for(auto& class_type : msg.class_map) - { - class_map[class_type.class_id] = class_type.class_name; - // cv::Mat - } - // for(auto& contour : contours) - // { - - // } - for(unsigned i = 0; i < msg.data.size(); i++){ - if(msg.data[i] == 0) - { - continue; - } - else if(msg.data[i] == 1) - { - int mx, my; - (void) mx; - (void) my; - // if(worldToMap(tracer_->Ray)) - } - } + latest_segmentation_message = *msg; } -void SemanticSegmentationLayer::extractPolygons(vision_msgs::msg::SemanticSegmentation& msg) +void SemanticSegmentationLayer::reset() { - (void) msg; + resetMaps(); + current_ = false; + was_reset_ = true; } } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/src/ray_caster.cpp b/nav2_costmap_2d/src/ray_caster.cpp new file mode 100644 index 0000000000..86bc35523e --- /dev/null +++ b/nav2_costmap_2d/src/ray_caster.cpp @@ -0,0 +1,129 @@ +#include "nav2_costmap_2d/ray_caster.hpp" + +namespace nav2_costmap_2d +{ +RayCaster::RayCaster() +{ +} + +void RayCaster::initialize( + rclcpp::Node::SharedPtr & parent_node, + std::string camera_info_topic_name, + std::string aligned_pc2_topic_name, + bool use_pointcloud, + tf2_ros::Buffer * tf2_buffer, + std::string global_frame, + tf2::Duration tf_tolerance, + double max_trace_distance +) +{ + parent_node_ = parent_node; + global_frame_ = global_frame; + + tf2_buffer_ = tf2_buffer; + camera_info_topic_name_ = camera_info_topic_name; + aligned_pc2_topic_name_ = aligned_pc2_topic_name; + tf_tolerance_ = tf_tolerance; + use_pointcloud_ = use_pointcloud; + max_trace_distance_ = max_trace_distance; + if(use_pointcloud_) + { + aligned_pc2_sub_ = parent_node_->create_subscription(aligned_pc2_topic_name_, rclcpp::SensorDataQoS(), std::bind(&RayCaster::pointCloudCb, this, std::placeholders::_1)); + RCLCPP_INFO(parent_node_->get_logger(), "Subscribed to %s", aligned_pc2_topic_name_.c_str()); + } + else + { + camera_info_sub_ = parent_node_->create_subscription(camera_info_topic_name_, rclcpp::SensorDataQoS(), std::bind(&RayCaster::cameraInfoCb, this, std::placeholders::_1)); + RCLCPP_INFO(parent_node_->get_logger(), "Subscribed to %s", camera_info_topic_name_.c_str()); + } +} + +bool RayCaster::worldToImage(geometry_msgs::msg::PointStamped& point, cv::Point2d& pixel){ + if (!tf2_buffer_->canTransform( + point.header.frame_id, sensor_frame_, tf2_ros::fromMsg(point.header.stamp))) + { + RCLCPP_ERROR( + parent_node_->get_logger(), "Ray caster can't transform from %s to %s", + sensor_frame_.c_str(), point.header.frame_id.c_str()); + return false; + } + geometry_msgs::msg::PointStamped transformed_point; + tf2_buffer_->transform(point, transformed_point, sensor_frame_, tf_tolerance_); + cv::Point3d cv_point{transformed_point.point.x, transformed_point.point.y, transformed_point.point.z}; + pixel = camera_model_.project3dToPixel(cv_point); + return true; +} + +bool RayCaster::imageToGroundPlane(cv::Point2d& pixel, geometry_msgs::msg::PointStamped& point) +{ + if (!tf2_buffer_->canTransform( + global_frame_, sensor_frame_, tf2_ros::fromMsg(point.header.stamp))) + { + RCLCPP_ERROR_THROTTLE( + parent_node_->get_logger(), *parent_node_->get_clock(), 10000, "Ray caster can't transform from %s to %s", + sensor_frame_.c_str(), point.header.frame_id.c_str()); + return false; + } + cv::Point3d cv_ray = camera_model_.projectPixelTo3dRay(pixel); + geometry_msgs::msg::PointStamped ray_end_sensor_frame = rayToPointStamped(cv_ray, sensor_frame_); + geometry_msgs::msg::PointStamped ray_end_world_frame; + geometry_msgs::msg::PointStamped camera_position_world_frame; + tf2_buffer_->transform(ray_end_sensor_frame, ray_end_world_frame, global_frame_, tf_tolerance_); + tf2_buffer_->transform(camera_origin_, camera_position_world_frame, global_frame_, tf_tolerance_); + if(ray_end_world_frame.point.z > camera_position_world_frame.point.z) + { + RCLCPP_DEBUG(parent_node_->get_logger(),"Point falls behind the camera, will not raycast"); + return false; + } + double ray_scale_factor = -camera_position_world_frame.point.z/(ray_end_world_frame.point.z - camera_position_world_frame.point.z); + geometry_msgs::msg::PointStamped intersection; + intersection.header.frame_id = global_frame_; + intersection.point.x = camera_position_world_frame.point.x + ray_scale_factor*(ray_end_world_frame.point.x - camera_position_world_frame.point.x); + intersection.point.y = camera_position_world_frame.point.y + ray_scale_factor*(ray_end_world_frame.point.y - camera_position_world_frame.point.y); + intersection.point.z = camera_position_world_frame.point.z + ray_scale_factor*(ray_end_world_frame.point.z - camera_position_world_frame.point.z); + geometry_msgs::msg::PointStamped intersection_camera_frame = tf2_buffer_->transform(intersection, sensor_frame_); + if(intersection_camera_frame.point.z > max_trace_distance_) + { + RCLCPP_DEBUG(parent_node_->get_logger(),"Point falls further than the max trace distance, will not raycast"); + return false; + } + point = intersection; + return true; +} + +geometry_msgs::msg::PointStamped RayCaster::rayToPointStamped(cv::Point3d& ray_end, std::string& frame_id) +{ + geometry_msgs::msg::PointStamped point; + point.point.x = ray_end.x; + point.point.y = ray_end.y; + point.point.z = ray_end.z; + point.header.frame_id = frame_id; + return point; +} + +void RayCaster::pointCloudCb(sensor_msgs::msg::PointCloud2::SharedPtr msg) +{ + pointcloud_msg_ = *msg; + ready_to_raytrace_ = true; +} + +void RayCaster::cameraInfoCb(sensor_msgs::msg::CameraInfo::SharedPtr msg) +{ + RCLCPP_INFO(parent_node_->get_logger(), "Troll"); + if(msg->header.frame_id.empty()) + { + RCLCPP_ERROR(parent_node_->get_logger(), "Frame id of camera info message is empty. Cannot raycast"); + return; + } + camera_model_.fromCameraInfo(*msg); + sensor_frame_ = msg->header.frame_id; + camera_origin_.header.frame_id = sensor_frame_; + camera_info_sub_.reset(); + ready_to_raytrace_ = true; + RCLCPP_INFO(parent_node_->get_logger(), "Received camera info message. Ready to raycast"); +} + + + + +} // namespace nav2_costmap_2d \ No newline at end of file diff --git a/nav2_costmap_2d/src/ray_tracer.cpp b/nav2_costmap_2d/src/ray_tracer.cpp deleted file mode 100644 index ac446362d3..0000000000 --- a/nav2_costmap_2d/src/ray_tracer.cpp +++ /dev/null @@ -1,122 +0,0 @@ -#include "nav2_costmap_2d/ray_tracer.hpp" - -namespace nav2_costmap_2d -{ -RayTracer::RayTracer() -{ -} - -void RayTracer::initialize( - rclcpp::Node::SharedPtr & parent_node, - std::string camera_info_topic_name, - std::string aligned_pc2_topic_name, - bool use_pointcloud, - tf2_ros::Buffer * tf2_buffer, - std::string global_frame, - std::string sensor_frame, - tf2::Duration tf_tolerance, - double max_trace_distance -) -{ - parent_node_ = parent_node; - global_frame_ = global_frame; - sensor_frame_ = sensor_frame; - tf2_buffer_ = tf2_buffer; - camera_info_topic_name_ = camera_info_topic_name; - aligned_pc2_topic_name_ = aligned_pc2_topic_name; - tf_tolerance_ = tf_tolerance; - use_pointcloud_ = use_pointcloud; - max_trace_distance_ = max_trace_distance; - if(use_pointcloud_) - { - aligned_pc2_sub_ = parent_node_->create_subscription(aligned_pc2_topic_name_, rclcpp::SensorDataQoS(), std::bind(&RayTracer::pointCloudCb, this, std::placeholders::_1)); - RCLCPP_INFO(parent_node_->get_logger(), "Subscribed to %s", aligned_pc2_topic_name_.c_str()); - } - else - { - camera_info_sub_ = parent_node_->create_subscription(camera_info_topic_name_, rclcpp::SensorDataQoS(), std::bind(&RayTracer::cameraInfoCb, this, std::placeholders::_1)); - RCLCPP_INFO(parent_node_->get_logger(), "Subscribed to %s", camera_info_topic_name_.c_str()); - } -} - -std::vector RayTracer::rayTracePixels(std::vector& pixels) -{ - std::vector ans = std::vector(); - if(!ready_to_raytrace_) - { - RCLCPP_WARN(parent_node_->get_logger(), "No messages received yet on the topic"); - return ans; - } - try - { - geometry_msgs::msg::TransformStamped sensor_to_global = tf2_buffer_->lookupTransform(sensor_frame_, global_frame_, rclcpp::Time(0)); - Eigen::Isometry3d eigen_transform = tf2::transformToEigen(sensor_to_global.transform); - for(auto& pixel : pixels) - { - cv::Point3d cv_ray = camera_model_.projectPixelTo3dRay(pixel); - geometry_msgs::msg::Point p = intersectWithGroundPlane(eigen_transform, cv_ray); - ans.push_back(p); - } - return ans; - } - catch(tf2::TransformException & ex) - { - RCLCPP_ERROR( - parent_node_->get_logger(), - "TF Exception that should never happen for sensor frame: %s, global_frame: %s, %s", - sensor_frame_.c_str(), - global_frame_.c_str(), ex.what()); - return ans; - } -} - -geometry_msgs::msg::Point RayTracer::intersectWithGroundPlane(Eigen::Isometry3d camera_to_ground_tf, cv::Point3d cv_ray) -{ - Eigen::Vector3d eigen_ray({cv_ray.x, cv_ray.y, cv_ray.z}); - Eigen::Vector3d camera_origin = camera_to_ground_tf.translation(); - Eigen::Vector3d ray_end = camera_to_ground_tf*eigen_ray; - Eigen::Vector3d ground_plane_origin({0.0, 0.0, 0.0}); - Eigen::Vector3d ground_plane_normal({0.0, 0.0, 1.0}); - Eigen::Vector3d u = ray_end - camera_origin; - Eigen::Vector3d w = camera_origin - ground_plane_origin; - double D = ground_plane_normal.dot(u); - double N = -ground_plane_normal.dot(w); - if(abs(D) < 0.0000001){ - return geometry_msgs::msg::Point(); - } - double sI = N / D; - Eigen::Vector3d intersection = camera_origin + sI * u; - return tf2::toMsg(intersection); -} - -cv::Point2d RayTracer::worldToImage(geometry_msgs::msg::PointStamped& point){ - if (point.header.frame_id != sensor_frame_) - { - RCLCPP_INFO( - parent_node_->get_logger(), "Point has to be on the %s frame but is in frame %s", - sensor_frame_.c_str(), point.header.frame_id.c_str()); - return cv::Point2d(); - } - cv::Point3d cv_point{point.point.x, point.point.y, point.point.z}; - cv::Point2d pixel = camera_model_.project3dToPixel(cv_point); - return pixel; -} - -void RayTracer::pointCloudCb(sensor_msgs::msg::PointCloud2::SharedPtr msg) -{ - pointcloud_msg_ = *msg; - ready_to_raytrace_ = true; -} - -void RayTracer::cameraInfoCb(sensor_msgs::msg::CameraInfo::SharedPtr msg) -{ - RCLCPP_INFO(parent_node_->get_logger(), "Received camera info message. Ready to raytrace"); - camera_model_.fromCameraInfo(*msg); - camera_info_sub_.reset(); - ready_to_raytrace_ = true; -} - - - - -} // namespace nav2_costmap_2d \ No newline at end of file From ba0c40b0b1206a8a10f7c0c1f5c59fba10aefd6e Mon Sep 17 00:00:00 2001 From: pepisg Date: Tue, 8 Mar 2022 18:09:30 -0500 Subject: [PATCH 04/13] Object buffer for storing semantic segmentation messages and transforms --- nav2_costmap_2d/CMakeLists.txt | 1 + .../include/nav2_costmap_2d/object_buffer.hpp | 124 ++++++++++++++++++ .../include/nav2_costmap_2d/ray_caster.hpp | 5 +- .../semantic_segmentation_layer.hpp | 11 +- .../plugins/semantic_segmentation_layer.cpp | 29 +++- 5 files changed, 161 insertions(+), 9 deletions(-) create mode 100644 nav2_costmap_2d/include/nav2_costmap_2d/object_buffer.hpp diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt index 204610fdbf..67fefd48e6 100644 --- a/nav2_costmap_2d/CMakeLists.txt +++ b/nav2_costmap_2d/CMakeLists.txt @@ -53,6 +53,7 @@ add_library(nav2_costmap_2d_core SHARED src/clear_costmap_service.cpp src/footprint_collision_checker.cpp plugins/costmap_filters/costmap_filter.cpp + include/nav2_costmap_2d/object_buffer.hpp ) # prevent pluginlib from using boost diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/object_buffer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/object_buffer.hpp new file mode 100644 index 0000000000..7f75761726 --- /dev/null +++ b/nav2_costmap_2d/include/nav2_costmap_2d/object_buffer.hpp @@ -0,0 +1,124 @@ +#ifndef NAV2_COSTMAP_2D__OBJECT_BUFFER_HPP_ +#define NAV2_COSTMAP_2D__OBJECT_BUFFER_HPP_ + +#include +#include +#include +#include +#include + +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2_ros/buffer.h" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/time.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" + +namespace nav2_costmap_2d +{ + using rclcpp::Time; + + template + struct TimedObject + { + ObjectT object; + Time time; + }; +/** + * @class ObjectBuffer + * @brief Class to orderly store Objects for a given period of time + */ +template +class ObjectBuffer +{ + public: + /** + * @brief Constructs an Object buffer. + * @param Object_keep_time Defines the persistence of Objects in seconds, 0 means only keep the latest + */ + ObjectBuffer( + const nav2_util::LifecycleNode::WeakPtr & parent, + double object_keep_time + ): + object_keep_time_(rclcpp::Duration::from_seconds(object_keep_time)) + { + auto node = parent.lock(); + clock_ = node->get_clock(); + logger_ = node->get_logger(); + } + + /** + * @brief Destructor... cleans up + */ + ~ObjectBuffer(){}; + + /** + * @brief stores a Object with an associated generation time + */ + void bufferObject(const ObjectT & object, const Time & generation_time) + { + TimedObject timed_object; + timed_object.object = object; + timed_object.time = generation_time; + objects_list_.push_front(timed_object); + } + + /** + * @brief Pushes copies of all current Objects onto the end of the vector passed in + * @param Objects The vector to be filled + */ + void getObjects(std::vector & objects) + { + // first... let's make sure that we don't have any stale objects + purgeStaleObjects(); + + // now we'll just copy the objects for the caller + for (auto obs_it = objects_list_.begin(); obs_it != objects_list_.end(); ++obs_it) { + objects.push_back(obs_it->object); + } + objects.clear(); + } + + + private: + /** + * @brief Removes any stale objects from the buffer list + */ + void purgeStaleObjects() + { + if(!objects_list_.empty()) + { + // if we're keeping objects for no time... then we'll only keep one object + auto obj_it = objects_list_.begin(); + if(object_keep_time_ == rclcpp::Duration(0.0)){ + objects_list_.erase(++obj_it, objects_list_.end()); + return; + } + + // otherwise... we'll have to loop through the objectss to see which ones are stale + for (obj_it = objects_list_.begin(); obj_it != objects_list_.end(); ++obj_it) + { + // check if the objects is out of date... and if it is, + // remove it and those that follow from the list + if ((clock_->now() - obj_it->time) > object_keep_time_) + { + objects_list_.erase(obj_it, objects_list_.end()); + return; + } + } + } + } + + rclcpp::Clock::SharedPtr clock_; + rclcpp::Logger logger_{rclcpp::get_logger("nav2_costmap_2d")}; + const rclcpp::Duration object_keep_time_; + std::string global_frame_; + std::string sensor_frame_; + std::list> objects_list_; + tf2::Duration tf_tolerance_; +}; + +} // namespace nav2_costmap_2d + + + +#endif // NAV2_COSTMAP_2D__OBJECT_BUFFER_HPP_ diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/ray_caster.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/ray_caster.hpp index 79dd0a27dd..32cffc3213 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/ray_caster.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/ray_caster.hpp @@ -2,8 +2,6 @@ #define NAV2_COSTMAP_2D__RAY_TRACER_HPP_ #include -#include -#include #include "image_geometry/pinhole_camera_model.h" #include "nav2_util/lifecycle_node.hpp" @@ -13,12 +11,11 @@ #include "opencv2/core.hpp" #include "sensor_msgs/msg/camera_info.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" -#include "message_filters/subscriber.h" #include "geometry_msgs/msg/transform_stamped.hpp" #include "geometry_msgs/msg/point.hpp" #include "geometry_msgs/msg/point_stamped.hpp" #include "tf2/convert.h" -#include "tf2_eigen/tf2_eigen.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 index 1cb7c24c05..09e0888f41 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/semantic_segmentation_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/semantic_segmentation_layer.hpp @@ -49,12 +49,19 @@ #include "nav2_costmap_2d/costmap_layer.hpp" #include "nav2_costmap_2d/layered_costmap.hpp" #include "nav2_costmap_2d/ray_caster.hpp" +#include "nav2_costmap_2d/object_buffer.hpp" #include "rclcpp/rclcpp.hpp" #include "vision_msgs/msg/semantic_segmentation.hpp" #include "opencv2/core.hpp" namespace nav2_costmap_2d { +struct MessageTf +{ + vision_msgs::msg::SemanticSegmentation message; + geometry_msgs::msg::Transform transform; +}; + class SemanticSegmentationLayer : public CostmapLayer { public: @@ -74,7 +81,9 @@ class SemanticSegmentationLayer : public CostmapLayer private: void segmentationCb(vision_msgs::msg::SemanticSegmentation::SharedPtr msg); - RayCaster tracer_; + RayCaster ray_caster_; + + std::shared_ptr> msg_buffer_; rclcpp::Subscription::SharedPtr semantic_segmentation_sub_; diff --git a/nav2_costmap_2d/plugins/semantic_segmentation_layer.cpp b/nav2_costmap_2d/plugins/semantic_segmentation_layer.cpp index b72ea1ac6a..63d8cc3307 100644 --- a/nav2_costmap_2d/plugins/semantic_segmentation_layer.cpp +++ b/nav2_costmap_2d/plugins/semantic_segmentation_layer.cpp @@ -70,7 +70,7 @@ void SemanticSegmentationLayer::onInitialize() } std::string segmentation_topic, camera_info_topic, pointcloud_topic; bool use_pointcloud; - double max_lookahead_distance; + double max_lookahead_distance, observation_keep_time; declareParameter("enabled", rclcpp::ParameterValue(true)); node->get_parameter(name_ + "." + "enabled", enabled_); @@ -84,6 +84,7 @@ void SemanticSegmentationLayer::onInitialize() node->get_parameter(name_ + "." + "camera_info_topic", camera_info_topic); declareParameter("pointcloud_topic", rclcpp::ParameterValue("")); node->get_parameter(name_ + "." + "pointcloud_topic", pointcloud_topic); + node->get_parameter(name_ + "." + "observation_persistence", observation_keep_time); global_frame_ = layered_costmap_->getGlobalFrameID(); rolling_window_ = layered_costmap_->isRolling(); @@ -92,8 +93,10 @@ void SemanticSegmentationLayer::onInitialize() semantic_segmentation_sub_ = node->create_subscription(segmentation_topic, rclcpp::SensorDataQoS(), std::bind(&SemanticSegmentationLayer::segmentationCb, this, std::placeholders::_1)); - tracer_.initialize(rclcpp_node_, camera_info_topic, pointcloud_topic, use_pointcloud, tf_, global_frame_, tf2::Duration(0), max_lookahead_distance); + ray_caster_.initialize(rclcpp_node_, camera_info_topic, pointcloud_topic, use_pointcloud, tf_, global_frame_, tf2::Duration(0), max_lookahead_distance); + msg_buffer_ = std::make_shared>(node, observation_keep_time); + } // The method is called to ask the plugin: which area of costmap it needs to update. @@ -119,7 +122,7 @@ void SemanticSegmentationLayer::updateBounds(double robot_x, double robot_y, dou pixel_idx.x = u; pixel_idx.y = v; geometry_msgs::msg::PointStamped world_point; - if(!tracer_.imageToGroundPlane(pixel_idx, world_point)) + if(!ray_caster_.imageToGroundPlane(pixel_idx, world_point)) { RCLCPP_DEBUG(logger_, "Could not raycast"); continue; @@ -185,7 +188,25 @@ void SemanticSegmentationLayer::updateCosts(nav2_costmap_2d::Costmap2D& master_g void SemanticSegmentationLayer::segmentationCb(vision_msgs::msg::SemanticSegmentation::SharedPtr msg) { - latest_segmentation_message = *msg; + // latest_segmentation_message = *msg; + geometry_msgs::msg::Transform current_transform; + try + { + current_transform = tf_->lookupTransform(msg->header.frame_id, global_frame_, msg->header.stamp).transform; + } + catch (tf2::TransformException & ex) { + // if an exception occurs, we need to remove the empty observation from the list + RCLCPP_ERROR( + logger_, + "TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s", + msg->header.frame_id.c_str(), + global_frame_.c_str(), ex.what()); + return; + } + MessageTf message_tf; + message_tf.message = *msg; + message_tf.transform = current_transform; + msg_buffer_->bufferObject(message_tf, msg->header.stamp); } void SemanticSegmentationLayer::reset() From 9dd3383a877eec89f066dd54052b17d40c92605c Mon Sep 17 00:00:00 2001 From: pepisg Date: Fri, 11 Mar 2022 16:06:18 -0500 Subject: [PATCH 05/13] Lookup table to store raycasts and object buffer to stores messages and transforms --- .../include/nav2_costmap_2d/object_buffer.hpp | 8 +- .../include/nav2_costmap_2d/ray_caster.hpp | 7 ++ .../semantic_segmentation_layer.hpp | 4 +- .../plugins/semantic_segmentation_layer.cpp | 115 +++++++++++++----- nav2_costmap_2d/src/ray_caster.cpp | 81 ++++++++++++ 5 files changed, 176 insertions(+), 39 deletions(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/object_buffer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/object_buffer.hpp index 7f75761726..66ec273fb1 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/object_buffer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/object_buffer.hpp @@ -70,12 +70,11 @@ class ObjectBuffer { // first... let's make sure that we don't have any stale objects purgeStaleObjects(); - // now we'll just copy the objects for the caller for (auto obs_it = objects_list_.begin(); obs_it != objects_list_.end(); ++obs_it) { objects.push_back(obs_it->object); } - objects.clear(); + objects_list_.clear(); } @@ -89,7 +88,7 @@ class ObjectBuffer { // if we're keeping objects for no time... then we'll only keep one object auto obj_it = objects_list_.begin(); - if(object_keep_time_ == rclcpp::Duration(0.0)){ + if(object_keep_time_ == rclcpp::Duration::from_seconds(0.0)){ objects_list_.erase(++obj_it, objects_list_.end()); return; } @@ -111,10 +110,7 @@ class ObjectBuffer rclcpp::Clock::SharedPtr clock_; rclcpp::Logger logger_{rclcpp::get_logger("nav2_costmap_2d")}; const rclcpp::Duration object_keep_time_; - std::string global_frame_; - std::string sensor_frame_; std::list> objects_list_; - tf2::Duration tf_tolerance_; }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/ray_caster.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/ray_caster.hpp index 32cffc3213..a9cb5c0fbe 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/ray_caster.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/ray_caster.hpp @@ -60,6 +60,8 @@ class RayCaster bool worldToImage(geometry_msgs::msg::PointStamped& point, cv::Point2d& pixel); bool imageToGroundPlane(cv::Point2d& pixel, geometry_msgs::msg::PointStamped& point); + + bool imageToGroundPlaneLookup(cv::Point2d& pixel, geometry_msgs::msg::PointStamped& point, geometry_msgs::msg::TransformStamped sensor_to_world_tf); private: @@ -69,6 +71,8 @@ class RayCaster void cameraInfoCb(sensor_msgs::msg::CameraInfo::SharedPtr msg); void pointCloudCb(sensor_msgs::msg::PointCloud2::SharedPtr msg); + + bool computeLookupTable(builtin_interfaces::msg::Time & time_msg); rclcpp::Node::SharedPtr parent_node_; std::string global_frame_; @@ -77,8 +81,11 @@ class RayCaster std::string camera_info_topic_name_; std::string aligned_pc2_topic_name_; tf2::Duration tf_tolerance_; + std::vector caster_lookup_table_; bool use_pointcloud_; double max_trace_distance_; + size_t img_width_; + size_t img_height_; bool ready_to_raytrace_ = false; 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 index 09e0888f41..966ddad9db 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/semantic_segmentation_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/semantic_segmentation_layer.hpp @@ -59,7 +59,7 @@ namespace nav2_costmap_2d { struct MessageTf { vision_msgs::msg::SemanticSegmentation message; - geometry_msgs::msg::Transform transform; + geometry_msgs::msg::TransformStamped transform; }; class SemanticSegmentationLayer : public CostmapLayer @@ -93,7 +93,7 @@ class SemanticSegmentationLayer : public CostmapLayer bool rolling_window_; bool was_reset_; - + int combination_method_; }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/plugins/semantic_segmentation_layer.cpp b/nav2_costmap_2d/plugins/semantic_segmentation_layer.cpp index 63d8cc3307..13df2f62cf 100644 --- a/nav2_costmap_2d/plugins/semantic_segmentation_layer.cpp +++ b/nav2_costmap_2d/plugins/semantic_segmentation_layer.cpp @@ -74,6 +74,8 @@ void SemanticSegmentationLayer::onInitialize() declareParameter("enabled", rclcpp::ParameterValue(true)); node->get_parameter(name_ + "." + "enabled", enabled_); + declareParameter("combination_method", rclcpp::ParameterValue(1)); + node->get_parameter(name_ + "." + "combination_method", combination_method_); declareParameter("use_pointcloud", rclcpp::ParameterValue(false)); node->get_parameter(name_ + "." + "use_pointcloud", use_pointcloud); declareParameter("max_lookahead_distance", rclcpp::ParameterValue(3.0)); @@ -84,6 +86,7 @@ void SemanticSegmentationLayer::onInitialize() node->get_parameter(name_ + "." + "camera_info_topic", camera_info_topic); declareParameter("pointcloud_topic", rclcpp::ParameterValue("")); node->get_parameter(name_ + "." + "pointcloud_topic", pointcloud_topic); + declareParameter("observation_persistence", rclcpp::ParameterValue(0.0)); node->get_parameter(name_ + "." + "observation_persistence", observation_keep_time); global_frame_ = layered_costmap_->getGlobalFrameID(); @@ -112,37 +115,77 @@ void SemanticSegmentationLayer::updateBounds(double robot_x, double robot_y, dou current_ = true; return; } - vision_msgs::msg::SemanticSegmentation current_segmentation = latest_segmentation_message; - int touched_cells = 0; - for(size_t v = 0; v < current_segmentation.height; v=v+5) - { - for(size_t u = 0; u < current_segmentation.width; u=u+5){ - uint8_t pixel = current_segmentation.data[v*current_segmentation.width+u]; - cv::Point2d pixel_idx; - pixel_idx.x = u; - pixel_idx.y = v; - geometry_msgs::msg::PointStamped world_point; - if(!ray_caster_.imageToGroundPlane(pixel_idx, world_point)) - { - RCLCPP_DEBUG(logger_, "Could not raycast"); - continue; - } - unsigned int mx, my; - if(!worldToMap(world_point.point.x, world_point.point.y, mx, my)) - { - RCLCPP_DEBUG(logger_, "Computing map coords failed"); - continue; - } - unsigned int index = getIndex(mx, my); - if(pixel!= 1){ - costmap_[index] = 100; - }else{ - costmap_[index] = 0; + std::vector segmentations; + msg_buffer_->getObjects(segmentations); + int processed_msgs = 0; + for(auto& segmentation : segmentations){ + int touched_cells = 0; + for(size_t v = 0; v < segmentation.message.height; v=v+2) + { + for(size_t u = 0; u < segmentation.message.width; u=u+2){ + uint8_t pixel = segmentation.message.data[v*segmentation.message.width+u]; + cv::Point2d pixel_idx; + pixel_idx.x = u; + pixel_idx.y = v; + geometry_msgs::msg::PointStamped world_point; + if(!ray_caster_.imageToGroundPlaneLookup(pixel_idx, world_point, segmentation.transform)) + { + RCLCPP_DEBUG(logger_, "Could not raycast"); + continue; + } + unsigned int mx, my; + if(!worldToMap(world_point.point.x, world_point.point.y, mx, my)) + { + RCLCPP_DEBUG(logger_, "Computing map coords failed"); + continue; + } + unsigned int index = getIndex(mx, my); + if(pixel!= 1){ + costmap_[index] = 100; + }else{ + costmap_[index] = 0; + } + touch(world_point.point.x, world_point.point.y, min_x, min_y, max_x, max_y); + touched_cells++; } - touch(world_point.point.x, world_point.point.y, min_x, min_y, max_x, max_y); - touched_cells++; } + processed_msgs++; } + if(processed_msgs>0) + { + RCLCPP_INFO(logger_, "Processed %i segmentations", processed_msgs); + } + // vision_msgs::msg::SemanticSegmentation current_segmentation = latest_segmentation_message; + // int touched_cells = 0; + // for(size_t v = 0; v < current_segmentation.height; v=v+5) + // { + // for(size_t u = 0; u < current_segmentation.width; u=u+5){ + // uint8_t pixel = current_segmentation.data[v*current_segmentation.width+u]; + // cv::Point2d pixel_idx; + // pixel_idx.x = u; + // pixel_idx.y = v; + // geometry_msgs::msg::PointStamped world_point; + // if(!ray_caster_.imageToGroundPlane(pixel_idx, world_point)) + // { + // RCLCPP_DEBUG(logger_, "Could not raycast"); + // continue; + // } + // unsigned int mx, my; + // if(!worldToMap(world_point.point.x, world_point.point.y, mx, my)) + // { + // RCLCPP_DEBUG(logger_, "Computing map coords failed"); + // continue; + // } + // unsigned int index = getIndex(mx, my); + // if(pixel!= 1){ + // costmap_[index] = 100; + // }else{ + // costmap_[index] = 0; + // } + // touch(world_point.point.x, world_point.point.y, min_x, min_y, max_x, max_y); + // touched_cells++; + // } + // } // RCLCPP_INFO(logger_, "touched %i cells", touched_cells); } @@ -183,16 +226,25 @@ void SemanticSegmentationLayer::updateCosts(nav2_costmap_2d::Costmap2D& master_g return; } // RCLCPP_INFO(logger_, "Updating costmap"); - updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j); + 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::segmentationCb(vision_msgs::msg::SemanticSegmentation::SharedPtr msg) { // latest_segmentation_message = *msg; - geometry_msgs::msg::Transform current_transform; + geometry_msgs::msg::TransformStamped current_transform; try { - current_transform = tf_->lookupTransform(msg->header.frame_id, global_frame_, msg->header.stamp).transform; + current_transform = tf_->lookupTransform(global_frame_, msg->header.frame_id, msg->header.stamp); } catch (tf2::TransformException & ex) { // if an exception occurs, we need to remove the empty observation from the list @@ -207,6 +259,7 @@ void SemanticSegmentationLayer::segmentationCb(vision_msgs::msg::SemanticSegment message_tf.message = *msg; message_tf.transform = current_transform; msg_buffer_->bufferObject(message_tf, msg->header.stamp); + RCLCPP_INFO(logger_, "msg buffered"); } void SemanticSegmentationLayer::reset() diff --git a/nav2_costmap_2d/src/ray_caster.cpp b/nav2_costmap_2d/src/ray_caster.cpp index 86bc35523e..f6fcd52bad 100644 --- a/nav2_costmap_2d/src/ray_caster.cpp +++ b/nav2_costmap_2d/src/ray_caster.cpp @@ -56,6 +56,7 @@ bool RayCaster::worldToImage(geometry_msgs::msg::PointStamped& point, cv::Point2 bool RayCaster::imageToGroundPlane(cv::Point2d& pixel, geometry_msgs::msg::PointStamped& point) { + // (void) sensor_to_world_tf; if (!tf2_buffer_->canTransform( global_frame_, sensor_frame_, tf2_ros::fromMsg(point.header.stamp))) { @@ -91,6 +92,40 @@ bool RayCaster::imageToGroundPlane(cv::Point2d& pixel, geometry_msgs::msg::Point return true; } +bool RayCaster::imageToGroundPlaneLookup(cv::Point2d& pixel, geometry_msgs::msg::PointStamped& point, geometry_msgs::msg::TransformStamped sensor_to_world_tf) +{ + if(!ready_to_raytrace_) + { + RCLCPP_WARN_THROTTLE(parent_node_->get_logger(), *parent_node_->get_clock(), 10000, "Not ready to raytrace"); + return false; + } + if (!tf2_buffer_->canTransform( + global_frame_, sensor_frame_, tf2_ros::fromMsg(point.header.stamp))) + { + RCLCPP_ERROR_THROTTLE( + parent_node_->get_logger(), *parent_node_->get_clock(), 10000, "Ray caster can't transform from %s to %s", + sensor_frame_.c_str(), point.header.frame_id.c_str()); + return false; + } + geometry_msgs::msg::PointStamped point_sensor_frame = caster_lookup_table_.at(pixel.y*img_width_+pixel.x); + if(point_sensor_frame.point.z > max_trace_distance_) + { + RCLCPP_DEBUG(parent_node_->get_logger(),"Point falls further than the max trace distance, will not raycast"); + return false; + } + if(point_sensor_frame.point.z < 0.0) + { + RCLCPP_DEBUG(parent_node_->get_logger(),"Point falls falls behind the camera, will not raycast"); + return false; + } + geometry_msgs::msg::PointStamped point_world_frame; + tf2::doTransform( + point_sensor_frame, point_world_frame, sensor_to_world_tf); + // tf2_buffer_->transform(point_sensor_frame, point_world_frame, global_frame_, tf_tolerance_); + point = point_world_frame; + return true; +} + geometry_msgs::msg::PointStamped RayCaster::rayToPointStamped(cv::Point3d& ray_end, std::string& frame_id) { geometry_msgs::msg::PointStamped point; @@ -118,12 +153,58 @@ void RayCaster::cameraInfoCb(sensor_msgs::msg::CameraInfo::SharedPtr msg) camera_model_.fromCameraInfo(*msg); sensor_frame_ = msg->header.frame_id; camera_origin_.header.frame_id = sensor_frame_; + img_width_ = msg->width; + img_height_ = msg->height; + if(!computeLookupTable(msg->header.stamp)) + { + RCLCPP_WARN(parent_node_->get_logger(), "Could not compute lookup table"); + return; + } camera_info_sub_.reset(); ready_to_raytrace_ = true; RCLCPP_INFO(parent_node_->get_logger(), "Received camera info message. Ready to raycast"); } +bool RayCaster::computeLookupTable(builtin_interfaces::msg::Time & time_msg) +{ + if (!tf2_buffer_->canTransform( + global_frame_, sensor_frame_, tf2_ros::fromMsg(time_msg))) + { + RCLCPP_ERROR_THROTTLE( + parent_node_->get_logger(), *parent_node_->get_clock(), 10000, "Ray caster can't transform from %s to %s", + sensor_frame_.c_str(), global_frame_.c_str()); + return false; + } + caster_lookup_table_.clear(); + caster_lookup_table_.reserve(img_width_*img_height_); + for(size_t v = 0; v < img_height_; v++) + { + for(size_t u = 0; u < img_width_; u++) + { + cv::Point2d pixel; + pixel.x = u; + pixel.y = v; + cv::Point3d cv_ray = camera_model_.projectPixelTo3dRay(pixel); + geometry_msgs::msg::PointStamped ray_end_sensor_frame = rayToPointStamped(cv_ray, sensor_frame_); + geometry_msgs::msg::PointStamped ray_end_world_frame; + geometry_msgs::msg::PointStamped camera_position_world_frame; + tf2_buffer_->transform(ray_end_sensor_frame, ray_end_world_frame, global_frame_, tf_tolerance_); + tf2_buffer_->transform(camera_origin_, camera_position_world_frame, global_frame_, tf_tolerance_); + double ray_scale_factor = -camera_position_world_frame.point.z/(ray_end_world_frame.point.z - camera_position_world_frame.point.z); + geometry_msgs::msg::PointStamped intersection; + intersection.header.frame_id = global_frame_; + intersection.point.x = camera_position_world_frame.point.x + ray_scale_factor*(ray_end_world_frame.point.x - camera_position_world_frame.point.x); + intersection.point.y = camera_position_world_frame.point.y + ray_scale_factor*(ray_end_world_frame.point.y - camera_position_world_frame.point.y); + intersection.point.z = camera_position_world_frame.point.z + ray_scale_factor*(ray_end_world_frame.point.z - camera_position_world_frame.point.z); + geometry_msgs::msg::PointStamped intersection_camera_frame = tf2_buffer_->transform(intersection, sensor_frame_); + caster_lookup_table_.emplace_back(intersection_camera_frame); + } + } + RCLCPP_INFO(parent_node_->get_logger(), "Lookup table computed successfully"); + return true; +} + } // namespace nav2_costmap_2d \ No newline at end of file From f0cc485058a0f6a1eebecbc4f66732444f986304 Mon Sep 17 00:00:00 2001 From: pepisg Date: Fri, 24 Jun 2022 11:09:01 -0500 Subject: [PATCH 06/13] Segmentation buffer and segmentation object --- nav2_costmap_2d/CMakeLists.txt | 3 +- .../include/nav2_costmap_2d/ray_caster.hpp | 106 ------ .../include/nav2_costmap_2d/segmentation.hpp | 119 ++++++ .../nav2_costmap_2d/segmentation_buffer.hpp | 155 ++++++++ .../semantic_segmentation_layer.hpp | 139 ++++--- nav2_costmap_2d/package.xml | 6 +- .../plugins/semantic_segmentation_layer.cpp | 360 ++++++++++-------- nav2_costmap_2d/src/segmentation_buffer.cpp | 256 +++++++++++++ 8 files changed, 826 insertions(+), 318 deletions(-) delete mode 100644 nav2_costmap_2d/include/nav2_costmap_2d/ray_caster.hpp create mode 100644 nav2_costmap_2d/include/nav2_costmap_2d/segmentation.hpp create mode 100644 nav2_costmap_2d/include/nav2_costmap_2d/segmentation_buffer.hpp create mode 100644 nav2_costmap_2d/src/segmentation_buffer.cpp diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt index 67fefd48e6..50abc0cd7a 100644 --- a/nav2_costmap_2d/CMakeLists.txt +++ b/nav2_costmap_2d/CMakeLists.txt @@ -49,11 +49,10 @@ add_library(nav2_costmap_2d_core SHARED src/footprint.cpp src/costmap_layer.cpp src/observation_buffer.cpp - src/ray_caster.cpp + src/segmentation_buffer.cpp src/clear_costmap_service.cpp src/footprint_collision_checker.cpp plugins/costmap_filters/costmap_filter.cpp - include/nav2_costmap_2d/object_buffer.hpp ) # prevent pluginlib from using boost diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/ray_caster.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/ray_caster.hpp deleted file mode 100644 index a9cb5c0fbe..0000000000 --- a/nav2_costmap_2d/include/nav2_costmap_2d/ray_caster.hpp +++ /dev/null @@ -1,106 +0,0 @@ -#ifndef NAV2_COSTMAP_2D__RAY_TRACER_HPP_ -#define NAV2_COSTMAP_2D__RAY_TRACER_HPP_ - -#include - -#include "image_geometry/pinhole_camera_model.h" -#include "nav2_util/lifecycle_node.hpp" -#include "rclcpp/rclcpp.hpp" -#include "tf2_ros/buffer.h" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "opencv2/core.hpp" -#include "sensor_msgs/msg/camera_info.hpp" -#include "sensor_msgs/msg/point_cloud2.hpp" -#include "geometry_msgs/msg/transform_stamped.hpp" -#include "geometry_msgs/msg/point.hpp" -#include "geometry_msgs/msg/point_stamped.hpp" -#include "tf2/convert.h" - - - - -namespace nav2_costmap_2d -{ -/** - * @class RayCaster - * @brief Takes in camera calibration data and traces pixels over a given plane - */ - -class RayCaster -{ -public: - /** - * @brief Constructs an observation buffer - * @param camera_info_topic_name The topic in which the camera calibration is published - * @param aligned_pc2_topic_name The topic of the image aligned pointcloud2 - * @param use_pointcloud Whether to use the pointcloud or the camera calibration for ray tracing - * @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 - */ - RayCaster(); - - /** - * @brief Destructor... cleans up - */ - ~RayCaster(){}; - - void initialize( - rclcpp::Node::SharedPtr & parent_node, - std::string camera_info_topic_name, - std::string aligned_pc2_topic_name, - bool use_pointcloud, - tf2_ros::Buffer * tf2_buffer, - std::string global_frame, - tf2::Duration tf_tolerance, - double max_trace_distance - ); - - bool worldToImage(geometry_msgs::msg::PointStamped& point, cv::Point2d& pixel); - - bool imageToGroundPlane(cv::Point2d& pixel, geometry_msgs::msg::PointStamped& point); - - bool imageToGroundPlaneLookup(cv::Point2d& pixel, geometry_msgs::msg::PointStamped& point, geometry_msgs::msg::TransformStamped sensor_to_world_tf); - - -private: - - geometry_msgs::msg::PointStamped rayToPointStamped(cv::Point3d& ray_end, std::string& frame_id); - - void cameraInfoCb(sensor_msgs::msg::CameraInfo::SharedPtr msg); - - void pointCloudCb(sensor_msgs::msg::PointCloud2::SharedPtr msg); - - bool computeLookupTable(builtin_interfaces::msg::Time & time_msg); - - rclcpp::Node::SharedPtr parent_node_; - std::string global_frame_; - std::string sensor_frame_; - tf2_ros::Buffer * tf2_buffer_; - std::string camera_info_topic_name_; - std::string aligned_pc2_topic_name_; - tf2::Duration tf_tolerance_; - std::vector caster_lookup_table_; - bool use_pointcloud_; - double max_trace_distance_; - size_t img_width_; - size_t img_height_; - - bool ready_to_raytrace_ = false; - - rclcpp::Subscription::SharedPtr camera_info_sub_; - rclcpp::Subscription::SharedPtr aligned_pc2_sub_; - - sensor_msgs::msg::PointCloud2 pointcloud_msg_; - image_geometry::PinholeCameraModel camera_model_ = image_geometry::PinholeCameraModel(); - - geometry_msgs::msg::PointStamped camera_origin_; - -}; - -} // namespace nav2_costmap_2d - - - -#endif // NAV2_COSTMAP_2D__RAY_TRACER_HPP_ \ No newline at end of file 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 0000000000..6b287cc9fd --- /dev/null +++ b/nav2_costmap_2d/include/nav2_costmap_2d/segmentation.hpp @@ -0,0 +1,119 @@ +/* + * 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: Conor McGann + */ + +#ifndef NAV2_COSTMAP_2D__SEGMENTATION_HPP_ +#define NAV2_COSTMAP_2D__SEGMENTATION_HPP_ + +#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::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::map class_map_; +}; + +} // 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 0000000000..1ec9d65804 --- /dev/null +++ b/nav2_costmap_2d/include/nav2_costmap_2d/segmentation_buffer.hpp @@ -0,0 +1,155 @@ +/********************************************************************* + * + * 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/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/semantic_segmentation.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 topic_name, + 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 vision_msgs::msg::SemanticSegmentation& segmentation); + + /** + * @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 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(); + + 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_; + 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 topic_name_; + 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 index 966ddad9db..bcfbd57ae7 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/semantic_segmentation_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/semantic_segmentation_layer.hpp @@ -33,67 +33,116 @@ * 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 + * Author: Pedro Gonzalez + *********************************************************************/ #ifndef SEMANTIC_SEGMENTATION_LAYER_HPP_ #define SEMANTIC_SEGMENTATION_LAYER_HPP_ #include "rclcpp/rclcpp.hpp" -#include "nav2_costmap_2d/layer.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/ray_caster.hpp" #include "nav2_costmap_2d/object_buffer.hpp" +#include "nav2_costmap_2d/segmentation.hpp" +#include "nav2_costmap_2d/segmentation_buffer.hpp" +#include "nav2_util/node_utils.hpp" +#include "opencv2/core.hpp" #include "rclcpp/rclcpp.hpp" +#include "tf2_ros/message_filter.h" #include "vision_msgs/msg/semantic_segmentation.hpp" -#include "opencv2/core.hpp" namespace nav2_costmap_2d { -struct MessageTf -{ - vision_msgs::msg::SemanticSegmentation message; - geometry_msgs::msg::TransformStamped transform; -}; - +/** + * @class SemanticSegmentationLayer + * @brief Takes in semantic segmentation messages and aligned pointclouds to populate the 2D costmap + */ class SemanticSegmentationLayer : public CostmapLayer { - public: - SemanticSegmentationLayer(); - - virtual void onInitialize(); - virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, - double* max_x, double* max_y); - virtual void updateCosts(nav2_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j); - - virtual void reset(); - - virtual void onFootprintChanged(); - - virtual bool isClearable() { return false; } - - private: - void segmentationCb(vision_msgs::msg::SemanticSegmentation::SharedPtr msg); - - RayCaster ray_caster_; - - std::shared_ptr> msg_buffer_; - - rclcpp::Subscription::SharedPtr semantic_segmentation_sub_; - - vision_msgs::msg::SemanticSegmentation latest_segmentation_message; - - std::string global_frame_; - - bool rolling_window_; - bool was_reset_; - int combination_method_; + 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; } + + private: + void syncSegmPointcloudCb( + const std::shared_ptr& segmentation, + const std::shared_ptr& pointcloud); + + // rclcpp::Subscription::SharedPtr + // semantic_segmentation_sub_; + std::shared_ptr> + semantic_segmentation_sub_; + std::shared_ptr> pointcloud_sub_; + std::shared_ptr> + segm_pc_sync_; + std::shared_ptr> pointcloud_tf_sub_; + + // debug publishers + std::shared_ptr> sgm_debug_pub_; + std::shared_ptr> orig_pointcloud_pub_; + std::shared_ptr> proc_pointcloud_pub_; + + std::shared_ptr segmentation_buffer_; + + std::string global_frame_; + + std::map class_map_; + + bool rolling_window_; + bool was_reset_; + bool debug_topics_; + int combination_method_; }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index 16f48d6ed7..aecc4d2fb1 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -1,6 +1,6 @@ - + nav2_costmap_2d 1.0.0 @@ -50,6 +50,6 @@ nav2_lifecycle_manager - ament_cmake + 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 index 13df2f62cf..c21b2e44c8 100644 --- a/nav2_costmap_2d/plugins/semantic_segmentation_layer.cpp +++ b/nav2_costmap_2d/plugins/semantic_segmentation_layer.cpp @@ -52,181 +52,203 @@ using nav2_costmap_2d::NO_INFORMATION; namespace nav2_costmap_2d { -SemanticSegmentationLayer::SemanticSegmentationLayer() -{ -} +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) + 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, sensor_frame; + std::vector class_types_string; + double max_obstacle_distance, min_obstacle_distance, observation_keep_time, transform_tolerance, + expected_update_rate; + + declareParameter("enabled", rclcpp::ParameterValue(true)); + node->get_parameter(name_ + "." + "enabled", enabled_); + declareParameter("combination_method", rclcpp::ParameterValue(1)); + node->get_parameter(name_ + "." + "combination_method", combination_method_); + declareParameter("publish_debug_topics", rclcpp::ParameterValue(false)); + node->get_parameter(name_ + "." + "publish_debug_topics", debug_topics_); + declareParameter("max_obstacle_distance", rclcpp::ParameterValue(5.0)); + node->get_parameter(name_ + "." + "max_obstacle_distance", max_obstacle_distance); + declareParameter("min_obstacle_distance", rclcpp::ParameterValue(0.3)); + node->get_parameter(name_ + "." + "min_obstacle_distance", min_obstacle_distance); + declareParameter("segmentation_topic", rclcpp::ParameterValue("")); + node->get_parameter(name_ + "." + "segmentation_topic", segmentation_topic); + declareParameter("pointcloud_topic", rclcpp::ParameterValue("")); + node->get_parameter(name_ + "." + "pointcloud_topic", pointcloud_topic); + declareParameter("observation_persistence", rclcpp::ParameterValue(0.0)); + node->get_parameter(name_ + "." + "observation_persistence", observation_keep_time); + declareParameter(name_ + "." + "expected_update_rate", rclcpp::ParameterValue(0.0)); + node->get_parameter(name_ + "." + "sensor_frame", sensor_frame); + node->get_parameter(name_ + "." + "expected_update_rate", expected_update_rate); + node->get_parameter("transform_tolerance", transform_tolerance); + + declareParameter("class_types", rclcpp::ParameterValue(std::vector({}))); + node->get_parameter(name_ + "." + "class_types", class_types_string); + if (class_types_string.empty()) + { + RCLCPP_ERROR(logger_, "no class types defined. Segmentation plugin cannot work this way"); + exit(-1); + } + + for (auto& source : class_types_string) + { + std::vector classes_ids; + uint8_t cost; + declareParameter(source + ".classes", rclcpp::ParameterValue(std::vector({}))); + declareParameter(source + ".cost", rclcpp::ParameterValue(0)); + node->get_parameter(name_ + "." + source + ".classes", classes_ids); + if (classes_ids.empty()) { - throw std::runtime_error{"Failed to lock node"}; + RCLCPP_ERROR(logger_, "no classes defined on type %s", source.c_str()); + continue; } - std::string segmentation_topic, camera_info_topic, pointcloud_topic; - bool use_pointcloud; - double max_lookahead_distance, observation_keep_time; + node->get_parameter(name_ + "." + source + ".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. Segmentation plugin cannot work this way"); + exit(-1); + } - declareParameter("enabled", rclcpp::ParameterValue(true)); - node->get_parameter(name_ + "." + "enabled", enabled_); - declareParameter("combination_method", rclcpp::ParameterValue(1)); - node->get_parameter(name_ + "." + "combination_method", combination_method_); - declareParameter("use_pointcloud", rclcpp::ParameterValue(false)); - node->get_parameter(name_ + "." + "use_pointcloud", use_pointcloud); - declareParameter("max_lookahead_distance", rclcpp::ParameterValue(3.0)); - node->get_parameter(name_ + "." + "max_lookahead_distance", max_lookahead_distance); - declareParameter("segmentation_topic", rclcpp::ParameterValue("")); - node->get_parameter(name_ + "." + "segmentation_topic", segmentation_topic); - declareParameter("camera_info_topic", rclcpp::ParameterValue("")); - node->get_parameter(name_ + "." + "camera_info_topic", camera_info_topic); - declareParameter("pointcloud_topic", rclcpp::ParameterValue("")); - node->get_parameter(name_ + "." + "pointcloud_topic", pointcloud_topic); - declareParameter("observation_persistence", rclcpp::ParameterValue(0.0)); - node->get_parameter(name_ + "." + "observation_persistence", observation_keep_time); + global_frame_ = layered_costmap_->getGlobalFrameID(); + rolling_window_ = layered_costmap_->isRolling(); - global_frame_ = layered_costmap_->getGlobalFrameID(); - rolling_window_ = layered_costmap_->isRolling(); + matchSize(); - matchSize(); + rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_sensor_data; - semantic_segmentation_sub_ = node->create_subscription(segmentation_topic, rclcpp::SensorDataQoS(), std::bind(&SemanticSegmentationLayer::segmentationCb, this, std::placeholders::_1)); + semantic_segmentation_sub_ = + std::make_shared>( + rclcpp_node_, segmentation_topic, custom_qos_profile); + pointcloud_sub_ = std::make_shared>( + rclcpp_node_, pointcloud_topic, custom_qos_profile); + pointcloud_tf_sub_ = std::make_shared>( + *pointcloud_sub_, *tf_, global_frame_, 50, rclcpp_node_, + tf2::durationFromSec(transform_tolerance)); + segm_pc_sync_ = + std::make_shared>( + *semantic_segmentation_sub_, *pointcloud_tf_sub_, 100); + segm_pc_sync_->registerCallback(std::bind(&SemanticSegmentationLayer::syncSegmPointcloudCb, this, + std::placeholders::_1, std::placeholders::_2)); - ray_caster_.initialize(rclcpp_node_, camera_info_topic, pointcloud_topic, use_pointcloud, tf_, global_frame_, tf2::Duration(0), max_lookahead_distance); + segmentation_buffer_ = std::make_shared( + node, pointcloud_topic, observation_keep_time, expected_update_rate, max_obstacle_distance, + min_obstacle_distance, *tf_, global_frame_, sensor_frame, + tf2::durationFromSec(transform_tolerance)); - msg_buffer_ = std::make_shared>(node, observation_keep_time); - + if (debug_topics_) + { + sgm_debug_pub_ = + node->create_publisher("/buffered_segmentation", 1); + orig_pointcloud_pub_ = + node->create_publisher("/buffered_pointcloud", 1); + proc_pointcloud_pub_ = + node->create_publisher("/processed_pointcloud", 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) + double* min_x, double* min_y, double* max_x, + double* max_y) { - if (rolling_window_) { - updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2); - } - if (!enabled_) { - current_ = true; - return; - } - std::vector segmentations; - msg_buffer_->getObjects(segmentations); - int processed_msgs = 0; - for(auto& segmentation : segmentations){ - int touched_cells = 0; - for(size_t v = 0; v < segmentation.message.height; v=v+2) - { - for(size_t u = 0; u < segmentation.message.width; u=u+2){ - uint8_t pixel = segmentation.message.data[v*segmentation.message.width+u]; - cv::Point2d pixel_idx; - pixel_idx.x = u; - pixel_idx.y = v; - geometry_msgs::msg::PointStamped world_point; - if(!ray_caster_.imageToGroundPlaneLookup(pixel_idx, world_point, segmentation.transform)) - { - RCLCPP_DEBUG(logger_, "Could not raycast"); - continue; - } - unsigned int mx, my; - if(!worldToMap(world_point.point.x, world_point.point.y, mx, my)) - { - RCLCPP_DEBUG(logger_, "Computing map coords failed"); - continue; - } - unsigned int index = getIndex(mx, my); - if(pixel!= 1){ - costmap_[index] = 100; - }else{ - costmap_[index] = 0; - } - touch(world_point.point.x, world_point.point.y, min_x, min_y, max_x, max_y); - touched_cells++; - } - } - processed_msgs++; + if (rolling_window_) + { + updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2); + } + if (!enabled_) + { + current_ = true; + return; + } + std::vector segmentations; + segmentation_buffer_->lock(); + segmentation_buffer_->getSegmentations(segmentations); + segmentation_buffer_->unlock(); + for (auto& segmentation : segmentations) + { + if (debug_topics_) + { + proc_pointcloud_pub_->publish(*segmentation.cloud_); } - if(processed_msgs>0) + 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++) { - RCLCPP_INFO(logger_, "Processed %i segmentations", processed_msgs); + 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 (!class_map_.count(segmentation.class_map_[class_id])) + { + RCLCPP_DEBUG(logger_, "Cost for class id %i was not defined, skipping", class_id); + continue; + } + costmap_[index] = class_map_[segmentation.class_map_[class_id]]; + touch(*(iter_x + point), *(iter_y + point), min_x, min_y, max_x, max_y); } - // vision_msgs::msg::SemanticSegmentation current_segmentation = latest_segmentation_message; - // int touched_cells = 0; - // for(size_t v = 0; v < current_segmentation.height; v=v+5) - // { - // for(size_t u = 0; u < current_segmentation.width; u=u+5){ - // uint8_t pixel = current_segmentation.data[v*current_segmentation.width+u]; - // cv::Point2d pixel_idx; - // pixel_idx.x = u; - // pixel_idx.y = v; - // geometry_msgs::msg::PointStamped world_point; - // if(!ray_caster_.imageToGroundPlane(pixel_idx, world_point)) - // { - // RCLCPP_DEBUG(logger_, "Could not raycast"); - // continue; - // } - // unsigned int mx, my; - // if(!worldToMap(world_point.point.x, world_point.point.y, mx, my)) - // { - // RCLCPP_DEBUG(logger_, "Computing map coords failed"); - // continue; - // } - // unsigned int index = getIndex(mx, my); - // if(pixel!= 1){ - // costmap_[index] = 100; - // }else{ - // costmap_[index] = 0; - // } - // touch(world_point.point.x, world_point.point.y, min_x, min_y, max_x, max_y); - // touched_cells++; - // } - // } - // RCLCPP_INFO(logger_, "touched %i cells", touched_cells); - + } } // 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()); + 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) +void SemanticSegmentationLayer::updateCosts(nav2_costmap_2d::Costmap2D& master_grid, int min_i, + int min_j, int max_i, int max_j) { - if (!enabled_) - { - return; - } + if (!enabled_) + { + return; + } - if (!current_ && was_reset_) { - was_reset_ = false; - current_ = true; - } - // (void) max_j; - // (void) master_grid; - // (void) min_i; - // (void) max_i; - // (void) min_j; - if(!costmap_) - { - return; - } - // RCLCPP_INFO(logger_, "Updating costmap"); - switch (combination_method_) { + 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; @@ -235,38 +257,52 @@ void SemanticSegmentationLayer::updateCosts(nav2_costmap_2d::Costmap2D& master_g break; default: // Nothing break; - } + } } -void SemanticSegmentationLayer::segmentationCb(vision_msgs::msg::SemanticSegmentation::SharedPtr msg) +void SemanticSegmentationLayer::syncSegmPointcloudCb( + const std::shared_ptr& segmentation, + const std::shared_ptr& pointcloud) { - // latest_segmentation_message = *msg; - geometry_msgs::msg::TransformStamped current_transform; - try - { - current_transform = tf_->lookupTransform(global_frame_, msg->header.frame_id, msg->header.stamp); - } - catch (tf2::TransformException & ex) { - // if an exception occurs, we need to remove the empty observation from the list - RCLCPP_ERROR( - logger_, - "TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s", - msg->header.frame_id.c_str(), - global_frame_.c_str(), ex.what()); - return; - } - MessageTf message_tf; - message_tf.message = *msg; - message_tf.transform = current_transform; - msg_buffer_->bufferObject(message_tf, msg->header.stamp); - RCLCPP_INFO(logger_, "msg buffered"); + 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 || + segmentation->confidence.size() < expected_array_size) + { + RCLCPP_WARN(logger_, + "segmentation arrays have wrong sizes: data->%lu, confidence->%lu, expected->%u. " + "Will not buffer message", + segmentation->data.size(), segmentation->confidence.size(), expected_array_size); + return; + } + if (segmentation->class_map.size() == 0) + { + RCLCPP_WARN(logger_, "Classs map is empty. Will not buffer message"); + return; + } + segmentation_buffer_->lock(); + segmentation_buffer_->bufferSegmentation(*pointcloud, *segmentation); + segmentation_buffer_->unlock(); + if (debug_topics_) + { + sgm_debug_pub_->publish(*segmentation); + orig_pointcloud_pub_->publish(*pointcloud); + } } void SemanticSegmentationLayer::reset() { - resetMaps(); - current_ = false; - was_reset_ = true; + resetMaps(); + current_ = false; + was_reset_ = true; } } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/src/segmentation_buffer.cpp b/nav2_costmap_2d/src/segmentation_buffer.cpp new file mode 100644 index 0000000000..15baa30b28 --- /dev/null +++ b/nav2_costmap_2d/src/segmentation_buffer.cpp @@ -0,0 +1,256 @@ +/********************************************************************* + * + * 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" +using namespace std::chrono_literals; + +namespace nav2_costmap_2d { +SegmentationBuffer::SegmentationBuffer(const nav2_util::LifecycleNode::WeakPtr& parent, + std::string topic_name, 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) + , 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) + , topic_name_(topic_name) + , 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::bufferSegmentation( + const sensor_msgs::msg::PointCloud2& cloud, + const vision_msgs::msg::SemanticSegmentation& segmentation) +{ + 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) = segmentation.confidence[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; + + // create tje class map from the classes contained on the segmentation message + std::map& segmentation_class_map = segmentation_list_.front().class_map_; + for (auto& semantic_class : segmentation.class_map) + { + segmentation_class_map[semantic_class.class_id] = semantic_class.class_name; + } + } 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(); +} + +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; + } + } + } +} + +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.", + topic_name_.c_str(), (clock_->now() - last_updated_).seconds(), + expected_update_rate_.seconds()); + } + return current; +} + +void SegmentationBuffer::resetLastUpdated() { last_updated_ = clock_->now(); } +} // namespace nav2_costmap_2d From 021ff36267d7334d0c63c636d6b7133a047341b5 Mon Sep 17 00:00:00 2001 From: pepisg Date: Fri, 24 Jun 2022 11:09:18 -0500 Subject: [PATCH 07/13] Remove ray casting --- nav2_costmap_2d/src/ray_caster.cpp | 210 ----------------------------- 1 file changed, 210 deletions(-) delete mode 100644 nav2_costmap_2d/src/ray_caster.cpp diff --git a/nav2_costmap_2d/src/ray_caster.cpp b/nav2_costmap_2d/src/ray_caster.cpp deleted file mode 100644 index f6fcd52bad..0000000000 --- a/nav2_costmap_2d/src/ray_caster.cpp +++ /dev/null @@ -1,210 +0,0 @@ -#include "nav2_costmap_2d/ray_caster.hpp" - -namespace nav2_costmap_2d -{ -RayCaster::RayCaster() -{ -} - -void RayCaster::initialize( - rclcpp::Node::SharedPtr & parent_node, - std::string camera_info_topic_name, - std::string aligned_pc2_topic_name, - bool use_pointcloud, - tf2_ros::Buffer * tf2_buffer, - std::string global_frame, - tf2::Duration tf_tolerance, - double max_trace_distance -) -{ - parent_node_ = parent_node; - global_frame_ = global_frame; - - tf2_buffer_ = tf2_buffer; - camera_info_topic_name_ = camera_info_topic_name; - aligned_pc2_topic_name_ = aligned_pc2_topic_name; - tf_tolerance_ = tf_tolerance; - use_pointcloud_ = use_pointcloud; - max_trace_distance_ = max_trace_distance; - if(use_pointcloud_) - { - aligned_pc2_sub_ = parent_node_->create_subscription(aligned_pc2_topic_name_, rclcpp::SensorDataQoS(), std::bind(&RayCaster::pointCloudCb, this, std::placeholders::_1)); - RCLCPP_INFO(parent_node_->get_logger(), "Subscribed to %s", aligned_pc2_topic_name_.c_str()); - } - else - { - camera_info_sub_ = parent_node_->create_subscription(camera_info_topic_name_, rclcpp::SensorDataQoS(), std::bind(&RayCaster::cameraInfoCb, this, std::placeholders::_1)); - RCLCPP_INFO(parent_node_->get_logger(), "Subscribed to %s", camera_info_topic_name_.c_str()); - } -} - -bool RayCaster::worldToImage(geometry_msgs::msg::PointStamped& point, cv::Point2d& pixel){ - if (!tf2_buffer_->canTransform( - point.header.frame_id, sensor_frame_, tf2_ros::fromMsg(point.header.stamp))) - { - RCLCPP_ERROR( - parent_node_->get_logger(), "Ray caster can't transform from %s to %s", - sensor_frame_.c_str(), point.header.frame_id.c_str()); - return false; - } - geometry_msgs::msg::PointStamped transformed_point; - tf2_buffer_->transform(point, transformed_point, sensor_frame_, tf_tolerance_); - cv::Point3d cv_point{transformed_point.point.x, transformed_point.point.y, transformed_point.point.z}; - pixel = camera_model_.project3dToPixel(cv_point); - return true; -} - -bool RayCaster::imageToGroundPlane(cv::Point2d& pixel, geometry_msgs::msg::PointStamped& point) -{ - // (void) sensor_to_world_tf; - if (!tf2_buffer_->canTransform( - global_frame_, sensor_frame_, tf2_ros::fromMsg(point.header.stamp))) - { - RCLCPP_ERROR_THROTTLE( - parent_node_->get_logger(), *parent_node_->get_clock(), 10000, "Ray caster can't transform from %s to %s", - sensor_frame_.c_str(), point.header.frame_id.c_str()); - return false; - } - cv::Point3d cv_ray = camera_model_.projectPixelTo3dRay(pixel); - geometry_msgs::msg::PointStamped ray_end_sensor_frame = rayToPointStamped(cv_ray, sensor_frame_); - geometry_msgs::msg::PointStamped ray_end_world_frame; - geometry_msgs::msg::PointStamped camera_position_world_frame; - tf2_buffer_->transform(ray_end_sensor_frame, ray_end_world_frame, global_frame_, tf_tolerance_); - tf2_buffer_->transform(camera_origin_, camera_position_world_frame, global_frame_, tf_tolerance_); - if(ray_end_world_frame.point.z > camera_position_world_frame.point.z) - { - RCLCPP_DEBUG(parent_node_->get_logger(),"Point falls behind the camera, will not raycast"); - return false; - } - double ray_scale_factor = -camera_position_world_frame.point.z/(ray_end_world_frame.point.z - camera_position_world_frame.point.z); - geometry_msgs::msg::PointStamped intersection; - intersection.header.frame_id = global_frame_; - intersection.point.x = camera_position_world_frame.point.x + ray_scale_factor*(ray_end_world_frame.point.x - camera_position_world_frame.point.x); - intersection.point.y = camera_position_world_frame.point.y + ray_scale_factor*(ray_end_world_frame.point.y - camera_position_world_frame.point.y); - intersection.point.z = camera_position_world_frame.point.z + ray_scale_factor*(ray_end_world_frame.point.z - camera_position_world_frame.point.z); - geometry_msgs::msg::PointStamped intersection_camera_frame = tf2_buffer_->transform(intersection, sensor_frame_); - if(intersection_camera_frame.point.z > max_trace_distance_) - { - RCLCPP_DEBUG(parent_node_->get_logger(),"Point falls further than the max trace distance, will not raycast"); - return false; - } - point = intersection; - return true; -} - -bool RayCaster::imageToGroundPlaneLookup(cv::Point2d& pixel, geometry_msgs::msg::PointStamped& point, geometry_msgs::msg::TransformStamped sensor_to_world_tf) -{ - if(!ready_to_raytrace_) - { - RCLCPP_WARN_THROTTLE(parent_node_->get_logger(), *parent_node_->get_clock(), 10000, "Not ready to raytrace"); - return false; - } - if (!tf2_buffer_->canTransform( - global_frame_, sensor_frame_, tf2_ros::fromMsg(point.header.stamp))) - { - RCLCPP_ERROR_THROTTLE( - parent_node_->get_logger(), *parent_node_->get_clock(), 10000, "Ray caster can't transform from %s to %s", - sensor_frame_.c_str(), point.header.frame_id.c_str()); - return false; - } - geometry_msgs::msg::PointStamped point_sensor_frame = caster_lookup_table_.at(pixel.y*img_width_+pixel.x); - if(point_sensor_frame.point.z > max_trace_distance_) - { - RCLCPP_DEBUG(parent_node_->get_logger(),"Point falls further than the max trace distance, will not raycast"); - return false; - } - if(point_sensor_frame.point.z < 0.0) - { - RCLCPP_DEBUG(parent_node_->get_logger(),"Point falls falls behind the camera, will not raycast"); - return false; - } - geometry_msgs::msg::PointStamped point_world_frame; - tf2::doTransform( - point_sensor_frame, point_world_frame, sensor_to_world_tf); - // tf2_buffer_->transform(point_sensor_frame, point_world_frame, global_frame_, tf_tolerance_); - point = point_world_frame; - return true; -} - -geometry_msgs::msg::PointStamped RayCaster::rayToPointStamped(cv::Point3d& ray_end, std::string& frame_id) -{ - geometry_msgs::msg::PointStamped point; - point.point.x = ray_end.x; - point.point.y = ray_end.y; - point.point.z = ray_end.z; - point.header.frame_id = frame_id; - return point; -} - -void RayCaster::pointCloudCb(sensor_msgs::msg::PointCloud2::SharedPtr msg) -{ - pointcloud_msg_ = *msg; - ready_to_raytrace_ = true; -} - -void RayCaster::cameraInfoCb(sensor_msgs::msg::CameraInfo::SharedPtr msg) -{ - RCLCPP_INFO(parent_node_->get_logger(), "Troll"); - if(msg->header.frame_id.empty()) - { - RCLCPP_ERROR(parent_node_->get_logger(), "Frame id of camera info message is empty. Cannot raycast"); - return; - } - camera_model_.fromCameraInfo(*msg); - sensor_frame_ = msg->header.frame_id; - camera_origin_.header.frame_id = sensor_frame_; - img_width_ = msg->width; - img_height_ = msg->height; - if(!computeLookupTable(msg->header.stamp)) - { - RCLCPP_WARN(parent_node_->get_logger(), "Could not compute lookup table"); - return; - } - camera_info_sub_.reset(); - ready_to_raytrace_ = true; - RCLCPP_INFO(parent_node_->get_logger(), "Received camera info message. Ready to raycast"); -} - - -bool RayCaster::computeLookupTable(builtin_interfaces::msg::Time & time_msg) -{ - if (!tf2_buffer_->canTransform( - global_frame_, sensor_frame_, tf2_ros::fromMsg(time_msg))) - { - RCLCPP_ERROR_THROTTLE( - parent_node_->get_logger(), *parent_node_->get_clock(), 10000, "Ray caster can't transform from %s to %s", - sensor_frame_.c_str(), global_frame_.c_str()); - return false; - } - caster_lookup_table_.clear(); - caster_lookup_table_.reserve(img_width_*img_height_); - for(size_t v = 0; v < img_height_; v++) - { - for(size_t u = 0; u < img_width_; u++) - { - cv::Point2d pixel; - pixel.x = u; - pixel.y = v; - cv::Point3d cv_ray = camera_model_.projectPixelTo3dRay(pixel); - geometry_msgs::msg::PointStamped ray_end_sensor_frame = rayToPointStamped(cv_ray, sensor_frame_); - geometry_msgs::msg::PointStamped ray_end_world_frame; - geometry_msgs::msg::PointStamped camera_position_world_frame; - tf2_buffer_->transform(ray_end_sensor_frame, ray_end_world_frame, global_frame_, tf_tolerance_); - tf2_buffer_->transform(camera_origin_, camera_position_world_frame, global_frame_, tf_tolerance_); - double ray_scale_factor = -camera_position_world_frame.point.z/(ray_end_world_frame.point.z - camera_position_world_frame.point.z); - geometry_msgs::msg::PointStamped intersection; - intersection.header.frame_id = global_frame_; - intersection.point.x = camera_position_world_frame.point.x + ray_scale_factor*(ray_end_world_frame.point.x - camera_position_world_frame.point.x); - intersection.point.y = camera_position_world_frame.point.y + ray_scale_factor*(ray_end_world_frame.point.y - camera_position_world_frame.point.y); - intersection.point.z = camera_position_world_frame.point.z + ray_scale_factor*(ray_end_world_frame.point.z - camera_position_world_frame.point.z); - geometry_msgs::msg::PointStamped intersection_camera_frame = tf2_buffer_->transform(intersection, sensor_frame_); - caster_lookup_table_.emplace_back(intersection_camera_frame); - } - } - RCLCPP_INFO(parent_node_->get_logger(), "Lookup table computed successfully"); - return true; -} - - - -} // namespace nav2_costmap_2d \ No newline at end of file From 04e85bf3f5dd1ab35d45d0db416018dd9eabbebe Mon Sep 17 00:00:00 2001 From: pepisg Date: Fri, 24 Jun 2022 11:19:19 -0500 Subject: [PATCH 08/13] Delete ray tracing related stuff --- nav2_costmap_2d/CMakeLists.txt | 2 - nav2_costmap_2d/costmap_plugins.xml | 17 ++- .../include/nav2_costmap_2d/object_buffer.hpp | 120 ------------------ nav2_costmap_2d/package.xml | 1 - 4 files changed, 8 insertions(+), 132 deletions(-) delete mode 100644 nav2_costmap_2d/include/nav2_costmap_2d/object_buffer.hpp diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt index 50abc0cd7a..81ffe69515 100644 --- a/nav2_costmap_2d/CMakeLists.txt +++ b/nav2_costmap_2d/CMakeLists.txt @@ -22,7 +22,6 @@ find_package(tf2_geometry_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_sensor_msgs REQUIRED) -find_package(image_geometry REQUIRED) find_package(visualization_msgs REQUIRED) find_package(angles REQUIRED) @@ -77,7 +76,6 @@ set(dependencies tf2_geometry_msgs tf2_ros tf2_sensor_msgs - image_geometry visualization_msgs angles ) diff --git a/nav2_costmap_2d/costmap_plugins.xml b/nav2_costmap_2d/costmap_plugins.xml index f6e212031b..c4fb17286b 100644 --- a/nav2_costmap_2d/costmap_plugins.xml +++ b/nav2_costmap_2d/costmap_plugins.xml @@ -1,32 +1,31 @@ - + Inflates obstacles to speed collision checking and to make robot prefer to stay away from obstacles. - + Listens to laser scan and point cloud messages and marks and clears grid cells. - + Listens to OccupancyGrid messages and copies them in, like from map_server. - + Similar to obstacle costmap, but uses 3D voxel grid to store data. A range-sensor (sonar, IR) based obstacle layer for costmap_2d - This is an example plugin which puts repeating costs gradients to costmap + A plugin for semantic segmentation data produced by RGBD cameras - + Prevents the robot from appearing in keepout zones marked on map. - + Restricts maximum speed of robot in speed-limit zones marked on map. - - + \ No newline at end of file diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/object_buffer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/object_buffer.hpp deleted file mode 100644 index 66ec273fb1..0000000000 --- a/nav2_costmap_2d/include/nav2_costmap_2d/object_buffer.hpp +++ /dev/null @@ -1,120 +0,0 @@ -#ifndef NAV2_COSTMAP_2D__OBJECT_BUFFER_HPP_ -#define NAV2_COSTMAP_2D__OBJECT_BUFFER_HPP_ - -#include -#include -#include -#include -#include - -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "tf2_ros/buffer.h" -#include "rclcpp/rclcpp.hpp" -#include "rclcpp/time.hpp" -#include "geometry_msgs/msg/transform_stamped.hpp" - -namespace nav2_costmap_2d -{ - using rclcpp::Time; - - template - struct TimedObject - { - ObjectT object; - Time time; - }; -/** - * @class ObjectBuffer - * @brief Class to orderly store Objects for a given period of time - */ -template -class ObjectBuffer -{ - public: - /** - * @brief Constructs an Object buffer. - * @param Object_keep_time Defines the persistence of Objects in seconds, 0 means only keep the latest - */ - ObjectBuffer( - const nav2_util::LifecycleNode::WeakPtr & parent, - double object_keep_time - ): - object_keep_time_(rclcpp::Duration::from_seconds(object_keep_time)) - { - auto node = parent.lock(); - clock_ = node->get_clock(); - logger_ = node->get_logger(); - } - - /** - * @brief Destructor... cleans up - */ - ~ObjectBuffer(){}; - - /** - * @brief stores a Object with an associated generation time - */ - void bufferObject(const ObjectT & object, const Time & generation_time) - { - TimedObject timed_object; - timed_object.object = object; - timed_object.time = generation_time; - objects_list_.push_front(timed_object); - } - - /** - * @brief Pushes copies of all current Objects onto the end of the vector passed in - * @param Objects The vector to be filled - */ - void getObjects(std::vector & objects) - { - // first... let's make sure that we don't have any stale objects - purgeStaleObjects(); - // now we'll just copy the objects for the caller - for (auto obs_it = objects_list_.begin(); obs_it != objects_list_.end(); ++obs_it) { - objects.push_back(obs_it->object); - } - objects_list_.clear(); - } - - - private: - /** - * @brief Removes any stale objects from the buffer list - */ - void purgeStaleObjects() - { - if(!objects_list_.empty()) - { - // if we're keeping objects for no time... then we'll only keep one object - auto obj_it = objects_list_.begin(); - if(object_keep_time_ == rclcpp::Duration::from_seconds(0.0)){ - objects_list_.erase(++obj_it, objects_list_.end()); - return; - } - - // otherwise... we'll have to loop through the objectss to see which ones are stale - for (obj_it = objects_list_.begin(); obj_it != objects_list_.end(); ++obj_it) - { - // check if the objects is out of date... and if it is, - // remove it and those that follow from the list - if ((clock_->now() - obj_it->time) > object_keep_time_) - { - objects_list_.erase(obj_it, objects_list_.end()); - return; - } - } - } - } - - rclcpp::Clock::SharedPtr clock_; - rclcpp::Logger logger_{rclcpp::get_logger("nav2_costmap_2d")}; - const rclcpp::Duration object_keep_time_; - std::list> objects_list_; -}; - -} // namespace nav2_costmap_2d - - - -#endif // NAV2_COSTMAP_2D__OBJECT_BUFFER_HPP_ diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index aecc4d2fb1..52aee123fe 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -37,7 +37,6 @@ tf2_geometry_msgs tf2_ros tf2_sensor_msgs - image_geometry visualization_msgs angles From bb3d07ac915d591b5b42b0735ca306853853a0cc Mon Sep 17 00:00:00 2001 From: pepisg Date: Fri, 24 Jun 2022 11:31:15 -0500 Subject: [PATCH 09/13] revert auto xml styling --- nav2_costmap_2d/costmap_plugins.xml | 15 ++++++++------- .../include/nav2_costmap_2d/segmentation.hpp | 1 + .../semantic_segmentation_layer.hpp | 2 -- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/nav2_costmap_2d/costmap_plugins.xml b/nav2_costmap_2d/costmap_plugins.xml index c4fb17286b..b5a6ae6550 100644 --- a/nav2_costmap_2d/costmap_plugins.xml +++ b/nav2_costmap_2d/costmap_plugins.xml @@ -1,15 +1,15 @@ - + Inflates obstacles to speed collision checking and to make robot prefer to stay away from obstacles. - + Listens to laser scan and point cloud messages and marks and clears grid cells. - + Listens to OccupancyGrid messages and copies them in, like from map_server. - + Similar to obstacle costmap, but uses 3D voxel grid to store data. @@ -21,11 +21,12 @@ - + Prevents the robot from appearing in keepout zones marked on map. - + Restricts maximum speed of robot in speed-limit zones marked on map. - \ No newline at end of file + + diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/segmentation.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/segmentation.hpp index 6b287cc9fd..94a38cda6c 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/segmentation.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/segmentation.hpp @@ -33,6 +33,7 @@ #define NAV2_COSTMAP_2D__SEGMENTATION_HPP_ #include +#include #include namespace nav2_costmap_2d { 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 index bcfbd57ae7..5aad54e7d1 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/semantic_segmentation_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/semantic_segmentation_layer.hpp @@ -46,11 +46,9 @@ #include "nav2_costmap_2d/costmap_layer.hpp" #include "nav2_costmap_2d/layer.hpp" #include "nav2_costmap_2d/layered_costmap.hpp" -#include "nav2_costmap_2d/object_buffer.hpp" #include "nav2_costmap_2d/segmentation.hpp" #include "nav2_costmap_2d/segmentation_buffer.hpp" #include "nav2_util/node_utils.hpp" -#include "opencv2/core.hpp" #include "rclcpp/rclcpp.hpp" #include "tf2_ros/message_filter.h" #include "vision_msgs/msg/semantic_segmentation.hpp" From 9288982266848d18b9ada65724a4b4a6e73dbe96 Mon Sep 17 00:00:00 2001 From: pepisg Date: Fri, 24 Jun 2022 14:12:57 -0500 Subject: [PATCH 10/13] Revert auto styling --- .../semantic_segmentation_layer.hpp | 2 -- nav2_costmap_2d/package.xml | 4 ++-- .../plugins/semantic_segmentation_layer.cpp | 20 +++++++++---------- 3 files changed, 12 insertions(+), 14 deletions(-) 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 index 5aad54e7d1..8077d5e7a5 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/semantic_segmentation_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/semantic_segmentation_layer.hpp @@ -116,8 +116,6 @@ class SemanticSegmentationLayer : public CostmapLayer const std::shared_ptr& segmentation, const std::shared_ptr& pointcloud); - // rclcpp::Subscription::SharedPtr - // semantic_segmentation_sub_; std::shared_ptr> semantic_segmentation_sub_; std::shared_ptr> pointcloud_sub_; diff --git a/nav2_costmap_2d/package.xml b/nav2_costmap_2d/package.xml index 52aee123fe..487a6c4c4a 100644 --- a/nav2_costmap_2d/package.xml +++ b/nav2_costmap_2d/package.xml @@ -1,6 +1,6 @@ - + nav2_costmap_2d 1.0.0 @@ -49,6 +49,6 @@ nav2_lifecycle_manager - ament_cmake + 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 index c21b2e44c8..8e947a281d 100644 --- a/nav2_costmap_2d/plugins/semantic_segmentation_layer.cpp +++ b/nav2_costmap_2d/plugins/semantic_segmentation_layer.cpp @@ -72,27 +72,27 @@ void SemanticSegmentationLayer::onInitialize() expected_update_rate; declareParameter("enabled", rclcpp::ParameterValue(true)); - node->get_parameter(name_ + "." + "enabled", enabled_); declareParameter("combination_method", rclcpp::ParameterValue(1)); - node->get_parameter(name_ + "." + "combination_method", combination_method_); declareParameter("publish_debug_topics", rclcpp::ParameterValue(false)); - node->get_parameter(name_ + "." + "publish_debug_topics", debug_topics_); declareParameter("max_obstacle_distance", rclcpp::ParameterValue(5.0)); - node->get_parameter(name_ + "." + "max_obstacle_distance", max_obstacle_distance); declareParameter("min_obstacle_distance", rclcpp::ParameterValue(0.3)); - node->get_parameter(name_ + "." + "min_obstacle_distance", min_obstacle_distance); declareParameter("segmentation_topic", rclcpp::ParameterValue("")); - node->get_parameter(name_ + "." + "segmentation_topic", segmentation_topic); declareParameter("pointcloud_topic", rclcpp::ParameterValue("")); - node->get_parameter(name_ + "." + "pointcloud_topic", pointcloud_topic); declareParameter("observation_persistence", rclcpp::ParameterValue(0.0)); - node->get_parameter(name_ + "." + "observation_persistence", observation_keep_time); declareParameter(name_ + "." + "expected_update_rate", rclcpp::ParameterValue(0.0)); + declareParameter("class_types", rclcpp::ParameterValue(std::vector({}))); + + node->get_parameter(name_ + "." + "enabled", enabled_); + node->get_parameter(name_ + "." + "combination_method", combination_method_); + node->get_parameter(name_ + "." + "publish_debug_topics", debug_topics_); + node->get_parameter(name_ + "." + "max_obstacle_distance", max_obstacle_distance); + node->get_parameter(name_ + "." + "min_obstacle_distance", min_obstacle_distance); + node->get_parameter(name_ + "." + "segmentation_topic", segmentation_topic); + node->get_parameter(name_ + "." + "pointcloud_topic", pointcloud_topic); + node->get_parameter(name_ + "." + "observation_persistence", observation_keep_time); node->get_parameter(name_ + "." + "sensor_frame", sensor_frame); node->get_parameter(name_ + "." + "expected_update_rate", expected_update_rate); node->get_parameter("transform_tolerance", transform_tolerance); - - declareParameter("class_types", rclcpp::ParameterValue(std::vector({}))); node->get_parameter(name_ + "." + "class_types", class_types_string); if (class_types_string.empty()) { From a3182c6d47a30954de0513ef11e66ce6ca2f8850 Mon Sep 17 00:00:00 2001 From: pepisg Date: Fri, 24 Jun 2022 17:50:05 -0500 Subject: [PATCH 11/13] Docstrings and comments --- .../include/nav2_costmap_2d/segmentation.hpp | 28 ++++++-------- .../nav2_costmap_2d/segmentation_buffer.hpp | 25 ++++++------ .../semantic_segmentation_layer.hpp | 19 +++++++++- .../plugins/semantic_segmentation_layer.cpp | 38 +++++++------------ nav2_costmap_2d/src/segmentation_buffer.cpp | 16 ++++++-- 5 files changed, 65 insertions(+), 61 deletions(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/segmentation.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/segmentation.hpp index 94a38cda6c..da1473c40a 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/segmentation.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/segmentation.hpp @@ -26,20 +26,23 @@ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * - * Authors: Conor McGann + * Authors: Pedro Gonzalez */ #ifndef NAV2_COSTMAP_2D__SEGMENTATION_HPP_ #define NAV2_COSTMAP_2D__SEGMENTATION_HPP_ -#include #include + +#include #include namespace nav2_costmap_2d { /** - * @brief Stores an segmentation in terms of a point cloud and the origin of the source + * @brief Stores an segmentation in terms of a point cloud containing the class and confidence + * of each point, the origin of the source and a class map containing the names of each class id + * in the pointcloud * @note Tried to make members and constructor arguments const but the compiler would not accept the * default assignment operator for vector insertion! */ @@ -51,7 +54,7 @@ class Segmentation */ Segmentation() : cloud_(new sensor_msgs::msg::PointCloud2()) {} /** - * @brief A destructor + * @brief A destructor. Deletes pointcloud pointer */ virtual ~Segmentation() { delete cloud_; } @@ -69,17 +72,11 @@ class Segmentation } /** - * @brief Creates an segmentation from an origin point and a point cloud + * @brief Creates an segmentation from an origin point, a point cloud and a class map * @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 + * @param cloud The point cloud of the segmentation. It must have the class and intensity channels + * @param class_map The name of each class id in the segmentation. i.e: 1: "Grass", 2: "Street" * 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::map class_map) @@ -101,10 +98,6 @@ class Segmentation /** * @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)) @@ -113,6 +106,7 @@ class Segmentation geometry_msgs::msg::Point origin_; sensor_msgs::msg::PointCloud2* cloud_; + ///< @brief To store the correspondence of each class id with its name std::map class_map_; }; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/segmentation_buffer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/segmentation_buffer.hpp index 1ec9d65804..07828a2227 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/segmentation_buffer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/segmentation_buffer.hpp @@ -32,7 +32,7 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * - * Author: Eitan Marder-Eppstein + * Author: Pedro Gonzalez *********************************************************************/ #ifndef NAV2_COSTMAP_2D__SEGMENTATION_BUFFER_HPP_ #define NAV2_COSTMAP_2D__SEGMENTATION_BUFFER_HPP_ @@ -53,7 +53,8 @@ namespace nav2_costmap_2d { /** * @class SegmentationBuffer - * @brief Takes in point clouds from sensors, transforms them to the desired frame, and stores them + * @brief Takes in segmentation and alingned pointclouds, and stores transformed pointclouds with + * extra fields for the class and inference confidence of each point */ class SegmentationBuffer { @@ -66,16 +67,10 @@ class SegmentationBuffer * 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 max_lookahead_distance The range to which the sensor should be trusted for projecting the + * segmentation mask + * @param min_lookahead_distance The range from which the sensor should be trusted for projecting the + * segmentation mask * @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 @@ -95,10 +90,12 @@ class SegmentationBuffer ~SegmentationBuffer(); /** - * @brief Transforms a PointCloud to the global frame and buffers it + * @brief Transforms a PointCloud to the global frame, adds channels for class and confidence of + * each point 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 + * @param cloud The cloud to be buffered. It should be aligned with the segmentation + * @param segmentation The segmentation mask containing the class and confidence of each pixel */ void bufferSegmentation(const sensor_msgs::msg::PointCloud2& cloud, const vision_msgs::msg::SemanticSegmentation& segmentation); 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 index 8077d5e7a5..ad97bbf5b2 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/semantic_segmentation_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/semantic_segmentation_layer.hpp @@ -112,27 +112,42 @@ class SemanticSegmentationLayer : public CostmapLayer virtual bool isClearable() { return true; } private: + /** + * @brief Callback called when a poincloud and a segmentation are synced and a tf is available + * @param segmentation a pointer segmentation mask containing the class and confidence of each pixel + * @param pointcloud A pointer to the aligned pointcloud + */ void syncSegmPointcloudCb( const std::shared_ptr& segmentation, const std::shared_ptr& pointcloud); + ///< @brief subscriber to segmentation messages std::shared_ptr> semantic_segmentation_sub_; + ///< @brief subscriber to aligned pointclouds std::shared_ptr> pointcloud_sub_; + ///< @brief subscriber to guarantee that transforms are available when pointclouds are received + std::shared_ptr> pointcloud_tf_sub_; + ///< @brief subscriber to sync transforms, pointclouds and segmentations std::shared_ptr> segm_pc_sync_; - std::shared_ptr> pointcloud_tf_sub_; // debug publishers + ///< @brief publisher to publish segmentation messages when buffered std::shared_ptr> sgm_debug_pub_; + ///< @brief publisher to publish pointcloud messages when buffered std::shared_ptr> orig_pointcloud_pub_; + ///< @brief publisher to publish pointclouds with class and confidence channels when put in the costmap std::shared_ptr> proc_pointcloud_pub_; + /// @brief Used to store segmentations and aligned pointclouds std::shared_ptr segmentation_buffer_; - std::string global_frame_; + std::string global_frame_; ///< @brief The global frame for the costmap + + ///< @brief Correspondence between class names and cost. i.e: "Grass": 254, "Floor": 0 std::map class_map_; bool rolling_window_; diff --git a/nav2_costmap_2d/plugins/semantic_segmentation_layer.cpp b/nav2_costmap_2d/plugins/semantic_segmentation_layer.cpp index 8e947a281d..1e74beaf26 100644 --- a/nav2_costmap_2d/plugins/semantic_segmentation_layer.cpp +++ b/nav2_costmap_2d/plugins/semantic_segmentation_layer.cpp @@ -33,12 +33,7 @@ * 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 + * Author: Pedro Gonzalez *********************************************************************/ #include "nav2_costmap_2d/semantic_segmentation_layer.hpp" @@ -54,9 +49,6 @@ 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; @@ -79,7 +71,7 @@ void SemanticSegmentationLayer::onInitialize() declareParameter("segmentation_topic", rclcpp::ParameterValue("")); declareParameter("pointcloud_topic", rclcpp::ParameterValue("")); declareParameter("observation_persistence", rclcpp::ParameterValue(0.0)); - declareParameter(name_ + "." + "expected_update_rate", rclcpp::ParameterValue(0.0)); + declareParameter("expected_update_rate", rclcpp::ParameterValue(0.0)); declareParameter("class_types", rclcpp::ParameterValue(std::vector({}))); node->get_parameter(name_ + "." + "enabled", enabled_); @@ -102,6 +94,7 @@ void SemanticSegmentationLayer::onInitialize() for (auto& source : class_types_string) { + // get the cost for each class std::vector classes_ids; uint8_t cost; declareParameter(source + ".classes", rclcpp::ParameterValue(std::vector({}))); @@ -140,6 +133,7 @@ void SemanticSegmentationLayer::onInitialize() pointcloud_tf_sub_ = std::make_shared>( *pointcloud_sub_, *tf_, global_frame_, 50, rclcpp_node_, tf2::durationFromSec(transform_tolerance)); + // make sure all data is synced: pointclouds, transforms and segmentations segm_pc_sync_ = std::make_shared>( @@ -147,11 +141,13 @@ void SemanticSegmentationLayer::onInitialize() segm_pc_sync_->registerCallback(std::bind(&SemanticSegmentationLayer::syncSegmPointcloudCb, this, std::placeholders::_1, std::placeholders::_2)); + // create a segmentation buffer segmentation_buffer_ = std::make_shared( node, pointcloud_topic, observation_keep_time, expected_update_rate, max_obstacle_distance, min_obstacle_distance, *tf_, global_frame_, sensor_frame, tf2::durationFromSec(transform_tolerance)); + // only for testing purposes if (debug_topics_) { sgm_debug_pub_ = @@ -163,9 +159,7 @@ void SemanticSegmentationLayer::onInitialize() } } -// 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) @@ -180,6 +174,7 @@ void SemanticSegmentationLayer::updateBounds(double robot_x, double robot_y, dou return; } std::vector segmentations; + // get the latest segmentations thread safely segmentation_buffer_->lock(); segmentation_buffer_->getSegmentations(segmentations); segmentation_buffer_->unlock(); @@ -192,6 +187,7 @@ void SemanticSegmentationLayer::updateBounds(double robot_x, double robot_y, dou sensor_msgs::PointCloud2ConstIterator iter_x(*segmentation.cloud_, "x"); sensor_msgs::PointCloud2ConstIterator iter_y(*segmentation.cloud_, "y"); sensor_msgs::PointCloud2ConstIterator iter_class(*segmentation.cloud_, "class"); + // For now, confidence of the inference is not used to adjust cost // sensor_msgs::PointCloud2ConstIterator iter_confidence(*segmentation.cloud_, // "confidence"); for (size_t point = 0; point < segmentation.cloud_->height * segmentation.cloud_->width; @@ -205,19 +201,19 @@ void SemanticSegmentationLayer::updateBounds(double robot_x, double robot_y, dou } unsigned int index = getIndex(mx, my); uint8_t class_id = *(iter_class + point); + // Check if no cost was defined for the given class if (!class_map_.count(segmentation.class_map_[class_id])) { RCLCPP_DEBUG(logger_, "Cost for class id %i was not defined, skipping", class_id); continue; } + // Get the class name from the buffered segmentation and then gets its cost from the class map costmap_[index] = class_map_[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"), @@ -225,10 +221,6 @@ void SemanticSegmentationLayer::onFootprintChanged() 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) { @@ -246,7 +238,6 @@ void SemanticSegmentationLayer::updateCosts(nav2_costmap_2d::Costmap2D& master_g { return; } - // RCLCPP_INFO(logger_, "Updating costmap"); switch (combination_method_) { case 0: // Overwrite @@ -264,6 +255,7 @@ void SemanticSegmentationLayer::syncSegmPointcloudCb( const std::shared_ptr& segmentation, const std::shared_ptr& pointcloud) { + // check if data has the right dimensions if (segmentation->width * segmentation->height != pointcloud->width * pointcloud->height) { RCLCPP_WARN(logger_, @@ -283,11 +275,13 @@ void SemanticSegmentationLayer::syncSegmPointcloudCb( segmentation->data.size(), segmentation->confidence.size(), expected_array_size); return; } + // check if class names and ids correspondence is contained in the message if (segmentation->class_map.size() == 0) { RCLCPP_WARN(logger_, "Classs map is empty. Will not buffer message"); return; } + // Store the pointcloud and segmentation segmentation_buffer_->lock(); segmentation_buffer_->bufferSegmentation(*pointcloud, *segmentation); segmentation_buffer_->unlock(); @@ -306,9 +300,5 @@ void SemanticSegmentationLayer::reset() } } // 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/segmentation_buffer.cpp b/nav2_costmap_2d/src/segmentation_buffer.cpp index 15baa30b28..5f9d47384a 100644 --- a/nav2_costmap_2d/src/segmentation_buffer.cpp +++ b/nav2_costmap_2d/src/segmentation_buffer.cpp @@ -32,7 +32,7 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * - * Author: Eitan Marder-Eppstein + * Author: Pedro Gonzalez *********************************************************************/ #include "nav2_costmap_2d/segmentation_buffer.hpp" @@ -103,8 +103,8 @@ void SegmentationBuffer::bufferSegmentation( 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 + // create a segmented pointcloud to store the original 3D data as well as the class and confidence + // of each point sensor_msgs::msg::PointCloud2& segmentation_cloud = *(segmentation_list_.front().cloud_); segmentation_cloud.height = global_frame_cloud.height; segmentation_cloud.width = global_frame_cloud.width; @@ -117,12 +117,15 @@ void SegmentationBuffer::bufferSegmentation( unsigned int cloud_size = global_frame_cloud.height * global_frame_cloud.width; sensor_msgs::PointCloud2Modifier modifier(segmentation_cloud); + // add the class and confidence fields to the pointcloud. 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); + + // update the point step and get iterators modifier.resize(cloud_size); sensor_msgs::PointCloud2Iterator iter_class_obs(segmentation_cloud, "class"); sensor_msgs::PointCloud2Iterator iter_confidence_obs(segmentation_cloud, "confidence"); @@ -130,6 +133,7 @@ void SegmentationBuffer::bufferSegmentation( sensor_msgs::PointCloud2Iterator iter_y_obs(segmentation_cloud, "y"); sensor_msgs::PointCloud2Iterator iter_z_obs(segmentation_cloud, "z"); + // get iterators for the original cloud 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"); @@ -146,14 +150,18 @@ void SegmentationBuffer::bufferSegmentation( { continue; } + // calculate the distance of the point to the sensor 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); + + // Remove points that are too far or too close if (sq_dist >= sq_max_lookahead_distance_ || sq_dist <= sq_min_lookahead_distance_) { continue; } + // assign the values to the segmentation and the original pointcloud to each point *(iter_class_obs + point_count) = segmentation.data[pixel_idx]; *(iter_confidence_obs + point_count) = segmentation.confidence[pixel_idx]; *(iter_x_obs + point_count) = *(iter_x_global + pixel_idx); @@ -168,7 +176,7 @@ void SegmentationBuffer::bufferSegmentation( segmentation_cloud.header.stamp = cloud.header.stamp; segmentation_cloud.header.frame_id = global_frame_cloud.header.frame_id; - // create tje class map from the classes contained on the segmentation message + // create the class map from the classes contained on the segmentation message std::map& segmentation_class_map = segmentation_list_.front().class_map_; for (auto& semantic_class : segmentation.class_map) { From c05182ca52b883c629f8def94c99fd393de4b2a0 Mon Sep 17 00:00:00 2001 From: pepisg Date: Fri, 24 Jun 2022 18:19:11 -0500 Subject: [PATCH 12/13] Remove old boost exclusion --- nav2_costmap_2d/CMakeLists.txt | 3 --- 1 file changed, 3 deletions(-) diff --git a/nav2_costmap_2d/CMakeLists.txt b/nav2_costmap_2d/CMakeLists.txt index 81ffe69515..73ffac934f 100644 --- a/nav2_costmap_2d/CMakeLists.txt +++ b/nav2_costmap_2d/CMakeLists.txt @@ -54,9 +54,6 @@ add_library(nav2_costmap_2d_core SHARED plugins/costmap_filters/costmap_filter.cpp ) -# prevent pluginlib from using boost -target_compile_definitions(nav2_costmap_2d_core PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") - set(dependencies geometry_msgs laser_geometry From 8a9725da84a4220dbb801cb7fae904ec9373a664 Mon Sep 17 00:00:00 2001 From: pepisg Date: Sun, 10 Dec 2023 16:09:32 +0000 Subject: [PATCH 13/13] Replace legacy segmentation message with labelinfo message --- .../include/nav2_costmap_2d/segmentation.hpp | 57 +- .../nav2_costmap_2d/segmentation_buffer.hpp | 89 +++- .../semantic_segmentation_layer.hpp | 90 ++-- .../plugins/semantic_segmentation_layer.cpp | 487 ++++++++++++------ nav2_costmap_2d/src/segmentation_buffer.cpp | 141 ++--- 5 files changed, 547 insertions(+), 317 deletions(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/segmentation.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/segmentation.hpp index da1473c40a..fd653d9ad2 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/segmentation.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/segmentation.hpp @@ -32,37 +32,36 @@ #ifndef NAV2_COSTMAP_2D__SEGMENTATION_HPP_ #define NAV2_COSTMAP_2D__SEGMENTATION_HPP_ -#include - #include +#include #include -namespace nav2_costmap_2d { +namespace nav2_costmap_2d +{ /** - * @brief Stores an segmentation in terms of a point cloud containing the class and confidence - * of each point, the origin of the source and a class map containing the names of each class id - * in the pointcloud + * @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: +public: /** * @brief Creates an empty segmentation */ - Segmentation() : cloud_(new sensor_msgs::msg::PointCloud2()) {} + Segmentation() + : cloud_(new sensor_msgs::msg::PointCloud2()) {} /** - * @brief A destructor. Deletes pointcloud pointer + * @brief A destructor */ - virtual ~Segmentation() { delete cloud_; } + virtual ~Segmentation() {delete cloud_;} /** * @brief Copy assignment operator * @param obs The segmentation to copy */ - Segmentation& operator=(const Segmentation& obs) + Segmentation & operator=(const Segmentation & obs) { origin_ = obs.origin_; cloud_ = new sensor_msgs::msg::PointCloud2(*(obs.cloud_)); @@ -72,15 +71,22 @@ class Segmentation } /** - * @brief Creates an segmentation from an origin point, a point cloud and a class map + * @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. It must have the class and intensity channels - * @param class_map The name of each class id in the segmentation. i.e: 1: "Grass", 2: "Street" + * @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::map class_map) - : origin_(origin), cloud_(new sensor_msgs::msg::PointCloud2(cloud)), class_map_(class_map) + 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) { } @@ -88,8 +94,8 @@ class Segmentation * @brief Copy constructor * @param obs The segmentation to copy */ - Segmentation(const Segmentation& obs) - : origin_(obs.origin_) + Segmentation(const Segmentation & obs) + : origin_(obs.origin_) , cloud_(new sensor_msgs::msg::PointCloud2(*(obs.cloud_))) , class_map_(obs.class_map_) { @@ -98,16 +104,19 @@ class Segmentation /** * @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)) + Segmentation(const sensor_msgs::msg::PointCloud2 & cloud) + : cloud_(new sensor_msgs::msg::PointCloud2(cloud)) { } geometry_msgs::msg::Point origin_; - sensor_msgs::msg::PointCloud2* cloud_; - ///< @brief To store the correspondence of each class id with its name - std::map class_map_; + sensor_msgs::msg::PointCloud2 * cloud_; + std::unordered_map class_map_; // contains class_id->cost relation }; } // 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 index 07828a2227..fa3ebcf86e 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/segmentation_buffer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/segmentation_buffer.hpp @@ -32,7 +32,7 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * - * Author: Pedro Gonzalez + * Author: Eitan Marder-Eppstein *********************************************************************/ #ifndef NAV2_COSTMAP_2D__SEGMENTATION_BUFFER_HPP_ #define NAV2_COSTMAP_2D__SEGMENTATION_BUFFER_HPP_ @@ -44,21 +44,22 @@ #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/semantic_segmentation.hpp" +#include "vision_msgs/msg/label_info.hpp" -namespace nav2_costmap_2d { +namespace nav2_costmap_2d +{ /** * @class SegmentationBuffer - * @brief Takes in segmentation and alingned pointclouds, and stores transformed pointclouds with - * extra fields for the class and inference confidence of each point + * @brief Takes in point clouds from sensors, transforms them to the desired frame, and stores them */ class SegmentationBuffer { - public: +public: /** * @brief Constructs an segmentation buffer * @param topic_name The topic of the segmentations, used as an identifier for error and warning @@ -67,10 +68,16 @@ class SegmentationBuffer * keep the latest * @param expected_update_rate How often this buffer is expected to be updated, 0 means there is * no limit - * @param max_lookahead_distance The range to which the sensor should be trusted for projecting the - * segmentation mask - * @param min_lookahead_distance The range from which the sensor should be trusted for projecting the - * segmentation mask + * @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 @@ -78,11 +85,13 @@ class SegmentationBuffer * @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 topic_name, - 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); + 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 @@ -90,21 +99,30 @@ class SegmentationBuffer ~SegmentationBuffer(); /** - * @brief Transforms a PointCloud to the global frame, adds channels for class and confidence of - * each point and buffers it + * @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. It should be aligned with the segmentation - * @param segmentation The segmentation mask containing the class and confidence of each pixel + * @param cloud The cloud to be buffered */ - void bufferSegmentation(const sensor_msgs::msg::PointCloud2& cloud, - const vision_msgs::msg::SemanticSegmentation& segmentation); + 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); + 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 @@ -115,19 +133,31 @@ class SegmentationBuffer /** * @brief Lock the segmentation buffer */ - inline void lock() { lock_.lock(); } + inline void lock() {lock_.lock();} /** * @brief Lock the segmentation buffer */ - inline void unlock() { lock_.unlock(); } + inline void unlock() {lock_.unlock();} /** * @brief Reset last updated timestamp */ void resetLastUpdated(); - private: + /** + * @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 */ @@ -135,15 +165,18 @@ class SegmentationBuffer rclcpp::Clock::SharedPtr clock_; rclcpp::Logger logger_{rclcpp::get_logger("nav2_costmap_2d")}; - tf2_ros::Buffer& tf2_buffer_; + 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 topic_name_; - std::recursive_mutex lock_; ///< @brief A lock for accessing data in callbacks safely + 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_; 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 index ad97bbf5b2..cefe4799d8 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/semantic_segmentation_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/semantic_segmentation_layer.hpp @@ -50,10 +50,12 @@ #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/semantic_segmentation.hpp" +#include "vision_msgs/msg/label_info.hpp" -namespace nav2_costmap_2d { +namespace nav2_costmap_2d +{ /** * @class SemanticSegmentationLayer @@ -61,7 +63,7 @@ namespace nav2_costmap_2d { */ class SemanticSegmentationLayer : public CostmapLayer { - public: +public: /** * @brief A constructor */ @@ -86,8 +88,9 @@ class SemanticSegmentationLayer : public CostmapLayer * @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); + 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 @@ -96,8 +99,9 @@ class SemanticSegmentationLayer : public CostmapLayer * @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); + 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 @@ -109,53 +113,59 @@ class SemanticSegmentationLayer : public CostmapLayer /** * @brief If clearing operations should be processed on this layer or not */ - virtual bool isClearable() { return true; } + virtual bool isClearable() {return true;} - private: - /** - * @brief Callback called when a poincloud and a segmentation are synced and a tf is available - * @param segmentation a pointer segmentation mask containing the class and confidence of each pixel - * @param pointcloud A pointer to the aligned pointcloud - */ + 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); - - ///< @brief subscriber to segmentation messages - std::shared_ptr> - semantic_segmentation_sub_; - ///< @brief subscriber to aligned pointclouds - std::shared_ptr> pointcloud_sub_; - ///< @brief subscriber to guarantee that transforms are available when pointclouds are received - std::shared_ptr> pointcloud_tf_sub_; - ///< @brief subscriber to sync transforms, pointclouds and segmentations - std::shared_ptr> - segm_pc_sync_; + 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 - ///< @brief publisher to publish segmentation messages when buffered - std::shared_ptr> sgm_debug_pub_; - ///< @brief publisher to publish pointcloud messages when buffered - std::shared_ptr> orig_pointcloud_pub_; - ///< @brief publisher to publish pointclouds with class and confidence channels when put in the costmap - std::shared_ptr> proc_pointcloud_pub_; + std::map>> proc_pointcloud_pubs_map_; - /// @brief Used to store segmentations and aligned pointclouds - std::shared_ptr segmentation_buffer_; + std::vector> segmentation_buffers_; + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; - std::string global_frame_; ///< @brief The global frame for the costmap + std::string global_frame_; + std::string topics_string_; - ///< @brief Correspondence between class names and cost. i.e: "Grass": 254, "Floor": 0 std::map class_map_; bool rolling_window_; bool was_reset_; - bool debug_topics_; int combination_method_; }; } // namespace nav2_costmap_2d -#endif // SEMANTIC_SEGMENTATION_LAYER_HPP_ \ No newline at end of file +#endif // SEMANTIC_SEGMENTATION_LAYER_HPP_ diff --git a/nav2_costmap_2d/plugins/semantic_segmentation_layer.cpp b/nav2_costmap_2d/plugins/semantic_segmentation_layer.cpp index 1e74beaf26..d1bb6e31a9 100644 --- a/nav2_costmap_2d/plugins/semantic_segmentation_layer.cpp +++ b/nav2_costmap_2d/plugins/semantic_segmentation_layer.cpp @@ -33,7 +33,12 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. * - * Author: Pedro Gonzalez + * 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" @@ -45,201 +50,278 @@ using nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE; using nav2_costmap_2d::LETHAL_OBSTACLE; using nav2_costmap_2d::NO_INFORMATION; -namespace nav2_costmap_2d { +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) - { + if (!node) { throw std::runtime_error{"Failed to lock node"}; } - std::string segmentation_topic, pointcloud_topic, sensor_frame; + 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)); - declareParameter("max_obstacle_distance", rclcpp::ParameterValue(5.0)); - declareParameter("min_obstacle_distance", rclcpp::ParameterValue(0.3)); - declareParameter("segmentation_topic", rclcpp::ParameterValue("")); - declareParameter("pointcloud_topic", rclcpp::ParameterValue("")); - declareParameter("observation_persistence", rclcpp::ParameterValue(0.0)); - declareParameter("expected_update_rate", rclcpp::ParameterValue(0.0)); - declareParameter("class_types", rclcpp::ParameterValue(std::vector({}))); node->get_parameter(name_ + "." + "enabled", enabled_); node->get_parameter(name_ + "." + "combination_method", combination_method_); - node->get_parameter(name_ + "." + "publish_debug_topics", debug_topics_); - node->get_parameter(name_ + "." + "max_obstacle_distance", max_obstacle_distance); - node->get_parameter(name_ + "." + "min_obstacle_distance", min_obstacle_distance); - node->get_parameter(name_ + "." + "segmentation_topic", segmentation_topic); - node->get_parameter(name_ + "." + "pointcloud_topic", pointcloud_topic); - node->get_parameter(name_ + "." + "observation_persistence", observation_keep_time); - node->get_parameter(name_ + "." + "sensor_frame", sensor_frame); - node->get_parameter(name_ + "." + "expected_update_rate", expected_update_rate); + node->get_parameter("track_unknown_space", track_unknown_space); node->get_parameter("transform_tolerance", transform_tolerance); - node->get_parameter(name_ + "." + "class_types", class_types_string); - if (class_types_string.empty()) - { - RCLCPP_ERROR(logger_, "no class types defined. Segmentation plugin cannot work this way"); - exit(-1); + + global_frame_ = layered_costmap_->getGlobalFrameID(); + rolling_window_ = layered_costmap_->isRolling(); + + if (track_unknown_space) { + default_value_ = NO_INFORMATION; + } else { + default_value_ = FREE_SPACE; } - for (auto& source : class_types_string) - { - // get the cost for each class - std::vector classes_ids; - uint8_t cost; - declareParameter(source + ".classes", rclcpp::ParameterValue(std::vector({}))); - declareParameter(source + ".cost", rclcpp::ParameterValue(0)); - node->get_parameter(name_ + "." + source + ".classes", classes_ids); - if (classes_ids.empty()) - { - RCLCPP_ERROR(logger_, "no classes defined on type %s", source.c_str()); - continue; + 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); } - node->get_parameter(name_ + "." + source + ".cost", cost); - for (auto& class_id : classes_ids) - { - class_map_.insert(std::pair(class_id, cost)); + + 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. Segmentation plugin cannot work this way"); - exit(-1); - } + if (class_map.empty()) { + RCLCPP_ERROR( + logger_, + "No classes defined for source %s. Segmentation plugin cannot work this way", + source.c_str()); + exit(-1); + } - global_frame_ = layered_costmap_->getGlobalFrameID(); - rolling_window_ = layered_costmap_->isRolling(); + //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; - matchSize(); + // 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; + // }); - rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_sensor_data; - - semantic_segmentation_sub_ = - std::make_shared>( - rclcpp_node_, segmentation_topic, custom_qos_profile); - pointcloud_sub_ = std::make_shared>( - rclcpp_node_, pointcloud_topic, custom_qos_profile); - pointcloud_tf_sub_ = std::make_shared>( - *pointcloud_sub_, *tf_, global_frame_, 50, rclcpp_node_, - tf2::durationFromSec(transform_tolerance)); - // make sure all data is synced: pointclouds, transforms and segmentations - segm_pc_sync_ = - std::make_shared>( - *semantic_segmentation_sub_, *pointcloud_tf_sub_, 100); - segm_pc_sync_->registerCallback(std::bind(&SemanticSegmentationLayer::syncSegmPointcloudCb, this, - std::placeholders::_1, std::placeholders::_2)); - - // create a segmentation buffer - segmentation_buffer_ = std::make_shared( - node, pointcloud_topic, observation_keep_time, expected_update_rate, max_obstacle_distance, - min_obstacle_distance, *tf_, global_frame_, sensor_frame, - tf2::durationFromSec(transform_tolerance)); - - // only for testing purposes - if (debug_topics_) - { - sgm_debug_pub_ = - node->create_publisher("/buffered_segmentation", 1); - orig_pointcloud_pub_ = - node->create_publisher("/buffered_pointcloud", 1); - proc_pointcloud_pub_ = - node->create_publisher("/processed_pointcloud", 1); + 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)); +} -void SemanticSegmentationLayer::updateBounds(double robot_x, double robot_y, double /*robot_yaw*/, - double* min_x, double* min_y, double* max_x, - double* max_y) +// 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) { - if (rolling_window_) - { + std::lock_guard guard(*getMutex()); + if (rolling_window_) { updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2); } - if (!enabled_) - { - current_ = true; + if (!enabled_) { return; } std::vector segmentations; - // get the latest segmentations thread safely - segmentation_buffer_->lock(); - segmentation_buffer_->getSegmentations(segmentations); - segmentation_buffer_->unlock(); - for (auto& segmentation : segmentations) - { - if (debug_topics_) - { - proc_pointcloud_pub_->publish(*segmentation.cloud_); - } + 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"); - // For now, confidence of the inference is not used to adjust cost // sensor_msgs::PointCloud2ConstIterator iter_confidence(*segmentation.cloud_, // "confidence"); for (size_t point = 0; point < segmentation.cloud_->height * segmentation.cloud_->width; - point++) + point++) { unsigned int mx, my; - if (!worldToMap(*(iter_x + point), *(iter_y + point), 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); - // Check if no cost was defined for the given class - if (!class_map_.count(segmentation.class_map_[class_id])) - { + if (!segmentation.class_map_.count(class_id)) { RCLCPP_DEBUG(logger_, "Cost for class id %i was not defined, skipping", class_id); continue; } - // Get the class name from the buffered segmentation and then gets its cost from the class map - costmap_[index] = class_map_[segmentation.class_map_[class_id]]; + 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()); + RCLCPP_DEBUG( + rclcpp::get_logger("nav2_costmap_2d"), + "SemanticSegmentationLayer::onFootprintChanged(): num footprint points: %lu", + layered_costmap_->getFootprint().size()); } -void SemanticSegmentationLayer::updateCosts(nav2_costmap_2d::Costmap2D& master_grid, int min_i, - int min_j, int max_i, int max_j) +// 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) { - if (!enabled_) - { + std::lock_guard guard(*getMutex()); + if (!enabled_) { return; } - if (!current_ && was_reset_) - { + if (!current_ && was_reset_) { was_reset_ = false; current_ = true; } - if (!costmap_) - { + if (!costmap_) { return; } - switch (combination_method_) - { + // RCLCPP_INFO(logger_, "Updating costmap"); + switch (combination_method_) { case 0: // Overwrite updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j); break; @@ -251,45 +333,48 @@ void SemanticSegmentationLayer::updateCosts(nav2_costmap_2d::Costmap2D& master_g } } +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 & segmentation, + const std::shared_ptr & pointcloud, + const std::shared_ptr & buffer) { - // check if data has the right dimensions - 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); + 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 || - segmentation->confidence.size() < expected_array_size) - { - RCLCPP_WARN(logger_, - "segmentation arrays have wrong sizes: data->%lu, confidence->%lu, expected->%u. " - "Will not buffer message", - segmentation->data.size(), segmentation->confidence.size(), expected_array_size); + 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; } - // check if class names and ids correspondence is contained in the message - if (segmentation->class_map.size() == 0) - { - RCLCPP_WARN(logger_, "Classs map is empty. Will not buffer message"); + 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; } - // Store the pointcloud and segmentation - segmentation_buffer_->lock(); - segmentation_buffer_->bufferSegmentation(*pointcloud, *segmentation); - segmentation_buffer_->unlock(); - if (debug_topics_) - { - sgm_debug_pub_->publish(*segmentation); - orig_pointcloud_pub_->publish(*pointcloud); - } + 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() @@ -299,6 +384,98 @@ void SemanticSegmentationLayer::reset() 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) \ No newline at end of file +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 index 5f9d47384a..a3c8f7a6c7 100644 --- a/nav2_costmap_2d/src/segmentation_buffer.cpp +++ b/nav2_costmap_2d/src/segmentation_buffer.cpp @@ -46,19 +46,24 @@ #include "tf2/convert.h" using namespace std::chrono_literals; -namespace nav2_costmap_2d { -SegmentationBuffer::SegmentationBuffer(const nav2_util::LifecycleNode::WeakPtr& parent, - std::string topic_name, 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) +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) - , topic_name_(topic_name) + , 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) @@ -71,9 +76,17 @@ SegmentationBuffer::SegmentationBuffer(const nav2_util::LifecycleNode::WeakPtr& 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 vision_msgs::msg::SemanticSegmentation& segmentation) + const sensor_msgs::msg::PointCloud2 & cloud, + const sensor_msgs::msg::Image & segmentation, + const sensor_msgs::msg::Image & confidence) { geometry_msgs::msg::PointStamped global_origin; @@ -84,8 +97,7 @@ void SegmentationBuffer::bufferSegmentation( // or whether we should get it from the cloud std::string origin_frame = sensor_frame_ == "" ? cloud.header.frame_id : sensor_frame_; - try - { + try { // given these segmentations come from sensors... // we'll need to store the origin pt of the sensor geometry_msgs::msg::PointStamped local_origin; @@ -103,9 +115,9 @@ void SegmentationBuffer::bufferSegmentation( tf2_buffer_.transform(cloud, global_frame_cloud, global_frame_, tf_tolerance_); global_frame_cloud.header.stamp = cloud.header.stamp; - // create a segmented pointcloud to store the original 3D data as well as the class and confidence - // of each point - sensor_msgs::msg::PointCloud2& segmentation_cloud = *(segmentation_list_.front().cloud_); + // 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; @@ -117,15 +129,14 @@ void SegmentationBuffer::bufferSegmentation( unsigned int cloud_size = global_frame_cloud.height * global_frame_cloud.width; sensor_msgs::PointCloud2Modifier modifier(segmentation_cloud); - // add the class and confidence fields to the pointcloud. segmentation_cloud.point_step = - addPointField(segmentation_cloud, "class", 1, sensor_msgs::msg::PointField::INT8, - 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); - - // update the point step and get iterators + 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"); @@ -133,37 +144,28 @@ void SegmentationBuffer::bufferSegmentation( sensor_msgs::PointCloud2Iterator iter_y_obs(segmentation_cloud, "y"); sensor_msgs::PointCloud2Iterator iter_z_obs(segmentation_cloud, "z"); - // get iterators for the original cloud 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++) - { + 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))) - { + if (!std::isfinite(*(iter_z_global + pixel_idx))) { continue; } - // calculate the distance of the point to the sensor 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); - - // Remove points that are too far or too close - if (sq_dist >= sq_max_lookahead_distance_ || sq_dist <= sq_min_lookahead_distance_) - { + if (sq_dist >= sq_max_lookahead_distance_ || sq_dist <= sq_min_lookahead_distance_) { continue; } - // assign the values to the segmentation and the original pointcloud to each point *(iter_class_obs + point_count) = segmentation.data[pixel_idx]; - *(iter_confidence_obs + point_count) = segmentation.confidence[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); @@ -176,19 +178,14 @@ void SegmentationBuffer::bufferSegmentation( segmentation_cloud.header.stamp = cloud.header.stamp; segmentation_cloud.header.frame_id = global_frame_cloud.header.frame_id; - // create the class map from the classes contained on the segmentation message - std::map& segmentation_class_map = segmentation_list_.front().class_map_; - for (auto& semantic_class : segmentation.class_map) - { - segmentation_class_map[semantic_class.class_id] = semantic_class.class_name; - } - } catch (tf2::TransformException& ex) - { + 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()); + 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; } @@ -200,40 +197,40 @@ void SegmentationBuffer::bufferSegmentation( } // returns a copy of the segmentations -void SegmentationBuffer::getSegmentations(std::vector& 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) - { + 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()) - { + 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)) - { + 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; + 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_) - { + if ((clock_->now() - obs.cloud_->header.stamp) > observation_keep_time_) { segmentation_list_.erase(obs_it, segmentation_list_.end()); return; } @@ -241,24 +238,28 @@ void SegmentationBuffer::purgeStaleSegmentations() } } +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)) - { + 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.", - topic_name_.c_str(), (clock_->now() - last_updated_).seconds(), - expected_update_rate_.seconds()); + 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(); } +void SegmentationBuffer::resetLastUpdated() {last_updated_ = clock_->now();} } // namespace nav2_costmap_2d