Skip to content

Fix Misplaced Footprint on Static Layer #4911

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 2 commits into
base: main
Choose a base branch
from
Open

Fix Misplaced Footprint on Static Layer #4911

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
8552eec
Fix misplaced footprint on static layer
HovorunBh Feb 10, 2025
08b9cb0
Fix build
HovorunBh Feb 10, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 4 additions & 1 deletion nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand Down Expand Up @@ -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_;
Expand Down
22 changes: 16 additions & 6 deletions nav2_costmap_2d/plugins/static_layer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -130,7 +130,6 @@
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));
Expand Down Expand Up @@ -159,15 +158,14 @@
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(
Expand Down Expand Up @@ -387,6 +385,18 @@
{
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_,

Check warning on line 392 in nav2_costmap_2d/plugins/static_layer.cpp

View check run for this annotation

Codecov / codecov/patch

nav2_costmap_2d/plugins/static_layer.cpp#L391-L392 

Added lines #L391 - L392 were not covered by tests
map_frame_, robot_base_frame_, transform_tolerance_
);
robot_x = robot_pose.pose.position.x;
robot_y = robot_pose.pose.position.y;

Check warning on line 396 in nav2_costmap_2d/plugins/static_layer.cpp

View check run for this annotation

Codecov / codecov/patch

nav2_costmap_2d/plugins/static_layer.cpp#L395-L396 

Added lines #L395 - L396 were not covered by tests
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++) {
Expand Down Expand Up @@ -436,7 +446,7 @@
try {
transform = tf_->lookupTransform(
map_frame_, global_frame_, tf2::TimePointZero,
transform_tolerance_);
tf2::durationFromSec(transform_tolerance_));

Check warning on line 449 in nav2_costmap_2d/plugins/static_layer.cpp

View check run for this annotation

Codecov / codecov/patch

nav2_costmap_2d/plugins/static_layer.cpp#L449 

Added line #L449 was not covered by tests
} catch (tf2::TransformException & ex) {
RCLCPP_ERROR(logger_, "StaticLayer: %s", ex.what());
return;
Expand Down Expand Up @@ -495,7 +505,7 @@
"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()) {
Expand Down
Loading