fix: check predicted footprints for LETHAL_OBSTACLE instead of INSCRIBED_INFLATED_OBSTACLE

src/path_tracking_pid_local_planner.cpp

@@ -220,14 +220,14 @@ bool TrackingPidLocalPlanner::computeVelocityCommands(geometry_msgs::Twist& cmd_
   {
     auto cost = projectedCollisionCost();
 
-    if (cost >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
+    if (cost >= costmap_2d::LETHAL_OBSTACLE)
     {
       pid_controller_.setVelMaxObstacle(0.0);
     }
     else if (pid_controller_.getConfig().obstacle_speed_reduction)
     {
       double max_vel = pid_controller_.getConfig().max_x_vel;
-      double reduction_factor = static_cast<double>(cost) / costmap_2d::INSCRIBED_INFLATED_OBSTACLE;
+      double reduction_factor = static_cast<double>(cost) / costmap_2d::LETHAL_OBSTACLE;
       double limit = max_vel * (1 - reduction_factor);
       ROS_DEBUG("Cost: %d, factor: %f, limit: %f", cost, reduction_factor, limit);
       pid_controller_.setVelMaxObstacle(limit);
@@ -456,14 +456,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);
 
@@ -492,7 +504,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;
@@ -533,8 +544,9 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost()
         mkCollisionIndicator.color.a = cell_cost / 255.0;
         point.z = mkCollisionIndicator.scale.z * 0.5;
         mkCollisionIndicator.pose.position = point;
-        if (max_cost >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE)
+        if (max_cost >= costmap_2d::LETHAL_OBSTACLE)
         {
+          max_projected_step_cost = max_cost;
           break;  // Collision detected, no need to evaluate further
         }
       }
@@ -565,7 +577,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,

test/test_path_tracking_pid.py

@@ -62,6 +62,10 @@ def test_exepath_action(self):
             initialpose_pub.publish(pose)
             rospy.sleep(0.1)  # Fill tf buffers
 
+        self.max_tracking_error_linear_x = rospy.get_param("~max_tracking_error_linear_x", 0.1)
+        self.max_tracking_error_linear_y = rospy.get_param("~max_tracking_error_linear_y", 0.1)
+        self.max_tracking_error_angular_z = rospy.get_param("~max_tracking_error_angular_z", 0.1)
+
         # Publisher for obstacles:
         self.obstacle_pub = rospy.Publisher("pointcloud", PointCloud, latch=True, queue_size=1)
         reconfigure = ReconfigureClient("/move_base_flex/PathTrackingPID", timeout=5)
@@ -109,9 +113,9 @@ def test_exepath_action(self):
             return
 
         # Check the errors
-        self.assertLess(error_catcher.error.linear.x, 0.1)
-        self.assertLess(error_catcher.error.linear.y, 0.1)
-        self.assertLess(error_catcher.error.angular.z, 0.1)
+        self.assertLess(error_catcher.error.linear.x, self.max_tracking_error_linear_x)
+        self.assertLess(error_catcher.error.linear.y, self.max_tracking_error_linear_y)
+        self.assertLess(error_catcher.error.angular.z, self.max_tracking_error_angular_z)
 
         # Do the same for backward movements if last path was a success
         if client.get_state() != GS.SUCCEEDED or rospy.get_param("backward", True):

test/test_path_tracking_pid.test

@@ -49,6 +49,7 @@
     <test test-name="rostest_path_lethal_obstacle" pkg="path_tracking_pid" type="test_path_tracking_pid.py" time-limit="300.0" >
         <rosparam param="obstacles">[[10.0, 10.0]]</rosparam>
         <rosparam param="outcome">4</rosparam>  <!-- cancelled -->
+        <rosparam param="max_tracking_error_linear_y">1.0</rosparam> <!-- not interested in tracking error for this test -->
     </test>
     <test test-name="rostest_path_close_obstacle" pkg="path_tracking_pid" type="test_path_tracking_pid.py" time-limit="300.0" >
         <rosparam param="obstacles">[[15.0, -8.0]]</rosparam>