improvements

Signed-off-by: Tony Najjar <[email protected]>

nav2_behavior_tree/plugins/action/remove_in_collision_goals_action.cpp

@@ -55,6 +55,13 @@ void RemoveInCollisionGoals::on_tick()
 BT::NodeStatus RemoveInCollisionGoals::on_completion(
   std::shared_ptr<nav2_msgs::srv::GetCosts::Response> response)
 {
+  if (response->costs.size() != input_goals_.size()) {
+    RCLCPP_ERROR(
+      node_->get_logger(),
+      "Received incorrect number of costs from GetCost service");
+    return BT::NodeStatus::FAILURE;
+  }
+
   Goals valid_goal_poses;
   for (size_t i = 0; i < response->costs.size(); ++i) {
     if ((response->costs[i] == 255 && !consider_unknown_as_obstacle_) ||

nav2_costmap_2d/src/costmap_2d_ros.cpp

@@ -837,14 +837,11 @@ void Costmap2DROS::getCostsCallback(
 
   for (const auto & pose : request->poses) {
     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(NO_INFORMATION);
-      continue;
+    if (!transformPoseToGlobalFrame(pose, pose_transformed)) {
+      response->costs.clear();
+      RCLCPP_ERROR(
+        get_logger(), "Failed to transform, returning empty costs");
+      return;
     }
     double yaw = tf2::getYaw(pose_transformed.pose.orientation);
 
@@ -866,6 +863,14 @@ void Costmap2DROS::getCostsCallback(
           pose_transformed.pose.position.x,
         pose_transformed.pose.position.y);
 
+      bool in_bounds = costmap->worldToMap(
+        pose_transformed.pose.position.x,
+        pose_transformed.pose.position.y, mx, my);
+
+      if (!in_bounds) {
+        response->costs.push_back(LETHAL_OBSTACLE);
+        continue;
+      }
       // Get the cost at the map coordinates
       response->costs.push_back(static_cast<float>(costmap->getCost(mx, my)));
     }