PR fixes

Signed-off-by: Tony Najjar <[email protected]>
ros-navigation · Aug 6, 2024 · e11923a · e11923a
1 parent 82e95fc
commit e11923a
Show file tree

Hide file tree

Showing 3 changed files with 45 additions and 46 deletions.
diff --git a/...avior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp b/...avior_tree/include/nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.hpp
@@ -45,10 +45,14 @@ class RemoveInCollisionGoals : public BT::ActionNodeBase
   {
     return {
       BT::InputPort<Goals>("input_goals", "Original goals to remove from"),
-      BT::InputPort<std::string>("costmap",
-          "Which costmap to use for checking collision. Choices: local, global, both"),
-      BT::InputPort<std::string>("cost_threshold",
+      BT::InputPort<std::string>("costmap_cost_service",
+          "Service to get cost from costmap"),
+      BT::InputPort<std::string>("costmap_cost_service_2",
+          "Second service name to get cost from costmap. If empty, only first service is used"),
+      BT::InputPort<double>("cost_threshold",
           "Cost threshold for considering a goal in collision"),
+      BT::InputPort<bool>("use_footprint",
+          "Whether to use footprint cost"),
       BT::OutputPort<Goals>("output_goals", "Goals with in-collision goals removed"),
     };
   }
@@ -58,11 +62,13 @@ class RemoveInCollisionGoals : public BT::ActionNodeBase
   BT::NodeStatus tick() override;
 
   rclcpp::Node::SharedPtr node_;
+  rclcpp::Client<nav2_msgs::srv::GetCost>::SharedPtr get_cost_client_;
+  rclcpp::Client<nav2_msgs::srv::GetCost>::SharedPtr get_cost_client_2_;
+  std::string costmap_cost_service_;
+  std::string costmap_cost_service_2_;
   bool initialized_;
-  std::string which_costmap_;
+  bool use_footprint_;
   double cost_threshold_;
-  rclcpp::Client<nav2_msgs::srv::GetCost>::SharedPtr get_global_cost_client_;
-  rclcpp::Client<nav2_msgs::srv::GetCost>::SharedPtr get_local_cost_client_;
   std::chrono::milliseconds server_timeout_;
 };
 

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
@@ -29,18 +29,27 @@ RemoveInCollisionGoals::RemoveInCollisionGoals(
   const BT::NodeConfiguration & conf)
 : BT::ActionNodeBase(name, conf),
   initialized_(false),
-  which_costmap_("both"),
+  costmap_cost_service_("/global_costmap/get_cost_global_costmap"),
+  costmap_cost_service_2_(""),
+  use_footprint_(true),
   cost_threshold_(253)
 {}
 
 void RemoveInCollisionGoals::initialize()
 {
   node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
-  get_global_cost_client_ = node_->create_client<nav2_msgs::srv::GetCost>(
-    "global_costmap/get_cost_global_costmap");
-  get_local_cost_client_ = node_->create_client<nav2_msgs::srv::GetCost>(
-    "local_costmap/get_cost_local_costmap");
   server_timeout_ = config().blackboard->template get<std::chrono::milliseconds>("server_timeout");
+
+  getInput("costmap_cost_service", costmap_cost_service_);
+  getInput("costmap_cost_service_2", costmap_cost_service_2_);
+
+  get_cost_client_ = node_->create_client<nav2_msgs::srv::GetCost>(
+    costmap_cost_service_);
+
+  if (!costmap_cost_service_2_.empty()) {
+    get_cost_client_2_ = node_->create_client<nav2_msgs::srv::GetCost>(
+      costmap_cost_service_2_);
+  }
 }
 
 inline BT::NodeStatus RemoveInCollisionGoals::tick()
@@ -51,7 +60,7 @@ inline BT::NodeStatus RemoveInCollisionGoals::tick()
     initialize();
   }
 
-  getInput("costmap", which_costmap_);
+  getInput("use_footprint", use_footprint_);
   getInput("cost_threshold", cost_threshold_);
 
   Goals goal_poses;
@@ -68,58 +77,41 @@ inline BT::NodeStatus RemoveInCollisionGoals::tick()
     request->x = goal.pose.position.x;
     request->y = goal.pose.position.y;
     request->theta = tf2::getYaw(goal.pose.orientation);
