diff --git a/nav2_bringup/bringup/params/nav2_params.yaml b/nav2_bringup/bringup/params/nav2_params.yaml index 1e08489296a..f232afedcc8 100644 --- a/nav2_bringup/bringup/params/nav2_params.yaml +++ b/nav2_bringup/bringup/params/nav2_params.yaml @@ -254,6 +254,8 @@ planner_server: GridBased: tolerance: 0.5 # tolerance for planning if unable to reach exact pose, in meters + downsample_costmap: false # whether or not to downsample the map to the specified resolution + downsampling_factor: 3 # multiplier for the resolution of the costmap layer allow_unknown: false # allow traveling in unknown space travel_cost_scale: 0.8 # [0,1] metric for how much you value staying out of higher-cost spaces (higher more adverse) max_iterations: -1 # maximum total iterations to search for before returning @@ -270,8 +272,8 @@ planner_server: w_curve: 30.0 #3.0 # range 0 - 50 w_dist: 0.0 #300 w/0 cost w_smooth: 15000.0 #15000 - w_cost: 0.025 - max_curve: 2.0 # translates to |delta angle| / |delta distance|, where distance is ~2x costmap resolution + w_cost: 0.025 # Use higher value when using the costmap downsampler (e.g. use 2.5 for downsampling_factor = 3) + max_curve: 2.0 # translates to |delta angle| / |delta distance|, where distance is ~2x costmap resolution cost_scaling_factor: 1.0 # this should match the inflation layer's parameter # 7.8 in 5cm resolution (45 deg / 10cm) # 4.0 in 10cm resolution (45 deg / 20 cm) diff --git a/nav2_bringup/bringup/rviz/nav2_default_view.rviz b/nav2_bringup/bringup/rviz/nav2_default_view.rviz index 04e989eb170..c07e3e0cba6 100644 --- a/nav2_bringup/bringup/rviz/nav2_default_view.rviz +++ b/nav2_bringup/bringup/rviz/nav2_default_view.rviz @@ -249,6 +249,20 @@ Visualization Manager: Value: /global_costmap/costmap Use Timestamp: false Value: true + - Alpha: 0.3 + Class: rviz_default_plugins/Map + Color Scheme: costmap + Draw Behind: false + Enabled: true + Name: Downsampled Costmap + Topic: + Depth: 1 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /downsampled_costmap + Use Timestamp: false + Value: true - Alpha: 1 Buffer Length: 1 Class: rviz_default_plugins/Path diff --git a/smac_planner/include/smac_planner/costmap_downsampler.hpp b/smac_planner/include/smac_planner/costmap_downsampler.hpp new file mode 100644 index 00000000000..2940192bf70 --- /dev/null +++ b/smac_planner/include/smac_planner/costmap_downsampler.hpp @@ -0,0 +1,194 @@ +// Copyright (c) 2020, Carlos Luis +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. Reserved. + +#ifndef SMAC_PLANNER__DOWNSAMPLER_HPP_ +#define SMAC_PLANNER__DOWNSAMPLER_HPP_ + +#include +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "smac_planner/constants.hpp" + +namespace smac_planner +{ + +/** + * @class smac_planner::CostmapDownsampler + * @brief A costmap downsampler for more efficient path planning + */ +class CostmapDownsampler +{ +public: + /** + * @brief A constructor for CostmapDownsampler + * @param node Lifecycle node pointer + */ + explicit CostmapDownsampler(const nav2_util::LifecycleNode::SharedPtr & node) + : _costmap(nullptr), + _downsampled_costmap(nullptr), + _downsampled_costmap_pub(nullptr), + _node(node) + { + } + + /** + * @brief A destructor for CostmapDownsampler + */ + ~CostmapDownsampler() + { + } + + /** + * @brief Initialize the downsampled costmap object and the ROS publisher + * @param global_frame The ID of the global frame used by the costmap + * @param topic_name The name of the topic to publish the downsampled costmap + * @param costmap The costmap we want to downsample + * @param downsampling_factor Multiplier for the costmap resolution + */ + void initialize( + const std::string & global_frame, + const std::string & topic_name, + nav2_costmap_2d::Costmap2D * const costmap, + const unsigned int & downsampling_factor) + { + _topic_name = topic_name; + _costmap = costmap; + _downsampling_factor = downsampling_factor; + updateCostmapSize(); + + _downsampled_costmap = std::make_unique + (_downsampled_size_x, _downsampled_size_y, _downsampled_resolution, + _costmap->getOriginX(), _costmap->getOriginY(), UNKNOWN); + + _downsampled_costmap_pub = std::make_unique( + _node, _downsampled_costmap.get(), global_frame, _topic_name, false); + } + + /** + * @brief Activate the publisher of the downsampled costmap + */ + void activatePublisher() { + _downsampled_costmap_pub->on_activate(); + } + + /** + * @brief Deactivate the publisher of the downsampled costmap + */ + void deactivatePublisher() { + _downsampled_costmap_pub->on_deactivate(); + } + + /** + * @brief Downsample the given costmap by the downsampling factor, and publish the downsampled costmap + * @param downsampling_factor Multiplier for the costmap resolution + * @return A ptr to the downsampled costmap + */ + nav2_costmap_2d::Costmap2D * downsample(const unsigned int & downsampling_factor) + { + _downsampling_factor = downsampling_factor; + updateCostmapSize(); + + // Adjust costmap size if needed + if (_downsampled_costmap->getSizeInCellsX() != _downsampled_size_x || + _downsampled_costmap->getSizeInCellsY() != _downsampled_size_y || + _downsampled_costmap->getResolution() != _downsampled_resolution) { + resizeCostmap(); + } + + // Assign costs + for (int i = 0; i < _downsampled_size_x; ++i) { + for (int j = 0; j < _downsampled_size_y; ++j) { + setCostOfCell(i, j); + } + } + + if (_node->count_subscribers(_topic_name) > 0) { + _downsampled_costmap_pub->publishCostmap(); + } + + return _downsampled_costmap.get(); + } + +private: + /** + * Update the sizes X-Y of the costmap and its downsampled version + */ + void updateCostmapSize() + { + _size_x = _costmap->getSizeInCellsX(); + _size_y = _costmap->getSizeInCellsY(); + _downsampled_size_x = ceil(static_cast(_size_x) / _downsampling_factor); + _downsampled_size_y = ceil(static_cast(_size_y) / _downsampling_factor); + _downsampled_resolution = _downsampling_factor * _costmap->getResolution(); + } + + /** + * Resize the downsampled costmap. Used in case the costmap changes and we need to update the downsampled version + */ + void resizeCostmap() + { + _downsampled_costmap->resizeMap( + _downsampled_size_x, + _downsampled_size_y, + _downsampled_resolution, + _costmap->getOriginX(), + _costmap->getOriginY()); + } + + /** + * @brief Explore all subcells of the original costmap and assign the max cost to the new (downsampled) cell + * @param new_mx The X-coordinate of the cell in the new costmap + * @param new_my The Y-coordinate of the cell in the new costmap + */ + void setCostOfCell( + const unsigned int & new_mx, + const unsigned int & new_my) + { + unsigned int mx, my; + unsigned char cost = 0; + unsigned int x_offset = new_mx * _downsampling_factor; + unsigned int y_offset = new_my * _downsampling_factor; + + for (int i = 0; i < _downsampling_factor; ++i) { + mx = x_offset + i; + if (mx >= _size_x) { + continue; + } + for (int j = 0; j < _downsampling_factor; ++j) { + my = y_offset + j; + if (my >= _size_y) { + continue; + } + cost = std::max(cost, _costmap->getCost(mx, my)); + } + } + + _downsampled_costmap->setCost(new_mx, new_my, cost); + } + + unsigned int _size_x; + unsigned int _size_y; + unsigned int _downsampled_size_x; + unsigned int _downsampled_size_y; + unsigned int _downsampling_factor; + float _downsampled_resolution; + std::string _topic_name; + nav2_util::LifecycleNode::SharedPtr _node; + nav2_costmap_2d::Costmap2D * _costmap; + std::unique_ptr _downsampled_costmap; + std::unique_ptr _downsampled_costmap_pub; +}; + +} // namespace smac_planner + +#endif // SMAC_PLANNER__DOWNSAMPLER_HPP_ diff --git a/smac_planner/include/smac_planner/smac_planner.hpp b/smac_planner/include/smac_planner/smac_planner.hpp index 9e8bd2b1286..dba2301a693 100644 --- a/smac_planner/include/smac_planner/smac_planner.hpp +++ b/smac_planner/include/smac_planner/smac_planner.hpp @@ -22,6 +22,8 @@ #include "smac_planner/a_star.hpp" #include "smac_planner/smoother.hpp" #include "smac_planner/upsampler.hpp" +#include "smac_planner/costmap_downsampler.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" #include "nav2_core/global_planner.hpp" #include "nav_msgs/msg/path.hpp" #include "nav2_costmap_2d/costmap_2d_ros.hpp" @@ -92,14 +94,17 @@ class SmacPlanner : public nav2_core::GlobalPlanner std::unique_ptr> _a_star; std::unique_ptr _smoother; std::unique_ptr _upsampler; + std::unique_ptr _costmap_downsampler; nav2_util::LifecycleNode::SharedPtr _node; nav2_costmap_2d::Costmap2D * _costmap; std::string _global_frame, _name; float _tolerance; + int _downsampling_factor; + bool _downsample_costmap; rclcpp_lifecycle::LifecyclePublisher::SharedPtr _raw_plan_publisher; - rclcpp_lifecycle::LifecyclePublisher::SharedPtr smoother_debug1_pub_; - rclcpp_lifecycle::LifecyclePublisher::SharedPtr smoother_debug2_pub_; - rclcpp_lifecycle::LifecyclePublisher::SharedPtr smoother_debug3_pub_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr _smoother_debug1_pub; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr _smoother_debug2_pub; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr _smoother_debug3_pub; SmootherParams _smoother_params; OptimizerParams _optimizer_params; int _upsampling_ratio; diff --git a/smac_planner/src/smac_planner.cpp b/smac_planner/src/smac_planner.cpp index bb18b08eb40..fc669b7cc04 100644 --- a/smac_planner/src/smac_planner.cpp +++ b/smac_planner/src/smac_planner.cpp @@ -74,7 +74,8 @@ SmacPlanner::SmacPlanner() _smoother(nullptr), _upsampler(nullptr), _node(nullptr), - _costmap(nullptr) + _costmap(nullptr), + _costmap_downsampler(nullptr) { } @@ -107,6 +108,12 @@ void SmacPlanner::configure( nav2_util::declare_parameter_if_not_declared( _node, name + ".tolerance", rclcpp::ParameterValue(0.125)); _tolerance = static_cast(_node->get_parameter(name + ".tolerance").as_double()); + nav2_util::declare_parameter_if_not_declared( + _node, name + ".downsample_costmap", rclcpp::ParameterValue(true)); + _node->get_parameter(name + ".downsample_costmap", _downsample_costmap); + nav2_util::declare_parameter_if_not_declared( + _node, name + ".downsampling_factor", rclcpp::ParameterValue(1)); + _node->get_parameter(name + ".downsampling_factor", _downsampling_factor); nav2_util::declare_parameter_if_not_declared( _node, name + ".allow_unknown", rclcpp::ParameterValue(true)); _node->get_parameter(name + ".allow_unknown", allow_unknown); @@ -187,10 +194,16 @@ void SmacPlanner::configure( } } + if (_downsample_costmap && _downsampling_factor > 1) { + std::string topic_name = "downsampled_costmap"; + _costmap_downsampler = std::make_unique(_node); + _costmap_downsampler->initialize(_global_frame, topic_name, _costmap, _downsampling_factor); + } + _raw_plan_publisher = _node->create_publisher("unsmoothed_plan", 1); - smoother_debug1_pub_= _node->create_publisher("debug1", 1); - smoother_debug2_pub_= _node->create_publisher("debug2", 1); - smoother_debug3_pub_= _node->create_publisher("debug3", 1); + _smoother_debug1_pub= _node->create_publisher("debug1", 1); + _smoother_debug2_pub= _node->create_publisher("debug2", 1); + _smoother_debug3_pub= _node->create_publisher("debug3", 1); RCLCPP_INFO( _node->get_logger(), "Configured plugin %s of type SmacPlanner with " @@ -207,9 +220,12 @@ void SmacPlanner::activate() _node->get_logger(), "Activating plugin %s of type SmacPlanner", _name.c_str()); _raw_plan_publisher->on_activate(); - smoother_debug1_pub_->on_activate(); - smoother_debug2_pub_->on_activate(); - smoother_debug3_pub_->on_activate(); + _smoother_debug1_pub->on_activate(); + _smoother_debug2_pub->on_activate(); + _smoother_debug3_pub->on_activate(); + if (_costmap_downsampler) { + _costmap_downsampler->activatePublisher(); + } } void SmacPlanner::deactivate() @@ -218,6 +234,9 @@ void SmacPlanner::deactivate() _node->get_logger(), "Deactivating plugin %s of type SmacPlanner", _name.c_str()); _raw_plan_publisher->on_deactivate(); + if (_costmap_downsampler) { + _costmap_downsampler->deactivatePublisher(); + } } void SmacPlanner::cleanup() @@ -228,7 +247,8 @@ void SmacPlanner::cleanup() _a_star.reset(); _smoother.reset(); _upsampler.reset(); - _raw_plan_publisher.reset(); + _costmap_downsampler.reset(); + _raw_plan_publisher.reset(); } nav_msgs::msg::Path SmacPlanner::createPlan( @@ -239,22 +259,28 @@ nav_msgs::msg::Path SmacPlanner::createPlan( steady_clock::time_point a = steady_clock::now(); #endif + // Choose which costmap to use for the planning + nav2_costmap_2d::Costmap2D * costmap = _costmap; + if (_costmap_downsampler) { + costmap = _costmap_downsampler->downsample(_downsampling_factor); + } + // Set Costmap - unsigned char * char_costmap = _costmap->getCharMap(); + unsigned char * char_costmap = costmap->getCharMap(); _a_star->setCosts( - _costmap->getSizeInCellsX(), - _costmap->getSizeInCellsY(), + costmap->getSizeInCellsX(), + costmap->getSizeInCellsY(), char_costmap); // Set starting point unsigned int mx, my, index; - _costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx, my); - index = _costmap->getIndex(mx, my); + costmap->worldToMap(start.pose.position.x, start.pose.position.y, mx, my); + index = costmap->getIndex(mx, my); _a_star->setStart(index); // Set goal point - _costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my); - index = _costmap->getIndex(mx, my); + costmap->worldToMap(goal.pose.position.x, goal.pose.position.y, mx, my); + index = costmap->getIndex(mx, my); _a_star->setGoal(index); // Setup message @@ -275,7 +301,7 @@ nav_msgs::msg::Path SmacPlanner::createPlan( std::string error; try { if (!_a_star->createPath( - path, num_iterations, _tolerance / static_cast(_costmap->getResolution()))) + path, num_iterations, _tolerance / static_cast(costmap->getResolution()))) { if (num_iterations < _a_star->getMaxIterations()) { error = std::string("no valid path found."); @@ -309,8 +335,8 @@ nav_msgs::msg::Path SmacPlanner::createPlan( } unsigned int index_x, index_y; double world_x, world_y; - _costmap->indexToCells(path[i], index_x, index_y); - _costmap->mapToWorld(index_x, index_y, world_x, world_y); + costmap->indexToCells(path[i], index_x, index_y); + costmap->mapToWorld(index_x, index_y, world_x, world_y); path_world.push_back(Eigen::Vector2d(world_x, world_y)); pose.pose.position.x = world_x; pose.pose.position.y = world_y; @@ -324,9 +350,9 @@ nav_msgs::msg::Path SmacPlanner::createPlan( // Smooth plan if (_smoother && path_world.size() > 4) { - MinimalCostmap mcmap(char_costmap, _costmap->getSizeInCellsX(), - _costmap->getSizeInCellsY(), _costmap->getOriginX(), _costmap->getOriginY(), - _costmap->getResolution()); + MinimalCostmap mcmap(char_costmap, costmap->getSizeInCellsX(), + costmap->getSizeInCellsY(), costmap->getOriginX(), costmap->getOriginY(), + costmap->getResolution()); if (!_smoother->smooth(path_world, & mcmap, _smoother_params)) { RCLCPP_WARN( _node->get_logger(), @@ -343,7 +369,7 @@ nav_msgs::msg::Path SmacPlanner::createPlan( pose.pose.position.y = path_world[i][1]; plan.poses[i] = pose; } - smoother_debug1_pub_->publish(plan); + _smoother_debug1_pub->publish(plan); ///////////////////////////////// DEBUG///////////////////////////////// // Upsample path