Improvements in RemoveInCollisionGoals and adjacent features (#4676)

* improvements

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* ament_uncrustify

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Fix formatting

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* fix building

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* fixes

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

* add stamp

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

---------

Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com>

nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp

@@ -52,17 +52,23 @@ class RemoveInCollisionGoals : public BtServiceNode<nav2_msgs::srv::GetCosts>
 
   static BT::PortsList providedPorts()
   {
-    return providedBasicPorts({
+    return providedBasicPorts(
+      {
         BT::InputPort<Goals>("input_goals", "Original goals to remove from"),
-        BT::InputPort<double>("cost_threshold", 254.0,
+        BT::InputPort<double>(
+          "cost_threshold", 254.0,
           "Cost threshold for considering a goal in collision"),
         BT::InputPort<bool>("use_footprint", true, "Whether to use footprint cost"),
+        BT::InputPort<bool>(
+          "consider_unknown_as_obstacle", false,
+          "Whether to consider unknown cost as obstacle"),
         BT::OutputPort<Goals>("output_goals", "Goals with in-collision goals removed"),
-    });
+      });
   }
 
 private:
   bool use_footprint_;
+  bool consider_unknown_as_obstacle_;
   double cost_threshold_;
   Goals input_goals_;
 };

nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp

@@ -37,6 +37,7 @@ void RemoveInCollisionGoals::on_tick()
   getInput("use_footprint", use_footprint_);
   getInput("cost_threshold", cost_threshold_);
   getInput("input_goals", input_goals_);
+  getInput("consider_unknown_as_obstacle", consider_unknown_as_obstacle_);
 
   if (input_goals_.empty()) {
     setOutput("output_goals", input_goals_);
@@ -47,11 +48,7 @@ void RemoveInCollisionGoals::on_tick()
   request_->use_footprint = use_footprint_;
 
   for (const auto & goal : input_goals_) {
-    geometry_msgs::msg::Pose2D pose;
-    pose.x = goal.pose.position.x;
-    pose.y = goal.pose.position.y;
-    pose.theta = tf2::getYaw(goal.pose.orientation);
-    request_->poses.push_back(pose);
+    request_->poses.push_back(goal);
   }
 }
 
@@ -60,10 +57,18 @@ BT::NodeStatus RemoveInCollisionGoals::on_completion(
 {
   Goals valid_goal_poses;
   for (size_t i = 0; i < response->costs.size(); ++i) {
-    if (response->costs[i] < cost_threshold_) {
+    if ((response->costs[i] == 255 && !consider_unknown_as_obstacle_) ||
+      response->costs[i] < cost_threshold_)
+    {
       valid_goal_poses.push_back(input_goals_[i]);
     }
   }
+  // Inform if all goals have been removed
+  if (valid_goal_poses.empty()) {
+    RCLCPP_INFO(
+      node_->get_logger(),
+      "All goals are in collision and have been removed from the list");
+  }
   setOutput("output_goals", valid_goal_poses);
   return BT::NodeStatus::SUCCESS;
 }

nav2_costmap_2d/src/costmap_2d_ros.cpp

@@ -254,7 +254,8 @@ Costmap2DROS::on_configure(const rclcpp_lifecycle::State & /*state*/)
   // Service to get the cost at a point
   get_cost_service_ = create_service<nav2_msgs::srv::GetCosts>(
     "get_cost_" + getName(),
-    std::bind(&Costmap2DROS::getCostsCallback, this, std::placeholders::_1, std::placeholders::_2,
+    std::bind(
+      &Costmap2DROS::getCostsCallback, this, std::placeholders::_1, std::placeholders::_2,
       std::placeholders::_3));
 
   // Add cleaning service
@@ -835,26 +836,35 @@ void Costmap2DROS::getCostsCallback(
   Costmap2D * costmap = layered_costmap_->getCostmap();
 
   for (const auto & pose : request->poses) {
-    bool in_bounds = costmap->worldToMap(pose.x, pose.y, mx, my);
+    geometry_msgs::msg::PoseStamped pose_transformed;
+    transformPoseToGlobalFrame(pose, pose_transformed);
+    bool in_bounds = costmap->worldToMap(
+      pose_transformed.pose.position.x,
+      pose_transformed.pose.position.y, mx, my);
 
     if (!in_bounds) {
-      response->costs.push_back(-1.0);
+      response->costs.push_back(NO_INFORMATION);
       continue;
     }
+    double yaw = tf2::getYaw(pose_transformed.pose.orientation);
 
     if (request->use_footprint) {
       Footprint footprint = layered_costmap_->getFootprint();
       FootprintCollisionChecker<Costmap2D *> collision_checker(costmap);
 
       RCLCPP_DEBUG(
         get_logger(), "Received request to get cost at footprint pose (%.2f, %.2f, %.2f)",
-        pose.x, pose.y, pose.theta);
+        pose_transformed.pose.position.x, pose_transformed.pose.position.y, yaw);
 
       response->costs.push_back(
-        collision_checker.footprintCostAtPose(pose.x, pose.y, pose.theta, footprint));
+        collision_checker.footprintCostAtPose(
+          pose_transformed.pose.position.x,
+          pose_transformed.pose.position.y, yaw, footprint));
     } else {
       RCLCPP_DEBUG(
-        get_logger(), "Received request to get cost at point (%f, %f)", pose.x, pose.y);
+        get_logger(), "Received request to get cost at point (%f, %f)",
+          pose_transformed.pose.position.x,
+        pose_transformed.pose.position.y);
 
       // Get the cost at the map coordinates
       response->costs.push_back(static_cast<float>(costmap->getCost(mx, my)));

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<std::vector<geometry_msgs::msg::Point>>())
 {
   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);
   }
 }
 

nav2_costmap_2d/test/unit/costmap_cost_service_test.cpp

@@ -50,9 +50,9 @@ class GetCostServiceTest : public ::testing::Test
 TEST_F(GetCostServiceTest, TestWithoutFootprint)
 {
   auto request = std::make_shared<nav2_msgs::srv::GetCosts::Request>();
-  geometry_msgs::msg::Pose2D pose;
-  pose.x = 0.5;
-  pose.y = 1.0;
+  geometry_msgs::msg::PoseStamped pose;
+  pose.pose.position.x = 0.5;
+  pose.pose.position.y = 1.0;
   request->poses.push_back(pose);
   request->use_footprint = false;
 
@@ -72,10 +72,12 @@ TEST_F(GetCostServiceTest, TestWithoutFootprint)
 TEST_F(GetCostServiceTest, TestWithFootprint)
 {
   auto request = std::make_shared<nav2_msgs::srv::GetCosts::Request>();
-  geometry_msgs::msg::Pose2D pose;
-  pose.x = 0.5;
-  pose.y = 1.0;
-  pose.theta = 0.5;
+  geometry_msgs::msg::PoseStamped pose;
+  pose.pose.position.x = 0.5;
+  pose.pose.position.y = 1.0;
+  tf2::Quaternion q;
+  q.setRPY(0, 0, 0.5);
+  pose.pose.orientation = tf2::toMsg(q);
   request->poses.push_back(pose);
   request->use_footprint = true;
 

nav2_msgs/srv/GetCosts.srv

@@ -1,6 +1,6 @@
 # Get costmap costs at given poses
 
 bool use_footprint
-geometry_msgs/Pose2D[] poses
+geometry_msgs/PoseStamped[] poses
 ---
 float32[] costs

nav2_rviz_plugins/src/costmap_cost_tool.cpp

@@ -94,23 +94,28 @@ int CostmapCostTool::processMouseEvent(rviz_common::ViewportMouseEvent & event)
 
 void CostmapCostTool::callCostService(float x, float y)
 {
+  rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node();
   // Create request for local costmap
   auto request = std::make_shared<nav2_msgs::srv::GetCosts::Request>();
-  geometry_msgs::msg::Pose2D pose;
-  pose.x = x;
-  pose.y = y;
+  geometry_msgs::msg::PoseStamped pose;
+  pose.header.frame_id = context_->getFixedFrame().toStdString();
+  pose.header.stamp = node->now();
+  pose.pose.position.x = x;
+  pose.pose.position.y = y;
   request->poses.push_back(pose);
   request->use_footprint = false;
 
   // Call local costmap service
   if (local_cost_client_->wait_for_service(std::chrono::seconds(1))) {
-    local_cost_client_->async_send_request(request,
+    local_cost_client_->async_send_request(
+      request,
       std::bind(&CostmapCostTool::handleLocalCostResponse, this, std::placeholders::_1));
   }
 
   // Call global costmap service
   if (global_cost_client_->wait_for_service(std::chrono::seconds(1))) {
-    global_cost_client_->async_send_request(request,
+    global_cost_client_->async_send_request(
+      request,
       std::bind(&CostmapCostTool::handleGlobalCostResponse, this, std::placeholders::_1));
   }
 }
@@ -120,23 +125,15 @@ void CostmapCostTool::handleLocalCostResponse(
 {
   rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node();
   auto response = future.get();
-  if (response->costs[0] != -1) {
-    RCLCPP_INFO(node->get_logger(), "Local costmap cost: %.1f", response->costs[0]);
-  } else {
-    RCLCPP_ERROR(node->get_logger(), "Failed to get local costmap cost");
-  }
+  RCLCPP_INFO(node->get_logger(), "Local costmap cost: %.1f", response->costs[0]);
 }
 
 void CostmapCostTool::handleGlobalCostResponse(
   rclcpp::Client<nav2_msgs::srv::GetCosts>::SharedFuture future)
 {
   rclcpp::Node::SharedPtr node = node_ptr_->get_raw_node();
   auto response = future.get();
-  if (response->costs[0] != -1) {
-    RCLCPP_INFO(node->get_logger(), "Global costmap cost: %.1f", response->costs[0]);
-  } else {
-    RCLCPP_ERROR(node->get_logger(), "Failed to get global costmap cost");
-  }
+  RCLCPP_INFO(node->get_logger(), "Global costmap cost: %.1f", response->costs[0]);
 }
 }  // namespace nav2_rviz_plugins