From ec88241772e980d7fb6f2dd07ca2e9290c099279 Mon Sep 17 00:00:00 2001 From: Tony Najjar Date: Mon, 16 Sep 2024 16:10:38 +0200 Subject: [PATCH] fixes Signed-off-by: Tony Najjar --- .../action/remove_in_collision_goals_action.cpp | 12 ++---------- nav2_costmap_2d/src/costmap_2d_ros.cpp | 2 +- nav2_costmap_2d/src/layered_costmap.cpp | 8 ++++---- 3 files changed, 7 insertions(+), 15 deletions(-) diff --git a/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp b/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp index 9f30189972d..d6f9ee662d0 100644 --- a/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp +++ b/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp @@ -55,14 +55,6 @@ void RemoveInCollisionGoals::on_tick() BT::NodeStatus RemoveInCollisionGoals::on_completion( std::shared_ptr response) { - for (size_t i = 0; i < input_goals_.size(); ++i) { - double yaw = tf2::getYaw(input_goals_[i].pose.orientation); - RCLCPP_INFO( - node_->get_logger(), - "Goal %ld: x: %f, y: %f, yaw: %f, costs: %f", i, - input_goals_[i].pose.position.x, input_goals_[i].pose.position.y, yaw, response->costs[i]); - } - Goals valid_goal_poses; for (size_t i = 0; i < response->costs.size(); ++i) { if ((response->costs[i] == 255 && !consider_unknown_as_obstacle_) || @@ -71,9 +63,9 @@ BT::NodeStatus RemoveInCollisionGoals::on_completion( valid_goal_poses.push_back(input_goals_[i]); } } - // Warn if all goals have been removed + // Inform if all goals have been removed if (valid_goal_poses.empty()) { - RCLCPP_WARN( + RCLCPP_INFO( node_->get_logger(), "All goals are in collision and have been removed from the list"); } diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 5ab99418057..fa11b60ebfe 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -843,7 +843,7 @@ void Costmap2DROS::getCostsCallback( pose_transformed.pose.position.y, mx, my); if (!in_bounds) { - response->costs.push_back(costmap->getDefaultValue()); + response->costs.push_back(NO_INFORMATION); continue; } double yaw = tf2::getYaw(pose_transformed.pose.orientation); diff --git a/nav2_costmap_2d/src/layered_costmap.cpp b/nav2_costmap_2d/src/layered_costmap.cpp index 33e9fc22aa5..2995ae03029 100644 --- a/nav2_costmap_2d/src/layered_costmap.cpp +++ b/nav2_costmap_2d/src/layered_costmap.cpp @@ -72,11 +72,11 @@ LayeredCostmap::LayeredCostmap(std::string global_frame, bool rolling_window, bo footprint_(std::make_shared>()) { if (track_unknown) { - primary_costmap_.setDefaultValue(255); - combined_costmap_.setDefaultValue(255); + primary_costmap_.setDefaultValue(NO_INFORMATION); + combined_costmap_.setDefaultValue(NO_INFORMATION); } else { - primary_costmap_.setDefaultValue(0); - combined_costmap_.setDefaultValue(0); + primary_costmap_.setDefaultValue(FREE_SPACE); + combined_costmap_.setDefaultValue(FREE_SPACE); } }