New Graceful Motion Controller (#4021)

* Initial commit

Signed-off-by: Alberto Tudela <[email protected]>

* Fix egopolar and add compute next pose

Signed-off-by: Alberto Tudela <[email protected]>

* Added simulated trajectory

Signed-off-by: Alberto Tudela <[email protected]>

* Added slowdown publisher

Signed-off-by: Alberto Tudela <[email protected]>

* Added first tests

Signed-off-by: Alberto Tudela <[email protected]>

* Added basic tests and smoth control

Signed-off-by: Alberto Tudela <[email protected]>

* Added initial rotation

Signed-off-by: Alberto Tudela <[email protected]>

* Added final rotation

Signed-off-by: Alberto Tudela <[email protected]>

* Added collision

Signed-off-by: Alberto Tudela <[email protected]>

* Improve last motion target

Signed-off-by: Alberto Tudela <[email protected]>

* Added new tests

Signed-off-by: Alberto Tudela <[email protected]>

* Set min velocity

Signed-off-by: Alberto Tudela <[email protected]>

* Added EOL

Signed-off-by: Alberto Tudela <[email protected]>

* Path test

Signed-off-by: Alberto Tudela <[email protected]>

* Add utils and minor fixes

Signed-off-by: Alberto Tudela <[email protected]>

* Update footprint calculation

Signed-off-by: Alberto Tudela <[email protected]>

* Added more tests

Signed-off-by: Alberto Tudela <[email protected]>

* Added nav2_controller to package test

Signed-off-by: Alberto Tudela <[email protected]>

* Improve comments

Signed-off-by: Alberto Tudela <[email protected]>

* Added backward motion

Signed-off-by: Alberto Tudela <[email protected]>

* Split in two libraries

Signed-off-by: Alberto Tudela <[email protected]>

* Improve rotation velocity

Signed-off-by: Alberto Tudela <[email protected]>

* Update documentation

Signed-off-by: Alberto Tudela <[email protected]>

* Minor fixes

Signed-off-by: Alberto Tudela <[email protected]>

* Revert collision_checker

Signed-off-by: Alberto Tudela <[email protected]>

* Pass costmap transform to simulate trajectory

Signed-off-by: Alberto Tudela <[email protected]>

* Retain old headers

Signed-off-by: Alberto Tudela <[email protected]>

* Update dyn parameters of control law

Signed-off-by: Alberto Tudela <[email protected]>

* Better comment in test

Signed-off-by: Alberto Tudela <[email protected]>

* Update setSpeedLimits with angular vel max

Signed-off-by: Alberto Tudela <[email protected]>

* Fix SpeedLimits

Signed-off-by: Alberto Tudela <[email protected]>

* Fixes in vel and collision

Signed-off-by: Alberto Tudela <[email protected]>

* Fix backward motion

Signed-off-by: Alberto Tudela <[email protected]>

---------

Signed-off-by: Alberto Tudela <[email protected]>

nav2_graceful_motion_controller/CMakeLists.txt

@@ -0,0 +1,80 @@
+cmake_minimum_required(VERSION 3.5)
+project(nav2_graceful_motion_controller)
+
+find_package(ament_cmake REQUIRED)
+find_package(nav2_common REQUIRED)
+find_package(nav2_core REQUIRED)
+find_package(nav2_costmap_2d REQUIRED)
+find_package(nav2_util REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(nav_msgs REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_geometry_msgs REQUIRED)
+find_package(nav_2d_utils REQUIRED)
+find_package(angles REQUIRED)
+
+nav2_package()
+
+include_directories(
+  include
+)
+
+set(dependencies
+  rclcpp
+  geometry_msgs
+  nav2_costmap_2d
+  pluginlib
+  nav_msgs
+  nav2_util
+  nav2_core
+  tf2
+  tf2_geometry_msgs
+  nav_2d_utils
+  angles
+)
+
+# Add Smooth Control Law as library
+add_library(smooth_control_law SHARED src/smooth_control_law.cpp)
+ament_target_dependencies(smooth_control_law ${dependencies})
+
+# Add Graceful Motion Controller
+set(library_name nav2_graceful_motion_controller)
+
+add_library(${library_name} SHARED
+  src/graceful_motion_controller.cpp
+  src/parameter_handler.cpp
+  src/path_handler.cpp
+  src/utils.cpp
+)
+
+target_link_libraries(${library_name} smooth_control_law)
+ament_target_dependencies(${library_name} ${dependencies})
+
+install(TARGETS smooth_control_law ${library_name}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+install(DIRECTORY include/
+  DESTINATION include/
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+
+  # the following line skips the linter which checks for copyrights
+  set(ament_cmake_copyright_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+  add_subdirectory(test)
+endif()
+
+ament_export_include_directories(include)
+ament_export_libraries(smooth_control_law ${library_name})
+ament_export_dependencies(${dependencies})
+
+pluginlib_export_plugin_description_file(nav2_core graceful_controller_plugin.xml)
+
+ament_package()

nav2_graceful_motion_controller/README.md

@@ -0,0 +1,42 @@
+# Graceful Motion Controller
+The graceful motion controller implements a controller based on the works of Jong Jin Park in "Graceful Navigation for Mobile Robots in Dynamic and Uncertain Environments". (2016). In this implementation, a `motion_target` is set at a distance away from the robot that is exponentially stable to generate a smooth trajectory for the robot to follow.
+
+See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-graceful-motion.html) for additional parameter descriptions.
+
+## Smooth control law
+The smooth control law is a pose-following kinematic control law that generates a smooth and confortable trajectory for the robot to follow. It is Lyapunov-based feedback control law made of three components:
+* The egocentric polar coordinates of the motion target (r, phi, delta) with respect to the robot frame.
+* A slow subsystem which describes the position of the robot.
+* A fast subsystem which describes the steering angle of the robot.
+
+![Trajectories](./doc/trajectories.png)
+
+## Parameters
+
+| Parameter | Description | 
+|-----|----|
+| `transform_tolerance` | The TF transform tolerance. | 
+| `motion_target_dist` | The lookahead distance to use to find the motion_target point. This distance should be a value around 1.0m but not much farther away. Greater values will cause the robot to generate smoother paths but not necessarily follow the path as closely. |
+| `max_robot_pose_search_dist` | Maximum integrated distance along the path to bound the search for the closest pose to the robot. This is set by default to the maximum costmap extent, so it shouldn't be set manually unless there are loops within the local costmap. | 
+| `k_phi` | Ratio of the rate of change in phi to the rate of change in r. Controls the convergence of the slow subsystem. If this value is equal to zero, the controller will behave as a pure waypoint follower. A high value offers extreme scenario of pose-following where theta is reduced much faster than r. **Note**: This variable is called k1 in earlier versions of the paper. | 
+| `k_delta` | Constant factor applied to the heading error feedback. Controls the convergence of the fast subsystem. The bigger the value, the robot converge faster to the reference heading. **Note**: This variable is called k2 in earlier versions of the paper. | 
+| `beta` | Constant factor applied to the path curvature. This value must be positive. Determines how fast the velocity drops when the curvature increases. | 
+| `lambda` | Constant factor applied to the path curvature. This value must be greater or equal to 1. Determines the sharpness of the curve: higher lambda implies sharper curves. | 
+| `v_linear_min` | Minimum linear velocity. Units: meters/sec. | 
+| `v_linear_max` | Maximum linear velocity. Units: meters/sec. | 
+| `v_angular_max` | Maximum angular velocity produced by the control law. Units: radians/sec. | 
+| `slowdown_radius` | Radius around the goal pose in which the robot will start to slow down. Units: meters. | 
+| `initial_rotation` | Enable a rotation in place to the goal before starting the path. The control law may generate large sweeping arcs to the goal pose, depending on the initial robot orientation and k_phi, k_delta. | 
+| `initial_rotation_min_angle` | The difference in the path orientation and the starting robot orientation to trigger a rotate in place, if `initial_rotation` is enabled. | 
+| `final_rotation` | Similar to `initial_rotation`, the control law can generate large arcs when the goal orientation is not aligned with the path. If this is enabled, the final pose will be ignored and the robot will follow the orientation of he path and will make a final rotation in place to the goal orientation. | 
+| `rotation_scaling_factor` | The scaling factor applied to the rotation in place velocity. | 
+| `allow_backward` | Whether to allow the robot to move backward. |
+
+## Topics
+
+| Topic  | Type | Description | 
+|-----|----|----|
+| `transformed_global_plan`  | `nav_msgs/Path` | The global plan transformed into the robot frame. | 
+| `local_plan`  | `nav_msgs/Path` | The local plan generated by appliyng iteratively the control law upon a set of motion targets along the global plan. | 
+| `motion_target`  | `geometry_msgs/PointStamped` | The current motion target used by the controller to compute the velocity commands. | 
+| `slowdown`  | `visualization_msgs/Marker` | A flat circle marker of radius slowdown_radius around the motion target. | 

nav2_graceful_motion_controller/graceful_controller_plugin.xml

@@ -0,0 +1,7 @@
+<class_libraries>
+    <library path="nav2_graceful_motion_controller">
+        <class type="nav2_graceful_motion_controller::GracefulMotionController" base_class_type="nav2_core::Controller">
+            <description>Graceful motion controller for Nav2</description>
+        </class>
+    </library>
+</class_libraries>

nav2_graceful_motion_controller/include/nav2_graceful_motion_controller/ego_polar_coords.hpp

@@ -0,0 +1,91 @@
+// Copyright (c) 2023 Alberto J. Tudela Roldán
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef NAV2_GRACEFUL_MOTION_CONTROLLER__EGO_POLAR_COORDS_HPP_
+#define NAV2_GRACEFUL_MOTION_CONTROLLER__EGO_POLAR_COORDS_HPP_
+
+#include <math.h>
+
+#include "angles/angles.h"
+#include "geometry_msgs/msg/pose.hpp"
+#include "tf2/utils.h"
+#include "tf2/transform_datatypes.h"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+
+namespace nav2_graceful_motion_controller
+{
+
+/**
+ * @brief Egocentric polar coordinates defined as the difference between the
+ * robot pose and the target pose relative to the robot position and orientation.
+ */
+struct EgocentricPolarCoordinates
+{
+  float r;       // Radial distance between the robot pose and the target pose.
+                 // Negative value if the robot is moving backwards.
+  float phi;     // Orientation of target with respect to the line of sight
+                 // from the robot to the target.
+  float delta;   // Steering angle of the robot with respect to the line of sight.
+
+  EgocentricPolarCoordinates(
+    const float & r_in = 0.0,
+    const float & phi_in = 0.0,
+    const float & delta_in = 0.0)
+  : r(r_in), phi(phi_in), delta(delta_in) {}
+
+  /**
+   * @brief Construct a new egocentric polar coordinates as the difference between the robot pose
+   * and the target pose relative to the robot position and orientation, both referenced to the same frame.
+   *
+   * Thus, r, phi and delta are always at the origin of the frame.
+   *
+   * @param target Target pose.
+   * @param current Current pose. Defaults to the origin.
+   * @param backward If true, the robot is moving backwards. Defaults to false.
+   */
+  explicit EgocentricPolarCoordinates(
+    const geometry_msgs::msg::Pose & target,
+    const geometry_msgs::msg::Pose & current = geometry_msgs::msg::Pose(), bool backward = false)
+  {
+    // Compute the difference between the target and the current pose
+    float dX = target.position.x - current.position.x;
+    float dY = target.position.y - current.position.y;
+    // Compute the line of sight from the robot to the target
+    // Flip it if the robot is moving backwards
+    float line_of_sight = backward ? (std::atan2(-dY, dX) + M_PI) : std::atan2(-dY, dX);
+    // Compute the ego polar coordinates
+    r = sqrt(dX * dX + dY * dY);
+    phi = angles::normalize_angle(tf2::getYaw(target.orientation) + line_of_sight);
+    delta = angles::normalize_angle(tf2::getYaw(current.orientation) + line_of_sight);
+    // If the robot is moving backwards, flip the sign of the radial distance
+    r *= backward ? -1.0 : 1.0;
+  }
+
+  /**
+   * @brief Construct a new egocentric polar coordinates for the target pose.
+   *
+   * @param target Target pose.
+   * @param backward If true, the robot is moving backwards. Defaults to false.
+   */
+  explicit EgocentricPolarCoordinates(
+    const geometry_msgs::msg::Pose & target,
+    bool backward = false)
+  {
+    EgocentricPolarCoordinates(target, geometry_msgs::msg::Pose(), backward);
+  }
+};
+
+}  // namespace nav2_graceful_motion_controller
+
+#endif  // NAV2_GRACEFUL_MOTION_CONTROLLER__EGO_POLAR_COORDS_HPP_