Merge pull request #20 from nobleo/fix/controller-is-too-conservative…

…-in-its-collision-checking

fix: check predicted footprints for LETHAL_OBSTACLE instead of INSCRIBED_INFLATED_OBSTACLE

src/path_tracking_pid_local_planner.cpp

@@ -244,11 +244,11 @@ std::optional<geometry_msgs::Twist> TrackingPidLocalPlanner::computeVelocityComm
   if (pid_controller_.getConfig().anti_collision) {
     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);
@@ -341,14 +341,27 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost()
     poses_on_path_points.push_back(projected_step_tf.getOrigin());
   }
 
+  costmap_2d::Costmap2D* costmap2d = costmap_->getCostmap();
   std::vector<tf2::Vector3> collision_footprint_points;
   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);
 
@@ -375,7 +388,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;
@@ -407,7 +419,8 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost()
         max_cost = cell_cost;
         // Set collision indicator on suspected cell with current cost
         collision_point = tf2_convert<tf2::Vector3>(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
         }
       }
@@ -430,7 +443,7 @@ uint8_t TrackingPidLocalPlanner::projectedCollisionCost()
   visualization_->publishCollisionFootprint(header, collision_footprint_points);
   visualization_->publishCollisionPolygon(header, collision_hull_points);
 
-  return max_cost;
+  return max_projected_step_cost;
 }
 
 uint32_t TrackingPidLocalPlanner::computeVelocityCommands(

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>