Regulated pure pursuit controller (#2152)

* regulated pure pursuit migration commit

* adding speed limit API

* adding review comments + adding rotate to goal heading

* adding test dir

* add some initial tests

* more tests

* remove old comment

* improve readme

* fix CI

* first attempt at changing algos in tests

* allowing full path parameter substitutions

* adding integration tests

* enable SMAC testing too with new changes

* swap algos

* revert

* Update angular velocity after constraining linear velocity (#2165)

This ensures the robot moves towards the lookahead point more closely.
If the angular velocity is not updated, then the robot tries to take cuts while turning,
which could lead to collisions when near obstacles

Signed-off-by: Shrijit Singh <[email protected]>

* Update cost scaling heuristic to vary speed linearly with distance (#2164)

* Update cost scaling to vary linearly with distance instead of relying on costmap cost

Signed-off-by: Shrijit Singh <[email protected]>

* Resolve suggested changes

Signed-off-by: Shrijit Singh <[email protected]>

* Add documentation for cost scaling parameters

Signed-off-by: Shrijit Singh <[email protected]>

* Improve parameter descriptions

Signed-off-by: Shrijit Singh <[email protected]>

* Comment cost scaling tests since layered costmap is not initialized

A valid layered costmap reference is needed to get the inscribed radius

Signed-off-by: Shrijit Singh <[email protected]>

Co-authored-by: Shrijit Singh <[email protected]>

nav2_common/nav2_common/launch/rewritten_yaml.py

@@ -106,11 +106,32 @@ def resolve_rewrites(self, context):
         return resolved_params, resolved_keys
 
     def substitute_params(self, yaml, param_rewrites):
+        # substitute leaf-only parameters
         for key in self.getYamlLeafKeys(yaml):
             if key.key() in param_rewrites:
                 raw_value = param_rewrites[key.key()]
                 key.setValue(self.convert(raw_value))
 
+        # substitute total path parameters
+        yaml_paths = self.pathify(yaml)
+        for path in yaml_paths:
+            if path in param_rewrites:
+                # this is an absolute path (ex. 'key.keyA.keyB.val')
+                rewrite_val = param_rewrites[path]
+                yaml_keys = path.split('.')
+                yaml = self.updateYamlPathVals(yaml, yaml_keys, rewrite_val)
+
+
+    def updateYamlPathVals(self, yaml, yaml_key_list, rewrite_val):
+        for key in yaml_key_list:
+            if key == yaml_key_list[-1]:
+                yaml[key] = rewrite_val
+                break
+            key = yaml_key_list.pop(0)
+            yaml[key] = self.updateYamlPathVals(yaml.get(key, {}), yaml_key_list, rewrite_val)
+
+        return yaml
+
     def substitute_keys(self, yaml, key_rewrites):
         if len(key_rewrites) != 0:
             for key, val in yaml.items():
@@ -129,6 +150,24 @@ def getYamlLeafKeys(self, yamlData):
         except AttributeError:
             return
 
+    def pathify(self, d, p=None, paths=None, joinchar='.'):
+        if p is None:
+            paths = {}
+            self.pathify(d, "", paths, joinchar=joinchar)
+            return paths
+        pn = p
+        if p != "":
+            pn += '.'
+        if isinstance(d, dict):
+            for k in d:
+                v = d[k]
+                self.pathify(v, pn + k, paths, joinchar=joinchar)
+        elif isinstance(d, list):
+            for idx, e in enumerate(d):
+                self.pathify(e, pn + str(idx), paths, joinchar=joinchar)
+        else:
+            paths[p] = d
+
     def convert(self, text_value):
         if self.__convert_types:
             # try converting to int or float

nav2_regulated_pure_pursuit_controller/CMakeLists.txt

@@ -0,0 +1,70 @@
+cmake_minimum_required(VERSION 3.5)
+project(nav2_regulated_pure_pursuit_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)
+
+nav2_package()
+set(CMAKE_CXX_STANDARD 17)
+
+include_directories(
+  include
+)
+
+set(dependencies
+  rclcpp
+  geometry_msgs
+  nav2_costmap_2d
+  pluginlib
+  nav_msgs
+  nav2_util
+  nav2_core
+  tf2
+)
+
+set(library_name nav2_regulated_pure_pursuit_controller)
+
+add_library(${library_name} SHARED
+        src/regulated_pure_pursuit_controller.cpp)
+
+# prevent pluginlib from using boost
+target_compile_definitions(${library_name} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
+
+ament_target_dependencies(${library_name}
+  ${dependencies}
+)
+
+install(TARGETS ${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(${library_name})
+ament_export_dependencies(${dependencies})
+
+pluginlib_export_plugin_description_file(nav2_core nav2_regulated_pure_pursuit_controller.xml)
+
+ament_package()
+

nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp

@@ -0,0 +1,258 @@
+// Copyright (c) 2020 Shrijit Singh
+// Copyright (c) 2020 Samsung Research America
+//
+// 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_REGULATED_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_
+#define NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_
+
+#include <string>
+#include <vector>
+#include <memory>
+#include <algorithm>
+
+#include "nav2_core/controller.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include "pluginlib/class_loader.hpp"
+#include "pluginlib/class_list_macros.hpp"
+#include "nav2_util/odometry_utils.hpp"
+#include "geometry_msgs/msg/pose2_d.hpp"
+
+namespace nav2_regulated_pure_pursuit_controller
+{
+
+/**
+ * @class nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController
+ * @brief Regulated pure pursuit controller plugin
+ */
+class RegulatedPurePursuitController : public nav2_core::Controller
+{
+public:
+  /**
+   * @brief Constructor for nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController
+   */
+  RegulatedPurePursuitController() = default;
+
+  /**
+   * @brief Destrructor for nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController
+   */
+  ~RegulatedPurePursuitController() override = default;
+
+  /**
+   * @brief Configure controller state machine
+   * @param parent WeakPtr to node
+   * @param name Name of plugin
+   * @param tf TF buffer
+   * @param costmap_ros Costmap2DROS object of environment
+   */
+  void configure(
+    const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
+    std::string name, const std::shared_ptr<tf2_ros::Buffer> & tf,
+    const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> & costmap_ros) override;
+
+  /**
+   * @brief Cleanup controller state machine
+   */
+  void cleanup() override;
+
+  /**
+   * @brief Activate controller state machine
+   */
+  void activate() override;
+
+  /**
+   * @brief Deactivate controller state machine
+   */
+  void deactivate() override;
+
+  /**
+   * @brief Compute the best command given the current pose and velocity, with possible debug information
+   *
+   * Same as above computeVelocityCommands, but with debug results.
+   * If the results pointer is not null, additional information about the twists
+   * evaluated will be in results after the call.
+   *
+   * @param pose      Current robot pose
+   * @param velocity  Current robot velocity
+   * @param results   Output param, if not NULL, will be filled in with full evaluation results
+   * @return          Best command
+   */
+  geometry_msgs::msg::TwistStamped computeVelocityCommands(
+    const geometry_msgs::msg::PoseStamped & pose,
+    const geometry_msgs::msg::Twist & velocity) override;
+
+  /**
+   * @brief nav2_core setPlan - Sets the global plan
+   * @param path The global plan
+   */
+  void setPlan(const nav_msgs::msg::Path & path) override;
+
+  /**
+   * @brief Limits the maximum linear speed of the robot.
+   * @param speed_limit expressed in percentage from maximum robot speed.
+   */
+  void setSpeedLimit(const double & speed_limit) override;
+
+protected:
+  /**
+   * @brief Transforms global plan into same frame as pose, clips far away poses and possibly prunes passed poses
+   * @param pose pose to transform
+   * @return Path in new frame
+   */
+  nav_msgs::msg::Path transformGlobalPlan(const geometry_msgs::msg::PoseStamped & pose);
+
+  /**
+   * @brief Transform a pose to another frame.
+   * @param frame Frame ID to transform to
+   * @param in_pose Pose input to transform
+   * @param out_pose transformed output
+   * @return bool if successful
+   */
+  bool transformPose(
+    const std::string frame,
+    const geometry_msgs::msg::PoseStamped & in_pose,
+    geometry_msgs::msg::PoseStamped & out_pose) const;
+
+  /**
+   * @brief Get lookahead distance
+   * @param cmd the current speed to use to compute lookahead point
+   * @return lookahead distance
+   */
+  double getLookAheadDistance(const geometry_msgs::msg::Twist &);
+
+  /**
+   * @brief Creates a PointStamped message for visualization
+   * @param carrot_pose Input carrot point as a PoseStamped
+   * @return CarrotMsg a carrot point marker, PointStamped
+   */
+  std::unique_ptr<geometry_msgs::msg::PointStamped> createCarrotMsg(
+    const geometry_msgs::msg::PoseStamped & carrot_pose);
+
+  /**
+   * @brief Whether robot should rotate to rough path heading
+   * @param carrot_pose current lookahead point
+   * @param angle_to_path Angle of robot output relatie to carrot marker
+   * @return Whether should rotate to path heading
+   */
+  bool shouldRotateToPath(
+    const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path);
+
+  /**
+   * @brief Whether robot should rotate to final goal orientation
+   * @param carrot_pose current lookahead point
+   * @return Whether should rotate to goal heading
+   */
+  bool shouldRotateToGoalHeading(const geometry_msgs::msg::PoseStamped & carrot_pose);
+
+  /**
+   * @brief Create a smooth and kinematically smoothed rotation command
+   * @param linear_vel linear velocity
+   * @param angular_vel angular velocity
+   * @param angle_to_path Angle of robot output relatie to carrot marker
+   * @param curr_speed the current robot speed
+   */
+  void rotateToHeading(double & linear_vel, double & angular_vel,
+    const double & angle_to_path, const geometry_msgs::msg::Twist & curr_speed);
+
+  /**
+   * @brief Whether collision is imminent
+   * @param robot_pose Pose of robot
+   * @param carrot_pose Pose of carrot
+   * @param linear_vel linear velocity to forward project
+   * @param angular_vel angular velocity to forward project
+   * @return Whether collision is imminent
+   */
+  bool isCollisionImminent(
+    const geometry_msgs::msg::PoseStamped &,
+    const geometry_msgs::msg::PoseStamped &,
+    const double &, const double &, const double &);
+
+  /**
+   * @brief Whether point is in collision
+   * @param x Pose of pose x
+   * @param y Pose of pose y
+   * @return Whether in collision
+   */
+  bool inCollision(const double & x, const double & y);
+
+  /**
+   * @brief Cost at a point
+   * @param x Pose of pose x
+   * @param y Pose of pose y
+   * @return Cost of pose in costmap
+   */
+  double costAtPose(const double & x, const double & y);
+
+  /**
+   * @brief apply regulation constraints to the system
+   * @param linear_vel robot command linear velocity input
+   * @param dist_error error in the carrot distance and lookahead distance
+   * @param lookahead_dist optimal lookahead distance
+   * @param curvature curvature of path
+   * @param speed Speed of robot
+   * @param pose_cost cost at this pose
+   */
+  void applyConstraints(
+    const double & dist_error, const double & lookahead_dist,
+    const double & curvature, const geometry_msgs::msg::Twist & speed,
+    const double & pose_cost, double & linear_vel);
+
+  /**
+   * @brief Get lookahead point
+   * @param lookahead_dist Optimal lookahead distance
+   * @param path Current global path
+   * @return Lookahead point
+   */
+  geometry_msgs::msg::PoseStamped getLookAheadPoint(const double &, const nav_msgs::msg::Path &);
+
+  std::shared_ptr<tf2_ros::Buffer> tf_;
+  std::string plugin_name_;
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
+  nav2_costmap_2d::Costmap2D * costmap_;
+  rclcpp::Logger logger_ {rclcpp::get_logger("RegulatedPurePursuitController")};
+
+  double desired_linear_vel_;
+  double lookahead_dist_;
+  double rotate_to_heading_angular_vel_;
+  double max_lookahead_dist_;
+  double min_lookahead_dist_;
+  double lookahead_time_;
+  double max_linear_accel_;
+  double max_linear_decel_;
+  bool use_velocity_scaled_lookahead_dist_;
+  tf2::Duration transform_tolerance_;
+  bool use_approach_vel_scaling_;
+  double min_approach_linear_velocity_;
+  double control_duration_;
+  double max_allowed_time_to_collision_;
+  bool use_regulated_linear_velocity_scaling_;
+  bool use_cost_regulated_linear_velocity_scaling_;
+  double cost_scaling_dist_;
+  double cost_scaling_gain_;
+  double inflation_cost_scaling_factor_;
+  double regulated_linear_scaling_min_radius_;
+  double regulated_linear_scaling_min_speed_;
+  bool use_rotate_to_heading_;
+  double max_angular_accel_;
+  double rotate_to_heading_min_angle_;
+  double goal_dist_tol_;
+
+  nav_msgs::msg::Path global_plan_;
+  std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> global_path_pub_;
+  std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PointStamped>> carrot_pub_;
+  std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> carrot_arc_pub_;
+};
+
+}  // namespace nav2_regulated_pure_pursuit_controller
+
+#endif  // NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_

nav2_regulated_pure_pursuit_controller/nav2_regulated_pure_pursuit_controller.xml

@@ -0,0 +1,10 @@
+<class_libraries>
+    <library path="nav2_regulated_pure_pursuit_controller">
+        <class type="nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" base_class_type="nav2_core::Controller">
+            <description>
+                nav2_regulated_pure_pursuit_controller
+            </description>
+        </class>
+    </library>
+</class_libraries>
+