Extend the dwb_local_planner with a split_path option

dwb_local_planner/README.md

@@ -72,7 +72,7 @@ virtual bool isGoalReached(const geometry_msgs::Pose2D& query_pose, const geomet
 
  * `void initialize(const ros::NodeHandle& planner_nh, std::string name, nav_core2::Costmap::Ptr costmap)` - called once on startup, and then calls `onInit`
  * `void onInit()` - May be overwritten to load parameters as needed.
- * `void reset()` - called at the beginning of every new navigation, i.e. when we get a new global goal via `setGoalPose`.
+ * `void reset()` - called at the beginning of every new navigation, i.e. when we get a new global plan.
  * `bool prepare(const geometry_msgs::Pose2D& pose, const nav_2d_msgs::Twist2D& vel, const geometry_msgs::Pose2D& goal, const nav_2d_msgs::Path2D& global_plan)` - called once per iteration of the planner, prior to the evaluation of all the trajectories
  * `double scoreTrajectory(const dwb_msgs::Trajectory2D& traj)` - called once per trajectory
  * `void debrief(const nav_2d_msgs::Twist2D& cmd_vel)` - called after all the trajectories to notify what trajectory was chosen.

dwb_local_planner/include/dwb_local_planner/dwb_local_planner.h

@@ -171,8 +171,21 @@ class DWBLocalPlanner: public nav_core2::LocalPlanner
    */
   geometry_msgs::Pose2D transformPoseToLocal(const nav_2d_msgs::Pose2DStamped& pose);
 
-  nav_2d_msgs::Path2D global_plan_;  ///< Saved Global Plan
+
+  /**
+   * @brief Reset the critics and other plugins, e.g. when getting a new plan,
+   *        or starting the next path segment.
+   */
+  virtual void resetPlugins();
+
+  std::vector<nav_2d_msgs::Path2D> global_plan_segments_;   ///< Path segments of same movement direction
+                                                            // (forward/backward/in-place rotation)
+  nav_2d_msgs::Path2D global_plan_;   ///< The currently active segment of the plan, or the whole plan if
+                                      // path segmentation is disabled.
+
   nav_2d_msgs::Pose2DStamped goal_pose_;  ///< Saved Goal Pose
+  nav_2d_msgs::Pose2DStamped intermediate_goal_pose_;   ///< Goal of current path segment
+
   bool prune_plan_;
   double prune_distance_;
   bool debug_trajectory_details_;

dwb_local_planner/include/dwb_local_planner/trajectory_critic.h

@@ -104,7 +104,7 @@ class TrajectoryCritic
   /**
    * @brief Reset the state of the critic
    *
-   * Reset is called when the planner receives a new global goal.
+   * Reset is called when the planner receives a new global plan.
    * This can be used to discard information specific to one plan.
    */
   virtual void reset() {}

dwb_local_planner/src/dwb_local_planner.cpp

@@ -37,6 +37,7 @@
 #include <dwb_local_planner/backwards_compatibility.h>
 #include <dwb_local_planner/illegal_trajectory_tracker.h>
 #include <nav_2d_utils/conversions.h>
+#include <nav_2d_utils/path_ops.h>
 #include <nav_2d_utils/tf_help.h>
 #include <nav_2d_msgs/Twist2D.h>
 #include <dwb_msgs/CriticScore.h>
@@ -148,11 +149,37 @@ bool DWBLocalPlanner::isGoalReached(const nav_2d_msgs::Pose2DStamped& pose, cons
 
   // Update time stamp of goal pose
   goal_pose_.header.stamp = pose.header.stamp;
+  intermediate_goal_pose_.header.stamp = pose.header.stamp;
 
-  bool ret = goal_checker_->isGoalReached(transformPoseToLocal(pose), transformPoseToLocal(goal_pose_), velocity);
+  // use the goal_checker_ to check if the intermediate goal was reached.
+  bool ret = goal_checker_->isGoalReached(
+              transformPoseToLocal(pose),
+              transformPoseToLocal(intermediate_goal_pose_),
+              velocity);
   if (ret)
   {
-    ROS_INFO_THROTTLE_NAMED(1.0, "DWBLocalPlanner", "Goal reached!");
+    if (global_plan_segments_.empty())
+      ROS_INFO_THROTTLE_NAMED(1.0, "DWBLocalPlanner", "Goal reached!");
+    else
+    {
+      ROS_INFO_THROTTLE_NAMED(1.0, "DWBLocalPlanner", "Intermediate goal reached!");
+      ret = false;   // reset to false, as we only finished one segment.
+
+      // activate next path segment
+      global_plan_ = global_plan_segments_[0];
+      global_plan_segments_.erase(global_plan_segments_.begin());
+      // set the next goal pose
+      intermediate_goal_pose_.header = global_plan_.header;
+      intermediate_goal_pose_.pose = global_plan_.poses.back();
+
+      // publish the next path segment
+      pub_.publishGlobalPlan(global_plan_);
+
+      // reset critics etc, as we changed the global_plan_
+      resetPlugins();
+      // prepare(...) will be called soon, automatically, when computing the
+      // velocity commands.
+    }
   }
   return ret;
 }
@@ -161,6 +188,48 @@ void DWBLocalPlanner::setGoalPose(const nav_2d_msgs::Pose2DStamped& goal_pose)
 {
   ROS_INFO_NAMED("DWBLocalPlanner", "New Goal Received.");
   goal_pose_ = goal_pose;
+  intermediate_goal_pose_ = goal_pose;   // For now assume that the path will not
+                                         // be split. Else the intermediate goal
+                                         // will be reset in setPlan.
+}
+
+void DWBLocalPlanner::setPlan(const nav_2d_msgs::Path2D& path)
+{
+  bool split_path;
+  planner_nh_.param("split_path", split_path, false);
+
+  if (split_path)
+  {
+    ROS_INFO_NAMED("DWBLocalPlanner", "Splitting path...");
+    global_plan_segments_ = nav_2d_utils::splitPlan(path);
+    ROS_INFO_STREAM("Split path into " << global_plan_segments_.size() << " segments.");
+  }
+  else
+  {
+    // split_path option is disabled, hence there is only one segment, which
+    // is the complete path.
+    global_plan_segments_.clear();
+    global_plan_segments_.push_back(path);
+  }
+
+  // set the global_plan_ to be the first segment
+  global_plan_ = global_plan_segments_[0];
+  global_plan_segments_.erase(global_plan_segments_.begin());
+
+  // publish not the complete path, but only the first segment.
+  pub_.publishGlobalPlan(global_plan_);
+
+  // set the intermediate goal
+  intermediate_goal_pose_.header = global_plan_.header;
+  intermediate_goal_pose_.pose = global_plan_.poses.back();
+
+  resetPlugins();
+  // prepare(...) to initialize the critics for the new segment does not need
+  // to be called here, as it is called at every computeVelocityCommands-call.
+}
+
+void DWBLocalPlanner::resetPlugins()
+{
   traj_generator_->reset();
   goal_checker_->reset();
   for (TrajectoryCritic::Ptr critic : critics_)
@@ -169,12 +238,6 @@ void DWBLocalPlanner::setGoalPose(const nav_2d_msgs::Pose2DStamped& goal_pose)
   }
 }
 
