compute lookahead as distance along path (#55)

graceful_controller_ros/CMakeLists.txt

@@ -76,6 +76,7 @@ if(CATKIN_ENABLE_TESTING)
   target_link_libraries(graceful_controller_tests
     ${catkin_LIBRARIES}
     ${GTEST_LIBRARIES}
+    graceful_controller_ros
   )
   add_dependencies(tests graceful_controller_tests)
   add_rostest(test/graceful_controller.test)

graceful_controller_ros/include/graceful_controller_ros/graceful_controller_ros.hpp

@@ -180,6 +180,15 @@ class GracefulControllerROS : public nav_core::BaseLocalPlanner
   geometry_msgs::PoseStamped robot_pose_;
 };
 
+/**
+ * @brief Compute distance of poses along a path. Assumes poses are in robot-centric frame.
+ * @param poses The poses that form the path.
+ * @param distances Computed distance for each pose from the robot, along the path.
+ *                  Returned by reference.
+ */
+void computeDistanceAlongPath(const std::vector<geometry_msgs::PoseStamped>& poses,
+                              std::vector<double>& distances);
+
 }  // namespace graceful_controller
 
 #endif  // GRACEFUL_CONTROLLER_ROS_GRACEFUL_CONTROLLER_ROS_HPP

graceful_controller_ros/src/graceful_controller_ros.cpp

@@ -400,20 +400,29 @@ bool GracefulControllerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_ve
     max_vel_x = std::max(max_vel_x, min_vel_x_);
   }
 
+  // Compute distance along path
+  std::vector<geometry_msgs::PoseStamped> target_poses;
+  std::vector<double> target_distances;
+  for (auto pose : transformed_plan)
+  {
+    // Transform potential target pose into base_link
+    geometry_msgs::PoseStamped transformed_pose;
+    tf2::doTransform(pose, transformed_pose, costmap_to_robot);
+    target_poses.push_back(transformed_pose);
+  }
+  computeDistanceAlongPath(target_poses, target_distances);
+
   // Work back from the end of plan to find valid target pose
   for (int i = transformed_plan.size() - 1; i >= 0; --i)
   {
     // Underlying control law needs a single target pose, which should:
     //  * Be as far away as possible from the robot (for smoothness)
     //  * But no further than the max_lookahed_ distance
     //  * Be feasible to reach in a collision free manner
-    geometry_msgs::PoseStamped target_pose;
-
-    // Transform potential target pose into base_link
-    tf2::doTransform(transformed_plan[i], target_pose, costmap_to_robot);
+    geometry_msgs::PoseStamped target_pose = target_poses[i];
+    double dist_to_target = target_distances[i];
 
     // Continue if target_pose is too far away from robot
-    double dist_to_target = std::hypot(target_pose.pose.position.x, target_pose.pose.position.y);
     if (dist_to_target > max_lookahead_)
     {
       continue;
@@ -749,6 +758,31 @@ void GracefulControllerROS::velocityCallback(const std_msgs::Float32::ConstPtr&
   max_vel_x_ = std::max(static_cast<double>(max_vel_x->data), min_vel_x_);
 }
 
+void computeDistanceAlongPath(const std::vector<geometry_msgs::PoseStamped>& poses,
+                              std::vector<double>& distances)
+{
+  distances.resize(poses.size());
+
+  // First compute distance from robot to pose
+  for (size_t i = 0; i < poses.size(); ++i)
+  {
+    // Determine distance from robot to pose
+    distances[i] = std::hypot(poses[i].pose.position.x, poses[i].pose.position.y);
+  }
+
+  // Find the closest target pose
+  auto closest = std::min_element(std::begin(distances), std::end(distances));
+
+  // Sum distances between poses, starting with the closest pose
+  // Yes, the poses behind the robot will still use euclidean distance from robot, but we don't use those anyways
+  for (size_t i = std::distance(std::begin(distances), closest) + 1; i < distances.size(); ++i)
+  {
+    distances[i] = distances[i - 1] +
+                   std::hypot(poses[i].pose.position.x - poses[i - 1].pose.position.x,
+                              poses[i].pose.position.y - poses[i - 1].pose.position.y);
+  }
+}
+
 }  // namespace graceful_controller
 
 #include <pluginlib/class_list_macros.h>

graceful_controller_ros/test/graceful_controller_tests.cpp

@@ -47,6 +47,9 @@
 #include <tf2_ros/transform_broadcaster.h>
 #include <tf2/utils.h>
 
+// Only needed for computeDistanceAlongPath tests
+#include <graceful_controller_ros/graceful_controller_ros.hpp>
+
 class ControllerFixture
 {
 public:
@@ -447,6 +450,55 @@ TEST(ControllerTests, test_collision_check)
   EXPECT_FALSE(controller->computeVelocityCommands(command));
 }
 
+TEST(ControllerTests, test_compute_distance_along_path)
+{
+  std::vector<geometry_msgs::PoseStamped> poses;
+  std::vector<double> distances;
+
+  // Simple set of poses
+  for (int i = 0; i < 5; ++i)
+  {
+    geometry_msgs::PoseStamped pose;
+    pose.pose.position.x = (i - 2);
+    pose.pose.position.y = 0;
+    poses.push_back(pose);
+  }
+
+  // Check distances
+  graceful_controller::computeDistanceAlongPath(poses, distances);
+  EXPECT_EQ(distances[0], 2);
+  EXPECT_EQ(distances[1], 1);
+  EXPECT_EQ(distances[2], 0);
+  EXPECT_EQ(distances[3], 1);
+  EXPECT_EQ(distances[4], 2);
+
+  // Make path wrap around
+  distances.clear();
+  {
+    geometry_msgs::PoseStamped pose;
+    pose.pose.position.x = 2;
+    pose.pose.position.y = 1;
+    poses.push_back(pose);
+    pose.pose.position.x = 1;
+    pose.pose.position.y = 1;
+    poses.push_back(pose);
+    pose.pose.position.x = 0;
+    pose.pose.position.y = 1;
+    poses.push_back(pose);
+  }
+
+  // Check distances
+  graceful_controller::computeDistanceAlongPath(poses, distances);
+  EXPECT_EQ(distances[0], 2);
+  EXPECT_EQ(distances[1], 1);
+  EXPECT_EQ(distances[2], 0);
+  EXPECT_EQ(distances[3], 1);
+  EXPECT_EQ(distances[4], 2);
+  EXPECT_EQ(distances[5], 3);
+  EXPECT_EQ(distances[6], 4);
+  EXPECT_EQ(distances[7], 5);
+}
+
 int main(int argc, char** argv)
 {
   ros::init(argc, argv, "graceful_controller_tests");