graceful_controller: implement iterative selection of control points (#4795)

* initial pass at iterative

Signed-off-by: Michael Ferguson <[email protected]>

* add v_angular_min_in_place

Signed-off-by: Michael Ferguson <[email protected]>

* add orientation filter, fix remaining TODOs

Signed-off-by: Michael Ferguson <[email protected]>

* try to increase coverage, fixup minor test issues

Signed-off-by: Michael Ferguson <[email protected]>

* address review comments

Signed-off-by: Michael Ferguson <[email protected]>

* review comments

* update defaults
* rename to in_place_collision_resolution

Signed-off-by: Michael Ferguson <[email protected]>

* revert change in default

Signed-off-by: Michael Ferguson <[email protected]>

---------

Signed-off-by: Michael Ferguson <[email protected]>

Author: mikeferguson

nav2_graceful_controller/include/nav2_graceful_controller/graceful_controller.hpp

@@ -107,42 +107,31 @@ class GracefulController : public nav2_core::Controller
   void setSpeedLimit(const double & speed_limit, const bool & percentage) override;
 
 protected:
-  /**
-   * @brief Get motion target point.
-   * @param motion_target_dist Optimal motion target distance
-   * @param path Current global path
-   * @return Motion target point
-   */
-  geometry_msgs::msg::PoseStamped getMotionTarget(
-    const double & motion_target_dist,
-    const nav_msgs::msg::Path & path);
-
   /**
    * @brief Simulate trajectory calculating in every step the new velocity command based on
    * a new curvature value and checking for collisions.
    *
-   * @param robot_pose Robot pose
-   * @param motion_target Motion target point
+   * @param motion_target Motion target point (in costmap local frame?)
    * @param costmap_transform Transform between global and local costmap
    * @param trajectory Simulated trajectory
+   * @param cmd_vel Initial command velocity during simulation
    * @param backward Flag to indicate if the robot is moving backward
    * @return true if the trajectory is collision free, false otherwise
    */
   bool simulateTrajectory(
-    const geometry_msgs::msg::PoseStamped & robot_pose,
     const geometry_msgs::msg::PoseStamped & motion_target,
     const geometry_msgs::msg::TransformStamped & costmap_transform,
     nav_msgs::msg::Path & trajectory,
-    const bool & backward);
+    geometry_msgs::msg::TwistStamped & cmd_vel,
+    bool backward);
 
   /**
    * @brief Rotate the robot to face the motion target with maximum angular velocity.
    *
    * @param angle_to_target Angle to the motion target
    * @return geometry_msgs::msg::Twist Velocity command
    */
-  geometry_msgs::msg::Twist rotateToTarget(
-    const double & angle_to_target);
+  geometry_msgs::msg::Twist rotateToTarget(double angle_to_target);
 
   /**
    * @brief Checks if the robot is in collision
@@ -153,6 +142,21 @@ class GracefulController : public nav2_core::Controller
    */
   bool inCollision(const double & x, const double & y, const double & theta);
 
+  /**
+   * @brief Compute the distance to each pose in a path
+   * @param poses Poses to compute distances with
+   * @param distances Computed distances
+   */
+  void computeDistanceAlongPath(
+    const std::vector<geometry_msgs::msg::PoseStamped> & poses,
+    std::vector<double> & distances);
+
+  /**
+   * @brief Control law requires proper orientations, not all planners provide them
+   * @param path Path to add orientations into, if required
+   */
+  void validateOrientations(std::vector<geometry_msgs::msg::PoseStamped> & path);
+
   std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
   std::string plugin_name_;
   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
@@ -164,6 +168,9 @@ class GracefulController : public nav2_core::Controller
   double goal_dist_tolerance_;
   bool goal_reached_;
 
+  // True from the time a new path arrives until we have completed an initial rotation
+  bool do_initial_rotation_;
+
   std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> transformed_plan_pub_;
   std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> local_plan_pub_;
   std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PointStamped>>

nav2_graceful_controller/include/nav2_graceful_controller/parameter_handler.hpp

@@ -33,7 +33,8 @@ namespace nav2_graceful_controller
 struct Parameters
 {
   double transform_tolerance;
-  double motion_target_dist;
+  double min_lookahead;
+  double max_lookahead;
   double max_robot_pose_search_dist;
   double k_phi;
   double k_delta;
@@ -44,12 +45,14 @@ struct Parameters
   double v_linear_max_initial;
   double v_angular_max;
   double v_angular_max_initial;
+  double v_angular_min_in_place;
   double slowdown_radius;
   bool initial_rotation;
-  double initial_rotation_min_angle;
-  bool final_rotation;
+  double initial_rotation_tolerance;
+  bool prefer_final_rotation;
   double rotation_scaling_factor;
   bool allow_backward;
+  double in_place_collision_resolution;
 };
 
 /**