-void DWBLocalPlanner::setPlan(const nav_2d_msgs::Path2D& path)
-{
-  pub_.publishGlobalPlan(path);
-  global_plan_ = path;
-}
-
 nav_2d_msgs::Twist2DStamped DWBLocalPlanner::computeVelocityCommands(const nav_2d_msgs::Pose2DStamped& pose,
                                                                      const nav_2d_msgs::Twist2D& velocity)
 {
@@ -209,9 +272,10 @@ void DWBLocalPlanner::prepare(const nav_2d_msgs::Pose2DStamped& pose, const nav_
 
   // Update time stamp of goal pose
   goal_pose_.header.stamp = pose.header.stamp;
+  intermediate_goal_pose_.header.stamp = pose.header.stamp;
 
   geometry_msgs::Pose2D local_start_pose = transformPoseToLocal(pose),
-                        local_goal_pose = transformPoseToLocal(goal_pose_);
+                        local_goal_pose = transformPoseToLocal(intermediate_goal_pose_);
 
   pub_.publishInputParams(costmap_->getInfo(), local_start_pose, velocity, local_goal_pose);
 

nav_2d_utils/CMakeLists.txt

@@ -65,6 +65,9 @@ if(CATKIN_ENABLE_TESTING)
 
   add_rostest_gtest(param_tests test/param_tests.launch test/param_tests.cpp)
   target_link_libraries(param_tests polygons ${catkin_LIBRARIES} ${GTEST_LIBRARIES})
+
+  catkin_add_gtest(split_plan_test test/split_plan_test.cpp)
+  target_link_libraries(split_plan_test path_ops ${catkin_LIBRARIES})
 endif()
 
 install(TARGETS conversions path_ops polygons

nav_2d_utils/include/nav_2d_utils/path_ops.h

@@ -36,6 +36,7 @@
 #define NAV_2D_UTILS_PATH_OPS_H
 
 #include <nav_2d_msgs/Path2D.h>
+#include <vector>
 
 namespace nav_2d_utils
 {
@@ -83,6 +84,35 @@ nav_2d_msgs::Path2D compressPlan(const nav_2d_msgs::Path2D& input_path, double e
  */
 void addPose(nav_2d_msgs::Path2D& path, double x, double y, double theta = 0.0);
 
+/**
+ * @brief Split the plan into segments when it is switching between going forwards / backwards / pure rotation
+ *
+ * Global planners like SBPL might create trajectories with complex
+ * maneuvers, switching between driving forwards and backwards, where it is
+ * crucial that the individual segments are carefully followed and completed
+ * before starting the next. This function splits the given path into such
+ * segments, so that they can be independently treated as separate plans.
+
+ * The segmentation is computed as follows:
+ * Given two poses in the path, we compute the vector v1 connecting them,
+ * the vector of orientation given through the angle theta, check the dot
+ * product of the vectors: If it is less than 0, the angle is over 90 deg,
+ * which is a coarse approximation for "driving backwards", and for "driving
+ * forwards" otherwise. In the special case that the vector connecting the
+ * two poses is null we have to deal with in-place-rotations of the robot.
+ * To avoid the robot from eagerly driving forward before having achieved the
+ * correct orientation, these situations are grouped in a third category,
+ * "pure rotation". The path is then split into segments of the same movement
+ * direction (forward/backward/pureRotation). When a cut is made, the last pose of the
+ * last segment is copied to be the first pose of the following segment in order to
+ * ensure a seamless path.
+ *
+ * @param global_plan_in input plan
+ * @param epsilon maximum linear translation threshold for "pure rotation" segments
+ * @return vector of plan segments
+ */
+std::vector<nav_2d_msgs::Path2D> splitPlan(const nav_2d_msgs::Path2D &global_plan_in, double epsilon = 1e-5);
+
 }  // namespace nav_2d_utils
 
 #endif  // NAV_2D_UTILS_PATH_OPS_H

nav_2d_utils/src/path_ops.cpp

@@ -189,4 +189,94 @@ void addPose(nav_2d_msgs::Path2D& path, double x, double y, double theta)
   pose.theta = theta;
   path.poses.push_back(pose);
 }
+
+std::vector<nav_2d_msgs::Path2D> splitPlan(const nav_2d_msgs::Path2D &global_plan_in, double epsilon)
+{
+  auto copy = global_plan_in;
+  std::vector<nav_2d_msgs::Path2D> global_plan_segments;   // Path segments of same movement direction
+                                                           // (forward/backward/in-place rotation)
+
+  while (copy.poses.size() > 1)   // need at least 2 poses in a path
+  {
+    // start a new segment
+    nav_2d_msgs::Path2D segment;
+    segment.header = global_plan_in.header;
+
+    // add the first pose
+    segment.poses.push_back(copy.poses[0]);
+    copy.poses.erase(copy.poses.begin());
+
+    // add the second pose and determine if we are going forward or backward
+    segment.poses.push_back(copy.poses[0]);
+    copy.poses.erase(copy.poses.begin());
+
+    // take the vector from the first to the second position and compare it
+    // to the orientation vector at the first position. If the angle is > 90 deg,
+    // assume we are driving backwards.
+    double v1[2] =
+        {
+            segment.poses[1].x - segment.poses[0].x,
+            segment.poses[1].y - segment.poses[0].y
+        };
+    double v2[2] =
+        {
+            cos(segment.poses[0].theta),
+            sin(segment.poses[0].theta)
+        };
+    double d = v1[0] * v2[0] + v1[1] * v2[1];   // dot product of the vectors. if < 0, the angle is over 90 degrees.
+    bool backwards = (d < 0);
+    bool pureRotation = fabs(v1[0]) < epsilon && fabs(v1[1]) < epsilon;
+    // if the translation vector is zero, the two positions are equal
+    // and the plan is to rotate on the spot. Since dwb would
+    // enthusiastically speed up at the start of a trajectory,
+    // even if it starts with in-place-rotation, it would
+    // miss the path by a lot. So let's split the path into
+    // forwards / backwards / pureRotation.
+
+    // add more poses while they lead into the same direction (driving forwards/backwards/pureRotation)
+    while (!copy.poses.empty())
+    {
+      // vector from the last pose in the segment to the next in the path
+      double v1[2] =
+          {
+              copy.poses[0].x - segment.poses.back().x,
+              copy.poses[0].y - segment.poses.back().y
+          };
+      // orientation at the last pose in the segment
+      double v2[2] =
+          {
+              cos(segment.poses.back().theta),
+              sin(segment.poses.back().theta)
+          };
+      double d = v1[0] * v2[0] + v1[1] * v2[1];   // dot product of the vectors. if < 0, the angle is over 90 degrees.
+      bool b = (d < 0);
+      bool rot = fabs(v1[0]) < epsilon && fabs(v1[1]) < epsilon;
+
+      if (b == backwards && rot == pureRotation)
+      {
+        // same direction -> just add the pose
+        segment.poses.push_back(copy.poses[0]);
+        copy.poses.erase(copy.poses.begin());
+      }
+      else
+      {
+        // direction changes -> add the last pose of the last segment back to
+        // the path, to start a new segment at the end of the last
+        copy.poses.insert(copy.poses.begin(), segment.poses.back());
+        break;
+      }
+    }
+
+    // add the created segment to the list
+    global_plan_segments.push_back(segment);
+  }
+  if (global_plan_segments.empty())
+  {
+    // global_plan_in doesn't have at least 2 poses, hence there is only one segment, which
+    // is the complete path.
+    global_plan_segments.push_back(global_plan_in);
+  }
+
+  return global_plan_segments;
+}
 }  // namespace nav_2d_utils