fix: make velocity scaledown consider costmap only at predicted base_…

…link

src/path_tracking_pid_local_planner.cpp

@@ -450,14 +450,26 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost()
     mkPosesOnPath.points.push_back(mkPointOnPath);
   }
 
+  costmap_2d::Costmap2D* costmap2d = costmap_->getCostmap();
   polygon_t previous_footprint_xy;
   polygon_t collision_polygon;
+  uint8_t max_projected_step_cost = 0;
   for (const auto& projection_tf : projected_steps_tf)
   {
-    // Project footprint forward
     double x = projection_tf.getOrigin().x();
     double y = projection_tf.getOrigin().y();
     double yaw = tf2::getYaw(projection_tf.getRotation());
+
+    // Calculate cost by checking base link location in costmap
+    int map_x, map_y;
+    costmap2d->worldToMapEnforceBounds(x, y, map_x, map_y);
+    uint8_t projected_step_cost = costmap2d->getCost(map_x, map_y);
+    if (projected_step_cost > max_projected_step_cost)
+    {
+      max_projected_step_cost = projected_step_cost;
+    }
+
+    // Project footprint forward
     std::vector<geometry_msgs::Point> footprint;
     costmap_2d::transformFootprint(x, y, yaw, costmap_->getRobotFootprint(), footprint);
 
@@ -486,7 +498,6 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost()
   }
 
   // Create a convex hull so we can use costmap2d->convexFillCells
-  costmap_2d::Costmap2D* costmap2d = costmap_->getCostmap();
   polygon_t collision_polygon_hull;
   boost::geometry::convex_hull(collision_polygon, collision_polygon_hull);
   std::vector<costmap_2d::MapLocation> collision_polygon_hull_map;
@@ -529,6 +540,7 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost()
         mkCollisionIndicator.pose.position = point;
         if (max_cost >= costmap_2d::LETHAL_OBSTACLE)
         {
+          max_projected_step_cost = max_cost;
           break;  // Collision detected, no need to evaluate further
         }
       }
@@ -559,7 +571,7 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost()
   }
   collision_marker_pub_.publish(mkCollision);
 
-  return max_cost;
+  return max_projected_step_cost;
 }
 
 uint32_t TrackingPidLocalPlanner::computeVelocityCommands(const geometry_msgs::PoseStamped& pose,