From 8562a7884b0b8274e45d668a6db666b3461d7b80 Mon Sep 17 00:00:00 2001 From: schupf2 Date: Tue, 17 Dec 2024 14:45:14 +0100 Subject: [PATCH 1/6] Only publish the costmap if it's current. Also provide an additional function in the costmap subscriber, to get the timestamp of the last received costmap Signed-off-by: schupf2 --- .../include/nav2_costmap_2d/costmap_subscriber.hpp | 4 ++++ nav2_costmap_2d/src/costmap_2d_ros.cpp | 10 +++++++--- nav2_costmap_2d/src/costmap_subscriber.cpp | 12 ++++++++++++ 3 files changed, 23 insertions(+), 3 deletions(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp index 4f50614876c..e252d9c889d 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp @@ -56,6 +56,10 @@ class CostmapSubscriber * @brief Get current costmap */ std::shared_ptr getCostmap(); + /** + * @brief Get the timestamp of the last costmap update + */ + rclcpp::Time getTimestampLastCostmapUpdate(); /** * @brief Callback for the costmap topic */ diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 98f5cbbef0c..be35e18b304 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -535,9 +535,10 @@ Costmap2DROS::mapUpdateLoop(double frequency) } 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 + auto map_current = isCurrent(); + if (map_current && // only update costmap if it's current + ((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 { RCLCPP_DEBUG(get_logger(), "Publish costmap at %s", name_.c_str()); costmap_publisher_->publishCostmap(); @@ -548,6 +549,9 @@ Costmap2DROS::mapUpdateLoop(double frequency) last_publish_ = current_time; } + else if (!map_current) { + RCLCPP_WARN(get_logger(), "Costmap was not published because it`s not current"); + } } } diff --git a/nav2_costmap_2d/src/costmap_subscriber.cpp b/nav2_costmap_2d/src/costmap_subscriber.cpp index 0426b87be30..bdba597309f 100644 --- a/nav2_costmap_2d/src/costmap_subscriber.cpp +++ b/nav2_costmap_2d/src/costmap_subscriber.cpp @@ -68,6 +68,18 @@ std::shared_ptr CostmapSubscriber::getCostmap() return costmap_; } +rclcpp::Time CostmapSubscriber::getTimestampLastCostmapUpdate() +{ + if (!costmap_received_) { + throw std::runtime_error("Costmap is not available"); + } + + std::lock_guard lock(costmap_msg_mutex_); + auto stamp = costmap_msg_->header.stamp; + + return stamp; +} + void CostmapSubscriber::costmapCallback(const nav2_msgs::msg::Costmap::SharedPtr msg) { { From 154707d8322461e3a9c8fbcd26c1e3f35b4ada74 Mon Sep 17 00:00:00 2001 From: schupf2 Date: Tue, 17 Dec 2024 14:45:47 +0100 Subject: [PATCH 2/6] Fix obstacle layer reporting current after reset even if it is not current Signed-off-by: schupf2 --- nav2_costmap_2d/plugins/obstacle_layer.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/nav2_costmap_2d/plugins/obstacle_layer.cpp b/nav2_costmap_2d/plugins/obstacle_layer.cpp index 51c018ae427..c5d95970b0a 100644 --- a/nav2_costmap_2d/plugins/obstacle_layer.cpp +++ b/nav2_costmap_2d/plugins/obstacle_layer.cpp @@ -539,10 +539,9 @@ ObstacleLayer::updateCosts( return; } - // if not current due to reset, set current now after clearing + // set was_reset to false again if (!current_ && was_reset_) { was_reset_ = false; - current_ = true; } if (footprint_clearing_enabled_) { @@ -758,7 +757,6 @@ void ObstacleLayer::reset() { resetMaps(); - resetBuffersLastUpdated(); current_ = false; was_reset_ = true; } From 886d9c8743451b743ea36c2e899a7aaab7b987dc Mon Sep 17 00:00:00 2001 From: schupf2 Date: Tue, 17 Dec 2024 16:25:44 +0100 Subject: [PATCH 3/6] Small fix in getTimestampLastCostmapUpdate Signed-off-by: schupf2 --- nav2_costmap_2d/src/costmap_subscriber.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_costmap_2d/src/costmap_subscriber.cpp b/nav2_costmap_2d/src/costmap_subscriber.cpp index bdba597309f..f401b1eec43 100644 --- a/nav2_costmap_2d/src/costmap_subscriber.cpp +++ b/nav2_costmap_2d/src/costmap_subscriber.cpp @@ -70,7 +70,7 @@ std::shared_ptr CostmapSubscriber::getCostmap() rclcpp::Time CostmapSubscriber::getTimestampLastCostmapUpdate() { - if (!costmap_received_) { + if (!isCostmapReceived()) { throw std::runtime_error("Costmap is not available"); } From 66c4b65f3a0e425cd5e9960af81b7edf117e1ee1 Mon Sep 17 00:00:00 2001 From: schupf2 Date: Wed, 18 Dec 2024 10:28:27 +0100 Subject: [PATCH 4/6] Fix coding style error Signed-off-by: schupf2 --- nav2_costmap_2d/src/costmap_2d_ros.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index be35e18b304..3bae6c6e9b8 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -548,8 +548,7 @@ Costmap2DROS::mapUpdateLoop(double frequency) } last_publish_ = current_time; - } - else if (!map_current) { + } else if (!map_current) { RCLCPP_WARN(get_logger(), "Costmap was not published because it`s not current"); } } From 049caa55ebf344f042686d2e9609fda6bc931f97 Mon Sep 17 00:00:00 2001 From: schupf2 Date: Wed, 18 Dec 2024 11:13:40 +0100 Subject: [PATCH 5/6] Remove unused API function. Signed-off-by: schupf2 --- .../include/nav2_costmap_2d/costmap_subscriber.hpp | 4 ---- nav2_costmap_2d/src/costmap_subscriber.cpp | 12 ------------ 2 files changed, 16 deletions(-) diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp index e252d9c889d..4f50614876c 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp @@ -56,10 +56,6 @@ class CostmapSubscriber * @brief Get current costmap */ std::shared_ptr getCostmap(); - /** - * @brief Get the timestamp of the last costmap update - */ - rclcpp::Time getTimestampLastCostmapUpdate(); /** * @brief Callback for the costmap topic */ diff --git a/nav2_costmap_2d/src/costmap_subscriber.cpp b/nav2_costmap_2d/src/costmap_subscriber.cpp index f401b1eec43..0426b87be30 100644 --- a/nav2_costmap_2d/src/costmap_subscriber.cpp +++ b/nav2_costmap_2d/src/costmap_subscriber.cpp @@ -68,18 +68,6 @@ std::shared_ptr CostmapSubscriber::getCostmap() return costmap_; } -rclcpp::Time CostmapSubscriber::getTimestampLastCostmapUpdate() -{ - if (!isCostmapReceived()) { - throw std::runtime_error("Costmap is not available"); - } - - std::lock_guard lock(costmap_msg_mutex_); - auto stamp = costmap_msg_->header.stamp; - - return stamp; -} - void CostmapSubscriber::costmapCallback(const nav2_msgs::msg::Costmap::SharedPtr msg) { { From 71d710e195a957e594b926db1db49c5e0d9c7dfd Mon Sep 17 00:00:00 2001 From: schupf2 Date: Wed, 18 Dec 2024 12:49:19 +0100 Subject: [PATCH 6/6] Additional coding style fix. Signed-off-by: schupf2 --- 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 3bae6c6e9b8..011efb23639 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -538,7 +538,7 @@ Costmap2DROS::mapUpdateLoop(double frequency) auto map_current = isCurrent(); if (map_current && // only update costmap if it's current ((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();