diff --git a/nav2_gradient_costmap_plugin/include/nav2_gradient_costmap_plugin/gradient_layer.hpp b/nav2_gradient_costmap_plugin/include/nav2_gradient_costmap_plugin/gradient_layer.hpp index 4ce7581..68b46ab 100644 --- a/nav2_gradient_costmap_plugin/include/nav2_gradient_costmap_plugin/gradient_layer.hpp +++ b/nav2_gradient_costmap_plugin/include/nav2_gradient_costmap_plugin/gradient_layer.hpp @@ -72,6 +72,8 @@ class GradientLayer : public nav2_costmap_2d::Layer virtual void onFootprintChanged(); + virtual bool isClearable() {return false;} + private: double last_min_x_, last_min_y_, last_max_x_, last_max_y_; diff --git a/nav2_pure_pursuit_controller/README.md b/nav2_pure_pursuit_controller/README.md deleted file mode 100644 index 253718b..0000000 --- a/nav2_pure_pursuit_controller/README.md +++ /dev/null @@ -1,11 +0,0 @@ -# Nav2 Pure pursuit controller -Tutorial code referenced in https://navigation.ros.org/plugin_tutorials/docs/writing_new_nav2controller_plugin.html - -This controller implements a the pure pursuit algorithm to track a path. - -## How the algorithm works -The global path is continuously pruned to the closest point to the robot (see the figure below). -Then the path is transformed to the robot frame and a lookahead point is determined. -This lookahead point will be given to the pure pursuite algorithm to calculate a command velocity. - -![Lookahead algorithm](./doc/lookahead_algorithm.png) diff --git a/nav2_pure_pursuit_controller/include/nav2_pure_pursuit_controller/pure_pursuit_controller.hpp b/nav2_pure_pursuit_controller/include/nav2_pure_pursuit_controller/pure_pursuit_controller.hpp deleted file mode 100644 index 2007c30..0000000 --- a/nav2_pure_pursuit_controller/include/nav2_pure_pursuit_controller/pure_pursuit_controller.hpp +++ /dev/null @@ -1,74 +0,0 @@ -/* - * SPDX-License-Identifier: BSD-3-Clause - * - * Author(s): Shrijit Singh - * - */ - -#ifndef NAV2_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_ -#define NAV2_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_ - -#include -#include -#include - -#include "nav2_core/controller.hpp" -#include "rclcpp/rclcpp.hpp" -#include "pluginlib/class_loader.hpp" -#include "pluginlib/class_list_macros.hpp" - -namespace nav2_pure_pursuit_controller -{ - -class PurePursuitController : public nav2_core::Controller -{ -public: - PurePursuitController() = default; - ~PurePursuitController() override = default; - - void configure( - const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, - std::string name, const std::shared_ptr & tf, - const std::shared_ptr & costmap_ros); - - - void cleanup() override; - void activate() override; - void deactivate() override; - - geometry_msgs::msg::TwistStamped computeVelocityCommands( - const geometry_msgs::msg::PoseStamped & pose, - const geometry_msgs::msg::Twist & velocity) override; - - void setPlan(const nav_msgs::msg::Path & path) override; - -protected: - nav_msgs::msg::Path transformGlobalPlan(const geometry_msgs::msg::PoseStamped & pose); - - bool transformPose( - const std::shared_ptr tf, - const std::string frame, - const geometry_msgs::msg::PoseStamped & in_pose, - geometry_msgs::msg::PoseStamped & out_pose, - const rclcpp::Duration & transform_tolerance - ) const; - - rclcpp_lifecycle::LifecycleNode::WeakPtr node_; - std::shared_ptr tf_; - std::string plugin_name_; - std::shared_ptr costmap_ros_; - rclcpp::Logger logger_ {rclcpp::get_logger("PurePursuitController")}; - rclcpp::Clock::SharedPtr clock_; - - double desired_linear_vel_; - double lookahead_dist_; - double max_angular_vel_; - rclcpp::Duration transform_tolerance_ {0, 0}; - - nav_msgs::msg::Path global_plan_; - std::shared_ptr> global_pub_; -}; - -} // namespace nav2_pure_pursuit_controller - -#endif // NAV2_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_ diff --git a/nav2_pure_pursuit_controller/nav2_pure_pursuit_controller.xml b/nav2_pure_pursuit_controller/nav2_pure_pursuit_controller.xml deleted file mode 100644 index e2276b5..0000000 --- a/nav2_pure_pursuit_controller/nav2_pure_pursuit_controller.xml +++ /dev/null @@ -1,10 +0,0 @@ - - - - - nav2_pure_pursuit_controller - - - - - diff --git a/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp b/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp deleted file mode 100644 index 412dce3..0000000 --- a/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp +++ /dev/null @@ -1,301 +0,0 @@ -/* - * SPDX-License-Identifier: BSD-3-Clause - * - * Author(s): Shrijit Singh - * - */ - -#include -#include -#include - -#include "nav2_core/exceptions.hpp" -#include "nav2_util/node_utils.hpp" -#include "nav2_pure_pursuit_controller/pure_pursuit_controller.hpp" -#include "nav2_util/geometry_utils.hpp" - -using std::hypot; -using std::min; -using std::max; -using std::abs; -using nav2_util::declare_parameter_if_not_declared; -using nav2_util::geometry_utils::euclidean_distance; - -namespace nav2_pure_pursuit_controller -{ - -/** - * Find element in iterator with the minimum calculated value - */ -template -Iter min_by(Iter begin, Iter end, Getter getCompareVal) -{ - if (begin == end) { - return end; - } - auto lowest = getCompareVal(*begin); - Iter lowest_it = begin; - for (Iter it = ++begin; it != end; ++it) { - auto comp = getCompareVal(*it); - if (comp < lowest) { - lowest = comp; - lowest_it = it; - } - } - return lowest_it; -} - -void PurePursuitController::configure( - const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, - std::string name, const std::shared_ptr & tf, - const std::shared_ptr & costmap_ros) -{ - node_ = parent; - - auto node = node_.lock(); - - costmap_ros_ = costmap_ros; - tf_ = tf; - plugin_name_ = name; - logger_ = node->get_logger(); - clock_ = node->get_clock(); - - declare_parameter_if_not_declared( - node, plugin_name_ + ".desired_linear_vel", rclcpp::ParameterValue( - 0.2)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".lookahead_dist", - rclcpp::ParameterValue(0.4)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".max_angular_vel", rclcpp::ParameterValue( - 1.0)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".transform_tolerance", rclcpp::ParameterValue( - 0.1)); - - node->get_parameter(plugin_name_ + ".desired_linear_vel", desired_linear_vel_); - node->get_parameter(plugin_name_ + ".lookahead_dist", lookahead_dist_); - node->get_parameter(plugin_name_ + ".max_angular_vel", max_angular_vel_); - double transform_tolerance; - node->get_parameter(plugin_name_ + ".transform_tolerance", transform_tolerance); - transform_tolerance_ = rclcpp::Duration::from_seconds(transform_tolerance); - - global_pub_ = node->create_publisher("received_global_plan", 1); -} - -void PurePursuitController::cleanup() -{ - RCLCPP_INFO( - logger_, - "Cleaning up controller: %s of type pure_pursuit_controller::PurePursuitController", - plugin_name_.c_str()); - global_pub_.reset(); -} - -void PurePursuitController::activate() -{ - RCLCPP_INFO( - logger_, - "Activating controller: %s of type pure_pursuit_controller::PurePursuitController\" %s", - plugin_name_.c_str()); - global_pub_->on_activate(); -} - -void PurePursuitController::deactivate() -{ - RCLCPP_INFO( - logger_, - "Dectivating controller: %s of type pure_pursuit_controller::PurePursuitController\" %s", - plugin_name_.c_str()); - global_pub_->on_deactivate(); -} - - -geometry_msgs::msg::TwistStamped PurePursuitController::computeVelocityCommands( - const geometry_msgs::msg::PoseStamped & pose, - const geometry_msgs::msg::Twist &) -{ - auto transformed_plan = transformGlobalPlan(pose); - - // Find the first pose which is at a distance greater than the specified lookahed distance - auto goal_pose_it = std::find_if( - transformed_plan.poses.begin(), transformed_plan.poses.end(), [&](const auto & ps) { - return hypot(ps.pose.position.x, ps.pose.position.y) >= lookahead_dist_; - }); - - // If the last pose is still within lookahed distance, take the last pose - if (goal_pose_it == transformed_plan.poses.end()) { - goal_pose_it = std::prev(transformed_plan.poses.end()); - } - auto goal_pose = goal_pose_it->pose; - - double linear_vel, angular_vel; - - // If the goal pose is in front of the robot then compute the velocity using the pure pursuit - // algorithm, else rotate with the max angular velocity until the goal pose is in front of the - // robot - if (goal_pose.position.x > 0) { - auto curvature = 2.0 * goal_pose.position.y / - (goal_pose.position.x * goal_pose.position.x + goal_pose.position.y * goal_pose.position.y); - linear_vel = desired_linear_vel_; - angular_vel = desired_linear_vel_ * curvature; - } else { - linear_vel = 0.0; - angular_vel = max_angular_vel_; - } - - // Create and publish a TwistStamped message with the desired velocity - geometry_msgs::msg::TwistStamped cmd_vel; - cmd_vel.header.frame_id = pose.header.frame_id; - cmd_vel.header.stamp = clock_->now(); - cmd_vel.twist.linear.x = linear_vel; - cmd_vel.twist.angular.z = max( - -1.0 * abs(max_angular_vel_), min( - angular_vel, abs( - max_angular_vel_))); - - return cmd_vel; -} - -void PurePursuitController::setPlan(const nav_msgs::msg::Path & path) -{ - global_pub_->publish(path); - global_plan_ = path; -} - -nav_msgs::msg::Path -PurePursuitController::transformGlobalPlan( - const geometry_msgs::msg::PoseStamped & pose) -{ - // Original mplementation taken fron nav2_dwb_controller - - if (global_plan_.poses.empty()) { - throw nav2_core::PlannerException("Received plan with zero length"); - } - - // let's get the pose of the robot in the frame of the plan - geometry_msgs::msg::PoseStamped robot_pose; - if (!transformPose( - tf_, global_plan_.header.frame_id, pose, - robot_pose, transform_tolerance_)) - { - throw nav2_core::PlannerException("Unable to transform robot pose into global plan's frame"); - } - - // We'll discard points on the plan that are outside the local costmap - nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); - double dist_threshold = std::max(costmap->getSizeInCellsX(), costmap->getSizeInCellsY()) * - costmap->getResolution() / 2.0; - - // First find the closest pose on the path to the robot - auto transformation_begin = - min_by( - global_plan_.poses.begin(), global_plan_.poses.end(), - [&robot_pose](const geometry_msgs::msg::PoseStamped & ps) { - return euclidean_distance(robot_pose, ps); - }); - - // From the closest point, look for the first point that's further then dist_threshold from the - // robot. These points are definitely outside of the costmap so we won't transform them. - auto transformation_end = std::find_if( - transformation_begin, end(global_plan_.poses), - [&](const auto & global_plan_pose) { - return euclidean_distance(robot_pose, global_plan_pose) > dist_threshold; - }); - - // Helper function for the transform below. Transforms a PoseStamped from global frame to local - auto transformGlobalPoseToLocal = [&](const auto & global_plan_pose) { - // We took a copy of the pose, let's lookup the transform at the current time - geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; - stamped_pose.header.frame_id = global_plan_.header.frame_id; - stamped_pose.header.stamp = pose.header.stamp; - stamped_pose.pose = global_plan_pose.pose; - transformPose( - tf_, costmap_ros_->getBaseFrameID(), - stamped_pose, transformed_pose, transform_tolerance_); - return transformed_pose; - }; - - // Transform the near part of the global plan into the robot's frame of reference. - nav_msgs::msg::Path transformed_plan; - std::transform( - transformation_begin, transformation_end, - std::back_inserter(transformed_plan.poses), - transformGlobalPoseToLocal); - transformed_plan.header.frame_id = costmap_ros_->getBaseFrameID(); - transformed_plan.header.stamp = pose.header.stamp; - - // Remove the portion of the global plan that we've already passed so we don't - // process it on the next iteration (this is called path pruning) - global_plan_.poses.erase(begin(global_plan_.poses), transformation_begin); - global_pub_->publish(transformed_plan); - - if (transformed_plan.poses.empty()) { - throw nav2_core::PlannerException("Resulting plan has 0 poses in it."); - } - - return transformed_plan; -} - -bool PurePursuitController::transformPose( - const std::shared_ptr tf, - const std::string frame, - const geometry_msgs::msg::PoseStamped & in_pose, - geometry_msgs::msg::PoseStamped & out_pose, - const rclcpp::Duration & transform_tolerance -) const -{ - // Implementation taken as is fron nav_2d_utils in nav2_dwb_controller - - if (in_pose.header.frame_id == frame) { - out_pose = in_pose; - return true; - } - - try { - tf->transform(in_pose, out_pose, frame); - return true; - } catch (tf2::ExtrapolationException & ex) { - auto transform = tf->lookupTransform( - frame, - in_pose.header.frame_id, - tf2::TimePointZero - ); - if ( - (rclcpp::Time(in_pose.header.stamp) - rclcpp::Time(transform.header.stamp)) > - transform_tolerance) - { - RCLCPP_ERROR( - rclcpp::get_logger("tf_help"), - "Transform data too old when converting from %s to %s", - in_pose.header.frame_id.c_str(), - frame.c_str() - ); - RCLCPP_ERROR( - rclcpp::get_logger("tf_help"), - "Data time: %ds %uns, Transform time: %ds %uns", - in_pose.header.stamp.sec, - in_pose.header.stamp.nanosec, - transform.header.stamp.sec, - transform.header.stamp.nanosec - ); - return false; - } else { - tf2::doTransform(in_pose, out_pose, transform); - return true; - } - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR( - rclcpp::get_logger("tf_help"), - "Exception in transformPose: %s", - ex.what() - ); - return false; - } - return false; -} - -} // namespace nav2_pure_pursuit_controller - -// Register this controller as a nav2_core plugin -PLUGINLIB_EXPORT_CLASS(nav2_pure_pursuit_controller::PurePursuitController, nav2_core::Controller) diff --git a/nav2_pure_pursuit_controller/CMakeLists.txt b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt similarity index 64% rename from nav2_pure_pursuit_controller/CMakeLists.txt rename to nav2_regulated_pure_pursuit_controller/CMakeLists.txt index 7419f3b..6094c3a 100644 --- a/nav2_pure_pursuit_controller/CMakeLists.txt +++ b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(nav2_pure_pursuit_controller) +project(nav2_regulated_pure_pursuit_controller) find_package(ament_cmake REQUIRED) find_package(nav2_common REQUIRED) @@ -13,6 +13,7 @@ find_package(pluginlib REQUIRED) find_package(tf2 REQUIRED) nav2_package() +set(CMAKE_CXX_STANDARD 17) include_directories( include @@ -29,17 +30,17 @@ set(dependencies tf2 ) -add_library(nav2_pure_pursuit_controller SHARED - src/pure_pursuit_controller.cpp) +add_library(nav2_regulated_pure_pursuit_controller SHARED + src/regulated_pure_pursuit_controller.cpp) # prevent pluginlib from using boost -target_compile_definitions(nav2_pure_pursuit_controller PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") +target_compile_definitions(nav2_regulated_pure_pursuit_controller PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") -ament_target_dependencies(nav2_pure_pursuit_controller +ament_target_dependencies(nav2_regulated_pure_pursuit_controller ${dependencies} ) -install(TARGETS nav2_pure_pursuit_controller +install(TARGETS nav2_regulated_pure_pursuit_controller ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin @@ -57,10 +58,10 @@ if(BUILD_TESTING) endif() ament_export_include_directories(include) -ament_export_libraries(nav2_pure_pursuit_controller) +ament_export_libraries(nav2_regulated_pure_pursuit_controller) ament_export_dependencies(${dependencies}) -pluginlib_export_plugin_description_file(nav2_core nav2_pure_pursuit_controller.xml) +pluginlib_export_plugin_description_file(nav2_core nav2_regulated_pure_pursuit_controller.xml) ament_package() diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md new file mode 100644 index 0000000..9ca3dc8 --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -0,0 +1,139 @@ +# Nav2 Regulated Pure Pursuit Controller + +This is a controller (local trajectory planner) that implements a variant on the pure pursuit algorithm to track a path. This variant we call the Regulated Pure Pursuit Algorithm, due to its additional regulation terms on collision and linear speed. It also implements the basics behind the Adaptive Pure Pursuit algorithm to vary lookahead distances by current speed. It was developed by [Shrijit Singh](https://www.linkedin.com/in/shrijitsingh99/) and [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/) as part of the Nav2 working group. + +Code based on a simplified version of this controller is referenced in the [Writing a New Nav2 Controller](https://navigation.ros.org/plugin_tutorials/docs/writing_new_nav2controller_plugin.html) tutorial. + +This plugin implements the `nav2_core::Controller` interface allowing it to be used acros the navigation stack as a local trajectory planner in the controller server's action server (`controller_server`). + +It builds on top of the ordinary pure pursuit algorithm in a number of ways. It also implements all the common variants of the pure pursuit algorithm such as adaptive pure pursuit. This controller is suitable for use on all types of robots, including differential, legged, and ackermann steering vehicles. It may also be used on omni-directional platforms, but won't be able to fully leverage the lateral movements of the base (you may consider DWB instead). + +This controller has been measured to run at well over 1 kHz on a modern intel processor. + +