-    request->use_footprint = true;
+    request->use_footprint = use_footprint_;
 
-    if (which_costmap_ == "global") {
-      auto future_global = get_global_cost_client_->async_send_request(request);
-      auto ret_global = rclcpp::spin_until_future_complete(node_, future_global, server_timeout_);
-      if (ret_global == rclcpp::FutureReturnCode::SUCCESS) {
-        if (future_global.get()->cost <= cost_threshold_) {
+    if (costmap_cost_service_2_.empty()) {
+      auto future = get_cost_client_->async_send_request(request);
+      auto ret = rclcpp::spin_until_future_complete(node_, future, server_timeout_);
+      if (ret == rclcpp::FutureReturnCode::SUCCESS) {
+        if (future.get()->cost <= cost_threshold_) {
           valid_goal_poses.push_back(goal);
         }
       } else {
         RCLCPP_ERROR(
           node_->get_logger(),
-          "RemoveInCollisionGoals BT node failed to call GetCost service of global costmap");
+          "RemoveInCollisionGoals BT node failed to call GetCost service of costmap");
         return BT::NodeStatus::FAILURE;
       }
-    } else if (which_costmap_ == "local") {
-      auto future_local = get_local_cost_client_->async_send_request(request);
-      auto ret_local = rclcpp::spin_until_future_complete(node_, future_local, server_timeout_);
-      if (ret_local == rclcpp::FutureReturnCode::SUCCESS) {
-        if (future_local.get()->cost <= cost_threshold_) {
-          valid_goal_poses.push_back(goal);
-        }
-      } else {
-        RCLCPP_ERROR(
-          node_->get_logger(),
-          "RemoveInCollisionGoals BT node failed to call GetCost service of local costmap");
-        return BT::NodeStatus::FAILURE;
-      }
-    } else if (which_costmap_ == "both") {
-      auto future_global = get_global_cost_client_->async_send_request(request);
-      auto future_local = get_local_cost_client_->async_send_request(request);
-      auto ret_global = rclcpp::spin_until_future_complete(node_, future_global, server_timeout_);
-      auto ret_local = rclcpp::spin_until_future_complete(node_, future_local, server_timeout_);
-      if (ret_local == rclcpp::FutureReturnCode::SUCCESS &&
-        ret_global == rclcpp::FutureReturnCode::SUCCESS)
+    } else {
+      auto future = get_cost_client_->async_send_request(request);
+      auto future_2 = get_cost_client_2_->async_send_request(request);
+      auto ret = rclcpp::spin_until_future_complete(node_, future, server_timeout_);
+      auto ret_2 = rclcpp::spin_until_future_complete(node_, future_2, server_timeout_);
+      if (ret == rclcpp::FutureReturnCode::SUCCESS &&
+        ret_2 == rclcpp::FutureReturnCode::SUCCESS)
       {
-        if (future_local.get()->cost <= cost_threshold_ &&
-          future_global.get()->cost <= cost_threshold_)
+        if (future.get()->cost <= cost_threshold_ &&
+          future_2.get()->cost <= cost_threshold_)
         {
           valid_goal_poses.push_back(goal);
         }
       } else {
         RCLCPP_ERROR(
           node_->get_logger(),
           "RemoveInCollisionGoals BT node failed to call GetCost "
-          "service of local or global costmap");
+          "service of one or two costmaps");
         return BT::NodeStatus::FAILURE;
       }
-    } else {
-      RCLCPP_ERROR(
-        node_->get_logger(), "The costmap parameter must be either 'local', 'global', or 'both'");
-      return BT::NodeStatus::FAILURE;
     }
   }
   setOutput("output_goals", valid_goal_poses);

diff --git a/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp b/nav2_behavior_tree/plugins/action/remove_passed_goals_action.cpp
@@ -43,6 +43,7 @@ void RemovePassedGoals::initialize()
 
   robot_base_frame_ = BT::deconflictPortAndParamFrame<std::string>(
     node, "robot_base_frame", this);
+  initialized_ = true;
 }
 
 inline BT::NodeStatus RemovePassedGoals::tick()