├── README.md └── non_planar_obstacle_layer ├── CMakeLists.txt ├── README.md ├── include └── non_planar_obstacle_layer │ ├── ObservationSource.hpp │ └── non_planar_obstacle_layer.hpp ├── non_planar_obstacle_layer.xml ├── package.xml └── src ├── ObservationSource.cpp └── non_planar_obstacle_layer.cpp /README.md: -------------------------------------------------------------------------------- 1 | # nav2_plugins 2 | 3 | This repo contains plugins for Nav2 for navigating in non-planar environments. 4 | -------------------------------------------------------------------------------- /non_planar_obstacle_layer/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(non_planar_obstacle_layer) 3 | 4 | set(lib_name ${PROJECT_NAME}_core) 5 | 6 | # === Environment === 7 | 8 | # Default to C99 9 | if(NOT CMAKE_C_STANDARD) 10 | set(CMAKE_C_STANDARD 99) 11 | endif() 12 | 13 | # Default to C++14 14 | if(NOT CMAKE_CXX_STANDARD) 15 | set(CMAKE_CXX_STANDARD 14) 16 | endif() 17 | 18 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 19 | add_compile_options(-Wall -Wextra -Wpedantic) 20 | endif() 21 | 22 | # === Dependencies === 23 | 24 | find_package(ament_cmake REQUIRED) 25 | find_package(rclcpp REQUIRED) 26 | find_package(nav2_costmap_2d REQUIRED) 27 | find_package(pluginlib REQUIRED) 28 | find_package(sensor_msgs) 29 | find_package(tf2_geometry_msgs) 30 | find_package(geometry_msgs) 31 | find_package(tf2_ros) 32 | find_package(tf2) 33 | find_package(grid_map_msgs) 34 | find_package(grid_map_ros) 35 | find_package(pcl_conversions REQUIRED) 36 | find_package(pcl_ros REQUIRED) 37 | find_package(tf2_eigen REQUIRED) 38 | 39 | set(dep_pkgs 40 | rclcpp 41 | nav2_costmap_2d 42 | pluginlib 43 | sensor_msgs 44 | tf2_geometry_msgs 45 | geometry_msgs 46 | tf2_ros 47 | tf2 48 | grid_map_msgs 49 | grid_map_ros 50 | pcl_conversions 51 | pcl_ros 52 | tf2_eigen) 53 | 54 | # === Build === 55 | 56 | add_library(${lib_name} SHARED 57 | src/ObservationSource.cpp 58 | src/non_planar_obstacle_layer.cpp) 59 | include_directories(include) 60 | 61 | # === Installation === 62 | 63 | install(TARGETS ${lib_name} 64 | DESTINATION lib) 65 | 66 | # === Ament work === 67 | 68 | # pluginlib_export_plugin_description_file() installs non_planar_obstacle_layer.xml 69 | # file into "share" directory and sets ament indexes for it. 70 | # This allows the plugin to be discovered as a plugin of required type. 71 | pluginlib_export_plugin_description_file(nav2_costmap_2d non_planar_obstacle_layer.xml) 72 | ament_target_dependencies(${lib_name} ${dep_pkgs}) 73 | ament_package() 74 | -------------------------------------------------------------------------------- /non_planar_obstacle_layer/README.md: -------------------------------------------------------------------------------- 1 | # Non-Planar Obstacle Layer 2 | 3 | [![](https://img.youtube.com/vi/TiyWV8fcg3s/0.jpg)](https://www.youtube.com/watch?v=TiyWV8fcg3s&feature=youtu.be "Click to play on You Tube") 4 | 5 | This plugin uses gridmap and different perception sources to detect obstacles in a non-planar environment. 6 | 7 | Usage: 8 | 9 | ``` 10 | global_costmap: 11 | global_costmap: 12 | ros__parameters: 13 | update_frequency: 1.0 14 | publish_frequency: 1.0 15 | global_frame: map 16 | robot_base_frame: base_link 17 | use_sim_time: True 18 | robot_radius: 0.22 19 | resolution: 0.05 20 | track_unknown_space: true 21 | plugins: ["static_layer", "obstacle_layer", "inflation_layer"] 22 | obstacle_layer: 23 | plugin: "non_planar_obstacle_layer/NonPlanarObstacleLayer" 24 | enabled: True 25 | gridmap_topic: /grid_map_map 26 | min_z: -20.0 27 | max_z: 20.0 28 | min_obstacle_height: 0.4 29 | max_obstacle_height: 2.0 30 | observation_sources: ["front_scan"] 31 | front_scan: 32 | topic: "/front_laser/scan" 33 | type: "LaserScan" 34 | static_layer: 35 | plugin: "nav2_costmap_2d::StaticLayer" 36 | map_subscribe_transient_local: True 37 | inflation_layer: 38 | plugin: "nav2_costmap_2d::InflationLayer" 39 | cost_scaling_factor: 3.0 40 | inflation_radius: 0.55 41 | always_send_full_costmap: True 42 | ``` 43 | 44 | ## Parameters 45 | 46 | * `enabled` (boolean, true): If the plugin is enabled 47 | * `gridmap_topic` (string, "grid_map_map"): The topic of the gridmap. 48 | * `min_z` (double, -inf): The minimum Z of the map elements. 49 | * `max_z` (double, -inf): The maximum Z of the map elements. 50 | * `min_obstacle_height` (double, 0.2): The minimum height above the surface of an obstacle. 51 | * `max_obstacle_height` (double, 2.0): The maximum height above the surface of an obstacle. 52 | * `bounds_size_x` (double, 40): This is the X size of the area to detect obstacles. 53 | * `bounds_size_y` (double, 40): This is the Y size of the area to detect obstacles. 54 | * `observation_sources` (string, ""): The perception sources to detect obstacles. 55 | * `type` (string, ""): The perception source type. Currently in [`LaserScan`, `PointCloud2`] 56 | * `topic` (string, "/perception"): The perception source topic. 57 | -------------------------------------------------------------------------------- /non_planar_obstacle_layer/include/non_planar_obstacle_layer/ObservationSource.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Intelligent Robotics Lab 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | 16 | #ifndef NON_PLANAR_OBSTACLE_LAYER__OBSERVATION_SOURCE_HPP_ 17 | #define NON_PLANAR_OBSTACLE_LAYER__OBSERVATION_SOURCE_HPP_ 18 | 19 | #include 20 | 21 | #include "tf2_ros/buffer.h" 22 | #include "tf2_ros/transform_listener.h" 23 | #include "tf2_ros/transform_broadcaster.h" 24 | #include "tf2/LinearMath/Transform.h" 25 | #include "tf2/transform_datatypes.h" 26 | #include "tf2_eigen/tf2_eigen.hpp" 27 | 28 | #include 29 | #include 30 | 31 | #include "sensor_msgs/msg/point_cloud2.hpp" 32 | #include "sensor_msgs/msg/laser_scan.hpp" 33 | #include "geometry_msgs/msg/transform_stamped.hpp" 34 | 35 | #include "rclcpp_lifecycle/lifecycle_node.hpp" 36 | #include "rclcpp/rclcpp.hpp" 37 | 38 | namespace non_planar_obstacle_layer 39 | { 40 | 41 | using std::placeholders::_1; 42 | using namespace std::chrono_literals; 43 | 44 | class ObservationSourceBase { 45 | public: 46 | virtual ~ObservationSourceBase() {}; 47 | 48 | virtual void clear_perception() = 0; 49 | virtual void insert_perception_in_pc( 50 | double min_x, double max_x, double min_y, double max_y, double min_z, double max_z, 51 | pcl::PointCloud::Ptr pc_out, double resolution) = 0; 52 | }; 53 | 54 | template 55 | class ObservationSource : public ObservationSourceBase 56 | { 57 | public: 58 | ObservationSource( 59 | const std::string & plugin_name, const std::string & name, rclcpp_lifecycle::LifecycleNode::SharedPtr node) 60 | : name_(name) 61 | { 62 | tf_buffer_ = 63 | std::make_unique(node->get_clock()); 64 | tf_listener_ = 65 | std::make_shared(*tf_buffer_); 66 | 67 | std::string topic; 68 | if (!node->has_parameter(plugin_name + "." + name + ".topic")) { 69 | node->declare_parameter(plugin_name + "." + name + ".topic", "/perception"); 70 | } 71 | node->get_parameter(plugin_name + "." + name + ".topic", topic); 72 | 73 | RCLCPP_INFO(node->get_logger(), "\t\tTopic = [%s]", topic.c_str()); 74 | 75 | percept_sub_ = node->create_subscription( 76 | topic, rclcpp::SensorDataQoS(), 77 | std::bind(&ObservationSource::perception_callback, this, _1)); 78 | } 79 | 80 | void set_last_perception(T & last_perception) { 81 | if (last_perception_ == nullptr) { 82 | last_perception_ = std::make_unique(); 83 | } 84 | *last_perception_ = last_perception; 85 | } 86 | 87 | virtual void clear_perception() { 88 | last_perception_ = nullptr; 89 | } 90 | 91 | bool get_transformation( 92 | const std::string & target_frame, std::string source_frame, 93 | const rclcpp::Time & time, Eigen::Affine3d & transform) 94 | { 95 | try { 96 | geometry_msgs::msg::TransformStamped sensor2map; 97 | sensor2map = tf_buffer_->lookupTransform( 98 | target_frame, source_frame, tf2_ros::fromRclcpp(time), 99 | tf2::Duration(200ms)); 100 | 101 | transform = tf2::transformToEigen(sensor2map); 102 | 103 | return true; 104 | } catch (tf2::TransformException& ex) { 105 | return false; 106 | } 107 | } 108 | 109 | public: 110 | typename T::UniquePtr last_perception_; 111 | 112 | protected: 113 | const std::string name_; 114 | typename rclcpp::Subscription::SharedPtr percept_sub_; 115 | 116 | std::shared_ptr tf_buffer_; 117 | std::shared_ptr tf_listener_; 118 | 119 | void perception_callback(typename T::UniquePtr msg) { 120 | last_perception_ = std::move(msg); 121 | } 122 | }; 123 | 124 | 125 | class LaserObservationSource : public ObservationSource 126 | { 127 | public: 128 | LaserObservationSource( 129 | const std::string & plugin_name, const std::string & name, rclcpp_lifecycle::LifecycleNode::SharedPtr node); 130 | 131 | void insert_perception_in_pc( 132 | double min_x, double max_x, double min_y, double max_y, double min_z, double max_z, 133 | pcl::PointCloud::Ptr pc_out, double resolution); 134 | }; 135 | 136 | class PointCloudObservationSource : public ObservationSource 137 | { 138 | public: 139 | PointCloudObservationSource( 140 | const std::string & plugin_name, const std::string & name, rclcpp_lifecycle::LifecycleNode::SharedPtr node); 141 | 142 | void insert_perception_in_pc( 143 | double min_x, double max_x, double min_y, double max_y, double min_z, double max_z, 144 | pcl::PointCloud::Ptr pc_out, double resolution); 145 | }; 146 | 147 | } // namespace non_planar_obstacle_layer 148 | 149 | #endif // NON_PLANAR_OBSTACLE_LAYER__OBSERVATION_SOURCE_HPP_ 150 | -------------------------------------------------------------------------------- /non_planar_obstacle_layer/include/non_planar_obstacle_layer/non_planar_obstacle_layer.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Intelligent Robotics Lab 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #ifndef NON_PLANAR_OBSTACLE_LAYER__NON_PLANAR_OBSTACLE_LAYER_HPP_ 16 | #define NON_PLANAR_OBSTACLE_LAYER__NON_PLANAR_OBSTACLE_LAYER_HPP_ 17 | 18 | 19 | #include "nav2_costmap_2d/layer.hpp" 20 | #include "nav2_costmap_2d/layered_costmap.hpp" 21 | 22 | #include "grid_map_msgs/msg/grid_map.hpp" 23 | #include "grid_map_ros/grid_map_ros.hpp" 24 | 25 | #include "non_planar_obstacle_layer/ObservationSource.hpp" 26 | 27 | #include "rclcpp/rclcpp.hpp" 28 | 29 | namespace non_planar_obstacle_layer 30 | { 31 | 32 | class NonPlanarObstacleLayer : public nav2_costmap_2d::Layer 33 | { 34 | public: 35 | NonPlanarObstacleLayer(); 36 | 37 | virtual void onInitialize(); 38 | virtual void updateBounds( 39 | double robot_x, double robot_y, double robot_yaw, double * min_x, 40 | double * min_y, 41 | double * max_x, 42 | double * max_y); 43 | virtual void updateCosts( 44 | nav2_costmap_2d::Costmap2D & master_grid, 45 | int min_i, int min_j, int max_i, int max_j); 46 | 47 | virtual void reset() 48 | { 49 | return; 50 | } 51 | 52 | virtual void onFootprintChanged(); 53 | virtual bool isClearable() {return false;} 54 | 55 | private: 56 | double min_z_, max_z_; 57 | 58 | double last_min_x_, last_min_y_, last_max_x_, last_max_y_; 59 | bool need_recalculation_; 60 | 61 | double min_obstacle_height_, max_obstacle_height_; 62 | double bounds_size_x_, bounds_size_y_; 63 | 64 | void gridmap_callback(const grid_map_msgs::msg::GridMap::ConstSharedPtr & msg); 65 | 66 | std::vector observation_sources_; 67 | 68 | rclcpp::Subscription::SharedPtr sub_gridmap_; 69 | std::shared_ptr gridmap_; 70 | }; 71 | 72 | } // namespace non_planar_obstacle_layer 73 | 74 | #endif // NON_PLANAR_OBSTACLE_LAYER__NON_PLANAR_OBSTACLE_LAYER_HPP_ 75 | -------------------------------------------------------------------------------- /non_planar_obstacle_layer/non_planar_obstacle_layer.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | This plugin provides a layer that incorporate obstacles to a costmap, taking into account that the environment is not plane 4 | 5 | 6 | -------------------------------------------------------------------------------- /non_planar_obstacle_layer/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | non_planar_obstacle_layer 5 | 1.0.0 6 | Run-time plugin for non-planar obstacle layer 7 | Francisco Martín Rico 8 | Apache-2.0 9 | 10 | ament_cmake 11 | 12 | ament_lint_auto 13 | ament_lint_common 14 | 15 | nav2_costmap_2d 16 | pluginlib 17 | rclcpp 18 | sensor_msgs 19 | tf2_geometry_msgs 20 | geometry_msgs 21 | tf2_ros 22 | tf2 23 | grid_map_msgs 24 | grid_map_ros 25 | pcl_conversions 26 | pcl_ros 27 | tf2_eigen 28 | 29 | 30 | 31 | ament_cmake 32 | 33 | 34 | -------------------------------------------------------------------------------- /non_planar_obstacle_layer/src/ObservationSource.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Intelligent Robotics Lab 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include "sensor_msgs/msg/point_cloud2.hpp" 25 | #include "sensor_msgs/msg/laser_scan.hpp" 26 | 27 | #include "tf2_ros/buffer_interface.h" 28 | 29 | #include "rclcpp_lifecycle/lifecycle_node.hpp" 30 | #include "rclcpp/rclcpp.hpp" 31 | 32 | #include "non_planar_obstacle_layer/ObservationSource.hpp" 33 | 34 | namespace non_planar_obstacle_layer 35 | { 36 | 37 | LaserObservationSource::LaserObservationSource( 38 | const std::string & plugin_name, const std::string & name, rclcpp_lifecycle::LifecycleNode::SharedPtr node) 39 | : ObservationSource(plugin_name, name, node) 40 | { 41 | } 42 | 43 | void 44 | LaserObservationSource::insert_perception_in_pc( 45 | double min_x, double max_x, double min_y, double max_y, double min_z, double max_z, 46 | pcl::PointCloud::Ptr pc_out, double resolution) 47 | { 48 | if (last_perception_ == nullptr) { 49 | return; 50 | } 51 | 52 | Eigen::Affine3d sensor2map; 53 | if (get_transformation("map", last_perception_->header.frame_id, 54 | rclcpp::Time(last_perception_->header.stamp), 55 | sensor2map)) 56 | { 57 | pcl::PointCloud::Ptr input_pc(new pcl::PointCloud); 58 | for (size_t i = 0; i < last_perception_->ranges.size(); i++) { 59 | if (std::isnan(last_perception_->ranges[i])) {continue;} 60 | 61 | double angle = last_perception_->angle_min + 62 | static_cast(i) * last_perception_->angle_increment; 63 | 64 | pcl::PointXYZ point; 65 | point.x = last_perception_->ranges[i] * cos(angle); 66 | point.y = last_perception_->ranges[i] * sin(angle); 67 | point.z = 0.0; 68 | 69 | input_pc->push_back(point); 70 | } 71 | 72 | // Reduce density 73 | pcl::PointCloud::Ptr reduced_pc(new pcl::PointCloud); 74 | pcl::VoxelGrid voxel_grid; 75 | voxel_grid.setInputCloud(input_pc); 76 | voxel_grid.setLeafSize(resolution, resolution, 10.0f); 77 | voxel_grid.filter(*reduced_pc); 78 | 79 | // Crop pc in base on coordinates 80 | Eigen::Vector4f minPoint(min_x, min_y, min_z, 1.0); // Minimum point coordinates (x, y, z, 1) 81 | Eigen::Vector4f maxPoint(max_x, max_y, max_z, 1.0); // Maximum point coordinates (x, y, z, 1) 82 | pcl::CropBox cropBoxFilter; 83 | cropBoxFilter.setInputCloud(reduced_pc); 84 | cropBoxFilter.setMin(minPoint); 85 | cropBoxFilter.setMax(maxPoint); 86 | cropBoxFilter.filter(*reduced_pc); 87 | 88 | pcl::PointCloud::Ptr temp_pc(new pcl::PointCloud); 89 | pcl::transformPointCloud(*reduced_pc, *temp_pc, sensor2map); 90 | 91 | pc_out->insert(pc_out->end(), temp_pc->begin(), temp_pc->end()); 92 | } 93 | } 94 | 95 | PointCloudObservationSource::PointCloudObservationSource( 96 | const std::string & plugin_name, const std::string & name, rclcpp_lifecycle::LifecycleNode::SharedPtr node) 97 | : ObservationSource(plugin_name, name, node) 98 | { 99 | } 100 | 101 | void 102 | PointCloudObservationSource::insert_perception_in_pc( 103 | double min_x, double max_x, double min_y, double max_y, double min_z, double max_z, 104 | pcl::PointCloud::Ptr pc_out, double resolution) 105 | { 106 | if (last_perception_ == nullptr) { 107 | return; 108 | } 109 | 110 | Eigen::Affine3d sensor2map; 111 | if (get_transformation("map", last_perception_->header.frame_id, 112 | rclcpp::Time(last_perception_->header.stamp), 113 | sensor2map)) 114 | { 115 | // Reduce density 116 | pcl::PointCloud::Ptr reduced_pc(new pcl::PointCloud); 117 | pcl::VoxelGrid voxel_grid; 118 | voxel_grid.setInputCloud(reduced_pc); 119 | voxel_grid.setLeafSize(resolution, resolution, 10.0f); 120 | voxel_grid.filter(*reduced_pc); 121 | 122 | // Crop pc in base on coordinates 123 | Eigen::Vector4f minPoint(min_x, min_y, min_z, 1.0); // Minimum point coordinates (x, y, z, 1) 124 | Eigen::Vector4f maxPoint(max_x, max_y, max_z, 1.0); // Maximum point coordinates (x, y, z, 1) 125 | pcl::CropBox cropBoxFilter; 126 | cropBoxFilter.setInputCloud(reduced_pc); 127 | cropBoxFilter.setMin(minPoint); 128 | cropBoxFilter.setMax(maxPoint); 129 | cropBoxFilter.filter(*reduced_pc); 130 | 131 | pcl::PointCloud::Ptr temp_pc(new pcl::PointCloud); 132 | pcl::transformPointCloud(*reduced_pc, *temp_pc, sensor2map); 133 | 134 | pc_out->insert(pc_out->end(), temp_pc->begin(), temp_pc->end()); 135 | } 136 | } 137 | 138 | } // namespace non_planar_obstacle_layer 139 | 140 | -------------------------------------------------------------------------------- /non_planar_obstacle_layer/src/non_planar_obstacle_layer.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Intelligent Robotics Lab 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include "non_planar_obstacle_layer/non_planar_obstacle_layer.hpp" 16 | 17 | #include 18 | #include 19 | 20 | #include "grid_map_msgs/msg/grid_map.hpp" 21 | #include "grid_map_ros/grid_map_ros.hpp" 22 | 23 | #include "nav2_costmap_2d/costmap_math.hpp" 24 | #include "nav2_costmap_2d/footprint.hpp" 25 | #include "rclcpp/parameter_events_filter.hpp" 26 | 27 | using std::placeholders::_1; 28 | 29 | using nav2_costmap_2d::LETHAL_OBSTACLE; 30 | using nav2_costmap_2d::INSCRIBED_INFLATED_OBSTACLE; 31 | using nav2_costmap_2d::NO_INFORMATION; 32 | 33 | namespace non_planar_obstacle_layer 34 | { 35 | 36 | NonPlanarObstacleLayer::NonPlanarObstacleLayer() 37 | : last_min_x_(-std::numeric_limits::max()), 38 | last_min_y_(-std::numeric_limits::max()), 39 | last_max_x_(std::numeric_limits::max()), 40 | last_max_y_(std::numeric_limits::max()), 41 | min_z_(-std::numeric_limits::max()), 42 | max_z_(std::numeric_limits::max()), 43 | min_obstacle_height_(0.2), 44 | max_obstacle_height_(2.0), 45 | bounds_size_x_(40.0), 46 | bounds_size_y_(40.0) 47 | { 48 | gridmap_ = std::make_shared(); 49 | 50 | current_ = true; 51 | } 52 | 53 | void 54 | NonPlanarObstacleLayer::onInitialize() 55 | { 56 | auto node = node_.lock(); 57 | 58 | RCLCPP_INFO(node->get_logger(), "Initializing NonPlanarObstacleLayer"); 59 | 60 | declareParameter("enabled", rclcpp::ParameterValue(true)); 61 | declareParameter("min_z", rclcpp::ParameterValue(min_z_)); 62 | declareParameter("max_z", rclcpp::ParameterValue(max_z_)); 63 | declareParameter("min_obstacle_height", rclcpp::ParameterValue(min_obstacle_height_)); 64 | declareParameter("max_obstacle_height", rclcpp::ParameterValue(max_obstacle_height_)); 65 | declareParameter("bounds_size_x", rclcpp::ParameterValue(bounds_size_x_)); 66 | declareParameter("bounds_size_y", rclcpp::ParameterValue(bounds_size_y_)); 67 | 68 | std::vector observation_sources; 69 | declareParameter("observation_sources", rclcpp::ParameterValue(observation_sources)); 70 | 71 | node->get_parameter(name_ + "." + "enabled", enabled_); 72 | node->get_parameter(name_ + "." + "min_z", min_z_); 73 | node->get_parameter(name_ + "." + "max_z", max_z_); 74 | node->get_parameter(name_ + "." + "min_obstacle_height", min_obstacle_height_); 75 | node->get_parameter(name_ + "." + "max_obstacle_height", max_obstacle_height_); 76 | node->get_parameter(name_ + "." + "bounds_size_x", bounds_size_x_); 77 | node->get_parameter(name_ + "." + "bounds_size_y", bounds_size_y_); 78 | 79 | RCLCPP_INFO(node->get_logger(), "\tZ = [%lf - %lf]", min_z_, min_z_); 80 | RCLCPP_INFO(node->get_logger(), "\tHeight = [%lf - %lf]", min_obstacle_height_, max_obstacle_height_); 81 | RCLCPP_INFO(node->get_logger(), "\tBounds = [%lf - %lf]", bounds_size_x_, bounds_size_y_); 82 | 83 | std::string gridmap_topic("grid_map_map"); 84 | declareParameter("gridmap_topic", rclcpp::ParameterValue(gridmap_topic)); 85 | node->get_parameter(name_ + "." + "gridmap_topic", gridmap_topic); 86 | 87 | RCLCPP_INFO(node->get_logger(), "\tGridmap Topic = [%s]", gridmap_topic.c_str()); 88 | 89 | sub_gridmap_ = node->create_subscription( 90 | gridmap_topic, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), 91 | std::bind(&NonPlanarObstacleLayer::gridmap_callback, this, _1)); 92 | 93 | declareParameter("observation_sources", rclcpp::ParameterValue(observation_sources)); 94 | node->get_parameter(name_ + "." + "observation_sources", observation_sources); 95 | 96 | for (const auto & observation_source : observation_sources) { 97 | std::string observation_source_type; 98 | declareParameter(observation_source + ".type", rclcpp::ParameterValue(observation_source_type)); 99 | node->get_parameter(name_ + "." + observation_source + ".type", observation_source_type); 100 | 101 | RCLCPP_INFO(node->get_logger(), "\tObservation Source = [%s]", observation_source.c_str()); 102 | RCLCPP_INFO(node->get_logger(), "\tObservation Type = [%s]", observation_source_type.c_str()); 103 | 104 | ObservationSourceBase * new_observation_source; 105 | if (observation_source_type == "LaserScan") { 106 | RCLCPP_INFO(node->get_logger(), "\t\tLaser"); 107 | new_observation_source = new LaserObservationSource(name_, observation_source, node); 108 | observation_sources_.push_back(new_observation_source); 109 | } else if (observation_source_type == "PointCloud2") { 110 | RCLCPP_INFO(node->get_logger(), "\t\tPointCloud2"); 111 | new_observation_source = new PointCloudObservationSource(name_, observation_source, node); 112 | observation_sources_.push_back(new_observation_source); 113 | } else { 114 | RCLCPP_WARN(node->get_logger(), "\tObservation source Type [%s] not valid", observation_source_type.c_str()); 115 | } 116 | 117 | RCLCPP_INFO( 118 | node->get_logger(), "Created observation source [%s] (type [%s])", 119 | observation_source.c_str(), observation_source_type.c_str()); 120 | } 121 | current_ = true; 122 | need_recalculation_ = false; 123 | } 124 | 125 | void 126 | NonPlanarObstacleLayer::gridmap_callback(const grid_map_msgs::msg::GridMap::ConstSharedPtr & msg) 127 | { 128 | grid_map::GridMapRosConverter::fromMessage(*msg, *gridmap_); 129 | } 130 | 131 | 132 | void 133 | NonPlanarObstacleLayer::onFootprintChanged() 134 | { 135 | need_recalculation_ = true; 136 | 137 | RCLCPP_DEBUG(rclcpp::get_logger( 138 | "nav2_costmap_2d"), "NonPlanarObstacleLayer::onFootprintChanged(): num footprint points: %lu", 139 | layered_costmap_->getFootprint().size()); 140 | } 141 | 142 | void 143 | NonPlanarObstacleLayer::updateBounds( 144 | double robot_x, double robot_y, double /*robot_yaw*/, double * min_x, 145 | double * min_y, double * max_x, double * max_y) 146 | { 147 | if (need_recalculation_) { 148 | last_min_x_ = *min_x; 149 | last_min_y_ = *min_y; 150 | last_max_x_ = *max_x; 151 | last_max_y_ = *max_y; 152 | // For some reason when I make these -::max() it does not 153 | // work with Costmap2D::worldToMapEnforceBounds(), so I'm using 154 | // -::max() instead. 155 | *min_x = -std::numeric_limits::max(); 156 | *min_y = -std::numeric_limits::max(); 157 | *max_x = std::numeric_limits::max(); 158 | *max_y = std::numeric_limits::max(); 159 | need_recalculation_ = false; 160 | } else { 161 | double tmp_min_x = last_min_x_; 162 | double tmp_min_y = last_min_y_; 163 | double tmp_max_x = last_max_x_; 164 | double tmp_max_y = last_max_y_; 165 | last_min_x_ = *min_x; 166 | last_min_y_ = *min_y; 167 | last_max_x_ = *max_x; 168 | last_max_y_ = *max_y; 169 | *min_x = std::min(tmp_min_x, *min_x); 170 | *min_y = std::min(tmp_min_y, *min_y); 171 | *max_x = std::max(tmp_max_x, *max_x); 172 | *max_y = std::max(tmp_max_y, *max_y); 173 | } 174 | } 175 | 176 | void 177 | NonPlanarObstacleLayer::updateCosts( 178 | nav2_costmap_2d::Costmap2D & master_grid, 179 | int min_i, int min_j, 180 | int max_i, int max_j) 181 | { 182 | if (!enabled_) { 183 | return; 184 | } 185 | 186 | unsigned char * master_array = master_grid.getCharMap(); 187 | double resolution = master_grid.getResolution(); 188 | 189 | double min_x, max_x, min_y, max_y; 190 | master_grid.mapToWorld(min_i, min_j, min_x, min_y); 191 | master_grid.mapToWorld(max_i, max_j, max_x, max_y); 192 | 193 | pcl::PointCloud::Ptr obstacle_cloud(new pcl::PointCloud); 194 | 195 | for (const auto observation_source : observation_sources_) { 196 | observation_source->insert_perception_in_pc( 197 | min_x, max_x, min_y, max_y, min_z_, max_z_, obstacle_cloud, resolution); 198 | } 199 | 200 | auto & gridpmap_pos = gridmap_->getPosition(); 201 | 202 | for (const auto & point : obstacle_cloud->points) { 203 | const float & px = point.x; 204 | const float & py = point.y; 205 | const float & pz = point.z; 206 | 207 | grid_map::Position particle_pos(px, py); 208 | float elevation = gridmap_->atPosition("elevation", particle_pos + gridpmap_pos); 209 | 210 | if (pz > (elevation + min_obstacle_height_) && pz < (elevation + max_obstacle_height_)) { 211 | unsigned int mx, my; 212 | if (!master_grid.worldToMap(px, py, mx, my)) { 213 | RCLCPP_INFO(logger_, "Computing map coords failed"); 214 | continue; 215 | } 216 | 217 | unsigned int index = master_grid.getIndex(mx, my); 218 | master_array[index] = LETHAL_OBSTACLE; 219 | } 220 | } 221 | } 222 | 223 | } // namespace non_planar_obstacle_layer 224 | 225 | #include "pluginlib/class_list_macros.hpp" 226 | PLUGINLIB_EXPORT_CLASS(non_planar_obstacle_layer::NonPlanarObstacleLayer, nav2_costmap_2d::Layer) 227 | --------------------------------------------------------------------------------