From fc4103f1525dea4fd92a7dedb96292911f2ec583 Mon Sep 17 00:00:00 2001 From: GoesM <130988564+GoesM@users.noreply.github.com> Date: Tue, 24 Oct 2023 12:06:47 +0800 Subject: [PATCH 01/33] image.hpp #3891 --- nav2_costmap_2d/include/nav2_costmap_2d/denoise/image.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image.hpp index db7ae8fce84..b7f518f72d0 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image.hpp @@ -50,7 +50,7 @@ class Image * Share image data between new and old object. * Changing data in a new object will affect the given one and vice versa */ - Image(Image & other); + Image(const Image & other); /** * @brief Create image from the other (move constructor) @@ -132,7 +132,7 @@ Image::Image(size_t rows, size_t columns, T * data, size_t step) } template -Image::Image(Image & other) +Image::Imagec(const Image & other) : data_start_{other.data_start_}, rows_{other.rows_}, columns_{other.columns_}, step_{other.step_} {} From 45f7de61fa85454e2e243ef54405a0add7558117 Mon Sep 17 00:00:00 2001 From: GoesM <130988564+GoesM@users.noreply.github.com> Date: Tue, 24 Oct 2023 21:32:29 +0800 Subject: [PATCH 02/33] Update image.hpp --- nav2_costmap_2d/include/nav2_costmap_2d/denoise/image.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image.hpp index b7f518f72d0..888455168d3 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image.hpp @@ -132,7 +132,7 @@ Image::Image(size_t rows, size_t columns, T * data, size_t step) } template -Image::Imagec(const Image & other) +Image::Image(const Image & other) : data_start_{other.data_start_}, rows_{other.rows_}, columns_{other.columns_}, step_{other.step_} {} From 93515e6096fa1925f562c302e42c8b64274cb223 Mon Sep 17 00:00:00 2001 From: GoesM <130988564+GoesM@users.noreply.github.com> Date: Sun, 12 Nov 2023 00:23:00 +0800 Subject: [PATCH 03/33] Update costmap_2d_ros.hpp --- nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp | 4 ++++ 1 file changed, 4 insertions(+) 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 f319e52f330..1d8a5799df7 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 @@ -169,9 +169,13 @@ class Costmap2DROS : public nav2_util::LifecycleNode /** @brief Same as getLayeredCostmap()->isCurrent(). */ bool isCurrent() { + // lock the costmap because no costmap-reset is allowed until the isCurrent() finished + std::unique_lock lock(*(layered_costmap_->getCostmap()->getMutex())); + return layered_costmap_->isCurrent(); } + /** * @brief Get the pose of the robot in the global frame of the costmap * @param global_pose Will be set to the pose of the robot in the global frame of the costmap From 590754d22ff29b991daf67a3e9faad4693840481 Mon Sep 17 00:00:00 2001 From: GoesM <130988564+GoesM@users.noreply.github.com> Date: Sun, 12 Nov 2023 15:16:34 +0800 Subject: [PATCH 04/33] uncrustify costmap_2d_ros.hpp --- nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp | 1 - 1 file changed, 1 deletion(-) 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 1d8a5799df7..a8c4ce6df69 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 @@ -171,7 +171,6 @@ class Costmap2DROS : public nav2_util::LifecycleNode { // lock the costmap because no costmap-reset is allowed until the isCurrent() finished std::unique_lock lock(*(layered_costmap_->getCostmap()->getMutex())); - return layered_costmap_->isCurrent(); } From 8e6c49d6fb0979a5db6d19ac7c03f27c7519fb35 Mon Sep 17 00:00:00 2001 From: GoesM <130988564+GoesM@users.noreply.github.com> Date: Mon, 13 Nov 2023 00:43:31 +0800 Subject: [PATCH 05/33] Update costmap_2d_ros.hpp --- .../nav2_costmap_2d/costmap_2d_ros.hpp | 35 ++++++------------- 1 file changed, 10 insertions(+), 25 deletions(-) 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 a8c4ce6df69..a7182293470 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 @@ -73,36 +73,23 @@ namespace nav2_costmap_2d class Costmap2DROS : public nav2_util::LifecycleNode { public: - /** - * @brief Constructor for the wrapper - */ - Costmap2DROS(); - /** * @brief Constructor for the wrapper, the node will * be placed in a namespace equal to the node's name * @param name Name of the costmap ROS node - * @param use_sim_time Whether to use simulation or real time */ - explicit Costmap2DROS(const std::string & name, const bool & use_sim_time = false); + explicit Costmap2DROS(const std::string & name); /** * @brief Constructor for the wrapper * @param name Name of the costmap ROS node * @param parent_namespace Absolute namespace of the node hosting the costmap node * @param local_namespace Namespace to append to the parent namespace - * @param use_sim_time Whether to use simulation or real time */ explicit Costmap2DROS( const std::string & name, const std::string & parent_namespace, - const std::string & local_namespace, - const bool & use_sim_time); - - /** - * @brief Common initialization for constructors - */ - void init(); + const std::string & local_namespace); /** * @brief A destructor @@ -169,8 +156,9 @@ class Costmap2DROS : public nav2_util::LifecycleNode /** @brief Same as getLayeredCostmap()->isCurrent(). */ bool isCurrent() { - // lock the costmap because no costmap-reset is allowed until the isCurrent() finished - std::unique_lock lock(*(layered_costmap_->getCostmap()->getMutex())); + //lock the layered_costmap because no ptr-free is allowed until other ptr-access finished + std::unique_lock lock(*(layered_costmap_->getCostmap()->getMutex_free_access())); + if(layered_costmap_==nullptr) return false; return layered_costmap_->isCurrent(); } @@ -323,9 +311,7 @@ class Costmap2DROS : public nav2_util::LifecycleNode // Publishers and subscribers rclcpp_lifecycle::LifecyclePublisher::SharedPtr footprint_pub_; - std::unique_ptr costmap_publisher_; - - std::vector> layer_publishers_; + std::unique_ptr costmap_publisher_{nullptr}; rclcpp::Subscription::SharedPtr footprint_sub_; rclcpp::Subscription::SharedPtr parameter_sub_; @@ -363,7 +349,7 @@ class Costmap2DROS : public nav2_util::LifecycleNode bool always_send_full_costmap_{false}; std::string footprint_; float footprint_padding_{0}; - std::string global_frame_; ///< The global frame for the costmap + std::string global_frame_; ///< The global frame for the costmap int map_height_meters_{0}; double map_publish_frequency_{0}; double map_update_frequency_{0}; @@ -377,12 +363,11 @@ class Costmap2DROS : public nav2_util::LifecycleNode std::vector filter_names_; std::vector filter_types_; double resolution_{0}; - std::string robot_base_frame_; ///< The frame_id of the robot base + std::string robot_base_frame_; ///< The frame_id of the robot base double robot_radius_; - bool rolling_window_{false}; ///< Whether to use a rolling window version of the costmap + bool rolling_window_{false}; ///< Whether to use a rolling window version of the costmap bool track_unknown_space_{false}; - double transform_tolerance_{0}; ///< The timeout before transform errors - double initial_transform_timeout_{0}; ///< The timeout before activation of the node errors + double transform_tolerance_{0}; ///< The timeout before transform errors // Derived parameters bool use_radius_{false}; From 0144fcb1710527b897e1fff0259e6e33bc6e544f Mon Sep 17 00:00:00 2001 From: GoesM <130988564+GoesM@users.noreply.github.com> Date: Mon, 13 Nov 2023 00:44:27 +0800 Subject: [PATCH 06/33] Update costmap_2d_ros.cpp --- nav2_costmap_2d/src/costmap_2d_ros.cpp | 114 +++++-------------------- 1 file changed, 21 insertions(+), 93 deletions(-) diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index c35933c9083..4802d8a8943 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -58,27 +58,13 @@ using rcl_interfaces::msg::ParameterType; namespace nav2_costmap_2d { -Costmap2DROS::Costmap2DROS(const std::string & name, const bool & use_sim_time) -: Costmap2DROS(name, "/", name, use_sim_time) {} - -Costmap2DROS::Costmap2DROS() -: nav2_util::LifecycleNode("costmap", ""), - name_("costmap"), - default_plugins_{"static_layer", "obstacle_layer", "inflation_layer"}, - default_types_{ - "nav2_costmap_2d::StaticLayer", - "nav2_costmap_2d::ObstacleLayer", - "nav2_costmap_2d::InflationLayer"} -{ - declare_parameter("map_topic", rclcpp::ParameterValue(std::string("map"))); - init(); -} +Costmap2DROS::Costmap2DROS(const std::string & name) +: Costmap2DROS(name, "/", name) {} Costmap2DROS::Costmap2DROS( const std::string & name, const std::string & parent_namespace, - const std::string & local_namespace, - const bool & use_sim_time) + const std::string & local_namespace) : nav2_util::LifecycleNode(name, "", // NodeOption arguments take precedence over the ones provided on the command line // use this to make sure the node is placed on the provided namespace @@ -87,8 +73,7 @@ Costmap2DROS::Costmap2DROS( rclcpp::NodeOptions().arguments({ "--ros-args", "-r", std::string("__ns:=") + nav2_util::add_namespaces(parent_namespace, local_namespace), - "--ros-args", "-r", name + ":" + std::string("__node:=") + name, - "--ros-args", "-p", "use_sim_time:=" + std::string(use_sim_time ? "true" : "false"), + "--ros-args", "-r", name + ":" + std::string("__node:=") + name })), name_(name), parent_namespace_(parent_namespace), @@ -97,14 +82,6 @@ Costmap2DROS::Costmap2DROS( "nav2_costmap_2d::StaticLayer", "nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::InflationLayer"} -{ - declare_parameter( - "map_topic", rclcpp::ParameterValue( - (parent_namespace_ == "/" ? "/" : parent_namespace_ + "/") + std::string("map"))); - init(); -} - -void Costmap2DROS::init() { RCLCPP_INFO(get_logger(), "Creating Costmap"); @@ -117,6 +94,9 @@ void Costmap2DROS::init() declare_parameter("height", rclcpp::ParameterValue(5)); declare_parameter("width", rclcpp::ParameterValue(5)); declare_parameter("lethal_cost_threshold", rclcpp::ParameterValue(100)); + declare_parameter( + "map_topic", rclcpp::ParameterValue( + (parent_namespace_ == "/" ? "/" : parent_namespace_ + "/") + std::string("map"))); declare_parameter("observation_sources", rclcpp::ParameterValue(std::string(""))); declare_parameter("origin_x", rclcpp::ParameterValue(0.0)); declare_parameter("origin_y", rclcpp::ParameterValue(0.0)); @@ -129,7 +109,6 @@ void Costmap2DROS::init() declare_parameter("rolling_window", rclcpp::ParameterValue(false)); declare_parameter("track_unknown_space", rclcpp::ParameterValue(false)); declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.3)); - declare_parameter("initial_transform_timeout", rclcpp::ParameterValue(60.0)); declare_parameter("trinary_costmap", rclcpp::ParameterValue(true)); declare_parameter("unknown_cost_value", rclcpp::ParameterValue(static_cast(0xff))); declare_parameter("update_frequency", rclcpp::ParameterValue(5.0)); @@ -145,13 +124,7 @@ nav2_util::CallbackReturn Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Configuring"); - try { - getParameters(); - } catch (const std::exception & e) { - RCLCPP_ERROR( - get_logger(), "Failed to configure costmap! %s.", e.what()); - return nav2_util::CallbackReturn::FAILURE; - } + getParameters(); callback_group_ = create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive, false); @@ -229,20 +202,6 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) layered_costmap_->getCostmap(), global_frame_, "costmap", always_send_full_costmap_); - auto layers = layered_costmap_->getPlugins(); - - for (auto & layer : *layers) { - auto costmap_layer = std::dynamic_pointer_cast(layer); - if (costmap_layer != nullptr) { - layer_publishers_.emplace_back( - std::make_unique( - shared_from_this(), - costmap_layer.get(), global_frame_, - layer->getName(), always_send_full_costmap_) - ); - } - } - // Set the footprint if (use_radius_) { setRobotFootprint(makeFootprintFromRadius(robot_radius_)); @@ -266,6 +225,9 @@ Costmap2DROS::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); + costmap_publisher_->on_activate(); + footprint_pub_->on_activate(); + // First, make sure that the transform between the robot base frame // and the global frame is available @@ -273,9 +235,6 @@ Costmap2DROS::on_activate(const rclcpp_lifecycle::State & /*state*/) RCLCPP_INFO(get_logger(), "Checking transform"); rclcpp::Rate r(2); - const auto initial_transform_timeout = rclcpp::Duration::from_seconds( - initial_transform_timeout_); - const auto initial_transform_timeout_point = now() + initial_transform_timeout; while (rclcpp::ok() && !tf_buffer_->canTransform( global_frame_, robot_base_frame_, tf2::TimePointZero, &tf_error)) @@ -285,29 +244,12 @@ Costmap2DROS::on_activate(const rclcpp_lifecycle::State & /*state*/) " to become available, tf error: %s", robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str()); - // Check timeout - if (now() > initial_transform_timeout_point) { - RCLCPP_ERROR( - get_logger(), "Failed to activate %s because transform from %s to %s did not become available before timeout", - get_name(), robot_base_frame_.c_str(), global_frame_.c_str()); - - return nav2_util::CallbackReturn::FAILURE; - } - // The error string will accumulate and errors will typically be the same, so the last // will do for the warning above. Reset the string here to avoid accumulation tf_error.clear(); r.sleep(); } - // Activate publishers - footprint_pub_->on_activate(); - costmap_publisher_->on_activate(); - - for (auto & layer_pub : layer_publishers_) { - layer_pub->on_activate(); - } - // Create a thread to handle updating the map stopped_ = true; // to active plugins stop_updates_ = false; @@ -340,12 +282,8 @@ Costmap2DROS::on_deactivate(const rclcpp_lifecycle::State & /*state*/) map_update_thread_->join(); } - footprint_pub_->on_deactivate(); costmap_publisher_->on_deactivate(); - - for (auto & layer_pub : layer_publishers_) { - layer_pub->on_deactivate(); - } + footprint_pub_->on_deactivate(); return nav2_util::CallbackReturn::SUCCESS; } @@ -354,13 +292,13 @@ nav2_util::CallbackReturn Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Cleaning up"); - - costmap_publisher_.reset(); - clear_costmap_service_.reset(); - - layer_publishers_.clear(); - + + //lock the layered_costmap because no ptr-free is allowed until other ptr-access finished + std::unique_lock lock(*(layered_costmap_->getCostmap()->getMutex_free_access())); layered_costmap_.reset(); + //unlock the layered_costmap_ to avoid pid stop + lock.unlock(); + tf_listener_.reset(); tf_buffer_.reset(); @@ -368,6 +306,8 @@ Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/) footprint_sub_.reset(); footprint_pub_.reset(); + costmap_publisher_.reset(); + clear_costmap_service_.reset(); executor_thread_.reset(); return nav2_util::CallbackReturn::SUCCESS; @@ -400,7 +340,6 @@ Costmap2DROS::getParameters() get_parameter("rolling_window", rolling_window_); get_parameter("track_unknown_space", track_unknown_space_); get_parameter("transform_tolerance", transform_tolerance_); - get_parameter("initial_transform_timeout", initial_transform_timeout_); get_parameter("update_frequency", map_update_frequency_); get_parameter("width", map_width_meters_); get_parameter("plugins", plugin_names_); @@ -510,22 +449,12 @@ Costmap2DROS::mapUpdateLoop(double frequency) layered_costmap_->getBounds(&x0, &xn, &y0, &yn); costmap_publisher_->updateBounds(x0, xn, y0, yn); - for (auto & layer_pub: layer_publishers_) { - layer_pub->updateBounds(x0, xn, y0, yn); - } - auto current_time = now(); if ((last_publish_ + publish_cycle_ < current_time) || // publish_cycle_ is due - (current_time < - last_publish_)) // time has moved backwards, probably due to a switch to sim_time // NOLINT + (current_time < last_publish_)) // time has moved backwards, probably due to a switch to sim_time // NOLINT { RCLCPP_DEBUG(get_logger(), "Publish costmap at %s", name_.c_str()); costmap_publisher_->publishCostmap(); - - for (auto & layer_pub: layer_publishers_) { - layer_pub->publishCostmap(); - } - last_publish_ = current_time; } } @@ -609,7 +538,6 @@ void Costmap2DROS::stop() { stop_updates_ = true; - // layered_costmap_ is set only if on_configure has been called if (layered_costmap_) { std::vector> * plugins = layered_costmap_->getPlugins(); From 913b62ff2fbcf329a079570eed9bc817b02c50b3 Mon Sep 17 00:00:00 2001 From: GoesM <130988564+GoesM@users.noreply.github.com> Date: Mon, 13 Nov 2023 00:44:56 +0800 Subject: [PATCH 07/33] Update costmap_2d.cpp --- nav2_costmap_2d/src/costmap_2d.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/nav2_costmap_2d/src/costmap_2d.cpp b/nav2_costmap_2d/src/costmap_2d.cpp index e72807da59b..2662131ac40 100644 --- a/nav2_costmap_2d/src/costmap_2d.cpp +++ b/nav2_costmap_2d/src/costmap_2d.cpp @@ -53,6 +53,7 @@ Costmap2D::Costmap2D( origin_y_(origin_y), costmap_(NULL), default_value_(default_value) { access_ = new mutex_t(); + free_access_ = new mutex_t();//only for UAF check // create the costmap initMaps(size_x_, size_y_); @@ -63,6 +64,7 @@ Costmap2D::Costmap2D(const nav_msgs::msg::OccupancyGrid & map) : default_value_(FREE_SPACE) { access_ = new mutex_t(); + free_access_ = new mutex_t();//only for UAF check // fill local variables size_x_ = map.info.width; From ed037ce15bcfd4e0801c4119cf7267216d141efb Mon Sep 17 00:00:00 2001 From: GoesM <130988564+GoesM@users.noreply.github.com> Date: Mon, 13 Nov 2023 00:50:31 +0800 Subject: [PATCH 08/33] Update costmap_2d_ros.cpp --- nav2_costmap_2d/src/costmap_2d_ros.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 4802d8a8943..72404f24a53 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -298,7 +298,7 @@ Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/) layered_costmap_.reset(); //unlock the layered_costmap_ to avoid pid stop lock.unlock(); - + tf_listener_.reset(); tf_buffer_.reset(); From fc27c4ebcd476095904c64fcd6aeb1097c6f1981 Mon Sep 17 00:00:00 2001 From: GoesM <130988564+GoesM@users.noreply.github.com> Date: Mon, 13 Nov 2023 00:50:49 +0800 Subject: [PATCH 09/33] Update costmap_2d_ros.hpp --- nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp | 1 - 1 file changed, 1 deletion(-) 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 a7182293470..cf7efc8f24a 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 @@ -162,7 +162,6 @@ class Costmap2DROS : public nav2_util::LifecycleNode return layered_costmap_->isCurrent(); } - /** * @brief Get the pose of the robot in the global frame of the costmap * @param global_pose Will be set to the pose of the robot in the global frame of the costmap From 41936a3b40558ee67a31e5e9f65a200827197efd Mon Sep 17 00:00:00 2001 From: GoesM <130988564+GoesM@users.noreply.github.com> Date: Mon, 13 Nov 2023 01:00:25 +0800 Subject: [PATCH 10/33] add UAF_lock into isCurrent --- .../nav2_costmap_2d/costmap_2d_ros.hpp | 30 ++++++++++++++----- 1 file changed, 23 insertions(+), 7 deletions(-) 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 cf7efc8f24a..436359f5c46 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 @@ -73,23 +73,36 @@ namespace nav2_costmap_2d class Costmap2DROS : public nav2_util::LifecycleNode { public: + /** + * @brief Constructor for the wrapper + */ + Costmap2DROS(); + /** * @brief Constructor for the wrapper, the node will * be placed in a namespace equal to the node's name * @param name Name of the costmap ROS node + * @param use_sim_time Whether to use simulation or real time */ - explicit Costmap2DROS(const std::string & name); + explicit Costmap2DROS(const std::string & name, const bool & use_sim_time = false); /** * @brief Constructor for the wrapper * @param name Name of the costmap ROS node * @param parent_namespace Absolute namespace of the node hosting the costmap node * @param local_namespace Namespace to append to the parent namespace + * @param use_sim_time Whether to use simulation or real time */ explicit Costmap2DROS( const std::string & name, const std::string & parent_namespace, - const std::string & local_namespace); + const std::string & local_namespace, + const bool & use_sim_time); + + /** + * @brief Common initialization for constructors + */ + void init(); /** * @brief A destructor @@ -310,7 +323,9 @@ class Costmap2DROS : public nav2_util::LifecycleNode // Publishers and subscribers rclcpp_lifecycle::LifecyclePublisher::SharedPtr footprint_pub_; - std::unique_ptr costmap_publisher_{nullptr}; + std::unique_ptr costmap_publisher_; + + std::vector> layer_publishers_; rclcpp::Subscription::SharedPtr footprint_sub_; rclcpp::Subscription::SharedPtr parameter_sub_; @@ -348,7 +363,7 @@ class Costmap2DROS : public nav2_util::LifecycleNode bool always_send_full_costmap_{false}; std::string footprint_; float footprint_padding_{0}; - std::string global_frame_; ///< The global frame for the costmap + std::string global_frame_; ///< The global frame for the costmap int map_height_meters_{0}; double map_publish_frequency_{0}; double map_update_frequency_{0}; @@ -362,11 +377,12 @@ class Costmap2DROS : public nav2_util::LifecycleNode std::vector filter_names_; std::vector filter_types_; double resolution_{0}; - std::string robot_base_frame_; ///< The frame_id of the robot base + std::string robot_base_frame_; ///< The frame_id of the robot base double robot_radius_; - bool rolling_window_{false}; ///< Whether to use a rolling window version of the costmap + bool rolling_window_{false}; ///< Whether to use a rolling window version of the costmap bool track_unknown_space_{false}; - double transform_tolerance_{0}; ///< The timeout before transform errors + double transform_tolerance_{0}; ///< The timeout before transform errors + double initial_transform_timeout_{0}; ///< The timeout before activation of the node errors // Derived parameters bool use_radius_{false}; From fe1ffb99e392011b93d234ecc4bbd6973f16c7e0 Mon Sep 17 00:00:00 2001 From: GoesM <130988564+GoesM@users.noreply.github.com> Date: Mon, 13 Nov 2023 01:02:47 +0800 Subject: [PATCH 11/33] Update costmap_2d_ros.cpp --- nav2_costmap_2d/src/costmap_2d_ros.cpp | 114 ++++++++++++++++++++----- 1 file changed, 93 insertions(+), 21 deletions(-) diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 72404f24a53..c35933c9083 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -58,13 +58,27 @@ using rcl_interfaces::msg::ParameterType; namespace nav2_costmap_2d { -Costmap2DROS::Costmap2DROS(const std::string & name) -: Costmap2DROS(name, "/", name) {} +Costmap2DROS::Costmap2DROS(const std::string & name, const bool & use_sim_time) +: Costmap2DROS(name, "/", name, use_sim_time) {} + +Costmap2DROS::Costmap2DROS() +: nav2_util::LifecycleNode("costmap", ""), + name_("costmap"), + default_plugins_{"static_layer", "obstacle_layer", "inflation_layer"}, + default_types_{ + "nav2_costmap_2d::StaticLayer", + "nav2_costmap_2d::ObstacleLayer", + "nav2_costmap_2d::InflationLayer"} +{ + declare_parameter("map_topic", rclcpp::ParameterValue(std::string("map"))); + init(); +} Costmap2DROS::Costmap2DROS( const std::string & name, const std::string & parent_namespace, - const std::string & local_namespace) + const std::string & local_namespace, + const bool & use_sim_time) : nav2_util::LifecycleNode(name, "", // NodeOption arguments take precedence over the ones provided on the command line // use this to make sure the node is placed on the provided namespace @@ -73,7 +87,8 @@ Costmap2DROS::Costmap2DROS( rclcpp::NodeOptions().arguments({ "--ros-args", "-r", std::string("__ns:=") + nav2_util::add_namespaces(parent_namespace, local_namespace), - "--ros-args", "-r", name + ":" + std::string("__node:=") + name + "--ros-args", "-r", name + ":" + std::string("__node:=") + name, + "--ros-args", "-p", "use_sim_time:=" + std::string(use_sim_time ? "true" : "false"), })), name_(name), parent_namespace_(parent_namespace), @@ -82,6 +97,14 @@ Costmap2DROS::Costmap2DROS( "nav2_costmap_2d::StaticLayer", "nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::InflationLayer"} +{ + declare_parameter( + "map_topic", rclcpp::ParameterValue( + (parent_namespace_ == "/" ? "/" : parent_namespace_ + "/") + std::string("map"))); + init(); +} + +void Costmap2DROS::init() { RCLCPP_INFO(get_logger(), "Creating Costmap"); @@ -94,9 +117,6 @@ Costmap2DROS::Costmap2DROS( declare_parameter("height", rclcpp::ParameterValue(5)); declare_parameter("width", rclcpp::ParameterValue(5)); declare_parameter("lethal_cost_threshold", rclcpp::ParameterValue(100)); - declare_parameter( - "map_topic", rclcpp::ParameterValue( - (parent_namespace_ == "/" ? "/" : parent_namespace_ + "/") + std::string("map"))); declare_parameter("observation_sources", rclcpp::ParameterValue(std::string(""))); declare_parameter("origin_x", rclcpp::ParameterValue(0.0)); declare_parameter("origin_y", rclcpp::ParameterValue(0.0)); @@ -109,6 +129,7 @@ Costmap2DROS::Costmap2DROS( declare_parameter("rolling_window", rclcpp::ParameterValue(false)); declare_parameter("track_unknown_space", rclcpp::ParameterValue(false)); declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.3)); + declare_parameter("initial_transform_timeout", rclcpp::ParameterValue(60.0)); declare_parameter("trinary_costmap", rclcpp::ParameterValue(true)); declare_parameter("unknown_cost_value", rclcpp::ParameterValue(static_cast(0xff))); declare_parameter("update_frequency", rclcpp::ParameterValue(5.0)); @@ -124,7 +145,13 @@ nav2_util::CallbackReturn Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Configuring"); - getParameters(); + try { + getParameters(); + } catch (const std::exception & e) { + RCLCPP_ERROR( + get_logger(), "Failed to configure costmap! %s.", e.what()); + return nav2_util::CallbackReturn::FAILURE; + } callback_group_ = create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive, false); @@ -202,6 +229,20 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) layered_costmap_->getCostmap(), global_frame_, "costmap", always_send_full_costmap_); + auto layers = layered_costmap_->getPlugins(); + + for (auto & layer : *layers) { + auto costmap_layer = std::dynamic_pointer_cast(layer); + if (costmap_layer != nullptr) { + layer_publishers_.emplace_back( + std::make_unique( + shared_from_this(), + costmap_layer.get(), global_frame_, + layer->getName(), always_send_full_costmap_) + ); + } + } + // Set the footprint if (use_radius_) { setRobotFootprint(makeFootprintFromRadius(robot_radius_)); @@ -225,9 +266,6 @@ Costmap2DROS::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); - costmap_publisher_->on_activate(); - footprint_pub_->on_activate(); - // First, make sure that the transform between the robot base frame // and the global frame is available @@ -235,6 +273,9 @@ Costmap2DROS::on_activate(const rclcpp_lifecycle::State & /*state*/) RCLCPP_INFO(get_logger(), "Checking transform"); rclcpp::Rate r(2); + const auto initial_transform_timeout = rclcpp::Duration::from_seconds( + initial_transform_timeout_); + const auto initial_transform_timeout_point = now() + initial_transform_timeout; while (rclcpp::ok() && !tf_buffer_->canTransform( global_frame_, robot_base_frame_, tf2::TimePointZero, &tf_error)) @@ -244,12 +285,29 @@ Costmap2DROS::on_activate(const rclcpp_lifecycle::State & /*state*/) " to become available, tf error: %s", robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str()); + // Check timeout + if (now() > initial_transform_timeout_point) { + RCLCPP_ERROR( + get_logger(), "Failed to activate %s because transform from %s to %s did not become available before timeout", + get_name(), robot_base_frame_.c_str(), global_frame_.c_str()); + + return nav2_util::CallbackReturn::FAILURE; + } + // The error string will accumulate and errors will typically be the same, so the last // will do for the warning above. Reset the string here to avoid accumulation tf_error.clear(); r.sleep(); } + // Activate publishers + footprint_pub_->on_activate(); + costmap_publisher_->on_activate(); + + for (auto & layer_pub : layer_publishers_) { + layer_pub->on_activate(); + } + // Create a thread to handle updating the map stopped_ = true; // to active plugins stop_updates_ = false; @@ -282,8 +340,12 @@ Costmap2DROS::on_deactivate(const rclcpp_lifecycle::State & /*state*/) map_update_thread_->join(); } - costmap_publisher_->on_deactivate(); footprint_pub_->on_deactivate(); + costmap_publisher_->on_deactivate(); + + for (auto & layer_pub : layer_publishers_) { + layer_pub->on_deactivate(); + } return nav2_util::CallbackReturn::SUCCESS; } @@ -292,13 +354,13 @@ nav2_util::CallbackReturn Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Cleaning up"); - - //lock the layered_costmap because no ptr-free is allowed until other ptr-access finished - std::unique_lock lock(*(layered_costmap_->getCostmap()->getMutex_free_access())); - layered_costmap_.reset(); - //unlock the layered_costmap_ to avoid pid stop - lock.unlock(); + costmap_publisher_.reset(); + clear_costmap_service_.reset(); + + layer_publishers_.clear(); + + layered_costmap_.reset(); tf_listener_.reset(); tf_buffer_.reset(); @@ -306,8 +368,6 @@ Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/) footprint_sub_.reset(); footprint_pub_.reset(); - costmap_publisher_.reset(); - clear_costmap_service_.reset(); executor_thread_.reset(); return nav2_util::CallbackReturn::SUCCESS; @@ -340,6 +400,7 @@ Costmap2DROS::getParameters() get_parameter("rolling_window", rolling_window_); get_parameter("track_unknown_space", track_unknown_space_); get_parameter("transform_tolerance", transform_tolerance_); + get_parameter("initial_transform_timeout", initial_transform_timeout_); get_parameter("update_frequency", map_update_frequency_); get_parameter("width", map_width_meters_); get_parameter("plugins", plugin_names_); @@ -449,12 +510,22 @@ Costmap2DROS::mapUpdateLoop(double frequency) layered_costmap_->getBounds(&x0, &xn, &y0, &yn); costmap_publisher_->updateBounds(x0, xn, y0, yn); + for (auto & layer_pub: layer_publishers_) { + layer_pub->updateBounds(x0, xn, y0, yn); + } + auto current_time = now(); if ((last_publish_ + publish_cycle_ < current_time) || // publish_cycle_ is due - (current_time < last_publish_)) // time has moved backwards, probably due to a switch to sim_time // NOLINT + (current_time < + last_publish_)) // time has moved backwards, probably due to a switch to sim_time // NOLINT { RCLCPP_DEBUG(get_logger(), "Publish costmap at %s", name_.c_str()); costmap_publisher_->publishCostmap(); + + for (auto & layer_pub: layer_publishers_) { + layer_pub->publishCostmap(); + } + last_publish_ = current_time; } } @@ -538,6 +609,7 @@ void Costmap2DROS::stop() { stop_updates_ = true; + // layered_costmap_ is set only if on_configure has been called if (layered_costmap_) { std::vector> * plugins = layered_costmap_->getPlugins(); From b3e6d0ec5ba9e7e6c1b85df015ae6330da4512be Mon Sep 17 00:00:00 2001 From: GoesM <130988564+GoesM@users.noreply.github.com> Date: Mon, 13 Nov 2023 01:03:13 +0800 Subject: [PATCH 12/33] Update costmap_2d_ros.cpp --- nav2_costmap_2d/src/costmap_2d_ros.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index c35933c9083..7f1b6cf6889 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -359,8 +359,12 @@ Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/) clear_costmap_service_.reset(); layer_publishers_.clear(); - + + //lock the layered_costmap because no ptr-free is allowed until other ptr-access finished + std::unique_lock lock(*(layered_costmap_->getCostmap()->getMutex_free_access())); layered_costmap_.reset(); + //unlock the layered_costmap_ to avoid pid stop + lock.unlock(); tf_listener_.reset(); tf_buffer_.reset(); From 63aadd07db32e472270ce1186f3fd8b21d7ce444 Mon Sep 17 00:00:00 2001 From: GoesM <130988564+GoesM@users.noreply.github.com> Date: Mon, 13 Nov 2023 01:07:16 +0800 Subject: [PATCH 13/33] add free_access_ lock only for UAF check --- nav2_costmap_2d/src/costmap_2d.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/nav2_costmap_2d/src/costmap_2d.cpp b/nav2_costmap_2d/src/costmap_2d.cpp index 2662131ac40..0bb281e6bc1 100644 --- a/nav2_costmap_2d/src/costmap_2d.cpp +++ b/nav2_costmap_2d/src/costmap_2d.cpp @@ -238,6 +238,7 @@ Costmap2D::Costmap2D(const Costmap2D & map) : costmap_(NULL) { access_ = new mutex_t(); + free_access_ = new mutex_t();//only for UAF check *this = map; } @@ -246,12 +247,14 @@ Costmap2D::Costmap2D() : size_x_(0), size_y_(0), resolution_(0.0), origin_x_(0.0), origin_y_(0.0), costmap_(NULL) { access_ = new mutex_t(); + free_access_ = new mutex_t();//only for UAF check } Costmap2D::~Costmap2D() { deleteMaps(); delete access_; + delete free_access_;//only for UAF check } unsigned int Costmap2D::cellDistance(double world_dist) From 5ffc63670cffc1210292332e90610f2d11215d8f Mon Sep 17 00:00:00 2001 From: GoesM <130988564+GoesM@users.noreply.github.com> Date: Mon, 13 Nov 2023 01:09:32 +0800 Subject: [PATCH 14/33] add free_access_ lock only for UAF check --- nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp | 5 +++++ 1 file changed, 5 insertions(+) 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 04b9f4daa43..82296a24b37 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp @@ -364,6 +364,10 @@ class Costmap2D { return access_; } + mutex_t * getMutex_free_access() + { + return free_access_;//only for UAF check + } protected: /** @@ -516,6 +520,7 @@ class Costmap2D } mutex_t * access_; + mutex_t * free_access_; //only for UAF check protected: unsigned int size_x_; From 1d1ba61bfe3431ea5f8d4d0eae0babba7b91225b Mon Sep 17 00:00:00 2001 From: GoesM <130988564+GoesM@users.noreply.github.com> Date: Mon, 13 Nov 2023 04:03:13 +0800 Subject: [PATCH 15/33] Add files via upload --- .../include/nav2_costmap_2d/costmap_2d.hpp | 5 --- .../nav2_costmap_2d/costmap_2d_ros.hpp | 34 +++++-------------- 2 files changed, 8 insertions(+), 31 deletions(-) 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 82296a24b37..04b9f4daa43 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d.hpp @@ -364,10 +364,6 @@ class Costmap2D { return access_; } - mutex_t * getMutex_free_access() - { - return free_access_;//only for UAF check - } protected: /** @@ -520,7 +516,6 @@ class Costmap2D } mutex_t * access_; - mutex_t * free_access_; //only for UAF check protected: unsigned int size_x_; 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 436359f5c46..2e6a6d72764 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 @@ -73,36 +73,23 @@ namespace nav2_costmap_2d class Costmap2DROS : public nav2_util::LifecycleNode { public: - /** - * @brief Constructor for the wrapper - */ - Costmap2DROS(); - /** * @brief Constructor for the wrapper, the node will * be placed in a namespace equal to the node's name * @param name Name of the costmap ROS node - * @param use_sim_time Whether to use simulation or real time */ - explicit Costmap2DROS(const std::string & name, const bool & use_sim_time = false); + explicit Costmap2DROS(const std::string & name); /** * @brief Constructor for the wrapper * @param name Name of the costmap ROS node * @param parent_namespace Absolute namespace of the node hosting the costmap node * @param local_namespace Namespace to append to the parent namespace - * @param use_sim_time Whether to use simulation or real time */ explicit Costmap2DROS( const std::string & name, const std::string & parent_namespace, - const std::string & local_namespace, - const bool & use_sim_time); - - /** - * @brief Common initialization for constructors - */ - void init(); + const std::string & local_namespace); /** * @brief A destructor @@ -169,9 +156,7 @@ class Costmap2DROS : public nav2_util::LifecycleNode /** @brief Same as getLayeredCostmap()->isCurrent(). */ bool isCurrent() { - //lock the layered_costmap because no ptr-free is allowed until other ptr-access finished - std::unique_lock lock(*(layered_costmap_->getCostmap()->getMutex_free_access())); - if(layered_costmap_==nullptr) return false; + std::unique_lock lock(*(layered_costmap_->getCostmap()->getMutex())); return layered_costmap_->isCurrent(); } @@ -323,9 +308,7 @@ class Costmap2DROS : public nav2_util::LifecycleNode // Publishers and subscribers rclcpp_lifecycle::LifecyclePublisher::SharedPtr footprint_pub_; - std::unique_ptr costmap_publisher_; - - std::vector> layer_publishers_; + std::unique_ptr costmap_publisher_{nullptr}; rclcpp::Subscription::SharedPtr footprint_sub_; rclcpp::Subscription::SharedPtr parameter_sub_; @@ -363,7 +346,7 @@ class Costmap2DROS : public nav2_util::LifecycleNode bool always_send_full_costmap_{false}; std::string footprint_; float footprint_padding_{0}; - std::string global_frame_; ///< The global frame for the costmap + std::string global_frame_; ///< The global frame for the costmap int map_height_meters_{0}; double map_publish_frequency_{0}; double map_update_frequency_{0}; @@ -377,12 +360,11 @@ class Costmap2DROS : public nav2_util::LifecycleNode std::vector filter_names_; std::vector filter_types_; double resolution_{0}; - std::string robot_base_frame_; ///< The frame_id of the robot base + std::string robot_base_frame_; ///< The frame_id of the robot base double robot_radius_; - bool rolling_window_{false}; ///< Whether to use a rolling window version of the costmap + bool rolling_window_{false}; ///< Whether to use a rolling window version of the costmap bool track_unknown_space_{false}; - double transform_tolerance_{0}; ///< The timeout before transform errors - double initial_transform_timeout_{0}; ///< The timeout before activation of the node errors + double transform_tolerance_{0}; ///< The timeout before transform errors // Derived parameters bool use_radius_{false}; From b2884d67c78b08043f20e0fd72d2eb8dca104757 Mon Sep 17 00:00:00 2001 From: GoesM <130988564+GoesM@users.noreply.github.com> Date: Mon, 13 Nov 2023 04:03:50 +0800 Subject: [PATCH 16/33] Add files via upload --- nav2_costmap_2d/src/costmap_2d.cpp | 5 -- nav2_costmap_2d/src/costmap_2d_ros.cpp | 112 ++++--------------------- 2 files changed, 16 insertions(+), 101 deletions(-) diff --git a/nav2_costmap_2d/src/costmap_2d.cpp b/nav2_costmap_2d/src/costmap_2d.cpp index 0bb281e6bc1..e72807da59b 100644 --- a/nav2_costmap_2d/src/costmap_2d.cpp +++ b/nav2_costmap_2d/src/costmap_2d.cpp @@ -53,7 +53,6 @@ Costmap2D::Costmap2D( origin_y_(origin_y), costmap_(NULL), default_value_(default_value) { access_ = new mutex_t(); - free_access_ = new mutex_t();//only for UAF check // create the costmap initMaps(size_x_, size_y_); @@ -64,7 +63,6 @@ Costmap2D::Costmap2D(const nav_msgs::msg::OccupancyGrid & map) : default_value_(FREE_SPACE) { access_ = new mutex_t(); - free_access_ = new mutex_t();//only for UAF check // fill local variables size_x_ = map.info.width; @@ -238,7 +236,6 @@ Costmap2D::Costmap2D(const Costmap2D & map) : costmap_(NULL) { access_ = new mutex_t(); - free_access_ = new mutex_t();//only for UAF check *this = map; } @@ -247,14 +244,12 @@ Costmap2D::Costmap2D() : size_x_(0), size_y_(0), resolution_(0.0), origin_x_(0.0), origin_y_(0.0), costmap_(NULL) { access_ = new mutex_t(); - free_access_ = new mutex_t();//only for UAF check } Costmap2D::~Costmap2D() { deleteMaps(); delete access_; - delete free_access_;//only for UAF check } unsigned int Costmap2D::cellDistance(double world_dist) diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 7f1b6cf6889..66fb7756471 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -58,27 +58,13 @@ using rcl_interfaces::msg::ParameterType; namespace nav2_costmap_2d { -Costmap2DROS::Costmap2DROS(const std::string & name, const bool & use_sim_time) -: Costmap2DROS(name, "/", name, use_sim_time) {} - -Costmap2DROS::Costmap2DROS() -: nav2_util::LifecycleNode("costmap", ""), - name_("costmap"), - default_plugins_{"static_layer", "obstacle_layer", "inflation_layer"}, - default_types_{ - "nav2_costmap_2d::StaticLayer", - "nav2_costmap_2d::ObstacleLayer", - "nav2_costmap_2d::InflationLayer"} -{ - declare_parameter("map_topic", rclcpp::ParameterValue(std::string("map"))); - init(); -} +Costmap2DROS::Costmap2DROS(const std::string & name) +: Costmap2DROS(name, "/", name) {} Costmap2DROS::Costmap2DROS( const std::string & name, const std::string & parent_namespace, - const std::string & local_namespace, - const bool & use_sim_time) + const std::string & local_namespace) : nav2_util::LifecycleNode(name, "", // NodeOption arguments take precedence over the ones provided on the command line // use this to make sure the node is placed on the provided namespace @@ -87,8 +73,7 @@ Costmap2DROS::Costmap2DROS( rclcpp::NodeOptions().arguments({ "--ros-args", "-r", std::string("__ns:=") + nav2_util::add_namespaces(parent_namespace, local_namespace), - "--ros-args", "-r", name + ":" + std::string("__node:=") + name, - "--ros-args", "-p", "use_sim_time:=" + std::string(use_sim_time ? "true" : "false"), + "--ros-args", "-r", name + ":" + std::string("__node:=") + name })), name_(name), parent_namespace_(parent_namespace), @@ -97,14 +82,6 @@ Costmap2DROS::Costmap2DROS( "nav2_costmap_2d::StaticLayer", "nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::InflationLayer"} -{ - declare_parameter( - "map_topic", rclcpp::ParameterValue( - (parent_namespace_ == "/" ? "/" : parent_namespace_ + "/") + std::string("map"))); - init(); -} - -void Costmap2DROS::init() { RCLCPP_INFO(get_logger(), "Creating Costmap"); @@ -117,6 +94,9 @@ void Costmap2DROS::init() declare_parameter("height", rclcpp::ParameterValue(5)); declare_parameter("width", rclcpp::ParameterValue(5)); declare_parameter("lethal_cost_threshold", rclcpp::ParameterValue(100)); + declare_parameter( + "map_topic", rclcpp::ParameterValue( + (parent_namespace_ == "/" ? "/" : parent_namespace_ + "/") + std::string("map"))); declare_parameter("observation_sources", rclcpp::ParameterValue(std::string(""))); declare_parameter("origin_x", rclcpp::ParameterValue(0.0)); declare_parameter("origin_y", rclcpp::ParameterValue(0.0)); @@ -129,7 +109,6 @@ void Costmap2DROS::init() declare_parameter("rolling_window", rclcpp::ParameterValue(false)); declare_parameter("track_unknown_space", rclcpp::ParameterValue(false)); declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.3)); - declare_parameter("initial_transform_timeout", rclcpp::ParameterValue(60.0)); declare_parameter("trinary_costmap", rclcpp::ParameterValue(true)); declare_parameter("unknown_cost_value", rclcpp::ParameterValue(static_cast(0xff))); declare_parameter("update_frequency", rclcpp::ParameterValue(5.0)); @@ -145,13 +124,7 @@ nav2_util::CallbackReturn Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Configuring"); - try { - getParameters(); - } catch (const std::exception & e) { - RCLCPP_ERROR( - get_logger(), "Failed to configure costmap! %s.", e.what()); - return nav2_util::CallbackReturn::FAILURE; - } + getParameters(); callback_group_ = create_callback_group( rclcpp::CallbackGroupType::MutuallyExclusive, false); @@ -229,20 +202,6 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/) layered_costmap_->getCostmap(), global_frame_, "costmap", always_send_full_costmap_); - auto layers = layered_costmap_->getPlugins(); - - for (auto & layer : *layers) { - auto costmap_layer = std::dynamic_pointer_cast(layer); - if (costmap_layer != nullptr) { - layer_publishers_.emplace_back( - std::make_unique( - shared_from_this(), - costmap_layer.get(), global_frame_, - layer->getName(), always_send_full_costmap_) - ); - } - } - // Set the footprint if (use_radius_) { setRobotFootprint(makeFootprintFromRadius(robot_radius_)); @@ -266,6 +225,9 @@ Costmap2DROS::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); + costmap_publisher_->on_activate(); + footprint_pub_->on_activate(); + // First, make sure that the transform between the robot base frame // and the global frame is available @@ -273,9 +235,6 @@ Costmap2DROS::on_activate(const rclcpp_lifecycle::State & /*state*/) RCLCPP_INFO(get_logger(), "Checking transform"); rclcpp::Rate r(2); - const auto initial_transform_timeout = rclcpp::Duration::from_seconds( - initial_transform_timeout_); - const auto initial_transform_timeout_point = now() + initial_transform_timeout; while (rclcpp::ok() && !tf_buffer_->canTransform( global_frame_, robot_base_frame_, tf2::TimePointZero, &tf_error)) @@ -285,29 +244,12 @@ Costmap2DROS::on_activate(const rclcpp_lifecycle::State & /*state*/) " to become available, tf error: %s", robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str()); - // Check timeout - if (now() > initial_transform_timeout_point) { - RCLCPP_ERROR( - get_logger(), "Failed to activate %s because transform from %s to %s did not become available before timeout", - get_name(), robot_base_frame_.c_str(), global_frame_.c_str()); - - return nav2_util::CallbackReturn::FAILURE; - } - // The error string will accumulate and errors will typically be the same, so the last // will do for the warning above. Reset the string here to avoid accumulation tf_error.clear(); r.sleep(); } - // Activate publishers - footprint_pub_->on_activate(); - costmap_publisher_->on_activate(); - - for (auto & layer_pub : layer_publishers_) { - layer_pub->on_activate(); - } - // Create a thread to handle updating the map stopped_ = true; // to active plugins stop_updates_ = false; @@ -340,12 +282,8 @@ Costmap2DROS::on_deactivate(const rclcpp_lifecycle::State & /*state*/) map_update_thread_->join(); } - footprint_pub_->on_deactivate(); costmap_publisher_->on_deactivate(); - - for (auto & layer_pub : layer_publishers_) { - layer_pub->on_deactivate(); - } + footprint_pub_->on_deactivate(); return nav2_util::CallbackReturn::SUCCESS; } @@ -355,16 +293,8 @@ Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Cleaning up"); - costmap_publisher_.reset(); - clear_costmap_service_.reset(); - - layer_publishers_.clear(); - - //lock the layered_costmap because no ptr-free is allowed until other ptr-access finished - std::unique_lock lock(*(layered_costmap_->getCostmap()->getMutex_free_access())); + std::unique_lock lock(*(layered_costmap_->getCostmap()->getMutex())); layered_costmap_.reset(); - //unlock the layered_costmap_ to avoid pid stop - lock.unlock(); tf_listener_.reset(); tf_buffer_.reset(); @@ -372,6 +302,8 @@ Costmap2DROS::on_cleanup(const rclcpp_lifecycle::State & /*state*/) footprint_sub_.reset(); footprint_pub_.reset(); + costmap_publisher_.reset(); + clear_costmap_service_.reset(); executor_thread_.reset(); return nav2_util::CallbackReturn::SUCCESS; @@ -404,7 +336,6 @@ Costmap2DROS::getParameters() get_parameter("rolling_window", rolling_window_); get_parameter("track_unknown_space", track_unknown_space_); get_parameter("transform_tolerance", transform_tolerance_); - get_parameter("initial_transform_timeout", initial_transform_timeout_); get_parameter("update_frequency", map_update_frequency_); get_parameter("width", map_width_meters_); get_parameter("plugins", plugin_names_); @@ -514,22 +445,12 @@ Costmap2DROS::mapUpdateLoop(double frequency) layered_costmap_->getBounds(&x0, &xn, &y0, &yn); costmap_publisher_->updateBounds(x0, xn, y0, yn); - for (auto & layer_pub: layer_publishers_) { - layer_pub->updateBounds(x0, xn, y0, yn); - } - auto current_time = now(); if ((last_publish_ + publish_cycle_ < current_time) || // publish_cycle_ is due - (current_time < - last_publish_)) // time has moved backwards, probably due to a switch to sim_time // NOLINT + (current_time < last_publish_)) // time has moved backwards, probably due to a switch to sim_time // NOLINT { RCLCPP_DEBUG(get_logger(), "Publish costmap at %s", name_.c_str()); costmap_publisher_->publishCostmap(); - - for (auto & layer_pub: layer_publishers_) { - layer_pub->publishCostmap(); - } - last_publish_ = current_time; } } @@ -613,7 +534,6 @@ void Costmap2DROS::stop() { stop_updates_ = true; - // layered_costmap_ is set only if on_configure has been called if (layered_costmap_) { std::vector> * plugins = layered_costmap_->getPlugins(); From 1a6893806a88310d1fb54b6593e79e8d06f67f00 Mon Sep 17 00:00:00 2001 From: GoesM <130988564+GoesM@users.noreply.github.com> Date: Mon, 13 Nov 2023 04:23:55 +0800 Subject: [PATCH 17/33] Add files via upload --- .../include/nav2_costmap_2d/cost_values.hpp | 30 +---------- .../nav2_costmap_2d/costmap_2d_ros.hpp | 3 +- .../costmap_filters/binary_filter.hpp | 1 + .../costmap_filters/costmap_filter.hpp | 14 +---- .../costmap_filters/keepout_filter.hpp | 3 +- .../costmap_filters/speed_filter.hpp | 1 + .../include/nav2_costmap_2d/costmap_layer.hpp | 20 ------- .../denoise/image_processing.hpp | 53 ++++++++----------- .../include/nav2_costmap_2d/footprint.hpp | 5 +- .../nav2_costmap_2d/inflation_layer.hpp | 12 +---- .../nav2_costmap_2d/layered_costmap.hpp | 13 ++--- .../nav2_costmap_2d/obstacle_layer.hpp | 2 +- nav2_costmap_2d/src/clear_costmap_service.cpp | 6 +-- nav2_costmap_2d/src/costmap_2d_cloud.cpp | 5 -- nav2_costmap_2d/src/costmap_2d_node.cpp | 2 +- nav2_costmap_2d/src/costmap_2d_publisher.cpp | 2 +- nav2_costmap_2d/src/costmap_2d_ros.cpp | 6 ++- nav2_costmap_2d/src/costmap_layer.cpp | 49 ----------------- nav2_costmap_2d/src/footprint.cpp | 13 +++-- nav2_costmap_2d/src/layered_costmap.cpp | 16 ++---- 20 files changed, 59 insertions(+), 197 deletions(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/cost_values.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/cost_values.hpp index 7ae3f4d1c76..dca22c9f7ff 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/cost_values.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/cost_values.hpp @@ -39,38 +39,10 @@ /** Provides a mapping for often used cost values */ namespace nav2_costmap_2d { - -/** - * @enum nav2_costmap_2d::CombinationMethod - * @brief Describes the method used to add data to master costmap, default to maximum. - */ -enum class CombinationMethod : int -{ - /** - * Overwrite means every valid value from this layer - * is written into the master grid (does not copy NO_INFORMATION) - */ - Overwrite = 0, - /** - * Sets the new value to the maximum of the master_grid's value - * and this layer's value. If the master value is NO_INFORMATION, - * it is overwritten. If the layer's value is NO_INFORMATION, - * the master value does not change - */ - Max = 1, - /** - * Sets the new value to the maximum of the master_grid's value - * and this layer's value. If the master value is NO_INFORMATION, - * it is NOT overwritten. If the layer's value is NO_INFORMATION, - * the master value does not change. - */ - MaxWithoutUnknownOverwrite = 2 -}; - static constexpr unsigned char NO_INFORMATION = 255; static constexpr unsigned char LETHAL_OBSTACLE = 254; static constexpr unsigned char INSCRIBED_INFLATED_OBSTACLE = 253; static constexpr unsigned char MAX_NON_OBSTACLE = 252; static constexpr unsigned char FREE_SPACE = 0; -} // namespace nav2_costmap_2d +} #endif // NAV2_COSTMAP_2D__COST_VALUES_HPP_ 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 2e6a6d72764..8648bf3e305 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 @@ -156,7 +156,8 @@ class Costmap2DROS : public nav2_util::LifecycleNode /** @brief Same as getLayeredCostmap()->isCurrent(). */ bool isCurrent() { - std::unique_lock lock(*(layered_costmap_->getCostmap()->getMutex())); + std::unique_lock lock(*(costmap_.getCostmap()->getMutex())); + if(layered_costmap_==nullptr) return false; return layered_costmap_->isCurrent(); } diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/binary_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/binary_filter.hpp index 0a813607836..e36507ef8fb 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/binary_filter.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/binary_filter.hpp @@ -109,6 +109,7 @@ class BinaryFilter : public CostmapFilter nav_msgs::msg::OccupancyGrid::SharedPtr filter_mask_; + std::string mask_frame_; // Frame where mask located in std::string global_frame_; // Frame of currnet layer (master_grid) double base_, multiplier_; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp index d1128b5ccc6..49aeab21e38 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp @@ -43,7 +43,6 @@ #include #include -#include #include "geometry_msgs/msg/pose2_d.hpp" #include "std_srvs/srv/set_bool.hpp" @@ -212,17 +211,6 @@ class CostmapFilter : public Layer return filter_mask->data[my * filter_mask->info.width + mx]; } - /** - * @brief Get the cost of a cell in the filter mask - * @param filter_mask Filter mask to get the cost from - * @param mx The x coordinate of the cell - * @param my The y coordinate of the cell - * @return The cost to set the cell to - */ - unsigned char getMaskCost( - nav_msgs::msg::OccupancyGrid::ConstSharedPtr filter_mask, - const unsigned int mx, const unsigned int & my) const; - /** * @brief: Name of costmap filter info topic */ @@ -234,7 +222,7 @@ class CostmapFilter : public Layer std::string mask_topic_; /** - * @brief: mask_frame->global_frame_ transform tolerance + * @brief: mask_frame_->global_frame_ transform tolerance */ tf2::Duration transform_tolerance_; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp index 2d7d9bf4aa4..54159cf6242 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp @@ -99,8 +99,9 @@ class KeepoutFilter : public CostmapFilter rclcpp::Subscription::SharedPtr filter_info_sub_; rclcpp::Subscription::SharedPtr mask_sub_; - nav_msgs::msg::OccupancyGrid::SharedPtr filter_mask_; + std::unique_ptr mask_costmap_; + std::string mask_frame_; // Frame where mask located in std::string global_frame_; // Frame of currnet layer (master_grid) }; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/speed_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/speed_filter.hpp index c9fdf9f0fa7..3ad4b9ba272 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/speed_filter.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/speed_filter.hpp @@ -103,6 +103,7 @@ class SpeedFilter : public CostmapFilter nav_msgs::msg::OccupancyGrid::SharedPtr filter_mask_; + std::string mask_frame_; // Frame where mask located in std::string global_frame_; // Frame of currnet layer (master_grid) double base_, multiplier_; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp index 43a82e0cde1..15285df5fe2 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_layer.hpp @@ -128,19 +128,6 @@ class CostmapLayer : public Layer, public Costmap2D nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, int max_j); - /* - * Updates the master_grid within the specified - * bounding box using this layer's values. - * - * Sets the new value to the maximum of the master_grid's value - * and this layer's value. If the master value is NO_INFORMATION, - * it is NOT overwritten. If the layer's value is NO_INFORMATION, - * the master value does not change. - */ - void updateWithMaxWithoutUnknownOverwrite( - nav2_costmap_2d::Costmap2D & master_grid, int min_i, int min_j, int max_i, - int max_j); - /* * Updates the master_grid within the specified * bounding box using this layer's values. @@ -185,13 +172,6 @@ class CostmapLayer : public Layer, public Costmap2D void useExtraBounds(double * min_x, double * min_y, double * max_x, double * max_y); bool has_extra_bounds_; - /** - * @brief Converts an integer to a CombinationMethod enum and logs on failure - * @param value The integer to convert - * @param function_name The name of the function calling this conversion (for logging) - */ - CombinationMethod combination_method_from_int(const int value); - private: double extra_min_x_, extra_max_x_, extra_min_y_, extra_max_y_; }; diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp index 99d24240d2c..ee798948475 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/denoise/image_processing.hpp @@ -21,8 +21,6 @@ #include #include #include -#include -#include namespace nav2_costmap_2d { @@ -79,7 +77,7 @@ void morphologyOperation( const Image & input, Image & output, const Image & shape, AggregateFn aggregate); -using ShapeBuffer3x3 = std::array; // NOLINT +using ShapeBuffer3x3 = std::array; inline Image createShape(ShapeBuffer3x3 & buffer, ConnectivityType connectivity); } // namespace imgproc_impl @@ -97,7 +95,7 @@ inline void dilate( const Image & input, Image & output, ConnectivityType connectivity, Max && max_function) { - using namespace imgproc_impl; // NOLINT + using namespace imgproc_impl; ShapeBuffer3x3 shape_buffer; Image shape = createShape(shape_buffer, connectivity); morphologyOperation(input, output, shape, max_function); @@ -378,7 +376,7 @@ struct EquivalenceLabelTreesBase struct LabelOverflow : public std::runtime_error { - explicit LabelOverflow(const std::string & message) + LabelOverflow(const std::string & message) : std::runtime_error(message) {} }; @@ -466,6 +464,7 @@ class EquivalenceLabelTrees : public EquivalenceLabelTreesBase { Label k = 1; for (Label i = 1; i < next_free_; ++i) { + if (labels_[i] < i) { labels_[i] = labels_[labels_[i]]; } else { @@ -505,7 +504,7 @@ class EquivalenceLabelTrees : public EquivalenceLabelTreesBase * '~' - row continuation in the same style */ max_labels = (rows * columns) / 3 + 1; } - ++max_labels; // add zero label + ++max_labels; // add zero label max_labels = std::min(max_labels, size_t(std::numeric_limits