From e82ce1aa69ee189c4637913702714407d11fd63b Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Wed, 2 Feb 2022 13:08:02 +0100 Subject: [PATCH 1/4] fix: check predicted footprints for LETHAL_OBSTACLE instead of INSCRIBED_INFLATED_OBSTACLE --- src/path_tracking_pid_local_planner.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index fd4f0177..52ed3b23 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/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(cost) / costmap_2d::INSCRIBED_INFLATED_OBSTACLE; + double reduction_factor = static_cast(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); @@ -533,7 +533,7 @@ 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) { break; // Collision detected, no need to evaluate further } From 05ee28a38603a442fdd83dcefb26c36bc3a29d29 Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Wed, 2 Feb 2022 17:02:50 +0100 Subject: [PATCH 2/4] fix: make velocity scaledown consider costmap only at predicted base_link --- src/path_tracking_pid_local_planner.cpp | 18 +++++++++++++++--- 1 file changed, 15 insertions(+), 3 deletions(-) diff --git a/src/path_tracking_pid_local_planner.cpp b/src/path_tracking_pid_local_planner.cpp index 52ed3b23..a9f452be 100644 --- a/src/path_tracking_pid_local_planner.cpp +++ b/src/path_tracking_pid_local_planner.cpp @@ -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 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 collision_polygon_hull_map; @@ -535,6 +546,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 } } @@ -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, From 3cc2b532c32f7c8a8cacdb4f58606aff52e48f1e Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Thu, 3 Feb 2022 11:48:41 +0100 Subject: [PATCH 3/4] test: make tracking error assessment configurable --- test/test_path_tracking_pid.py | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/test/test_path_tracking_pid.py b/test/test_path_tracking_pid.py index 7c5a407d..aa4efb8e 100755 --- a/test/test_path_tracking_pid.py +++ b/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): From 12f8c95b990dd59eb1b049991e2a6c347fa30e2d Mon Sep 17 00:00:00 2001 From: Rokus Ottervanger Date: Thu, 3 Feb 2022 11:49:49 +0100 Subject: [PATCH 4/4] test: disregard final tracking error in lethal obstacle test --- test/test_path_tracking_pid.test | 1 + 1 file changed, 1 insertion(+) diff --git a/test/test_path_tracking_pid.test b/test/test_path_tracking_pid.test index 84d67483..46a14c88 100644 --- a/test/test_path_tracking_pid.test +++ b/test/test_path_tracking_pid.test @@ -49,6 +49,7 @@ [[10.0, 10.0]] 4 + 1.0 [[15.0, -8.0]]