Regulated pure pursuit controller

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>
+