From 8552eecc0f16cfc91efadaa30afe4d63f412cf0e Mon Sep 17 00:00:00 2001 From: HovorunBh Date: Mon, 10 Feb 2025 11:01:30 +0100 Subject: [PATCH 1/2] Fix misplaced footprint on static layer Signed-off-by: HovorunBh --- .../include/nav2_costmap_2d/static_layer.hpp | 5 ++++- nav2_costmap_2d/plugins/static_layer.cpp | 22 ++++++++++++++----- 2 files changed, 20 insertions(+), 7 deletions(-) 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 c938f30f55e..79c602a3115 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp @@ -49,6 +49,8 @@ #include "nav_msgs/msg/occupancy_grid.hpp" #include "rclcpp/rclcpp.hpp" #include "nav2_costmap_2d/footprint.hpp" +#include "nav2_util/robot_utils.hpp" +#include "tf2/utils.hpp" namespace nav2_costmap_2d { @@ -197,7 +199,8 @@ class StaticLayer : public CostmapLayer bool trinary_costmap_; bool map_received_{false}; bool map_received_in_update_bounds_{false}; - tf2::Duration transform_tolerance_; + double transform_tolerance_; + std::string robot_base_frame_; nav_msgs::msg::OccupancyGrid::SharedPtr map_buffer_; // Dynamic parameters handler rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr dyn_params_handler_; diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index 9f0a69f80ed..34149666c2f 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -130,7 +130,6 @@ void StaticLayer::getParameters() { int temp_lethal_threshold = 0; - double temp_tf_tol = 0.0; declareParameter("enabled", rclcpp::ParameterValue(true)); declareParameter("subscribe_to_updates", rclcpp::ParameterValue(false)); @@ -159,15 +158,14 @@ StaticLayer::getParameters() node->get_parameter("lethal_cost_threshold", temp_lethal_threshold); node->get_parameter("unknown_cost_value", unknown_cost_value_); node->get_parameter("trinary_costmap", trinary_costmap_); - node->get_parameter("transform_tolerance", temp_tf_tol); + node->get_parameter("transform_tolerance", transform_tolerance_); + node->get_parameter("robot_base_frame", robot_base_frame_); // Enforce bounds lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0); map_received_ = false; map_received_in_update_bounds_ = false; - transform_tolerance_ = tf2::durationFromSec(temp_tf_tol); - // Add callback for dynamic parameters dyn_params_handler_ = node->add_on_set_parameters_callback( std::bind( @@ -387,6 +385,18 @@ StaticLayer::updateFootprint( { if (!footprint_clearing_enabled_) {return;} + if (map_frame_.empty()) {return;} + if (map_frame != global_frame_) { + geometry_msgs::msg::PoseStamped robot_pose; + nav2_util::getCurrentPose( + robot_pose, *tf_, + map_frame_, robot_base_frame_, transform_tolerance_ + ); + robot_x = robot_pose.pose.position.x; + robot_y = robot_pose.pose.position.y; + robot_yaw = tf2::getYaw(robot_pose.pose.orientation); + } + transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(), transformed_footprint_); for (unsigned int i = 0; i < transformed_footprint_.size(); i++) { @@ -436,7 +446,7 @@ StaticLayer::updateCosts( try { transform = tf_->lookupTransform( map_frame_, global_frame_, tf2::TimePointZero, - transform_tolerance_); + tf2::durationFromSec(transform_tolerance_)); } catch (tf2::TransformException & ex) { RCLCPP_ERROR(logger_, "StaticLayer: %s", ex.what()); return; @@ -495,7 +505,7 @@ StaticLayer::dynamicParametersCallback( "cannot be changed while running. Rejecting parameter update.", param_name.c_str()); } else if (param_type == ParameterType::PARAMETER_DOUBLE) { if (param_name == name_ + "." + "transform_tolerance") { - transform_tolerance_ = tf2::durationFromSec(parameter.as_double()); + transform_tolerance_ = parameter.as_double(); } } else if (param_type == ParameterType::PARAMETER_BOOL) { if (param_name == name_ + "." + "enabled" && enabled_ != parameter.as_bool()) { From 08b9cb09d76a01c4463bb6f87bf896bfae032100 Mon Sep 17 00:00:00 2001 From: HovorunBh Date: Mon, 10 Feb 2025 11:20:25 +0100 Subject: [PATCH 2/2] Fix build Signed-off-by: HovorunBh --- nav2_costmap_2d/plugins/static_layer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index 34149666c2f..49934aa0b8a 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -386,7 +386,7 @@ StaticLayer::updateFootprint( if (!footprint_clearing_enabled_) {return;} if (map_frame_.empty()) {return;} - if (map_frame != global_frame_) { + if (map_frame_ != global_frame_) { geometry_msgs::msg::PoseStamped robot_pose; nav2_util::getCurrentPose( robot_pose, *tf_,