From 59bc0ff695fbcecd8eeefd119f4c2b4301542afc Mon Sep 17 00:00:00 2001 From: andriimaistruk <71632363+andriimaistruk@users.noreply.github.com> Date: Mon, 28 Dec 2020 17:48:16 +0200 Subject: [PATCH] Costmap out of bounds access exception handling (#14) * add exception for costmap get cost && change planner_server launch file * added path planning failure behaviour when static layer costmap wasn't initialized * added out of bounds error message printing --- .../bringup/launch/planner_server_launch.py | 2 +- .../include/nav2_costmap_2d/costmap_2d.hpp | 9 +---- .../nav2_costmap_2d/costmap_2d_ros.hpp | 4 ++ .../include/nav2_costmap_2d/layer.hpp | 2 + .../include/nav2_costmap_2d/static_layer.hpp | 2 + nav2_costmap_2d/src/costmap_2d.cpp | 37 +++++++++++++++++-- nav2_costmap_2d/src/costmap_2d_ros.cpp | 10 +++++ .../nav2_util/simple_action_server.hpp | 2 +- .../include/smac_planner/smac_planner.hpp | 1 + smac_planner/src/smac_planner.cpp | 14 +++++++ 10 files changed, 70 insertions(+), 13 deletions(-) diff --git a/nav2_bringup/bringup/launch/planner_server_launch.py b/nav2_bringup/bringup/launch/planner_server_launch.py index 24156c93ee9..b3d1f77aa8c 100644 --- a/nav2_bringup/bringup/launch/planner_server_launch.py +++ b/nav2_bringup/bringup/launch/planner_server_launch.py @@ -108,7 +108,7 @@ def generate_launch_description(): condition=IfCondition(use_lifecycle_mgr), package='nav2_lifecycle_manager', executable='lifecycle_manager', - name='lifecycle_manager_navigation', + name='lifecycle_manager_planner_server', #arguments=['--ros-args', '--log-level', 'DEBUG'], output='screen', parameters=[{'use_sim_time': use_sim_time}, diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp index d74895b40a2..4bb434c99bf 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp @@ -127,8 +127,7 @@ class Costmap2D /** * @brief Get the cost of a cell in the costmap - * @param mx The x coordinate of the cell - * @param my The y coordinate of the cell + * @param index The cell index * @return The cost of the cell */ unsigned char getCost(unsigned int index) const; @@ -186,11 +185,7 @@ class Costmap2D * @param my The y coordinate * @return The associated index */ - inline unsigned int getIndex(unsigned int mx, unsigned int my) const - { - return my * size_x_ + mx; - } - + unsigned int getIndex(unsigned int mx, unsigned int my) const; /** * @brief Given an index... compute the associated map coordinates * @param index The index diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp index 124d1666a51..54d50214edd 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp @@ -256,6 +256,9 @@ class Costmap2DROS : public nav2_util::LifecycleNode */ bool getUseRadius() {return use_radius_;} + + bool is_static_layer_costmap_initialized(); + protected: rclcpp::Node::SharedPtr client_node_; @@ -313,6 +316,7 @@ class Costmap2DROS : public nav2_util::LifecycleNode std::vector padded_footprint_; std::unique_ptr clear_costmap_service_; + std::shared_ptr static_layer_plugin; }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp index b0eca7251a1..eab62603c54 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/layer.hpp @@ -128,6 +128,8 @@ class Layer // TODO(mjeronimo): public nav2_util::LifecycleHelperInterface void undeclareAllParameters(); std::string getFullName(const std::string & param_name); + virtual bool map_received() { return false; } + protected: LayeredCostmap * layered_costmap_; std::string name_; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp index bbd5daa757e..aa7bbbf33f4 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp @@ -72,6 +72,8 @@ class StaticLayer : public CostmapLayer virtual void matchSize(); + inline bool map_received() { return map_received_; } + private: void getParameters(); void processMap(const nav_msgs::msg::OccupancyGrid & new_map); diff --git a/nav2_costmap_2d/src/costmap_2d.cpp b/nav2_costmap_2d/src/costmap_2d.cpp index 976d09ce263..381b91ca0b2 100644 --- a/nav2_costmap_2d/src/costmap_2d.cpp +++ b/nav2_costmap_2d/src/costmap_2d.cpp @@ -41,6 +41,10 @@ #include #include #include +#include +#include "nav2_costmap_2d/cost_values.hpp" + +using nav2_costmap_2d::NO_INFORMATION; namespace nav2_costmap_2d { @@ -207,19 +211,40 @@ unsigned char * Costmap2D::getCharMap() const return costmap_; } +unsigned int Costmap2D::getIndex(unsigned int mx, unsigned int my) const +{ + auto cell_index = my * size_x_ + mx; + if (cell_index < size_x_ * size_y_) { + return cell_index; + } else { + std::cout << "ERROR: Costmap2D::getIndex out of bounds x, y cell coordinates" << std::endl; + throw std::runtime_error("Access to out of bounds costmap cell"); + } +} + unsigned char Costmap2D::getCost(unsigned int mx, unsigned int my) const { - return costmap_[getIndex(mx, my)]; + try { + return costmap_[getIndex(mx, my)]; + } catch (const std::runtime_error& e) { + return nav2_costmap_2d::NO_INFORMATION; + } } unsigned char Costmap2D::getCost(unsigned int undex) const { + // no need to do bounds checks here + // it is already done in smac_planner/node_se2.cpp:373 return costmap_[undex]; } void Costmap2D::setCost(unsigned int mx, unsigned int my, unsigned char cost) { - costmap_[getIndex(mx, my)] = cost; + try { + costmap_[getIndex(mx, my)] = cost; + } catch (const std::runtime_error& e) { + return; + } } void Costmap2D::mapToWorld(unsigned int mx, unsigned int my, double & wx, double & wy) const @@ -351,8 +376,12 @@ bool Costmap2D::setConvexPolygonCost( // set the cost of those cells for (unsigned int i = 0; i < polygon_cells.size(); ++i) { - unsigned int index = getIndex(polygon_cells[i].x, polygon_cells[i].y); - costmap_[index] = cost_value; + try { + unsigned int index = getIndex(polygon_cells[i].x, polygon_cells[i].y); + costmap_[index] = cost_value; + } catch (const std::runtime_error& e) { + continue; + } } return true; } diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 89c3d28aca8..a9e9bb32999 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -154,6 +154,10 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) layered_costmap_, plugin_names_[i], tf_buffer_.get(), shared_from_this(), client_node_, rclcpp_node_); + if (plugin_names_[i] == "static_layer") { + static_layer_plugin = plugin; + } + RCLCPP_INFO(get_logger(), "Initialized plugin \"%s\"", plugin_names_[i].c_str()); } @@ -538,4 +542,10 @@ Costmap2DROS::getRobotPose(geometry_msgs::msg::PoseStamped & global_pose) global_frame_, robot_base_frame_, transform_tolerance_); } +bool +Costmap2DROS::is_static_layer_costmap_initialized() +{ + // return static layer boolean variable map_received_ + return static_layer_plugin->map_received(); +} } // namespace nav2_costmap_2d diff --git a/nav2_util/include/nav2_util/simple_action_server.hpp b/nav2_util/include/nav2_util/simple_action_server.hpp index e4767abf5f8..9b425f39340 100644 --- a/nav2_util/include/nav2_util/simple_action_server.hpp +++ b/nav2_util/include/nav2_util/simple_action_server.hpp @@ -145,7 +145,7 @@ class SimpleActionServer } catch (std::exception & ex) { RCLCPP_ERROR( node_logging_interface_->get_logger(), - "Action server failed while executing action callback: \"%s\"", ex.what()); + "Action server %s failed while executing action callback: \"%s\"", action_name_, ex.what()); terminate_all(); return; } diff --git a/smac_planner/include/smac_planner/smac_planner.hpp b/smac_planner/include/smac_planner/smac_planner.hpp index 379f0c92a5b..adfc0a8c57a 100644 --- a/smac_planner/include/smac_planner/smac_planner.hpp +++ b/smac_planner/include/smac_planner/smac_planner.hpp @@ -124,6 +124,7 @@ class SmacPlanner : public nav2_core::GlobalPlanner SmootherParams _smoother_params; OptimizerParams _optimizer_params; double _max_planning_time; + std::shared_ptr _costmap_ros; }; } // namespace smac_planner diff --git a/smac_planner/src/smac_planner.cpp b/smac_planner/src/smac_planner.cpp index f3db10830ad..8d69bc8410b 100644 --- a/smac_planner/src/smac_planner.cpp +++ b/smac_planner/src/smac_planner.cpp @@ -50,6 +50,7 @@ void SmacPlanner::configure( std::shared_ptr costmap_ros) { _node = parent; + _costmap_ros = costmap_ros; _costmap = costmap_ros->getCostmap(); _name = name; _global_frame = costmap_ros->getGlobalFrameID(); @@ -217,6 +218,19 @@ nav_msgs::msg::Path SmacPlanner::createPlan( std::unique_lock lock(*(_costmap->getMutex())); + // check if static layer was initialized + if (!_costmap_ros->is_static_layer_costmap_initialized()) { + // no map for static layer was published + // abort planning + RCLCPP_WARN( + _node->get_logger(), "static layer costmap haven't received any initial occupancy grid", + _name.c_str()); + nav_msgs::msg::Path plan; + plan.header.stamp = _node->now(); + plan.header.frame_id = _global_frame; + // returning empty path equals to planning failure + return plan; + } // Downsample costmap, if required nav2_costmap_2d::Costmap2D * costmap = _costmap; if (_costmap_downsampler) {