+ +

+ +## Pure Pursuit Basics + +The Pure Pursuit algorithm has been in use for over 30 years. You can read more about the details of the pure pursuit controller in its [introduction paper](http://www.enseignement.polytechnique.fr/profs/informatique/Eric.Goubault/MRIS/coulter_r_craig_1992_1.pdf). The core idea is to find a point on the path in front of the robot and find the linear and angular velocity to help drive towards it. Once it moves forward, a new point is selected, and the process repeats until the end of the path. The distance used to find the point to drive towards is the `lookahead` distance. + +In order to simply book-keeping, the global path is continuously pruned to the closest point to the robot (see the figure below) so that we only have to process useful path points. Then, the section of the path within the local costmap bounds is transformed to the robot frame and a lookahead point is determined using a predefined distance. + +Finally, the lookahead point will be given to the pure pursuit algorithm which finds the curvature of the path required to drive the robot to the lookahead point. This curvature is then applied to the velocity commands to allow the robot to drive. + +![Lookahead algorithm](./doc/lookahead_algorithm.png) + +## Regulated Pure Pursuit Features + +We have created a new variation on the pure pursuit algorithm that we dubb the Regulated Pure Pursuit algorithm. We combine the features of the Adaptive Pure Pursuit algorithm with rules around linear velocity with a focus on consumer, industrial, and service robot's needs. We also implement several common-sense safety mechanisms like collision detection and ensuring that commands are kinematically feasible that are missing from all other variants of pure pursuit (for some remarkable reason). + +The Regulated Pure Pursuit controller implements active collision detection. We use a parameter to set the maximum allowable time before a potential collision on the current velocity command. Using the current linear and angular velocity, we project forward in time that duration and check for collisions. Intuitively, you may think that collision checking between the robot and the lookahead point seems logical. However, if you're maneuvering in tight spaces, it makes alot of sense to only search forward a given amount of time to give the system a little leeway to get itself out. In confined spaces especially, we want to make sure that we're collision checking a reasonable amount of space for the current action being taken (e.g. if moving at 0.1 m/s, it makes no sense to look 10 meters ahead to the carrot, or 100 seconds into the future). This helps look further at higher speeds / angular rotations and closer with fine, slow motions in constrained environments so it doesn't over report collisions from valid motions near obstacles. If you set the maximum allowable to a large number, it will collision check all the way, but not exceeding, the lookahead point. We visualize the collision checking arc on the `lookahead_arc` topic. + +The regulated pure pursuit algorithm also makes use of the common variations on the pure pursuit algorithm. We implement the adaptive pure pursuit's main contribution of having velocity-scaled lookahead point distances. This helps make the controller more stable over a larger range of potential linear velocities. There are parameters for setting the lookahead gain (or lookahead time) and thresholded values for minimum and maximum. + +We also implement kinematic speed limits on the linear velocities in operations and angular velocities during pure rotations. This makes sure that the output commands are smooth, safe, and kinematically feasible. This is especially important at the beginning and end of a path tracking task, where you are ramping up to speed and slowing down to the goal. + +The final minor improvement we make is slowing on approach to the goal. Knowing that the optimal lookahead distance is `X`, we can take the difference in `X` and the actual distance of the lookahead point found to find the lookahead point error. During operations, the variation in this error should be exceptionally small and won't be triggered. However, at the end of the path, there are no more points at a lookahead distance away from the robot, so it uses the last point on the path. So as the robot approaches a target, its error will grow and the robot's velocity will be reduced proportional to this error until a minimum threshold. This is also tracked by the kinematic speed limits to ensure drivability. + +The major improvements that this work implements is the regulations on the linear velocity based on some cost functions. They were selected to remove long-standing bad behavior within the pure pursuit algorithm. A major downside of this approach is the overshoot and poor handling in particularly high curvature (or extremely rapidly changing curvature) environments. It is commonly known that this will cause the robot to overshoot from the path and potentially collide with the environment. These cost functions were also chosen based on common requirements and needs of mobile robots uses in service, commercial, and industrial use-cases; scaling by curvature creates intuitive behavior of slowing the robot when making sharp turns and slowing when its near a potential collision so that small variations don't clip obstacles. + +The cost functions penalize the robot's speed based on its proximity to obstacles and the curvature of the path. This is helpful to slow the robot when moving close to things in narrow spaces and scaling down the linear velocity by curvature helps to stabilize the controller over a larger range of lookahead point distances. This also has the added benefit of removing the sensitive tuning of the lookahead point / range, as the robot will track paths far better. Tuning is still required, but it is substantially easier to get reasonable behavior with minor adjustments. + +An unintended tertiary benefit of scaling the linear velocities by curvature is that a robot will natively rotate to rough path heading when using holonomic planners that don't start aligned with the robot pose orientation. As the curvature will be very high, the linear velocity drops and the angular velocity takes over to rotate to heading. While not perfect, it does dramatically reduce the need to rotate to a close path heading before following and opens up a broader range of planning techniques. Pure Pursuit controllers otherwise would be completely unable to recover from this in even modestly confined spaces. + +Mixing the proximity and curvature regulated linear velocities with the time-scaled collision checker, we see a near-perfect combination allowing the regulated pure pursuit algorithm to handle high starting deviations from the path and navigate collision-free in tight spaces without overshoot. + +## Configuration + +| Parameter | Description | +|-----|----| +| `desired_linear_vel` | The desired maximum linear velocity to use. | +| `max_linear_accel` | Acceleration for linear velocity | +| `max_linear_decel` | Deceleration for linear velocity | +| `lookahead_dist` | The lookahead distance to use to find the lookahead point | +| `min_lookahead_dist` | The minimum lookahead distance threshold when using velocity scaled lookahead distances | +| `max_lookahead_dist` | The maximum lookahead distance threshold when using velocity scaled lookahead distances | +| `lookahead_time` | The time to project the velocity by to find the velocity scaled lookahead distance. Also known as the lookahead gain. | +| `rotate_to_heading_angular_vel` | If rotate to heading is used, this is the angular velocity to use. | +| `transform_tolerance` | The TF transform tolerance | +| `use_velocity_scaled_lookahead_dist` | Whether to use the velocity scaled lookahead distances or constant `lookahead_distance` | +| `min_approach_linear_velocity` | The minimum velocity threshold to apply when approaching the goal | +| `use_approach_linear_velocity_scaling` | Whether to scale the linear velocity down on approach to the goal for a smooth stop | +| `max_allowed_time_to_collision` | The time to project a velocity command to check for collisions | +| `use_regulated_linear_velocity_scaling` | Whether to use the regulated features for curvature | +| `use_cost_regulated_linear_velocity_scaling` | Whether to use the regulated features for proximity to obstacles | +| `regulated_linear_scaling_min_radius` | The turning radius for which the regulation features are triggered. Remember, sharper turns have smaller radii | +| `regulated_linear_scaling_min_speed` | The minimum speed for which the regulated features can send, to ensure process is still achievable even in high cost spaces with high curvature. | +| `use_rotate_to_heading` | Whether to enable rotating to rough heading when using holonomic planners. Recommended on for all robot types except ackermann, which cannot rotate in place. | +| `rotate_to_heading_min_angle` | The difference in the path orientation and the starting robot orientation to trigger a rotate in place, if enabled. | +| `max_angular_accel` | Maximum allowable angular acceleration while rotating to heading, if enabled | + + +Example fully-described XML with default parameter values: + +``` +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + progress_checker_plugin: "progress_checker" + goal_checker_plugin: "goal_checker" + controller_plugins: ["FollowPath"] + + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + goal_checker: + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + stateful: True + FollowPath: + plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" + desired_linear_vel: 0.5 + max_linear_accel: 2.5 + max_linear_decel: 2.5 + lookahead_dist: 0.6 + min_lookahead_dist: 0.3 + max_lookahead_dist: 0.9 + lookahead_time: 1.5 + rotate_to_heading_angular_vel: 1.8 + transform_tolerance: 0.1 + use_velocity_scaled_lookahead_dist: false + min_approach_linear_velocity: 0.05 + use_approach_linear_velocity_scaling: true + max_allowed_time_to_collision: 1.0 + use_regulated_linear_velocity_scaling: true + use_cost_regulated_linear_velocity_scaling: false + regulated_linear_scaling_min_radius: 0.9 + regulated_linear_scaling_min_speed: 0.25 + use_rotate_to_heading: true + rotate_to_heading_min_angle: 0.785 + max_angular_accel: 3.2 +``` + +## Topics + +| Topic | Type | Description | +|-----|----|----| +| `lookahead_point` | `geometry_msgs/PointStamped` | The current lookahead point on the path | +| `lookahead_arc` | `nav_msgs/Path` | The drivable arc between the robot and the carrot. Arc length depends on `max_allowed_time_to_collision`, forward simulating from the robot pose at the commanded `Twist` by that time. In a collision state, the last published arc will be the points leading up to, and including, the first point in collision. | + +Note: The `lookahead_arc` is also a really great speed indicator, when "full" to carrot or max time, you know you're at full speed. If 20% less, you can tell the robot is approximately 20% below maximum speed. Think of it as the collision checking bounds but also a speed guage. + +## Notes to users + +By default, the `use_cost_regulated_linear_velocity_scaling` is set to `false` because the generic sandbox environment we have setup is the TB3 world. This is a highly constrained environment so it overly triggers to slow the robot as everywhere has high costs. This is recommended to be set to `true` when not working in constantly high-cost spaces. + +To tune to get Adaptive Pure Pursuit behaviors, set all boolean parameters to false except `use_velocity_scaled_lookahead_dist` and make sure to tune `lookahead_time`, `min_lookahead_dist` and `max_lookahead_dist`. + +To tune to get Pure Pursuit behaviors, set all boolean parameters to false and make sure to tune `lookahead_dist`. + +Currently, there is no rotate to goal behaviors, so it is expected that the path approach orientations are the orientations of the goal or the goal checker has been set with a generous `min_theta_velocity_threshold`. Implementations for rotating to goal heading are on the way. + +The choice of lookahead distances are highly dependent on robot size, responsiveness, controller update rate, and speed. Please make sure to tune this for your platform, although the `regulated` features do largely make heavy tuning of this value unnecessary. If you see wiggling, increase the distance or scale. If it's not converging as fast to the path as you'd like, decrease it. diff --git a/nav2_pure_pursuit_controller/doc/lookahead_algorithm.png b/nav2_regulated_pure_pursuit_controller/doc/lookahead_algorithm.png similarity index 100% rename from nav2_pure_pursuit_controller/doc/lookahead_algorithm.png rename to nav2_regulated_pure_pursuit_controller/doc/lookahead_algorithm.png diff --git a/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp new file mode 100644 index 0000000..7d04272 --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -0,0 +1,131 @@ +// 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 +#include +#include +#include + +#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 RegulatedPurePursuitController : public nav2_core::Controller +{ +public: + RegulatedPurePursuitController() = default; + + ~RegulatedPurePursuitController() override = default; + + void configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + std::string name, const std::shared_ptr & tf, + const std::shared_ptr & costmap_ros) override; + + + void cleanup() override; + + void activate() override; + + void deactivate() override; + + geometry_msgs::msg::TwistStamped computeVelocityCommands( + const geometry_msgs::msg::PoseStamped & pose, + const geometry_msgs::msg::Twist & velocity) override; + + void setPlan(const nav_msgs::msg::Path & path) override; + +protected: + nav_msgs::msg::Path transformGlobalPlan(const geometry_msgs::msg::PoseStamped & pose); + + bool transformPose( + const std::string frame, + const geometry_msgs::msg::PoseStamped & in_pose, + geometry_msgs::msg::PoseStamped & out_pose) const; + + double getLookAheadDistance(const geometry_msgs::msg::Twist &); + + std::unique_ptr createCarrotMsg( + const geometry_msgs::msg::PoseStamped & carrot_pose); + + bool shouldRotateToPath( + const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path); + + void rotateToHeading(double & linear_vel, double & angular_vel, + const double & angle_to_path, const geometry_msgs::msg::Twist & curr_speed); + + bool isCollisionImminent( + const geometry_msgs::msg::PoseStamped &, + const geometry_msgs::msg::PoseStamped &, + const double &, const double &, const double &); + + bool inCollision(const double & x, const double & y); + + double costAtPose(const double & x, const double & y); + + void applyConstraints( + double & linear_vel, + const double & dist_error, const double & lookahead_dist, + const double & curvature, const geometry_msgs::msg::Twist & speed, + const double & pose_cost); + + geometry_msgs::msg::PoseStamped getLookAheadPoint(const double &, const nav_msgs::msg::Path &); + + std::shared_ptr tf_; + std::string plugin_name_; + std::shared_ptr 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 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_; + + nav_msgs::msg::Path global_plan_; + std::shared_ptr> global_pub_; + std::shared_ptr> carrot_pub_; + std::shared_ptr> carrot_arc_pub_; +}; + +} // namespace nav2_regulated_pure_pursuit_controller + +#endif // NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_ diff --git a/nav2_regulated_pure_pursuit_controller/nav2_regulated_pure_pursuit_controller.xml b/nav2_regulated_pure_pursuit_controller/nav2_regulated_pure_pursuit_controller.xml new file mode 100644 index 0000000..d695bce --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/nav2_regulated_pure_pursuit_controller.xml @@ -0,0 +1,10 @@ + + + + + nav2_regulated_pure_pursuit_controller + + + + + diff --git a/nav2_pure_pursuit_controller/package.xml b/nav2_regulated_pure_pursuit_controller/package.xml similarity index 76% rename from nav2_pure_pursuit_controller/package.xml rename to nav2_regulated_pure_pursuit_controller/package.xml index cb210c8..2e47a8e 100644 --- a/nav2_pure_pursuit_controller/package.xml +++ b/nav2_regulated_pure_pursuit_controller/package.xml @@ -1,11 +1,12 @@ - nav2_pure_pursuit_controller + nav2_regulated_pure_pursuit_controller 1.0.0 - Pure pursuit controller + Regulated Pure Pursuit Controller + Steve Macenski Shrijit Singh - BSD-3-Clause + Apache-2.0 ament_cmake diff --git a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp new file mode 100644 index 0000000..20f246a --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -0,0 +1,568 @@ +// 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. + +#include +#include +#include + +#include "nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp" +#include "nav2_core/exceptions.hpp" +#include "nav2_util/node_utils.hpp" +#include "nav2_util/geometry_utils.hpp" + +using std::hypot; +using std::min; +using std::max; +using std::abs; +using nav2_util::declare_parameter_if_not_declared; +using nav2_util::geometry_utils::euclidean_distance; +using namespace nav2_costmap_2d; // NOLINT + +namespace nav2_regulated_pure_pursuit_controller +{ + +/** + * Find element in iterator with the minimum calculated value + */ +template +Iter min_by(Iter begin, Iter end, Getter getCompareVal) +{ + if (begin == end) { + return end; + } + auto lowest = getCompareVal(*begin); + Iter lowest_it = begin; + for (Iter it = ++begin; it != end; ++it) { + auto comp = getCompareVal(*it); + if (comp < lowest) { + lowest = comp; + lowest_it = it; + } + } + return lowest_it; +} + +void RegulatedPurePursuitController::configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, + std::string name, const std::shared_ptr & tf, + const std::shared_ptr & costmap_ros) +{ + auto node = parent.lock(); + if (!node) { + throw std::runtime_error("Unable to lock node!"); + } + + costmap_ros_ = costmap_ros; + costmap_ = costmap_ros_->getCostmap(); + tf_ = tf; + plugin_name_ = name; + logger_ = node->get_logger(); + + double transform_tolerance = 0.1; + double control_frequency = 20.0; + + declare_parameter_if_not_declared( + node, plugin_name_ + ".desired_linear_vel", rclcpp::ParameterValue(0.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_linear_accel", rclcpp::ParameterValue(2.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_linear_decel", rclcpp::ParameterValue(2.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".lookahead_dist", rclcpp::ParameterValue(0.6)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".min_lookahead_dist", rclcpp::ParameterValue(0.3)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_lookahead_dist", rclcpp::ParameterValue(0.9)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".lookahead_time", rclcpp::ParameterValue(1.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".rotate_to_heading_angular_vel", rclcpp::ParameterValue(1.8)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".transform_tolerance", rclcpp::ParameterValue(0.1)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_velocity_scaled_lookahead_dist", + rclcpp::ParameterValue(false)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".min_approach_linear_velocity", rclcpp::ParameterValue(0.05)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_approach_linear_velocity_scaling", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_allowed_time_to_collision", rclcpp::ParameterValue(1.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_regulated_linear_velocity_scaling", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_cost_regulated_linear_velocity_scaling", rclcpp::ParameterValue(false)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".regulated_linear_scaling_min_radius", rclcpp::ParameterValue(0.90)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".regulated_linear_scaling_min_speed", rclcpp::ParameterValue(0.25)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".use_rotate_to_heading", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".rotate_to_heading_min_angle", rclcpp::ParameterValue(0.785)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(3.2)); + + node->get_parameter(plugin_name_ + ".desired_linear_vel", desired_linear_vel_); + node->get_parameter(plugin_name_ + ".max_linear_accel", max_linear_accel_); + node->get_parameter(plugin_name_ + ".max_linear_decel", max_linear_decel_); + node->get_parameter(plugin_name_ + ".lookahead_dist", lookahead_dist_); + node->get_parameter(plugin_name_ + ".min_lookahead_dist", min_lookahead_dist_); + node->get_parameter(plugin_name_ + ".max_lookahead_dist", max_lookahead_dist_); + node->get_parameter(plugin_name_ + ".lookahead_time", lookahead_time_); + node->get_parameter(plugin_name_ + ".rotate_to_heading_angular_vel", rotate_to_heading_angular_vel_); + node->get_parameter(plugin_name_ + ".transform_tolerance", transform_tolerance); + node->get_parameter(plugin_name_ + ".use_velocity_scaled_lookahead_dist", + use_velocity_scaled_lookahead_dist_); + node->get_parameter(plugin_name_ + ".min_approach_linear_velocity", min_approach_linear_velocity_); + node->get_parameter(plugin_name_ + ".use_approach_linear_velocity_scaling", use_approach_vel_scaling_); + node->get_parameter(plugin_name_ + ".max_allowed_time_to_collision", max_allowed_time_to_collision_); + node->get_parameter(plugin_name_ + ".use_regulated_linear_velocity_scaling", use_regulated_linear_velocity_scaling_); + node->get_parameter(plugin_name_ + ".use_cost_regulated_linear_velocity_scaling", use_cost_regulated_linear_velocity_scaling_); + node->get_parameter(plugin_name_ + ".regulated_linear_scaling_min_radius", regulated_linear_scaling_min_radius_); + node->get_parameter(plugin_name_ + ".regulated_linear_scaling_min_speed", regulated_linear_scaling_min_speed_); + node->get_parameter(plugin_name_ + ".use_rotate_to_heading", use_rotate_to_heading_); + node->get_parameter(plugin_name_ + ".rotate_to_heading_min_angle", rotate_to_heading_min_angle_); + node->get_parameter(plugin_name_ + ".max_angular_accel", max_angular_accel_); + node->get_parameter("control_frequency", control_frequency); + + transform_tolerance_ = tf2::durationFromSec(transform_tolerance); + control_duration_ = 1.0 / control_frequency; + + global_pub_ = node->create_publisher("received_global_plan", 1); + carrot_pub_ = node->create_publisher("lookahead_point", 1); + carrot_arc_pub_ = node->create_publisher("lookahead_collision_arc", 1); +} + +void RegulatedPurePursuitController::cleanup() +{ + RCLCPP_INFO( + logger_, + "Cleaning up controller: %s of type" + " regulated_pure_pursuit_controller::RegulatedPurePursuitController", + plugin_name_.c_str()); + global_pub_.reset(); + carrot_pub_.reset(); + carrot_arc_pub_.reset(); +} + +void RegulatedPurePursuitController::activate() +{ + RCLCPP_INFO( + logger_, + "Activating controller: %s of type " + "regulated_pure_pursuit_controller::RegulatedPurePursuitController\" %s", + plugin_name_.c_str()); + global_pub_->on_activate(); + carrot_pub_->on_activate(); + carrot_arc_pub_->on_activate(); +} + +void RegulatedPurePursuitController::deactivate() +{ + RCLCPP_INFO( + logger_, + "Deactivating controller: %s of type " + "regulated_pure_pursuit_controller::RegulatedPurePursuitController\" %s", + plugin_name_.c_str()); + global_pub_->on_deactivate(); + carrot_pub_->on_deactivate(); + carrot_arc_pub_->on_deactivate(); +} + +double RegulatedPurePursuitController::getLookAheadDistance(const geometry_msgs::msg::Twist & speed) +{ + // If using velocity-scaled look ahead distances, find and clamp the dist + // Else, use the static look ahead distance + double lookahead_dist = 0.0; + if (use_velocity_scaled_lookahead_dist_) { + lookahead_dist = speed.linear.x * lookahead_time_; + lookahead_dist = std::clamp(lookahead_dist, min_lookahead_dist_, max_lookahead_dist_); + } else { + lookahead_dist = lookahead_dist_; + } + + return lookahead_dist; +} + +std::unique_ptr RegulatedPurePursuitController::createCarrotMsg( + const geometry_msgs::msg::PoseStamped & carrot_pose) +{ + auto carrot_msg = std::make_unique(); + carrot_msg->header = carrot_pose.header; + carrot_msg->point.x = carrot_pose.pose.position.x; + carrot_msg->point.y = carrot_pose.pose.position.y; + carrot_msg->point.z = 0.01; // publish right over map to stand out + return carrot_msg; +} + +geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocityCommands( + const geometry_msgs::msg::PoseStamped & pose, + const geometry_msgs::msg::Twist & speed) +{ + // Transform path to robot base frame + auto transformed_plan = transformGlobalPlan(pose); + + // Find look ahead distance and point on path and publish + const double lookahead_dist = getLookAheadDistance(speed); + auto carrot_pose = getLookAheadPoint(lookahead_dist, transformed_plan); + carrot_pub_->publish(std::move(createCarrotMsg(carrot_pose))); + + double linear_vel, angular_vel; + + // Find distance^2 to look ahead point (carrot) in robot base frame + // This is the chord length of the circle + const double carrot_dist2 = + (carrot_pose.pose.position.x * carrot_pose.pose.position.x) + + (carrot_pose.pose.position.y * carrot_pose.pose.position.y); + + // Find curvature of circle (k = 1 / R) + double curvature = 0.0; + if (carrot_dist2 > 0.001) { + curvature = 2.0 * carrot_pose.pose.position.y / carrot_dist2; + } + + // Apply curvature to angular velocity + linear_vel = desired_linear_vel_; + angular_vel = desired_linear_vel_ * curvature; + + // Make sure we're in compliance with basic constraints + double angle_to_path; + if (shouldRotateToPath(carrot_pose, angle_to_path)) { + rotateToHeading(linear_vel, angular_vel, angle_to_path, speed); + } else { + applyConstraints( + linear_vel, fabs(lookahead_dist - sqrt(carrot_dist2)), + lookahead_dist, curvature, speed, + costAtPose(pose.pose.position.x, pose.pose.position.y)); + } + + // Collision checking on this velocity heading + if (isCollisionImminent(pose, carrot_pose, curvature, linear_vel, angular_vel)) { + RCLCPP_ERROR(logger_, "Collision imminent!"); + throw std::runtime_error("RegulatedPurePursuitController detected collision ahead!"); + } + + // populate and return message + geometry_msgs::msg::TwistStamped cmd_vel; + cmd_vel.header = pose.header; + cmd_vel.twist.linear.x = linear_vel; + cmd_vel.twist.angular.z = angular_vel; + return cmd_vel; +} + +bool RegulatedPurePursuitController::shouldRotateToPath( + const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path) +{ + // Whether we should rotate robot to rough path heading + angle_to_path = atan2(carrot_pose.pose.position.y, carrot_pose.pose.position.x); + return use_rotate_to_heading_ && fabs(angle_to_path) > rotate_to_heading_min_angle_; +} + +void RegulatedPurePursuitController::rotateToHeading( + double & linear_vel, double & angular_vel, + const double & angle_to_path, const geometry_msgs::msg::Twist & curr_speed) +{ + // Rotate in place using max angular velocity / acceleration possible + linear_vel = 0.0; + const double sign = angle_to_path > 0.0 ? 1.0 : -1.0; + angular_vel = sign * rotate_to_heading_angular_vel_; + + const double & dt = control_duration_; + const double min_feasible_angular_speed = curr_speed.angular.z - max_angular_accel_ * dt; + const double max_feasible_angular_speed = curr_speed.angular.z + max_angular_accel_ * dt; + angular_vel = std::clamp(angular_vel, min_feasible_angular_speed, max_feasible_angular_speed); +} + +geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoint( + const double & lookahead_dist, + const nav_msgs::msg::Path & transformed_plan) +{ + // Find the first pose which is at a distance greater than the lookahead distance + auto goal_pose_it = std::find_if( + transformed_plan.poses.begin(), transformed_plan.poses.end(), [&](const auto & ps) { + return hypot(ps.pose.position.x, ps.pose.position.y) >= lookahead_dist; + }); + + // If the no pose is not far enough, take the last pose + if (goal_pose_it == transformed_plan.poses.end()) { + goal_pose_it = std::prev(transformed_plan.poses.end()); + } + + return *goal_pose_it; +} + +bool RegulatedPurePursuitController::isCollisionImminent( + const geometry_msgs::msg::PoseStamped & robot_pose, + const geometry_msgs::msg::PoseStamped & carrot_pose, + const double & curvature, const double & linear_vel, + const double & angular_vel) +{ + // Note(stevemacenski): This may be a bit unusual, but the robot_pose is in + // odom frame and the carrot_pose is in robot base frame. We need to collision + // check in odom frame, so all values will be relative to robot base pose. + // But we can still use the carrot pose in odom to find various quantities. + + geometry_msgs::msg::PoseStamped carrot_in_odom; + if (!transformPose(costmap_ros_->getGlobalFrameID(), carrot_pose, carrot_in_odom)) + { + RCLCPP_ERROR(logger_, "Unable to get carrot pose in odom frame, failing collision check!"); + return true; + } + + // check current point and carrot point are OK, most likely ones to be in collision + if (inCollision(carrot_in_odom.pose.position.x, carrot_in_odom.pose.position.y) || + inCollision(robot_pose.pose.position.x, robot_pose.pose.position.y)) + { + return true; + } + + // debug messages + nav_msgs::msg::Path arc_pts_msg; + arc_pts_msg.header.frame_id = costmap_ros_->getGlobalFrameID(); + arc_pts_msg.header.stamp = robot_pose.header.stamp; + geometry_msgs::msg::PoseStamped pose_msg; + pose_msg.header.frame_id = arc_pts_msg.header.frame_id; + pose_msg.header.stamp = arc_pts_msg.header.stamp; + + // Using curvature (k = 1 / R) and carrot distance (chord on circle R), project the command + const double chord_len = hypot(carrot_pose.pose.position.x, carrot_pose.pose.position.y); + + // Find the number of increments by finding the arc length of the chord on circle + const double r = fabs(1.0 / curvature); + const double alpha = 2.0 * asin(chord_len / (2 * r)); // central angle + const double arc_len = alpha * r; + const double delta_dist = costmap_->getResolution(); + const unsigned int num_pts = static_cast(ceil(arc_len / delta_dist)); + const double projection_time = costmap_->getResolution() / linear_vel; + + geometry_msgs::msg::Pose2D curr_pose; + curr_pose.x = robot_pose.pose.position.x; + curr_pose.y = robot_pose.pose.position.y; + curr_pose.theta = tf2::getYaw(robot_pose.pose.orientation); + + for (unsigned int i = 1; i < num_pts; i++) { + // only forward simulate within time requested + if (i * projection_time > max_allowed_time_to_collision_) { + break; + } + + // apply velocity at curr_pose over distance delta_dist + curr_pose.x += projection_time * (linear_vel * cos(curr_pose.theta)); + curr_pose.y += projection_time * (linear_vel * sin(curr_pose.theta)); + curr_pose.theta += projection_time * angular_vel; + + // store it for visualization + pose_msg.pose.position.x = curr_pose.x; + pose_msg.pose.position.y = curr_pose.y; + pose_msg.pose.position.z = 0.01; + arc_pts_msg.poses.push_back(pose_msg); + + // check for collision at this point + if (inCollision(curr_pose.x, curr_pose.y)) { + carrot_arc_pub_->publish(arc_pts_msg); + return true; + } + } + + carrot_arc_pub_->publish(arc_pts_msg); + + return false; +} + +bool RegulatedPurePursuitController::inCollision(const double & x, const double & y) +{ + unsigned int mx, my; + costmap_->worldToMap(x, y, mx, my); + + unsigned char cost = costmap_->getCost(mx, my); + + if (costmap_ros_->getLayeredCostmap()->isTrackingUnknown()) { + return cost >= INSCRIBED_INFLATED_OBSTACLE && cost != NO_INFORMATION; + } else { + return cost >= INSCRIBED_INFLATED_OBSTACLE; + } +} + +double RegulatedPurePursuitController::costAtPose(const double & x, const double & y) +{ + unsigned int mx, my; + costmap_->worldToMap(x, y, mx, my); + + unsigned char cost = costmap_->getCost(mx, my); + return static_cast(cost); +} + +void RegulatedPurePursuitController::applyConstraints( + double & linear_vel, + const double & dist_error, const double & lookahead_dist, + const double & curvature, const geometry_msgs::msg::Twist & curr_speed, + const double & pose_cost) +{ + // limit the linear velocity by curvature + const double radius = fabs(1.0 / curvature); + const double & min_rad = regulated_linear_scaling_min_radius_; + if (use_regulated_linear_velocity_scaling_ && radius < min_rad) { + linear_vel *= 1.0 - (fabs(radius - min_rad) / min_rad); + if (linear_vel < regulated_linear_scaling_min_speed_) { + linear_vel = regulated_linear_scaling_min_speed_; + } + } + + // limit the linear velocity by proximity to obstacles + if (use_cost_regulated_linear_velocity_scaling_ && + pose_cost != static_cast(NO_INFORMATION)) + { + // Note(stevemacenski): We can use the cost at a pose as a derived value proportional to + // distance to obstacle since we lack a distance map. [0-128] is the freespace costed + // range, above 128 is possibly inscribed, so it should max out at 128 to be a minimum speed + // in when in questionable states. This creates a linear mapping of + // cost [0, 128] to speed [min regulated speed, linear vel] + double max_non_collision = 128.0; + if (pose_cost > max_non_collision) { + linear_vel = regulated_linear_scaling_min_speed_; + } else { + const double slope = (regulated_linear_scaling_min_speed_ - linear_vel) / max_non_collision; + linear_vel = slope * pose_cost + linear_vel; + } + } + + // if the actual lookahead distance is shorter than requested, that means we're at the + // end of the path. We'll scale linear velocity by error to slow to a smooth stop + if (use_approach_vel_scaling_ && dist_error > 2.0 * costmap_->getResolution()) { + double velocity_scaling = 1.0 - (dist_error / lookahead_dist); + double unbounded_vel = linear_vel * velocity_scaling; + if (unbounded_vel < min_approach_linear_velocity_) { + linear_vel = min_approach_linear_velocity_; + } else { + linear_vel *= velocity_scaling; + } + } + + // Limit linear velocities to be kinematically feasible, v = v0 + a * dt + double & dt = control_duration_; + const double max_feasible_linear_speed = curr_speed.linear.x + max_linear_accel_ * dt; + const double min_feasible_linear_speed = curr_speed.linear.x - max_linear_decel_ * dt; + linear_vel = std::clamp(linear_vel, min_feasible_linear_speed, max_feasible_linear_speed); + + // Note(stevemacenski): for the moment, no smoothing on angular velocities, we find the existing + // commands are very smooth and we don't want to artifically reduce them if the hardware + // can handle it. Plus deviating from the commands here can be collision-inducing if users + // don't properly set these values, so hiding that complexity from them that would likely + // become the #1 cause of issues. + // const double max_feasible_angular_speed = curr_speed.angular.z + angular_accel_ * dt; + // const double min_feasible_angular_speed = curr_speed.angular.z - angular_accel_ * dt; + // angular_vel = std::clamp(angular_vel, min_feasible_angular_speed, max_feasible_angular_speed); + + // Limit range to generally valid velocities + linear_vel = std::clamp(linear_vel, 0.0, desired_linear_vel_); +} + +void RegulatedPurePursuitController::setPlan(const nav_msgs::msg::Path & path) +{ + global_plan_ = path; +} + +nav_msgs::msg::Path RegulatedPurePursuitController::transformGlobalPlan( + const geometry_msgs::msg::PoseStamped & pose) +{ + if (global_plan_.poses.empty()) { + throw nav2_core::PlannerException("Received plan with zero length"); + } + + // let's get the pose of the robot in the frame of the plan + geometry_msgs::msg::PoseStamped robot_pose; + if (!transformPose(global_plan_.header.frame_id, pose, robot_pose)) + { + throw nav2_core::PlannerException("Unable to transform robot pose into global plan's frame"); + } + + // We'll discard points on the plan that are outside the local costmap + nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap(); + const double max_costmap_dim = std::max(costmap->getSizeInCellsX(), costmap->getSizeInCellsY()); + const double max_transform_dist = max_costmap_dim * costmap->getResolution() / 2.0; + + // First find the closest pose on the path to the robot + auto transformation_begin = + min_by( + global_plan_.poses.begin(), global_plan_.poses.end(), + [&robot_pose](const geometry_msgs::msg::PoseStamped & ps) { + return euclidean_distance(robot_pose, ps); + }); + + // Find points definitely outside of the costmap so we won't transform them. + auto transformation_end = std::find_if( + transformation_begin, end(global_plan_.poses), + [&](const auto & global_plan_pose) { + return euclidean_distance(robot_pose, global_plan_pose) > max_transform_dist; + }); + + // Lambda to transform a PoseStamped from global frame to local + auto transformGlobalPoseToLocal = [&](const auto & global_plan_pose) { + geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; + stamped_pose.header.frame_id = global_plan_.header.frame_id; + stamped_pose.header.stamp = robot_pose.header.stamp; + stamped_pose.pose = global_plan_pose.pose; + transformPose(costmap_ros_->getBaseFrameID(), stamped_pose, transformed_pose); + return transformed_pose; + }; + + // Transform the near part of the global plan into the robot's frame of reference. + nav_msgs::msg::Path transformed_plan; + std::transform( + transformation_begin, transformation_end, + std::back_inserter(transformed_plan.poses), + transformGlobalPoseToLocal); + transformed_plan.header.frame_id = costmap_ros_->getBaseFrameID(); + transformed_plan.header.stamp = robot_pose.header.stamp; + + // Remove the portion of the global plan that we've already passed so we don't + // process it on the next iteration (this is called path pruning) + global_plan_.poses.erase(begin(global_plan_.poses), transformation_begin); + global_pub_->publish(transformed_plan); + + if (transformed_plan.poses.empty()) { + throw nav2_core::PlannerException("Resulting plan has 0 poses in it."); + } + + return transformed_plan; +} + +bool RegulatedPurePursuitController::transformPose( + const std::string frame, + const geometry_msgs::msg::PoseStamped & in_pose, + geometry_msgs::msg::PoseStamped & out_pose) const +{ + if (in_pose.header.frame_id == frame) { + out_pose = in_pose; + return true; + } + + try { + tf_->transform(in_pose, out_pose, frame, transform_tolerance_); + out_pose.header.frame_id = frame; + return true; + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR(logger_, "Exception in transformPose: %s", ex.what()); + } + return false; +} + +} // namespace nav2_pure_pursuit_controller + +// Register this controller as a nav2_core plugin +PLUGINLIB_EXPORT_CLASS( + nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController, + nav2_core::Controller)