From dc871e9dd612014383431f698c6e584e63302f63 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 4 Dec 2020 17:41:32 -0800 Subject: [PATCH 01/32] adding simplifications and outlines for new featuers --- .../pure_pursuit_controller.hpp | 13 +- .../src/pure_pursuit_controller.cpp | 197 ++++++++++-------- 2 files changed, 119 insertions(+), 91 deletions(-) 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 index 2007c30..a0a6cb0 100644 --- 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 @@ -46,12 +46,11 @@ class PurePursuitController : public nav2_core::Controller 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; + geometry_msgs::msg::PoseStamped & out_pose) const; + + double getLookAheadDistance(); rclcpp_lifecycle::LifecycleNode::WeakPtr node_; std::shared_ptr tf_; @@ -63,6 +62,12 @@ class PurePursuitController : public nav2_core::Controller double desired_linear_vel_; double lookahead_dist_; double max_angular_vel_; + double max_lookahead_dist_; + double min_lookahead_dist_; + double lookahead_gain_; + double max_accel_; + double max_decel_; + bool use_velocity_scaled_lookahead_dist_; rclcpp::Duration transform_tolerance_ {0, 0}; nav_msgs::msg::Path global_plan_; diff --git a/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp b/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp index 412dce3..0d502fc 100644 --- a/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp +++ b/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp @@ -53,6 +53,9 @@ void PurePursuitController::configure( node_ = parent; auto node = node_.lock(); + if (!node) { + throw std::runtime_error("Unable to lock node!"); + } costmap_ros_ = costmap_ros; tf_ = tf; @@ -61,23 +64,42 @@ void PurePursuitController::configure( clock_ = node->get_clock(); declare_parameter_if_not_declared( - node, plugin_name_ + ".desired_linear_vel", rclcpp::ParameterValue( - 0.2)); + node, plugin_name_ + ".desired_linear_vel", rclcpp::ParameterValue(0.5)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_accel", rclcpp::ParameterValue(1.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_decel", rclcpp::ParameterValue(1.0)); 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)); + node, plugin_name_ + ".min_lookahead_dist", + rclcpp::ParameterValue(0.3)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_lookahead_dist", + rclcpp::ParameterValue(0.6)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".lookahead_gain", + rclcpp::ParameterValue(1.5)); declare_parameter_if_not_declared( - node, plugin_name_ + ".transform_tolerance", rclcpp::ParameterValue( - 0.1)); + node, plugin_name_ + ".max_angular_vel", rclcpp::ParameterValue(1.0)); + 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)); node->get_parameter(plugin_name_ + ".desired_linear_vel", desired_linear_vel_); + node->get_parameter(plugin_name_ + ".max_accel", max_accel_); + node->get_parameter(plugin_name_ + ".max_decel", max_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_gain", lookahead_gain_); node->get_parameter(plugin_name_ + ".max_angular_vel", max_angular_vel_); double transform_tolerance; node->get_parameter(plugin_name_ + ".transform_tolerance", transform_tolerance); + node->get_parameter(plugin_name_ + ".use_velocity_scaled_lookahead_dist", use_velocity_scaled_lookahead_dist_); transform_tolerance_ = rclcpp::Duration::from_seconds(transform_tolerance); global_pub_ = node->create_publisher("received_global_plan", 1); @@ -110,50 +132,97 @@ void PurePursuitController::deactivate() global_pub_->on_deactivate(); } +double PurePursuitController::getLookAheadDistance() +{ + // If using velocity-scaled look ahead distances, find and clamp the dist + // Else, use the default look ahead distance + double lookahead_dist = 0.0; + if (use_velocity_scaled_lookahead_dist_) { + lookahead_dist = speed.linear.x * lookahead_gain_; + lookahead_dist = std::clamp(lookahead_dist, min_lookahead_dist_, max_lookahead_dist_); + } else { + lookahead_dist = lookahead_dist_; + } + + return lookahead_dist; +} + +// PROFILES +// [] acceleration profiling, ramp up and down +// [] if carrot dist < requested, scale down speeds because on approach to goal +// [] collision checking robot pose + along carrot +// [DONE] scale look ahead dist by speed + +// ROTATE (separate class this uses that can be used elsewhere too?) +// rotate after goal to get orientation +// rotate to heading to start geometry_msgs::msg::TwistStamped PurePursuitController::computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, - const geometry_msgs::msg::Twist &) + const geometry_msgs::msg::Twist & speed) { + // Transform path to robot base frame auto transformed_plan = transformGlobalPlan(pose); - // Find the first pose which is at a distance greater than the specified lookahed distance + // Find look ahead distance + const double lookahead_dist = getLookAheadDistance(); + + // 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_; + 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 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()); } - auto goal_pose = goal_pose_it->pose; + auto carrot_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_; + // Find distance to look ahead point (carrot) + double carrot_dist = hypot( + pose.pose.position.x - carrot_pose.position.x, + pose.pose.position.y - carrot_pose.position.y); + + // Find curvature of circular arc + double curvature = 0.0; + if (carrot_dist > 0.001) { + curvature = 2.0 * carrot_pose.position.y / (carrot_dist * carrot_dist); } - // Create and publish a TwistStamped message with the desired velocity + // Apply curvature to angular velocity + linear_vel = desired_linear_vel_; + angular_vel = desired_linear_vel_ * curvature; + + // TODO apply acceleration profiles and ramp down on carrot dist + // does ramp down + // carrot_dist_err = fabs(carrot_dist - lookahead_dist) + // if (carrot_dist_err > costmap_resolution) + // v linear out = v linear * (carrot_dist_error / lookahead_dist); + // + // store last velocities / timestamp + // v - v0 = at + // if a < a max, OK (check if speeding up or slowing down to apply accel / decel max) + // else, set v = v0 + at + // apply to linear and angular (helps with ramp up and angular changes) + + // make sure in range of valid velocities + angular_vel = std::clamp(angular_vel, -max_angular_vel_, max_angular_vel_); + + // TODO collision check robot pose -> carrot (function) + // get pose robot and carrot + // interpolate between them costmap resolution + // check for collisions + // return bool true/false + + // populate and return message 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_))); - + cmd_vel.twist.angular.z = angular_vel; return cmd_vel; } @@ -167,25 +236,21 @@ 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_)) + 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(); - double dist_threshold = std::max(costmap->getSizeInCellsX(), costmap->getSizeInCellsY()) * - costmap->getResolution() / 2.0; + 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 = @@ -195,35 +260,30 @@ PurePursuitController::transformGlobalPlan( 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. + // 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) > dist_threshold; + return euclidean_distance(robot_pose, global_plan_pose) > max_transform_dist; }); - // Helper function for the transform below. Transforms a PoseStamped from global frame to local + // Lambda to transform a PoseStamped from global frame to local + geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; 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.header.stamp = robot_pose.header.stamp; stamped_pose.pose = global_plan_pose.pose; - transformPose( - tf_, costmap_ros_->getBaseFrameID(), - stamped_pose, transformed_pose, transform_tolerance_); + 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 = pose.header.stamp; + 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) @@ -238,12 +298,9 @@ PurePursuitController::transformGlobalPlan( } 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 + geometry_msgs::msg::PoseStamped & out_pose) const { // Implementation taken as is fron nav_2d_utils in nav2_dwb_controller @@ -253,44 +310,10 @@ bool PurePursuitController::transformPose( } try { - tf->transform(in_pose, out_pose, frame); + tf_->transform(in_pose, out_pose, frame, transform_tolerance_); 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; + RCLCPP_ERROR(logger_, "Exception in transformPose: %s", ex.what()); } return false; } From f56a1df5c398e59896c1e80d97e34ac79e4e4b30 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 4 Dec 2020 18:11:21 -0800 Subject: [PATCH 02/32] adding kinematic constraints --- .../pure_pursuit_controller.hpp | 7 ++ .../src/pure_pursuit_controller.cpp | 72 ++++++++++++++----- 2 files changed, 60 insertions(+), 19 deletions(-) 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 index a0a6cb0..2f4232e 100644 --- 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 @@ -52,6 +52,12 @@ class PurePursuitController : public nav2_core::Controller double getLookAheadDistance(); + bool isCollisionImminent(const geometry_msgs::msg::PoseStamped &, const geometry_msgs::msg::PoseStamped &); + + void applyKinematicConstraints( + double & linear_vel, double & angular_vel, + const double & dist_error, const double & lookahead_dist, const double & dt); + rclcpp_lifecycle::LifecycleNode::WeakPtr node_; std::shared_ptr tf_; std::string plugin_name_; @@ -69,6 +75,7 @@ class PurePursuitController : public nav2_core::Controller double max_decel_; bool use_velocity_scaled_lookahead_dist_; rclcpp::Duration transform_tolerance_ {0, 0}; + geometry_msgs::msg::TwistStamped last_cmd_; nav_msgs::msg::Path global_plan_; std::shared_ptr> global_pub_; diff --git a/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp b/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp index 0d502fc..0e03a19 100644 --- a/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp +++ b/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp @@ -149,7 +149,7 @@ double PurePursuitController::getLookAheadDistance() // PROFILES // [] acceleration profiling, ramp up and down -// [] if carrot dist < requested, scale down speeds because on approach to goal +// [DONE] if carrot dist < requested, scale down speeds because on approach to goal // [] collision checking robot pose + along carrot // [DONE] scale look ahead dist by speed @@ -178,11 +178,11 @@ geometry_msgs::msg::TwistStamped PurePursuitController::computeVelocityCommands( goal_pose_it = std::prev(transformed_plan.poses.end()); } - auto carrot_pose = goal_pose_it->pose; + auto & carrot_pose = goal_pose_it->pose; double linear_vel, angular_vel; // Find distance to look ahead point (carrot) - double carrot_dist = hypot( + const double carrot_dist = hypot( pose.pose.position.x - carrot_pose.position.x, pose.pose.position.y - carrot_pose.position.y); @@ -196,26 +196,18 @@ geometry_msgs::msg::TwistStamped PurePursuitController::computeVelocityCommands( linear_vel = desired_linear_vel_; angular_vel = desired_linear_vel_ * curvature; - // TODO apply acceleration profiles and ramp down on carrot dist - // does ramp down - // carrot_dist_err = fabs(carrot_dist - lookahead_dist) - // if (carrot_dist_err > costmap_resolution) - // v linear out = v linear * (carrot_dist_error / lookahead_dist); - // - // store last velocities / timestamp - // v - v0 = at - // if a < a max, OK (check if speeding up or slowing down to apply accel / decel max) - // else, set v = v0 + at - // apply to linear and angular (helps with ramp up and angular changes) + rclcpp::Duration dt = pose.header.stamp - last_cmd_.header.stamp; + applyKinematicConstraints( + linear_vel, angular_vel, fabs(lookahead_dist - carrot_dist), lookahead_dist, dt.seconds()); // make sure in range of valid velocities angular_vel = std::clamp(angular_vel, -max_angular_vel_, max_angular_vel_); + linear_vel = std::clamp(linear_vel, 0.0, desired_linear_vel_); - // TODO collision check robot pose -> carrot (function) - // get pose robot and carrot - // interpolate between them costmap resolution - // check for collisions - // return bool true/false + if (isCollisionImminent(pose, carrot_pose)) { + RCLCPP_ERROR(logger_, "Collision imminent!"); + throw std::runtime_error("PurePursuitController detected collision ahead!"); + } // populate and return message geometry_msgs::msg::TwistStamped cmd_vel; @@ -223,9 +215,50 @@ geometry_msgs::msg::TwistStamped PurePursuitController::computeVelocityCommands( cmd_vel.header.stamp = clock_->now(); cmd_vel.twist.linear.x = linear_vel; cmd_vel.twist.angular.z = angular_vel; + last_cmd_ = cmd_vel; return cmd_vel; } +bool PurePursuitController::isCollisionImminent( + const geometry_msgs::msg::PoseStamped & robot_pose, + const geometry_msgs::msg::PoseStamped & carrot_pose) +{ + // TODO + // interpolate between carrot/robot at costmap resolution + // check for collisions + // return bool true/false +} + +void PurePursuitController::applyKinematicConstraints( + double & linear_vel, double & angular_vel, + const double & dist_error, const double & lookahead_dist, const double & dt) +{ + // TODO need separate accel/decel for linear and angular velocities + + // 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 (dist_error > 2.0 * costmap_ros_->getCostmap()->getResolution()) { + linear_vel = linear_vel * (dist_error / lookahead_dist); + } + + // if we're accelerating or decelerating too fast, limit linear velocity + const double dt = ...; + double measured_lin_accel = (linear_vel - last_cmd_.twist.linear.x) / dt; + if (measured_lin_accel > max_accel_) { + linear_vel = last_cmd_.twist.linear.x + max_accel_ * dt; + } else if (measured_lin_accel < -max_decel) { + linear_vel = last_cmd_.twist.linear.x - max_decel_ * dt; + } + + // if we're accelerating or decelerating too fast, limit angular velocity + double measured_ang_accel = (angular_vel - last_cmd_.twist.angular.z) / dt; + if (measured_ang_accel > max_accel) { + angular_vel = last_cmd_.twist.angular.z + max_accel_ * dt; + } else if (measured_ang_accel < -max_decel) { + angular_vel = last_cmd_.twist.angular.z - max_decel_ * dt; + } +} + void PurePursuitController::setPlan(const nav_msgs::msg::Path & path) { global_pub_->publish(path); @@ -278,6 +311,7 @@ PurePursuitController::transformGlobalPlan( }; // 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), From becd371fb2372b1ed93fb48e076a28ae3b4b85ed Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 4 Dec 2020 18:16:43 -0800 Subject: [PATCH 03/32] adding max acceleration and deceleration for angular vel --- .../pure_pursuit_controller.hpp | 6 +++-- .../src/pure_pursuit_controller.cpp | 27 ++++++++++--------- 2 files changed, 19 insertions(+), 14 deletions(-) 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 index 2f4232e..2aa75bd 100644 --- 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 @@ -71,8 +71,10 @@ class PurePursuitController : public nav2_core::Controller double max_lookahead_dist_; double min_lookahead_dist_; double lookahead_gain_; - double max_accel_; - double max_decel_; + double max_linear_accel_; + double max_linear_decel_; + double max_angular_accel_; + double max_angular_decel_; bool use_velocity_scaled_lookahead_dist_; rclcpp::Duration transform_tolerance_ {0, 0}; geometry_msgs::msg::TwistStamped last_cmd_; diff --git a/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp b/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp index 0e03a19..d52f2e3 100644 --- a/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp +++ b/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp @@ -66,9 +66,13 @@ void PurePursuitController::configure( declare_parameter_if_not_declared( node, plugin_name_ + ".desired_linear_vel", rclcpp::ParameterValue(0.5)); declare_parameter_if_not_declared( - node, plugin_name_ + ".max_accel", rclcpp::ParameterValue(1.0)); + node, plugin_name_ + ".max_linear_accel", rclcpp::ParameterValue(1.0)); declare_parameter_if_not_declared( - node, plugin_name_ + ".max_decel", rclcpp::ParameterValue(1.0)); + node, plugin_name_ + ".max_linear_decel", rclcpp::ParameterValue(1.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(1.0)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_angular_decel", rclcpp::ParameterValue(1.0)); declare_parameter_if_not_declared( node, plugin_name_ + ".lookahead_dist", rclcpp::ParameterValue(0.4)); @@ -90,8 +94,10 @@ void PurePursuitController::configure( false)); node->get_parameter(plugin_name_ + ".desired_linear_vel", desired_linear_vel_); - node->get_parameter(plugin_name_ + ".max_accel", max_accel_); - node->get_parameter(plugin_name_ + ".max_decel", max_decel_); + 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_ + ".max_angular_accel", max_angular_accel_); + node->get_parameter(plugin_name_ + ".max_angular_decel", max_angular_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_); @@ -148,7 +154,7 @@ double PurePursuitController::getLookAheadDistance() } // PROFILES -// [] acceleration profiling, ramp up and down +// [DONE] acceleration profiling, ramp up and down // [DONE] if carrot dist < requested, scale down speeds because on approach to goal // [] collision checking robot pose + along carrot @@ -233,8 +239,6 @@ void PurePursuitController::applyKinematicConstraints( double & linear_vel, double & angular_vel, const double & dist_error, const double & lookahead_dist, const double & dt) { - // TODO need separate accel/decel for linear and angular velocities - // 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 (dist_error > 2.0 * costmap_ros_->getCostmap()->getResolution()) { @@ -242,20 +246,19 @@ void PurePursuitController::applyKinematicConstraints( } // if we're accelerating or decelerating too fast, limit linear velocity - const double dt = ...; double measured_lin_accel = (linear_vel - last_cmd_.twist.linear.x) / dt; if (measured_lin_accel > max_accel_) { - linear_vel = last_cmd_.twist.linear.x + max_accel_ * dt; + linear_vel = last_cmd_.twist.linear.x + max_linear_accel_ * dt; } else if (measured_lin_accel < -max_decel) { - linear_vel = last_cmd_.twist.linear.x - max_decel_ * dt; + linear_vel = last_cmd_.twist.linear.x - max_linear_decel_ * dt; } // if we're accelerating or decelerating too fast, limit angular velocity double measured_ang_accel = (angular_vel - last_cmd_.twist.angular.z) / dt; if (measured_ang_accel > max_accel) { - angular_vel = last_cmd_.twist.angular.z + max_accel_ * dt; + angular_vel = last_cmd_.twist.angular.z + max_angular_accel_ * dt; } else if (measured_ang_accel < -max_decel) { - angular_vel = last_cmd_.twist.angular.z - max_decel_ * dt; + angular_vel = last_cmd_.twist.angular.z - max_angular_decel_ * dt; } } From 2b7401004b7889d2d9368499a9736500770704da Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 7 Dec 2020 14:40:04 -0800 Subject: [PATCH 04/32] rename and move files around a bit --- .../CMakeLists.txt | 5 +- .../README.md | 0 .../doc/lookahead_algorithm.png | Bin .../pure_pursuit_controller.hpp | 16 +- .../nav2_pure_pursuit.xml | 0 .../package.xml | 2 +- .../src/pure_pursuit_controller.cpp | 157 ++++++++++++------ 7 files changed, 120 insertions(+), 60 deletions(-) rename {nav2_pure_pursuit_controller => nav2_pure_pursuit}/CMakeLists.txt (95%) rename {nav2_pure_pursuit_controller => nav2_pure_pursuit}/README.md (100%) rename {nav2_pure_pursuit_controller => nav2_pure_pursuit}/doc/lookahead_algorithm.png (100%) rename {nav2_pure_pursuit_controller/include/nav2_pure_pursuit_controller => nav2_pure_pursuit/include/nav2_pure_pursuit}/pure_pursuit_controller.hpp (82%) rename nav2_pure_pursuit_controller/nav2_pure_pursuit_controller.xml => nav2_pure_pursuit/nav2_pure_pursuit.xml (100%) rename {nav2_pure_pursuit_controller => nav2_pure_pursuit}/package.xml (95%) rename {nav2_pure_pursuit_controller => nav2_pure_pursuit}/src/pure_pursuit_controller.cpp (75%) diff --git a/nav2_pure_pursuit_controller/CMakeLists.txt b/nav2_pure_pursuit/CMakeLists.txt similarity index 95% rename from nav2_pure_pursuit_controller/CMakeLists.txt rename to nav2_pure_pursuit/CMakeLists.txt index 7419f3b..cc36cc5 100644 --- a/nav2_pure_pursuit_controller/CMakeLists.txt +++ b/nav2_pure_pursuit/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(nav2_pure_pursuit_controller) +project(nav2_pure_pursuit) 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 @@ -60,7 +61,7 @@ ament_export_include_directories(include) ament_export_libraries(nav2_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_pure_pursuit.xml) ament_package() diff --git a/nav2_pure_pursuit_controller/README.md b/nav2_pure_pursuit/README.md similarity index 100% rename from nav2_pure_pursuit_controller/README.md rename to nav2_pure_pursuit/README.md diff --git a/nav2_pure_pursuit_controller/doc/lookahead_algorithm.png b/nav2_pure_pursuit/doc/lookahead_algorithm.png similarity index 100% rename from nav2_pure_pursuit_controller/doc/lookahead_algorithm.png rename to nav2_pure_pursuit/doc/lookahead_algorithm.png diff --git a/nav2_pure_pursuit_controller/include/nav2_pure_pursuit_controller/pure_pursuit_controller.hpp b/nav2_pure_pursuit/include/nav2_pure_pursuit/pure_pursuit_controller.hpp similarity index 82% rename from nav2_pure_pursuit_controller/include/nav2_pure_pursuit_controller/pure_pursuit_controller.hpp rename to nav2_pure_pursuit/include/nav2_pure_pursuit/pure_pursuit_controller.hpp index 2aa75bd..246beff 100644 --- a/nav2_pure_pursuit_controller/include/nav2_pure_pursuit_controller/pure_pursuit_controller.hpp +++ b/nav2_pure_pursuit/include/nav2_pure_pursuit/pure_pursuit_controller.hpp @@ -2,15 +2,17 @@ * SPDX-License-Identifier: BSD-3-Clause * * Author(s): Shrijit Singh + * Author(s): Steve Macenski * */ -#ifndef NAV2_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_ -#define NAV2_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_ +#ifndef NAV2_PURE_PURSUIT__PURE_PURSUIT_CONTROLLER_HPP_ +#define NAV2_PURE_PURSUIT__PURE_PURSUIT_CONTROLLER_HPP_ #include #include #include +#include #include "nav2_core/controller.hpp" #include "rclcpp/rclcpp.hpp" @@ -29,7 +31,7 @@ class PurePursuitController : public nav2_core::Controller void configure( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, std::string name, const std::shared_ptr & tf, - const std::shared_ptr & costmap_ros); + const std::shared_ptr & costmap_ros) override; void cleanup() override; @@ -50,18 +52,22 @@ class PurePursuitController : public nav2_core::Controller const geometry_msgs::msg::PoseStamped & in_pose, geometry_msgs::msg::PoseStamped & out_pose) const; - double getLookAheadDistance(); + double getLookAheadDistance(const geometry_msgs::msg::Twist &); bool isCollisionImminent(const geometry_msgs::msg::PoseStamped &, const geometry_msgs::msg::PoseStamped &); + bool inCollision(const double & x, const double & y); void applyKinematicConstraints( double & linear_vel, double & angular_vel, const double & dist_error, const double & lookahead_dist, const double & dt); + geometry_msgs::msg::PoseStamped getLookAheadMarker(const double &, const nav_msgs::msg::Path &); + rclcpp_lifecycle::LifecycleNode::WeakPtr node_; std::shared_ptr tf_; std::string plugin_name_; std::shared_ptr costmap_ros_; + nav2_costmap_2d::Costmap2D * costmap_; rclcpp::Logger logger_ {rclcpp::get_logger("PurePursuitController")}; rclcpp::Clock::SharedPtr clock_; @@ -76,7 +82,7 @@ class PurePursuitController : public nav2_core::Controller double max_angular_accel_; double max_angular_decel_; bool use_velocity_scaled_lookahead_dist_; - rclcpp::Duration transform_tolerance_ {0, 0}; + tf2::Duration transform_tolerance_; geometry_msgs::msg::TwistStamped last_cmd_; nav_msgs::msg::Path global_plan_; diff --git a/nav2_pure_pursuit_controller/nav2_pure_pursuit_controller.xml b/nav2_pure_pursuit/nav2_pure_pursuit.xml similarity index 100% rename from nav2_pure_pursuit_controller/nav2_pure_pursuit_controller.xml rename to nav2_pure_pursuit/nav2_pure_pursuit.xml diff --git a/nav2_pure_pursuit_controller/package.xml b/nav2_pure_pursuit/package.xml similarity index 95% rename from nav2_pure_pursuit_controller/package.xml rename to nav2_pure_pursuit/package.xml index cb210c8..5bbc422 100644 --- a/nav2_pure_pursuit_controller/package.xml +++ b/nav2_pure_pursuit/package.xml @@ -1,7 +1,7 @@ - nav2_pure_pursuit_controller + nav2_pure_pursuit 1.0.0 Pure pursuit controller Shrijit Singh diff --git a/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp b/nav2_pure_pursuit/src/pure_pursuit_controller.cpp similarity index 75% rename from nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp rename to nav2_pure_pursuit/src/pure_pursuit_controller.cpp index d52f2e3..8f98311 100644 --- a/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp +++ b/nav2_pure_pursuit/src/pure_pursuit_controller.cpp @@ -2,16 +2,18 @@ * SPDX-License-Identifier: BSD-3-Clause * * Author(s): Shrijit Singh + * Author(s): Steve Macenski * */ #include #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_pure_pursuit/pure_pursuit_controller.hpp" #include "nav2_util/geometry_utils.hpp" using std::hypot; @@ -58,10 +60,12 @@ void PurePursuitController::configure( } costmap_ros_ = costmap_ros; + costmap_ = costmap_ros_->getCostmap(); tf_ = tf; plugin_name_ = name; logger_ = node->get_logger(); clock_ = node->get_clock(); + last_cmd_.header.stamp = clock_->now(); declare_parameter_if_not_declared( node, plugin_name_ + ".desired_linear_vel", rclcpp::ParameterValue(0.5)); @@ -105,8 +109,9 @@ void PurePursuitController::configure( node->get_parameter(plugin_name_ + ".max_angular_vel", max_angular_vel_); double transform_tolerance; node->get_parameter(plugin_name_ + ".transform_tolerance", transform_tolerance); - node->get_parameter(plugin_name_ + ".use_velocity_scaled_lookahead_dist", use_velocity_scaled_lookahead_dist_); - transform_tolerance_ = rclcpp::Duration::from_seconds(transform_tolerance); + node->get_parameter(plugin_name_ + ".use_velocity_scaled_lookahead_dist", + use_velocity_scaled_lookahead_dist_); + transform_tolerance_ = tf2::durationFromSec(transform_tolerance); global_pub_ = node->create_publisher("received_global_plan", 1); } @@ -133,12 +138,12 @@ void PurePursuitController::deactivate() { RCLCPP_INFO( logger_, - "Dectivating controller: %s of type pure_pursuit_controller::PurePursuitController\" %s", + "Deactivating controller: %s of type pure_pursuit_controller::PurePursuitController\" %s", plugin_name_.c_str()); global_pub_->on_deactivate(); } -double PurePursuitController::getLookAheadDistance() +double PurePursuitController::getLookAheadDistance(const geometry_msgs::msg::Twist & speed) { // If using velocity-scaled look ahead distances, find and clamp the dist // Else, use the default look ahead distance @@ -153,16 +158,6 @@ double PurePursuitController::getLookAheadDistance() return lookahead_dist; } -// PROFILES -// [DONE] acceleration profiling, ramp up and down -// [DONE] if carrot dist < requested, scale down speeds because on approach to goal - -// [] collision checking robot pose + along carrot -// [DONE] scale look ahead dist by speed - -// ROTATE (separate class this uses that can be used elsewhere too?) -// rotate after goal to get orientation -// rotate to heading to start geometry_msgs::msg::TwistStamped PurePursuitController::computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & speed) @@ -170,46 +165,34 @@ geometry_msgs::msg::TwistStamped PurePursuitController::computeVelocityCommands( // Transform path to robot base frame auto transformed_plan = transformGlobalPlan(pose); - // Find look ahead distance - const double lookahead_dist = getLookAheadDistance(); + // Find look ahead distance and point on path + const double lookahead_dist = getLookAheadDistance(speed); + geometry_msgs::msg::PoseStamped carrot_pose = getLookAheadMarker(lookahead_dist, 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()); - } - - auto & carrot_pose = goal_pose_it->pose; double linear_vel, angular_vel; - // Find distance to look ahead point (carrot) - const double carrot_dist = hypot( - pose.pose.position.x - carrot_pose.position.x, - pose.pose.position.y - carrot_pose.position.y); + // Find distance^2 to look ahead point (carrot) in robot base frame + 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 circular arc double curvature = 0.0; - if (carrot_dist > 0.001) { - curvature = 2.0 * carrot_pose.position.y / (carrot_dist * carrot_dist); + 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 kinematics rclcpp::Duration dt = pose.header.stamp - last_cmd_.header.stamp; applyKinematicConstraints( - linear_vel, angular_vel, fabs(lookahead_dist - carrot_dist), lookahead_dist, dt.seconds()); - - // make sure in range of valid velocities - angular_vel = std::clamp(angular_vel, -max_angular_vel_, max_angular_vel_); - linear_vel = std::clamp(linear_vel, 0.0, desired_linear_vel_); + linear_vel, angular_vel, + fabs(lookahead_dist - sqrt(carrot_dist2)), lookahead_dist, dt.seconds()); + // Collision checking if (isCollisionImminent(pose, carrot_pose)) { RCLCPP_ERROR(logger_, "Collision imminent!"); throw std::runtime_error("PurePursuitController detected collision ahead!"); @@ -225,14 +208,81 @@ geometry_msgs::msg::TwistStamped PurePursuitController::computeVelocityCommands( return cmd_vel; } +geometry_msgs::msg::PoseStamped PurePursuitController::getLookAheadMarker( + 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 PurePursuitController::isCollisionImminent( const geometry_msgs::msg::PoseStamped & robot_pose, const geometry_msgs::msg::PoseStamped & carrot_pose) { - // TODO - // interpolate between carrot/robot at costmap resolution - // check for collisions - // return bool true/false + // 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 to find quantities and convert to odom. + + const double carrot_dist = hypot(carrot_pose.pose.position.x, carrot_pose.pose.position.y); + const unsigned int num_pts = static_cast(ceil(carrot_dist / costmap_->getResolution())); + const double pt_distance = carrot_dist / num_pts; + + geometry_msgs::msg::PoseStamped carrot_in_odom; + try { + tf_->transform(carrot_pose, carrot_in_odom, + costmap_ros_->getGlobalFrameID(), transform_tolerance_); + return true; + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR( + logger_, + "Exception attempting to get look ahead point in odom frame: %s", ex.what()); + } + + geometry_msgs::msg::Vector3 unit_vector; + unit_vector.x = carrot_in_odom.pose.position.x / carrot_dist; + unit_vector.y = carrot_in_odom.pose.position.y / carrot_dist; + + double curr_dist; + geometry_msgs::msg::Vector3 cur_pt; + + for (unsigned int i = 0; i != num_pts; i++) { + curr_dist = i * pt_distance; + cur_pt.x = robot_pose.pose.position.x + (curr_dist * unit_vector.x); + cur_pt.y = robot_pose.pose.position.y + (curr_dist * unit_vector.y); + if (inCollision(cur_pt.x, cur_pt.y)) { + return true; + } + } + + return false; +} + +bool PurePursuitController::inCollision(const double & x, const double & y) +{ + using namespace nav2_costmap_2d; // NOLINT + + 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; + } } void PurePursuitController::applyKinematicConstraints( @@ -241,25 +291,29 @@ void PurePursuitController::applyKinematicConstraints( { // 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 (dist_error > 2.0 * costmap_ros_->getCostmap()->getResolution()) { + if (dist_error > 2.0 * costmap_->getResolution()) { linear_vel = linear_vel * (dist_error / lookahead_dist); } // if we're accelerating or decelerating too fast, limit linear velocity double measured_lin_accel = (linear_vel - last_cmd_.twist.linear.x) / dt; - if (measured_lin_accel > max_accel_) { + if (measured_lin_accel > max_linear_accel_) { linear_vel = last_cmd_.twist.linear.x + max_linear_accel_ * dt; - } else if (measured_lin_accel < -max_decel) { + } else if (measured_lin_accel < -max_linear_decel_) { linear_vel = last_cmd_.twist.linear.x - max_linear_decel_ * dt; } // if we're accelerating or decelerating too fast, limit angular velocity double measured_ang_accel = (angular_vel - last_cmd_.twist.angular.z) / dt; - if (measured_ang_accel > max_accel) { + if (measured_ang_accel > max_angular_accel_) { angular_vel = last_cmd_.twist.angular.z + max_angular_accel_ * dt; - } else if (measured_ang_accel < -max_decel) { + } else if (measured_ang_accel < -max_angular_decel_) { angular_vel = last_cmd_.twist.angular.z - max_angular_decel_ * dt; } + + // make sure in range of valid velocities + angular_vel = std::clamp(angular_vel, -max_angular_vel_, max_angular_vel_); + linear_vel = std::clamp(linear_vel, 0.0, desired_linear_vel_); } void PurePursuitController::setPlan(const nav_msgs::msg::Path & path) @@ -304,8 +358,8 @@ PurePursuitController::transformGlobalPlan( }); // Lambda to transform a PoseStamped from global frame to local - geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose; 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; @@ -339,8 +393,6 @@ bool PurePursuitController::transformPose( const geometry_msgs::msg::PoseStamped & in_pose, geometry_msgs::msg::PoseStamped & out_pose) 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; @@ -348,6 +400,7 @@ bool PurePursuitController::transformPose( 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()); From 3fa599d5693c49a95bcbbb1357bc4cb813c94271 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 7 Dec 2020 14:41:45 -0800 Subject: [PATCH 05/32] fix time type issues --- nav2_pure_pursuit/src/pure_pursuit_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_pure_pursuit/src/pure_pursuit_controller.cpp b/nav2_pure_pursuit/src/pure_pursuit_controller.cpp index 8f98311..faaaad7 100644 --- a/nav2_pure_pursuit/src/pure_pursuit_controller.cpp +++ b/nav2_pure_pursuit/src/pure_pursuit_controller.cpp @@ -187,7 +187,7 @@ geometry_msgs::msg::TwistStamped PurePursuitController::computeVelocityCommands( angular_vel = desired_linear_vel_ * curvature; // Make sure we're in compliance with basic kinematics - rclcpp::Duration dt = pose.header.stamp - last_cmd_.header.stamp; + rclcpp::Duration dt = rclcpp::Time(pose.header.stamp) - rclcpp::Time(last_cmd_.header.stamp); applyKinematicConstraints( linear_vel, angular_vel, fabs(lookahead_dist - sqrt(carrot_dist2)), lookahead_dist, dt.seconds()); From adc068114d4a462599852b26c47b0a88682fc054 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 9 Dec 2020 15:59:28 -0800 Subject: [PATCH 06/32] adding review comments --- .../gradient_layer.hpp | 2 ++ nav2_pure_pursuit/src/pure_pursuit_controller.cpp | 14 +++++--------- 2 files changed, 7 insertions(+), 9 deletions(-) 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/src/pure_pursuit_controller.cpp b/nav2_pure_pursuit/src/pure_pursuit_controller.cpp index faaaad7..233a5d3 100644 --- a/nav2_pure_pursuit/src/pure_pursuit_controller.cpp +++ b/nav2_pure_pursuit/src/pure_pursuit_controller.cpp @@ -240,19 +240,15 @@ bool PurePursuitController::isCollisionImminent( const double pt_distance = carrot_dist / num_pts; geometry_msgs::msg::PoseStamped carrot_in_odom; - try { - tf_->transform(carrot_pose, carrot_in_odom, - costmap_ros_->getGlobalFrameID(), transform_tolerance_); + 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; - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR( - logger_, - "Exception attempting to get look ahead point in odom frame: %s", ex.what()); } geometry_msgs::msg::Vector3 unit_vector; - unit_vector.x = carrot_in_odom.pose.position.x / carrot_dist; - unit_vector.y = carrot_in_odom.pose.position.y / carrot_dist; + unit_vector.x = (carrot_in_odom.pose.position.x - robot_pose.pose.position.x) / carrot_dist; + unit_vector.y = (carrot_in_odom.pose.position.y - robot_pose.pose.position.y) / carrot_dist; double curr_dist; geometry_msgs::msg::Vector3 cur_pt; From 83b068ea6ef15958efc70873f7f9cb067a5c2879 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 9 Dec 2020 16:02:39 -0800 Subject: [PATCH 07/32] using lookahead time vs gain --- .../include/nav2_pure_pursuit/pure_pursuit_controller.hpp | 2 +- nav2_pure_pursuit/src/pure_pursuit_controller.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/nav2_pure_pursuit/include/nav2_pure_pursuit/pure_pursuit_controller.hpp b/nav2_pure_pursuit/include/nav2_pure_pursuit/pure_pursuit_controller.hpp index 246beff..6ac3960 100644 --- a/nav2_pure_pursuit/include/nav2_pure_pursuit/pure_pursuit_controller.hpp +++ b/nav2_pure_pursuit/include/nav2_pure_pursuit/pure_pursuit_controller.hpp @@ -76,7 +76,7 @@ class PurePursuitController : public nav2_core::Controller double max_angular_vel_; double max_lookahead_dist_; double min_lookahead_dist_; - double lookahead_gain_; + double lookahead_time_; double max_linear_accel_; double max_linear_decel_; double max_angular_accel_; diff --git a/nav2_pure_pursuit/src/pure_pursuit_controller.cpp b/nav2_pure_pursuit/src/pure_pursuit_controller.cpp index 233a5d3..be9f521 100644 --- a/nav2_pure_pursuit/src/pure_pursuit_controller.cpp +++ b/nav2_pure_pursuit/src/pure_pursuit_controller.cpp @@ -87,7 +87,7 @@ void PurePursuitController::configure( node, plugin_name_ + ".max_lookahead_dist", rclcpp::ParameterValue(0.6)); declare_parameter_if_not_declared( - node, plugin_name_ + ".lookahead_gain", + node, plugin_name_ + ".lookahead_time", rclcpp::ParameterValue(1.5)); declare_parameter_if_not_declared( node, plugin_name_ + ".max_angular_vel", rclcpp::ParameterValue(1.0)); @@ -105,7 +105,7 @@ void PurePursuitController::configure( 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_gain", lookahead_gain_); + node->get_parameter(plugin_name_ + ".lookahead_time", lookahead_time_); node->get_parameter(plugin_name_ + ".max_angular_vel", max_angular_vel_); double transform_tolerance; node->get_parameter(plugin_name_ + ".transform_tolerance", transform_tolerance); @@ -149,7 +149,7 @@ double PurePursuitController::getLookAheadDistance(const geometry_msgs::msg::Twi // Else, use the default look ahead distance double lookahead_dist = 0.0; if (use_velocity_scaled_lookahead_dist_) { - lookahead_dist = speed.linear.x * lookahead_gain_; + lookahead_dist = speed.linear.x * lookahead_time_; lookahead_dist = std::clamp(lookahead_dist, min_lookahead_dist_, max_lookahead_dist_); } else { lookahead_dist = lookahead_dist_; From 08c05faf09580c394e5e2398aa41daf909bb1aa3 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 9 Dec 2020 17:43:00 -0800 Subject: [PATCH 08/32] some linting --- .../src/pure_pursuit_controller.cpp | 20 ++++++++----------- 1 file changed, 8 insertions(+), 12 deletions(-) diff --git a/nav2_pure_pursuit/src/pure_pursuit_controller.cpp b/nav2_pure_pursuit/src/pure_pursuit_controller.cpp index be9f521..ea9578e 100644 --- a/nav2_pure_pursuit/src/pure_pursuit_controller.cpp +++ b/nav2_pure_pursuit/src/pure_pursuit_controller.cpp @@ -78,24 +78,20 @@ void PurePursuitController::configure( declare_parameter_if_not_declared( node, plugin_name_ + ".max_angular_decel", rclcpp::ParameterValue(1.0)); declare_parameter_if_not_declared( - node, plugin_name_ + ".lookahead_dist", - rclcpp::ParameterValue(0.4)); + node, plugin_name_ + ".lookahead_dist", rclcpp::ParameterValue(0.4)); declare_parameter_if_not_declared( - node, plugin_name_ + ".min_lookahead_dist", - rclcpp::ParameterValue(0.3)); + node, plugin_name_ + ".min_lookahead_dist", rclcpp::ParameterValue(0.3)); declare_parameter_if_not_declared( - node, plugin_name_ + ".max_lookahead_dist", - rclcpp::ParameterValue(0.6)); + node, plugin_name_ + ".max_lookahead_dist", rclcpp::ParameterValue(0.6)); declare_parameter_if_not_declared( - node, plugin_name_ + ".lookahead_time", - rclcpp::ParameterValue(1.5)); + node, plugin_name_ + ".lookahead_time", rclcpp::ParameterValue(1.5)); 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)); declare_parameter_if_not_declared( - node, plugin_name_ + ".use_velocity_scaled_lookahead_dist", rclcpp::ParameterValue( - false)); + node, plugin_name_ + ".use_velocity_scaled_lookahead_dist", + rclcpp::ParameterValue(false)); node->get_parameter(plugin_name_ + ".desired_linear_vel", desired_linear_vel_); node->get_parameter(plugin_name_ + ".max_linear_accel", max_linear_accel_); @@ -167,7 +163,7 @@ geometry_msgs::msg::TwistStamped PurePursuitController::computeVelocityCommands( // Find look ahead distance and point on path const double lookahead_dist = getLookAheadDistance(speed); - geometry_msgs::msg::PoseStamped carrot_pose = getLookAheadMarker(lookahead_dist, transformed_plan); + auto carrot_pose = getLookAheadMarker(lookahead_dist, transformed_plan); double linear_vel, angular_vel; @@ -236,7 +232,7 @@ bool PurePursuitController::isCollisionImminent( // But we can still use the carrot pose to find quantities and convert to odom. const double carrot_dist = hypot(carrot_pose.pose.position.x, carrot_pose.pose.position.y); - const unsigned int num_pts = static_cast(ceil(carrot_dist / costmap_->getResolution())); + unsigned int num_pts = static_cast(ceil(carrot_dist / costmap_->getResolution())); const double pt_distance = carrot_dist / num_pts; geometry_msgs::msg::PoseStamped carrot_in_odom; From 9741303f56917849a759487a6cdb634d809ce4be Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Wed, 9 Dec 2020 18:03:04 -0800 Subject: [PATCH 09/32] adding carrot publisher for debug / visualization and complete copyright headers --- .../pure_pursuit_controller.hpp | 34 +++++++++++-- nav2_pure_pursuit/package.xml | 1 + .../src/pure_pursuit_controller.cpp | 48 ++++++++++++++++--- 3 files changed, 74 insertions(+), 9 deletions(-) diff --git a/nav2_pure_pursuit/include/nav2_pure_pursuit/pure_pursuit_controller.hpp b/nav2_pure_pursuit/include/nav2_pure_pursuit/pure_pursuit_controller.hpp index 6ac3960..ebc75a3 100644 --- a/nav2_pure_pursuit/include/nav2_pure_pursuit/pure_pursuit_controller.hpp +++ b/nav2_pure_pursuit/include/nav2_pure_pursuit/pure_pursuit_controller.hpp @@ -1,9 +1,36 @@ /* - * SPDX-License-Identifier: BSD-3-Clause + * Software License Agreement (BSD License) * - * Author(s): Shrijit Singh - * Author(s): Steve Macenski + * Copyright (c) 2020, Shrijit Singh + * Copyright (c) 2020, Samsung Research America + * All rights reserved. * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. */ #ifndef NAV2_PURE_PURSUIT__PURE_PURSUIT_CONTROLLER_HPP_ @@ -87,6 +114,7 @@ class PurePursuitController : public nav2_core::Controller nav_msgs::msg::Path global_plan_; std::shared_ptr> global_pub_; + std::shared_ptr> carrot_pub_; }; } // namespace nav2_pure_pursuit_controller diff --git a/nav2_pure_pursuit/package.xml b/nav2_pure_pursuit/package.xml index 5bbc422..17b6c6d 100644 --- a/nav2_pure_pursuit/package.xml +++ b/nav2_pure_pursuit/package.xml @@ -4,6 +4,7 @@ nav2_pure_pursuit 1.0.0 Pure pursuit controller + Steve Macenski Shrijit Singh BSD-3-Clause diff --git a/nav2_pure_pursuit/src/pure_pursuit_controller.cpp b/nav2_pure_pursuit/src/pure_pursuit_controller.cpp index ea9578e..a4648ef 100644 --- a/nav2_pure_pursuit/src/pure_pursuit_controller.cpp +++ b/nav2_pure_pursuit/src/pure_pursuit_controller.cpp @@ -1,9 +1,36 @@ /* - * SPDX-License-Identifier: BSD-3-Clause + * Software License Agreement (BSD License) * - * Author(s): Shrijit Singh - * Author(s): Steve Macenski + * Copyright (c) 2020, Shrijit Singh + * Copyright (c) 2020, Samsung Research America + * All rights reserved. * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. */ #include @@ -11,9 +38,9 @@ #include #include +#include "nav2_pure_pursuit/pure_pursuit_controller.hpp" #include "nav2_core/exceptions.hpp" #include "nav2_util/node_utils.hpp" -#include "nav2_pure_pursuit/pure_pursuit_controller.hpp" #include "nav2_util/geometry_utils.hpp" using std::hypot; @@ -110,6 +137,7 @@ void PurePursuitController::configure( transform_tolerance_ = tf2::durationFromSec(transform_tolerance); global_pub_ = node->create_publisher("received_global_plan", 1); + carrot_pub_ = node->create_publisher("lookahead_point", 1); } void PurePursuitController::cleanup() @@ -119,6 +147,7 @@ void PurePursuitController::cleanup() "Cleaning up controller: %s of type pure_pursuit_controller::PurePursuitController", plugin_name_.c_str()); global_pub_.reset(); + carrot_pub_.reset(); } void PurePursuitController::activate() @@ -128,6 +157,7 @@ void PurePursuitController::activate() "Activating controller: %s of type pure_pursuit_controller::PurePursuitController\" %s", plugin_name_.c_str()); global_pub_->on_activate(); + carrot_pub_->on_activate(); } void PurePursuitController::deactivate() @@ -137,6 +167,7 @@ void PurePursuitController::deactivate() "Deactivating controller: %s of type pure_pursuit_controller::PurePursuitController\" %s", plugin_name_.c_str()); global_pub_->on_deactivate(); + carrot_pub_->on_deactivate(); } double PurePursuitController::getLookAheadDistance(const geometry_msgs::msg::Twist & speed) @@ -161,9 +192,15 @@ geometry_msgs::msg::TwistStamped PurePursuitController::computeVelocityCommands( // Transform path to robot base frame auto transformed_plan = transformGlobalPlan(pose); - // Find look ahead distance and point on path + // Find look ahead distance and point on path and publish const double lookahead_dist = getLookAheadDistance(speed); auto carrot_pose = getLookAheadMarker(lookahead_dist, transformed_plan); + 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 + carrot_pub_->publish(std::move(carrot_msg)); double linear_vel, angular_vel; @@ -310,7 +347,6 @@ void PurePursuitController::applyKinematicConstraints( void PurePursuitController::setPlan(const nav_msgs::msg::Path & path) { - global_pub_->publish(path); global_plan_ = path; } From f872e3510ffbf66b4138f071da12ee512d413adf Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 11 Dec 2020 16:24:43 -0800 Subject: [PATCH 10/32] renaming files and collision detections online --- .../CMakeLists.txt | 4 +- .../README.md | 0 .../doc/lookahead_algorithm.png | Bin .../pure_pursuit_controller.hpp | 19 +- .../nav2_pure_pursuit_controller.xml | 0 .../package.xml | 2 +- .../src/pure_pursuit_controller.cpp | 167 +++++++++++++----- 7 files changed, 138 insertions(+), 54 deletions(-) rename {nav2_pure_pursuit => nav2_pure_pursuit_controller}/CMakeLists.txt (96%) rename {nav2_pure_pursuit => nav2_pure_pursuit_controller}/README.md (100%) rename {nav2_pure_pursuit => nav2_pure_pursuit_controller}/doc/lookahead_algorithm.png (100%) rename {nav2_pure_pursuit/include/nav2_pure_pursuit => nav2_pure_pursuit_controller/include/nav2_pure_pursuit_controller}/pure_pursuit_controller.hpp (87%) rename nav2_pure_pursuit/nav2_pure_pursuit.xml => nav2_pure_pursuit_controller/nav2_pure_pursuit_controller.xml (100%) rename {nav2_pure_pursuit => nav2_pure_pursuit_controller}/package.xml (95%) rename {nav2_pure_pursuit => nav2_pure_pursuit_controller}/src/pure_pursuit_controller.cpp (71%) diff --git a/nav2_pure_pursuit/CMakeLists.txt b/nav2_pure_pursuit_controller/CMakeLists.txt similarity index 96% rename from nav2_pure_pursuit/CMakeLists.txt rename to nav2_pure_pursuit_controller/CMakeLists.txt index cc36cc5..af658d9 100644 --- a/nav2_pure_pursuit/CMakeLists.txt +++ b/nav2_pure_pursuit_controller/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(nav2_pure_pursuit) +project(nav2_pure_pursuit_controller) find_package(ament_cmake REQUIRED) find_package(nav2_common REQUIRED) @@ -61,7 +61,7 @@ ament_export_include_directories(include) ament_export_libraries(nav2_pure_pursuit_controller) ament_export_dependencies(${dependencies}) -pluginlib_export_plugin_description_file(nav2_core nav2_pure_pursuit.xml) +pluginlib_export_plugin_description_file(nav2_core nav2_pure_pursuit_controller.xml) ament_package() diff --git a/nav2_pure_pursuit/README.md b/nav2_pure_pursuit_controller/README.md similarity index 100% rename from nav2_pure_pursuit/README.md rename to nav2_pure_pursuit_controller/README.md diff --git a/nav2_pure_pursuit/doc/lookahead_algorithm.png b/nav2_pure_pursuit_controller/doc/lookahead_algorithm.png similarity index 100% rename from nav2_pure_pursuit/doc/lookahead_algorithm.png rename to nav2_pure_pursuit_controller/doc/lookahead_algorithm.png diff --git a/nav2_pure_pursuit/include/nav2_pure_pursuit/pure_pursuit_controller.hpp b/nav2_pure_pursuit_controller/include/nav2_pure_pursuit_controller/pure_pursuit_controller.hpp similarity index 87% rename from nav2_pure_pursuit/include/nav2_pure_pursuit/pure_pursuit_controller.hpp rename to nav2_pure_pursuit_controller/include/nav2_pure_pursuit_controller/pure_pursuit_controller.hpp index ebc75a3..9fc26b5 100644 --- a/nav2_pure_pursuit/include/nav2_pure_pursuit/pure_pursuit_controller.hpp +++ b/nav2_pure_pursuit_controller/include/nav2_pure_pursuit_controller/pure_pursuit_controller.hpp @@ -33,8 +33,8 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef NAV2_PURE_PURSUIT__PURE_PURSUIT_CONTROLLER_HPP_ -#define NAV2_PURE_PURSUIT__PURE_PURSUIT_CONTROLLER_HPP_ +#ifndef NAV2_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_ +#define NAV2_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_ #include #include @@ -45,6 +45,8 @@ #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_pure_pursuit_controller { @@ -81,12 +83,15 @@ class PurePursuitController : public nav2_core::Controller double getLookAheadDistance(const geometry_msgs::msg::Twist &); - bool isCollisionImminent(const geometry_msgs::msg::PoseStamped &, const geometry_msgs::msg::PoseStamped &); + 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); - void applyKinematicConstraints( + void applyConstraints( double & linear_vel, double & angular_vel, - const double & dist_error, const double & lookahead_dist, const double & dt); + const double & dist_error, const double & lookahead_dist); geometry_msgs::msg::PoseStamped getLookAheadMarker(const double &, const nav_msgs::msg::Path &); @@ -110,11 +115,13 @@ class PurePursuitController : public nav2_core::Controller double max_angular_decel_; bool use_velocity_scaled_lookahead_dist_; tf2::Duration transform_tolerance_; - geometry_msgs::msg::TwistStamped last_cmd_; + bool approach_vel_scaling_; + double min_approach_vel_scaling_; nav_msgs::msg::Path global_plan_; std::shared_ptr> global_pub_; std::shared_ptr> carrot_pub_; + std::shared_ptr> carrot_arc_pub_; }; } // namespace nav2_pure_pursuit_controller diff --git a/nav2_pure_pursuit/nav2_pure_pursuit.xml b/nav2_pure_pursuit_controller/nav2_pure_pursuit_controller.xml similarity index 100% rename from nav2_pure_pursuit/nav2_pure_pursuit.xml rename to nav2_pure_pursuit_controller/nav2_pure_pursuit_controller.xml diff --git a/nav2_pure_pursuit/package.xml b/nav2_pure_pursuit_controller/package.xml similarity index 95% rename from nav2_pure_pursuit/package.xml rename to nav2_pure_pursuit_controller/package.xml index 17b6c6d..0a6f007 100644 --- a/nav2_pure_pursuit/package.xml +++ b/nav2_pure_pursuit_controller/package.xml @@ -1,7 +1,7 @@ - nav2_pure_pursuit + nav2_pure_pursuit_controller 1.0.0 Pure pursuit controller Steve Macenski diff --git a/nav2_pure_pursuit/src/pure_pursuit_controller.cpp b/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp similarity index 71% rename from nav2_pure_pursuit/src/pure_pursuit_controller.cpp rename to nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp index a4648ef..1903492 100644 --- a/nav2_pure_pursuit/src/pure_pursuit_controller.cpp +++ b/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp @@ -38,7 +38,7 @@ #include #include -#include "nav2_pure_pursuit/pure_pursuit_controller.hpp" +#include "nav2_pure_pursuit_controller/pure_pursuit_controller.hpp" #include "nav2_core/exceptions.hpp" #include "nav2_util/node_utils.hpp" #include "nav2_util/geometry_utils.hpp" @@ -53,6 +53,14 @@ using nav2_util::geometry_utils::euclidean_distance; namespace nav2_pure_pursuit_controller { +// TEST +// general performance: follows path well, doesn't veer off, defaults, can get on path OK +// make it so able to scale down linear velocity on sharper turn to make sure it can match? + +// kinematic constraints +// slowing on approach -- generally speaking working, but not ready yet +// dynamic scaling look ahead + /** * Find element in iterator with the minimum calculated value */ @@ -92,7 +100,6 @@ void PurePursuitController::configure( plugin_name_ = name; logger_ = node->get_logger(); clock_ = node->get_clock(); - last_cmd_.header.stamp = clock_->now(); declare_parameter_if_not_declared( node, plugin_name_ + ".desired_linear_vel", rclcpp::ParameterValue(0.5)); @@ -105,7 +112,7 @@ void PurePursuitController::configure( declare_parameter_if_not_declared( node, plugin_name_ + ".max_angular_decel", rclcpp::ParameterValue(1.0)); declare_parameter_if_not_declared( - node, plugin_name_ + ".lookahead_dist", rclcpp::ParameterValue(0.4)); + 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( @@ -118,7 +125,11 @@ void PurePursuitController::configure( 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)); + rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".min_approach_vel_scaling", rclcpp::ParameterValue(0.10)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".approach_vel_scaling", rclcpp::ParameterValue(true)); node->get_parameter(plugin_name_ + ".desired_linear_vel", desired_linear_vel_); node->get_parameter(plugin_name_ + ".max_linear_accel", max_linear_accel_); @@ -134,10 +145,16 @@ void PurePursuitController::configure( 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_vel_scaling", min_approach_vel_scaling_); + node->get_parameter(plugin_name_ + ".approach_vel_scaling", approach_vel_scaling_); + + std::string wheel_odom_topic; + node->get_parameter(plugin_name_ + ".wheel_odom_topic", wheel_odom_topic); transform_tolerance_ = tf2::durationFromSec(transform_tolerance); 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 PurePursuitController::cleanup() @@ -148,6 +165,7 @@ void PurePursuitController::cleanup() plugin_name_.c_str()); global_pub_.reset(); carrot_pub_.reset(); + carrot_arc_pub_.reset(); } void PurePursuitController::activate() @@ -158,6 +176,7 @@ void PurePursuitController::activate() plugin_name_.c_str()); global_pub_->on_activate(); carrot_pub_->on_activate(); + carrot_arc_pub_->on_activate(); } void PurePursuitController::deactivate() @@ -168,6 +187,7 @@ void PurePursuitController::deactivate() plugin_name_.c_str()); global_pub_->on_deactivate(); carrot_pub_->on_deactivate(); + carrot_arc_pub_->on_deactivate(); } double PurePursuitController::getLookAheadDistance(const geometry_msgs::msg::Twist & speed) @@ -205,11 +225,12 @@ geometry_msgs::msg::TwistStamped PurePursuitController::computeVelocityCommands( 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 circular arc + // 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; @@ -220,13 +241,12 @@ geometry_msgs::msg::TwistStamped PurePursuitController::computeVelocityCommands( angular_vel = desired_linear_vel_ * curvature; // Make sure we're in compliance with basic kinematics - rclcpp::Duration dt = rclcpp::Time(pose.header.stamp) - rclcpp::Time(last_cmd_.header.stamp); - applyKinematicConstraints( + applyConstraints( linear_vel, angular_vel, - fabs(lookahead_dist - sqrt(carrot_dist2)), lookahead_dist, dt.seconds()); + fabs(lookahead_dist - sqrt(carrot_dist2)), lookahead_dist); // Collision checking - if (isCollisionImminent(pose, carrot_pose)) { + if (isCollisionImminent(pose, carrot_pose, curvature, linear_vel, angular_vel)) { RCLCPP_ERROR(logger_, "Collision imminent!"); throw std::runtime_error("PurePursuitController detected collision ahead!"); } @@ -237,7 +257,6 @@ geometry_msgs::msg::TwistStamped PurePursuitController::computeVelocityCommands( cmd_vel.header.stamp = clock_->now(); cmd_vel.twist.linear.x = linear_vel; cmd_vel.twist.angular.z = angular_vel; - last_cmd_ = cmd_vel; return cmd_vel; } @@ -261,16 +280,14 @@ geometry_msgs::msg::PoseStamped PurePursuitController::getLookAheadMarker( bool PurePursuitController::isCollisionImminent( const geometry_msgs::msg::PoseStamped & robot_pose, - const geometry_msgs::msg::PoseStamped & carrot_pose) + const geometry_msgs::msg::PoseStamped & carrot_pose, + const double & curvature, const double & linear_vel, + const double & angular_vel) { // 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 to find quantities and convert to odom. - - const double carrot_dist = hypot(carrot_pose.pose.position.x, carrot_pose.pose.position.y); - unsigned int num_pts = static_cast(ceil(carrot_dist / costmap_->getResolution())); - const double pt_distance = carrot_dist / num_pts; + // 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)) @@ -279,22 +296,58 @@ bool PurePursuitController::isCollisionImminent( return true; } - geometry_msgs::msg::Vector3 unit_vector; - unit_vector.x = (carrot_in_odom.pose.position.x - robot_pose.pose.position.x) / carrot_dist; - unit_vector.y = (carrot_in_odom.pose.position.y - robot_pose.pose.position.y) / carrot_dist; - - double curr_dist; - geometry_msgs::msg::Vector3 cur_pt; + // 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; + } - for (unsigned int i = 0; i != num_pts; i++) { - curr_dist = i * pt_distance; - cur_pt.x = robot_pose.pose.position.x + (curr_dist * unit_vector.x); - cur_pt.y = robot_pose.pose.position.y + (curr_dist * unit_vector.y); - if (inCollision(cur_pt.x, cur_pt.y)) { + // debug messages + nav_msgs::msg::Path arc_pts_msg; + arc_pts_msg.header.frame_id = costmap_ros_->getGlobalFrameID(); + arc_pts_msg.header.stamp = clock_->now(); + 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 = 1 / 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++) { + // 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; } @@ -314,31 +367,55 @@ bool PurePursuitController::inCollision(const double & x, const double & y) } } -void PurePursuitController::applyKinematicConstraints( +// LIN ACCEL +// ANG ACCEL +// LIN DECEL +// ANG DECEL +// CARROT DIST on approach slow + +// how handle active -> pause -> go +// go -> preempt -> go +// go -> pause -> go + +void PurePursuitController::applyConstraints( double & linear_vel, double & angular_vel, - const double & dist_error, const double & lookahead_dist, const double & dt) + const double & dist_error, const double & lookahead_dist) { // 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 (dist_error > 2.0 * costmap_->getResolution()) { - linear_vel = linear_vel * (dist_error / lookahead_dist); + if (approach_vel_scaling_ && dist_error > 2.0 * costmap_->getResolution()) { + double velocity_scaling = 1.0 - (dist_error / lookahead_dist); + if (velocity_scaling < min_approach_vel_scaling_) { + velocity_scaling = min_approach_vel_scaling_; + } + linear_vel = linear_vel * velocity_scaling; } - // if we're accelerating or decelerating too fast, limit linear velocity - double measured_lin_accel = (linear_vel - last_cmd_.twist.linear.x) / dt; - if (measured_lin_accel > max_linear_accel_) { - linear_vel = last_cmd_.twist.linear.x + max_linear_accel_ * dt; - } else if (measured_lin_accel < -max_linear_decel_) { - linear_vel = last_cmd_.twist.linear.x - max_linear_decel_ * dt; - } + // TODO try to apply those inverse contraints on linear velocity when curvature is high? - // if we're accelerating or decelerating too fast, limit angular velocity - double measured_ang_accel = (angular_vel - last_cmd_.twist.angular.z) / dt; - if (measured_ang_accel > max_angular_accel_) { - angular_vel = last_cmd_.twist.angular.z + max_angular_accel_ * dt; - } else if (measured_ang_accel < -max_angular_decel_) { - angular_vel = last_cmd_.twist.angular.z - max_angular_decel_ * dt; - } + + + // using the odom smoother has failed, go back to command-based kinematics + // but how deal with edge cases above? (in set plan if stamp > N s, reset to curret odom?) + // double & dt = control_duration_; + + // // if we're accelerating or decelerating too fast, limit linear velocity + // double measured_lin_accel = (linear_vel - last_odom_.linear.x) / dt; + // std::cout << "Dt: " << dt << " linV: " << linear_vel << " last linV: " << last_odom_.linear.x << " accel: " << measured_lin_accel << std::endl; + // if (measured_lin_accel > max_linear_accel_) { + // linear_vel = last_odom_.linear.x + max_linear_accel_ * dt; + // } else if (measured_lin_accel < -max_linear_decel_) { + // linear_vel = last_odom_.linear.x - max_linear_decel_ * dt; + // } + + // // if we're accelerating or decelerating too fast, limit angular velocity + // double measured_ang_accel = (angular_vel - last_odom_.angular.z) / dt; + // std::cout << "Dt: " << dt << " angV: " << angular_vel << " last angV: " << last_odom_.angular.z << " accel: " << measured_ang_accel << std::endl; + // if (measured_ang_accel > max_angular_accel_) { + // angular_vel = last_odom_.angular.z + max_angular_accel_ * dt; + // } else if (measured_ang_accel < -max_angular_decel_) { + // angular_vel = last_odom_.angular.z - max_angular_decel_ * dt; + // } // make sure in range of valid velocities angular_vel = std::clamp(angular_vel, -max_angular_vel_, max_angular_vel_); From 1a592d9a22a69064c71db923cbc3bfcfd673c538 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 11 Dec 2020 16:26:31 -0800 Subject: [PATCH 11/32] finish dynamic scaling of look ahead points --- nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp b/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp index 1903492..7a7a9d0 100644 --- a/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp +++ b/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp @@ -59,7 +59,6 @@ namespace nav2_pure_pursuit_controller // kinematic constraints // slowing on approach -- generally speaking working, but not ready yet -// dynamic scaling look ahead /** * Find element in iterator with the minimum calculated value From 11b242da401528f3b0fdfa2b16f6cd9cecb17127 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 11 Dec 2020 17:27:33 -0800 Subject: [PATCH 12/32] smoothing more kinematics --- nav2_pure_pursuit_controller/README.md | 15 +++- .../pure_pursuit_controller.hpp | 7 +- .../src/pure_pursuit_controller.cpp | 88 ++++++++----------- 3 files changed, 57 insertions(+), 53 deletions(-) diff --git a/nav2_pure_pursuit_controller/README.md b/nav2_pure_pursuit_controller/README.md index 253718b..b91bef2 100644 --- a/nav2_pure_pursuit_controller/README.md +++ b/nav2_pure_pursuit_controller/README.md @@ -1,5 +1,6 @@ # Nav2 Pure pursuit controller -Tutorial code referenced in https://navigation.ros.org/plugin_tutorials/docs/writing_new_nav2controller_plugin.html + +Tutorial code example is 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. @@ -9,3 +10,15 @@ Then the path is transformed to the robot frame and a lookahead point is determi This lookahead point will be given to the pure pursuite algorithm to calculate a command velocity. ![Lookahead algorithm](./doc/lookahead_algorithm.png) + +## Features + +This implementation has a number of non-standard features. +- Collision avoidance in the computed velocity direction between the robot in the look ahead point to ensure safe path following. Uses a maximum collision time parameter to inform the amount of time to forward simulate for collisions. Set to a very large number to always forward simulate to the carrot location +- Optional dynamic scaling of the look ahead point distance proportional to velocity. This helps you have a more stable robot path tracking over a broader range of velocity inputs if your robot has a large band of operating velocities. There are parameters for the minimum and maximum distances as well. +- Optional slow on approach to the goal. The default algorithm tracks a path at a given linear velocity. This feature allows you to slow the robot on its approach to a goal and also set the minimum percentage (from 0.0-1.0) on approach so it doesn't approach 0% before attaining the goal. +- Kinematic speed limiting to make sure the resulting trajectories are kinematically feasible. + +This package will also publish a few useful topics: +- lookahead point: the position at which the lookahead point is located the robot is attempting to follow +- lookahead arc: the arc produced by the pure pursuit algorithm for debug visualization while tuning. It is also the points on the path that the collision detector is running. In a collision state, the last published arc will be the points leading up to, but not including, the collision. 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 index 9fc26b5..1b67dff 100644 --- 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 @@ -91,7 +91,8 @@ class PurePursuitController : public nav2_core::Controller void applyConstraints( double & linear_vel, double & angular_vel, - const double & dist_error, const double & lookahead_dist); + const double & dist_error, const double & lookahead_dist, + const geometry_msgs::msg::Twist & speed); geometry_msgs::msg::PoseStamped getLookAheadMarker(const double &, const nav_msgs::msg::Path &); @@ -111,12 +112,12 @@ class PurePursuitController : public nav2_core::Controller double lookahead_time_; double max_linear_accel_; double max_linear_decel_; - double max_angular_accel_; - double max_angular_decel_; bool use_velocity_scaled_lookahead_dist_; tf2::Duration transform_tolerance_; bool approach_vel_scaling_; double min_approach_vel_scaling_; + double control_duration_; + double max_time_to_collision_; nav_msgs::msg::Path global_plan_; std::shared_ptr> global_pub_; diff --git a/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp b/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp index 7a7a9d0..a2c15bf 100644 --- a/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp +++ b/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp @@ -60,6 +60,8 @@ namespace nav2_pure_pursuit_controller // kinematic constraints // slowing on approach -- generally speaking working, but not ready yet +// rotate to heading option + /** * Find element in iterator with the minimum calculated value */ @@ -103,38 +105,35 @@ void PurePursuitController::configure( 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(1.0)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".max_linear_decel", rclcpp::ParameterValue(1.0)); - declare_parameter_if_not_declared( - node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(1.0)); + node, plugin_name_ + ".max_linear_accel", rclcpp::ParameterValue(2.5)); declare_parameter_if_not_declared( - node, plugin_name_ + ".max_angular_decel", rclcpp::ParameterValue(1.0)); + 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.6)); + 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_ + ".max_angular_vel", rclcpp::ParameterValue(1.0)); + node, plugin_name_ + ".max_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(true)); + rclcpp::ParameterValue(false)); declare_parameter_if_not_declared( node, plugin_name_ + ".min_approach_vel_scaling", rclcpp::ParameterValue(0.10)); declare_parameter_if_not_declared( node, plugin_name_ + ".approach_vel_scaling", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".max_time_to_collision", rclcpp::ParameterValue(1.0)); + 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_ + ".max_angular_accel", max_angular_accel_); - node->get_parameter(plugin_name_ + ".max_angular_decel", max_angular_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_); @@ -146,10 +145,13 @@ void PurePursuitController::configure( use_velocity_scaled_lookahead_dist_); node->get_parameter(plugin_name_ + ".min_approach_vel_scaling", min_approach_vel_scaling_); node->get_parameter(plugin_name_ + ".approach_vel_scaling", approach_vel_scaling_); + node->get_parameter(plugin_name_ + ".max_time_to_collision", max_time_to_collision_); + + double control_frequency = 20.0; + node->get_parameter("control_frequency", control_frequency); - std::string wheel_odom_topic; - node->get_parameter(plugin_name_ + ".wheel_odom_topic", wheel_odom_topic); 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); @@ -242,7 +244,7 @@ geometry_msgs::msg::TwistStamped PurePursuitController::computeVelocityCommands( // Make sure we're in compliance with basic kinematics applyConstraints( linear_vel, angular_vel, - fabs(lookahead_dist - sqrt(carrot_dist2)), lookahead_dist); + fabs(lookahead_dist - sqrt(carrot_dist2)), lookahead_dist, speed); // Collision checking if (isCollisionImminent(pose, carrot_pose, curvature, linear_vel, angular_vel)) { @@ -327,6 +329,10 @@ bool PurePursuitController::isCollisionImminent( curr_pose.theta = tf2::getYaw(robot_pose.pose.orientation); for (unsigned int i = 1; i < num_pts; i++) { + if (i * projection_time > max_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)); @@ -367,18 +373,13 @@ bool PurePursuitController::inCollision(const double & x, const double & y) } // LIN ACCEL -// ANG ACCEL // LIN DECEL -// ANG DECEL // CARROT DIST on approach slow -// how handle active -> pause -> go -// go -> preempt -> go -// go -> pause -> go - void PurePursuitController::applyConstraints( double & linear_vel, double & angular_vel, - const double & dist_error, const double & lookahead_dist) + const double & dist_error, const double & lookahead_dist, + const geometry_msgs::msg::Twist & curr_speed) { // 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 @@ -390,35 +391,24 @@ void PurePursuitController::applyConstraints( linear_vel = linear_vel * velocity_scaling; } - // TODO try to apply those inverse contraints on linear velocity when curvature is high? - - - - // using the odom smoother has failed, go back to command-based kinematics - // but how deal with edge cases above? (in set plan if stamp > N s, reset to curret odom?) - // double & dt = control_duration_; - - // // if we're accelerating or decelerating too fast, limit linear velocity - // double measured_lin_accel = (linear_vel - last_odom_.linear.x) / dt; - // std::cout << "Dt: " << dt << " linV: " << linear_vel << " last linV: " << last_odom_.linear.x << " accel: " << measured_lin_accel << std::endl; - // if (measured_lin_accel > max_linear_accel_) { - // linear_vel = last_odom_.linear.x + max_linear_accel_ * dt; - // } else if (measured_lin_accel < -max_linear_decel_) { - // linear_vel = last_odom_.linear.x - max_linear_decel_ * dt; - // } - - // // if we're accelerating or decelerating too fast, limit angular velocity - // double measured_ang_accel = (angular_vel - last_odom_.angular.z) / dt; - // std::cout << "Dt: " << dt << " angV: " << angular_vel << " last angV: " << last_odom_.angular.z << " accel: " << measured_ang_accel << std::endl; - // if (measured_ang_accel > max_angular_accel_) { - // angular_vel = last_odom_.angular.z + max_angular_accel_ * dt; - // } else if (measured_ang_accel < -max_angular_decel_) { - // angular_vel = last_odom_.angular.z - max_angular_decel_ * dt; - // } - - // make sure in range of valid velocities - angular_vel = std::clamp(angular_vel, -max_angular_vel_, max_angular_vel_); + // make sure linear velocities are kinematically feasible, v = v0 + a*t + 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); + + // 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); + + // make sure in range of generally valid velocities linear_vel = std::clamp(linear_vel, 0.0, desired_linear_vel_); + angular_vel = std::clamp(angular_vel, -max_angular_vel_, max_angular_vel_); } void PurePursuitController::setPlan(const nav_msgs::msg::Path & path) From 8f25bbea794d94fea742b77b9d72bc72c1e2aad6 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 11 Dec 2020 17:31:16 -0800 Subject: [PATCH 13/32] rename variable --- .../pure_pursuit_controller.hpp | 2 +- .../src/pure_pursuit_controller.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) 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 index 1b67dff..1e6a257 100644 --- 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 @@ -117,7 +117,7 @@ class PurePursuitController : public nav2_core::Controller bool approach_vel_scaling_; double min_approach_vel_scaling_; double control_duration_; - double max_time_to_collision_; + double max_allowed_time_to_collision; nav_msgs::msg::Path global_plan_; std::shared_ptr> global_pub_; diff --git a/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp b/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp index a2c15bf..47c6156 100644 --- a/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp +++ b/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp @@ -128,7 +128,7 @@ void PurePursuitController::configure( declare_parameter_if_not_declared( node, plugin_name_ + ".approach_vel_scaling", rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( - node, plugin_name_ + ".max_time_to_collision", rclcpp::ParameterValue(1.0)); + node, plugin_name_ + ".max_allowed_time_to_collision", rclcpp::ParameterValue(0.25)); node->get_parameter(plugin_name_ + ".desired_linear_vel", desired_linear_vel_); @@ -145,7 +145,7 @@ void PurePursuitController::configure( use_velocity_scaled_lookahead_dist_); node->get_parameter(plugin_name_ + ".min_approach_vel_scaling", min_approach_vel_scaling_); node->get_parameter(plugin_name_ + ".approach_vel_scaling", approach_vel_scaling_); - node->get_parameter(plugin_name_ + ".max_time_to_collision", max_time_to_collision_); + node->get_parameter(plugin_name_ + ".max_allowed_time_to_collision", max_allowed_time_to_collision_); double control_frequency = 20.0; node->get_parameter("control_frequency", control_frequency); @@ -329,7 +329,7 @@ bool PurePursuitController::isCollisionImminent( curr_pose.theta = tf2::getYaw(robot_pose.pose.orientation); for (unsigned int i = 1; i < num_pts; i++) { - if (i * projection_time > max_time_to_collision_) { + if (i * projection_time > max_allowed_time_to_collision) { break; } From 664d933b4ea2042ef18f7e52e372904d674d0f1f Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Fri, 11 Dec 2020 17:50:17 -0800 Subject: [PATCH 14/32] removing testing artifacts --- nav2_pure_pursuit_controller/README.md | 4 ++-- .../pure_pursuit_controller.hpp | 2 +- .../src/pure_pursuit_controller.cpp | 24 +++++-------------- 3 files changed, 9 insertions(+), 21 deletions(-) diff --git a/nav2_pure_pursuit_controller/README.md b/nav2_pure_pursuit_controller/README.md index b91bef2..ddfd15d 100644 --- a/nav2_pure_pursuit_controller/README.md +++ b/nav2_pure_pursuit_controller/README.md @@ -17,8 +17,8 @@ This implementation has a number of non-standard features. - Collision avoidance in the computed velocity direction between the robot in the look ahead point to ensure safe path following. Uses a maximum collision time parameter to inform the amount of time to forward simulate for collisions. Set to a very large number to always forward simulate to the carrot location - Optional dynamic scaling of the look ahead point distance proportional to velocity. This helps you have a more stable robot path tracking over a broader range of velocity inputs if your robot has a large band of operating velocities. There are parameters for the minimum and maximum distances as well. - Optional slow on approach to the goal. The default algorithm tracks a path at a given linear velocity. This feature allows you to slow the robot on its approach to a goal and also set the minimum percentage (from 0.0-1.0) on approach so it doesn't approach 0% before attaining the goal. -- Kinematic speed limiting to make sure the resulting trajectories are kinematically feasible. +- Kinematic speed limiting on linear velocities to make sure the resulting trajectories are kinematically feasible on getting up to speed and slowing to a stop. This package will also publish a few useful topics: - lookahead point: the position at which the lookahead point is located the robot is attempting to follow -- lookahead arc: the arc produced by the pure pursuit algorithm for debug visualization while tuning. It is also the points on the path that the collision detector is running. In a collision state, the last published arc will be the points leading up to, but not including, the collision. +- lookahead arc: the arc produced by the pure pursuit algorithm for debug visualization while tuning. It is also the points on the path that the collision detector is running. In a collision state, the last published arc will be the points leading up to, and including, the first point in collision. 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 index 1e6a257..f981f61 100644 --- 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 @@ -117,7 +117,7 @@ class PurePursuitController : public nav2_core::Controller bool approach_vel_scaling_; double min_approach_vel_scaling_; double control_duration_; - double max_allowed_time_to_collision; + double max_allowed_time_to_collision_; nav_msgs::msg::Path global_plan_; std::shared_ptr> global_pub_; diff --git a/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp b/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp index 47c6156..bb0f557 100644 --- a/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp +++ b/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp @@ -53,15 +53,6 @@ using nav2_util::geometry_utils::euclidean_distance; namespace nav2_pure_pursuit_controller { -// TEST -// general performance: follows path well, doesn't veer off, defaults, can get on path OK -// make it so able to scale down linear velocity on sharper turn to make sure it can match? - -// kinematic constraints -// slowing on approach -- generally speaking working, but not ready yet - -// rotate to heading option - /** * Find element in iterator with the minimum calculated value */ @@ -128,7 +119,7 @@ void PurePursuitController::configure( declare_parameter_if_not_declared( node, plugin_name_ + ".approach_vel_scaling", rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( - node, plugin_name_ + ".max_allowed_time_to_collision", rclcpp::ParameterValue(0.25)); + node, plugin_name_ + ".max_allowed_time_to_collision", rclcpp::ParameterValue(1.0)); node->get_parameter(plugin_name_ + ".desired_linear_vel", desired_linear_vel_); @@ -241,12 +232,12 @@ geometry_msgs::msg::TwistStamped PurePursuitController::computeVelocityCommands( linear_vel = desired_linear_vel_; angular_vel = desired_linear_vel_ * curvature; - // Make sure we're in compliance with basic kinematics + // Make sure we're in compliance with basic constraints applyConstraints( linear_vel, angular_vel, fabs(lookahead_dist - sqrt(carrot_dist2)), lookahead_dist, speed); - // Collision checking + // 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("PurePursuitController detected collision ahead!"); @@ -329,7 +320,8 @@ bool PurePursuitController::isCollisionImminent( curr_pose.theta = tf2::getYaw(robot_pose.pose.orientation); for (unsigned int i = 1; i < num_pts; i++) { - if (i * projection_time > max_allowed_time_to_collision) { + // only forward simulate within time requested + if (i * projection_time > max_allowed_time_to_collision_) { break; } @@ -372,10 +364,6 @@ bool PurePursuitController::inCollision(const double & x, const double & y) } } -// LIN ACCEL -// LIN DECEL -// CARROT DIST on approach slow - void PurePursuitController::applyConstraints( double & linear_vel, double & angular_vel, const double & dist_error, const double & lookahead_dist, @@ -397,7 +385,7 @@ void PurePursuitController::applyConstraints( 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); - // for the moment, no smoothing on angular velocities, we find the existing + // 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 From c4af1780575c47282995c7ef2e97da3ca54f7bfd Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 14 Dec 2020 16:47:55 -0800 Subject: [PATCH 15/32] adding curvature constraint terms --- nav2_pure_pursuit_controller/README.md | 7 +++++ .../pure_pursuit_controller.hpp | 4 ++- .../src/pure_pursuit_controller.cpp | 31 ++++++++++++++++--- 3 files changed, 36 insertions(+), 6 deletions(-) diff --git a/nav2_pure_pursuit_controller/README.md b/nav2_pure_pursuit_controller/README.md index ddfd15d..666df3c 100644 --- a/nav2_pure_pursuit_controller/README.md +++ b/nav2_pure_pursuit_controller/README.md @@ -18,7 +18,14 @@ This implementation has a number of non-standard features. - Optional dynamic scaling of the look ahead point distance proportional to velocity. This helps you have a more stable robot path tracking over a broader range of velocity inputs if your robot has a large band of operating velocities. There are parameters for the minimum and maximum distances as well. - Optional slow on approach to the goal. The default algorithm tracks a path at a given linear velocity. This feature allows you to slow the robot on its approach to a goal and also set the minimum percentage (from 0.0-1.0) on approach so it doesn't approach 0% before attaining the goal. - Kinematic speed limiting on linear velocities to make sure the resulting trajectories are kinematically feasible on getting up to speed and slowing to a stop. +- Optionally scaling linear velocity by its proximity to obstacles and curvature. 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. The major benefit of scaling by curvature is to create intuitive behavior of slowing the robot when making sharp turns and also de-sensitising the look ahead distances which would otherwise be more sensitive to overshoot in some such cases (e.g. average users require little tuning to get reasonable behavior). A secondary benefit of scaling by curvature is to all the robot to natively rotate to rough path heading when using holonomic planners without explicit 'rotate to heading' commands. This package will also publish a few useful topics: - lookahead point: the position at which the lookahead point is located the robot is attempting to follow - lookahead arc: the arc produced by the pure pursuit algorithm for debug visualization while tuning. It is also the points on the path that the collision detector is running. In a collision state, the last published arc will be the points leading up to, and including, the first point in collision. + +--- + +Adaptive regulated/restricted pure pursuit. Adqptive for changing pt and regulated on speed to avoid collisions with nearby things/collision checking/high curvature where devation might be more prone + +Any more principled approaches I can take? Think about the tools in my toolbox (KF, PF, geometry, NLLS, unit analysis, kinematics, cost function term formulations) 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 index f981f61..696f3b7 100644 --- 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 @@ -92,7 +92,7 @@ class PurePursuitController : public nav2_core::Controller void applyConstraints( double & linear_vel, double & angular_vel, const double & dist_error, const double & lookahead_dist, - const geometry_msgs::msg::Twist & speed); + const double & curvature, const geometry_msgs::msg::Twist & speed); geometry_msgs::msg::PoseStamped getLookAheadMarker(const double &, const nav_msgs::msg::Path &); @@ -118,6 +118,8 @@ class PurePursuitController : public nav2_core::Controller double min_approach_vel_scaling_; double control_duration_; double max_allowed_time_to_collision_; + bool linear_velocity_scaling_; + double linear_scaling_min_radius_; nav_msgs::msg::Path global_plan_; std::shared_ptr> global_pub_; diff --git a/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp b/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp index bb0f557..a595985 100644 --- a/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp +++ b/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp @@ -90,8 +90,8 @@ void PurePursuitController::configure( costmap_ = costmap_ros_->getCostmap(); tf_ = tf; plugin_name_ = name; - logger_ = node->get_logger(); clock_ = node->get_clock(); + logger_ = node->get_logger(); declare_parameter_if_not_declared( node, plugin_name_ + ".desired_linear_vel", rclcpp::ParameterValue(0.5)); @@ -120,7 +120,10 @@ void PurePursuitController::configure( node, plugin_name_ + ".approach_vel_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_ + ".linear_velocity_scaling", rclcpp::ParameterValue(true)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".linear_scaling_min_radius", rclcpp::ParameterValue(0.90)); node->get_parameter(plugin_name_ + ".desired_linear_vel", desired_linear_vel_); node->get_parameter(plugin_name_ + ".max_linear_accel", max_linear_accel_); @@ -137,6 +140,8 @@ void PurePursuitController::configure( node->get_parameter(plugin_name_ + ".min_approach_vel_scaling", min_approach_vel_scaling_); node->get_parameter(plugin_name_ + ".approach_vel_scaling", approach_vel_scaling_); node->get_parameter(plugin_name_ + ".max_allowed_time_to_collision", max_allowed_time_to_collision_); + node->get_parameter(plugin_name_ + ".linear_velocity_scaling", linear_velocity_scaling_); + node->get_parameter(plugin_name_ + ".linear_scaling_min_radius", linear_scaling_min_radius_); double control_frequency = 20.0; node->get_parameter("control_frequency", control_frequency); @@ -235,7 +240,7 @@ geometry_msgs::msg::TwistStamped PurePursuitController::computeVelocityCommands( // Make sure we're in compliance with basic constraints applyConstraints( linear_vel, angular_vel, - fabs(lookahead_dist - sqrt(carrot_dist2)), lookahead_dist, speed); + fabs(lookahead_dist - sqrt(carrot_dist2)), lookahead_dist, curvature, speed); // Collision checking on this velocity heading if (isCollisionImminent(pose, carrot_pose, curvature, linear_vel, angular_vel)) { @@ -307,7 +312,7 @@ bool PurePursuitController::isCollisionImminent( 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 = 1 / curvature; + 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(); @@ -367,7 +372,7 @@ bool PurePursuitController::inCollision(const double & x, const double & y) void PurePursuitController::applyConstraints( double & linear_vel, double & angular_vel, const double & dist_error, const double & lookahead_dist, - const geometry_msgs::msg::Twist & curr_speed) + const double & curvature, const geometry_msgs::msg::Twist & curr_speed) { // 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 @@ -379,6 +384,22 @@ void PurePursuitController::applyConstraints( linear_vel = linear_vel * velocity_scaling; } + + // limit the linear velocity by curvature + const double radius = fabs(1.0 / curvature); + if (linear_velocity_scaling_ && radius < linear_scaling_min_radius_) { + std::cout << radius; + std::cout << " Before: " << linear_vel; + linear_vel *= 1.0 - (fabs(radius - linear_scaling_min_radius_) / linear_scaling_min_radius_); + std::cout << " After: " << linear_vel << std::endl; + } + + // // limit the linear velocity by distance to obstacles (N meters away thresh) OR cost at cell as approximation for proximity (N cost thresh) + // if (linear_velocity_scaling_ && cost != NO_INFORMATION) { + // // (0-253) range (non inclusive) of valid options. 128 = possibly inscribed + // linear vel = + // } + // make sure linear velocities are kinematically feasible, v = v0 + a*t double & dt = control_duration_; const double max_feasible_linear_speed = curr_speed.linear.x + max_linear_accel_ * dt; From 360e165235f1a254eafe067cbb80bafc25a41436 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 14 Dec 2020 17:44:05 -0800 Subject: [PATCH 16/32] parameter updates --- nav2_pure_pursuit_controller/README.md | 25 ++++++--- .../pure_pursuit_controller.hpp | 7 +-- .../src/pure_pursuit_controller.cpp | 54 +++++++++++-------- 3 files changed, 55 insertions(+), 31 deletions(-) diff --git a/nav2_pure_pursuit_controller/README.md b/nav2_pure_pursuit_controller/README.md index 666df3c..fe9a349 100644 --- a/nav2_pure_pursuit_controller/README.md +++ b/nav2_pure_pursuit_controller/README.md @@ -2,7 +2,7 @@ Tutorial code example is 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. +This controller 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. ## How the algorithm works The global path is continuously pruned to the closest point to the robot (see the figure below). @@ -18,14 +18,27 @@ This implementation has a number of non-standard features. - Optional dynamic scaling of the look ahead point distance proportional to velocity. This helps you have a more stable robot path tracking over a broader range of velocity inputs if your robot has a large band of operating velocities. There are parameters for the minimum and maximum distances as well. - Optional slow on approach to the goal. The default algorithm tracks a path at a given linear velocity. This feature allows you to slow the robot on its approach to a goal and also set the minimum percentage (from 0.0-1.0) on approach so it doesn't approach 0% before attaining the goal. - Kinematic speed limiting on linear velocities to make sure the resulting trajectories are kinematically feasible on getting up to speed and slowing to a stop. -- Optionally scaling linear velocity by its proximity to obstacles and curvature. 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. The major benefit of scaling by curvature is to create intuitive behavior of slowing the robot when making sharp turns and also de-sensitising the look ahead distances which would otherwise be more sensitive to overshoot in some such cases (e.g. average users require little tuning to get reasonable behavior). A secondary benefit of scaling by curvature is to all the robot to natively rotate to rough path heading when using holonomic planners without explicit 'rotate to heading' commands. +- Optionally scaling linear velocity by its proximity to obstacles and curvature. 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. The major benefit of scaling by curvature is to create intuitive behavior of slowing the robot when making sharp turns and also de-sensitising the look ahead distances which would otherwise be more sensitive to overshoot in some such cases (e.g. average users require little tuning to get reasonable behavior). A secondary benefit of scaling by curvature is to all the robot to natively rotate to rough path heading when using holonomic planners (e.g. don't align starting pose orientation with current robot orientation or in preempted commands changing path directions) without explicit 'rotate to heading' commands. + +## Topics and parameters This package will also publish a few useful topics: - lookahead point: the position at which the lookahead point is located the robot is attempting to follow -- lookahead arc: the arc produced by the pure pursuit algorithm for debug visualization while tuning. It is also the points on the path that the collision detector is running. In a collision state, the last published arc will be the points leading up to, and including, the first point in collision. +- lookahead arc: the arc produced by the pure pursuit algorithm for debug visualization while tuning. It is also the points on the path that the collision detector is running. In a collision state, the last published arc will be the points leading up to, and including, the first point in collision. It 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. + +Parameters table ---- +## Notes to users -Adaptive regulated/restricted pure pursuit. Adqptive for changing pt and regulated on speed to avoid collisions with nearby things/collision checking/high curvature where devation might be more prone +- On tuning look ahead point for desired behavior (wiggling / overshoot / cuttoff stuff) +- On using velocity based lookahead range rather than static +- On on-approach velocity scaling +- On linear velocity scaling by curvature / cost +- On rotate to heading / goal (?) -Any more principled approaches I can take? Think about the tools in my toolbox (KF, PF, geometry, NLLS, unit analysis, kinematics, cost function term formulations) +## TODO +- param table +- topics table +- better intro / description / gif / images +- fill in notes to users +- cleanup features section \ No newline at end of file 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 index 696f3b7..f479c28 100644 --- 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 @@ -114,12 +114,13 @@ class PurePursuitController : public nav2_core::Controller double max_linear_decel_; bool use_velocity_scaled_lookahead_dist_; tf2::Duration transform_tolerance_; - bool approach_vel_scaling_; + bool use_approach_vel_scaling_; double min_approach_vel_scaling_; double control_duration_; double max_allowed_time_to_collision_; - bool linear_velocity_scaling_; - double linear_scaling_min_radius_; + bool use_linear_velocity_scaling_; + double restricted_linear_scaling_min_radius_; + double restricted_linear_scaling_min_speed_; nav_msgs::msg::Path global_plan_; std::shared_ptr> global_pub_; diff --git a/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp b/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp index a595985..f58af35 100644 --- a/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp +++ b/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp @@ -93,6 +93,9 @@ void PurePursuitController::configure( clock_ = node->get_clock(); 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( @@ -117,13 +120,15 @@ void PurePursuitController::configure( declare_parameter_if_not_declared( node, plugin_name_ + ".min_approach_vel_scaling", rclcpp::ParameterValue(0.10)); declare_parameter_if_not_declared( - node, plugin_name_ + ".approach_vel_scaling", rclcpp::ParameterValue(true)); + 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_ + ".linear_velocity_scaling", rclcpp::ParameterValue(true)); + node, plugin_name_ + ".use_restricted_linear_velocity_scaling", rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( - node, plugin_name_ + ".linear_scaling_min_radius", rclcpp::ParameterValue(0.90)); + node, plugin_name_ + ".restricted_linear_scaling_min_radius", rclcpp::ParameterValue(0.90)); + declare_parameter_if_not_declared( + node, plugin_name_ + ".restricted_linear_scaling_min_speed", rclcpp::ParameterValue(0.25)); node->get_parameter(plugin_name_ + ".desired_linear_vel", desired_linear_vel_); node->get_parameter(plugin_name_ + ".max_linear_accel", max_linear_accel_); @@ -133,17 +138,15 @@ void PurePursuitController::configure( 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_ + ".max_angular_vel", max_angular_vel_); - double transform_tolerance; 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_vel_scaling", min_approach_vel_scaling_); - node->get_parameter(plugin_name_ + ".approach_vel_scaling", approach_vel_scaling_); + 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_ + ".linear_velocity_scaling", linear_velocity_scaling_); - node->get_parameter(plugin_name_ + ".linear_scaling_min_radius", linear_scaling_min_radius_); - - double control_frequency = 20.0; + node->get_parameter(plugin_name_ + ".use_restricted_linear_velocity_scaling", use_linear_velocity_scaling_); + node->get_parameter(plugin_name_ + ".restricted_linear_scaling_min_radius", restricted_linear_scaling_min_radius_); + node->get_parameter(plugin_name_ + ".restricted_linear_scaling_min_speed", restricted_linear_scaling_min_speed_); node->get_parameter("control_frequency", control_frequency); transform_tolerance_ = tf2::durationFromSec(transform_tolerance); @@ -281,8 +284,8 @@ bool PurePursuitController::isCollisionImminent( const double & curvature, const double & linear_vel, const double & angular_vel) { - // 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 + // 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. @@ -376,7 +379,7 @@ void PurePursuitController::applyConstraints( { // 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 (approach_vel_scaling_ && dist_error > 2.0 * costmap_->getResolution()) { + if (use_approach_vel_scaling_ && dist_error > 2.0 * costmap_->getResolution()) { double velocity_scaling = 1.0 - (dist_error / lookahead_dist); if (velocity_scaling < min_approach_vel_scaling_) { velocity_scaling = min_approach_vel_scaling_; @@ -384,23 +387,30 @@ void PurePursuitController::applyConstraints( linear_vel = linear_vel * velocity_scaling; } - // limit the linear velocity by curvature const double radius = fabs(1.0 / curvature); - if (linear_velocity_scaling_ && radius < linear_scaling_min_radius_) { + const double & min_rad = restricted_linear_scaling_min_radius_; + if (use_linear_velocity_scaling_ && radius < min_rad) { std::cout << radius; - std::cout << " Before: " << linear_vel; - linear_vel *= 1.0 - (fabs(radius - linear_scaling_min_radius_) / linear_scaling_min_radius_); - std::cout << " After: " << linear_vel << std::endl; + linear_vel *= 1.0 - (fabs(radius - min_rad) / min_rad); + if (linear_vel < restricted_linear_scaling_min_speed_) { + linear_vel = restricted_linear_scaling_min_speed_; + } } - // // limit the linear velocity by distance to obstacles (N meters away thresh) OR cost at cell as approximation for proximity (N cost thresh) - // if (linear_velocity_scaling_ && cost != NO_INFORMATION) { - // // (0-253) range (non inclusive) of valid options. 128 = possibly inscribed - // linear vel = + // // limit the linear velocity by proximity to obstacles + // if (use_linear_velocity_scaling_ && cost != NO_INFORMATION) { + // // 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 + // if (cost > static_cast(128)) { + // linear_vel = restricted_linear_scaling_min_speed_; + // } else { + // linear_vel *= fabs(static_cast(cost) - 128.0) / 128.0; + // } // } - // make sure linear velocities are kinematically feasible, v = v0 + a*t + // make sure linear velocities are 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; From 95e7de53d300860e5df29163301301a7bb7f0be7 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 14 Dec 2020 18:28:25 -0800 Subject: [PATCH 17/32] adding velocity scaled costs --- nav2_pure_pursuit_controller/README.md | 4 +- .../pure_pursuit_controller.hpp | 10 ++- .../src/pure_pursuit_controller.cpp | 88 +++++++++++-------- 3 files changed, 59 insertions(+), 43 deletions(-) diff --git a/nav2_pure_pursuit_controller/README.md b/nav2_pure_pursuit_controller/README.md index fe9a349..8361f4c 100644 --- a/nav2_pure_pursuit_controller/README.md +++ b/nav2_pure_pursuit_controller/README.md @@ -18,7 +18,7 @@ This implementation has a number of non-standard features. - Optional dynamic scaling of the look ahead point distance proportional to velocity. This helps you have a more stable robot path tracking over a broader range of velocity inputs if your robot has a large band of operating velocities. There are parameters for the minimum and maximum distances as well. - Optional slow on approach to the goal. The default algorithm tracks a path at a given linear velocity. This feature allows you to slow the robot on its approach to a goal and also set the minimum percentage (from 0.0-1.0) on approach so it doesn't approach 0% before attaining the goal. - Kinematic speed limiting on linear velocities to make sure the resulting trajectories are kinematically feasible on getting up to speed and slowing to a stop. -- Optionally scaling linear velocity by its proximity to obstacles and curvature. 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. The major benefit of scaling by curvature is to create intuitive behavior of slowing the robot when making sharp turns and also de-sensitising the look ahead distances which would otherwise be more sensitive to overshoot in some such cases (e.g. average users require little tuning to get reasonable behavior). A secondary benefit of scaling by curvature is to all the robot to natively rotate to rough path heading when using holonomic planners (e.g. don't align starting pose orientation with current robot orientation or in preempted commands changing path directions) without explicit 'rotate to heading' commands. +- Optionally scaling linear velocity by its proximity to obstacles and curvature. 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. The major benefit of scaling by curvature is to create intuitive behavior of slowing the robot when making sharp turns and also de-sensitising the look ahead distances which would otherwise be more sensitive to overshoot in some such cases (e.g. average users require little tuning to get reasonable behavior). A secondary benefit of scaling by curvature is to all the robot to natively rotate to rough path heading when using holonomic planners (e.g. don't align starting pose orientation with current robot orientation or in preempted commands changing path directions) without explicit 'rotate to heading' commands. Pure Pursuit controllers otherwise would be unable to recover from this in confined spaces. ## Topics and parameters @@ -41,4 +41,4 @@ Parameters table - topics table - better intro / description / gif / images - fill in notes to users -- cleanup features section \ No newline at end of file +- cleanup features section 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 index f479c28..776f03c 100644 --- 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 @@ -88,11 +88,13 @@ class PurePursuitController : public nav2_core::Controller 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, double & angular_vel, const double & dist_error, const double & lookahead_dist, - const double & curvature, const geometry_msgs::msg::Twist & speed); + const double & curvature, const geometry_msgs::msg::Twist & speed, + const double & pose_cost); geometry_msgs::msg::PoseStamped getLookAheadMarker(const double &, const nav_msgs::msg::Path &); @@ -118,9 +120,9 @@ class PurePursuitController : public nav2_core::Controller double min_approach_vel_scaling_; double control_duration_; double max_allowed_time_to_collision_; - bool use_linear_velocity_scaling_; - double restricted_linear_scaling_min_radius_; - double restricted_linear_scaling_min_speed_; + bool use_regulated_linear_velocity_scaling_; + double regulated_linear_scaling_min_radius_; + double regulated_linear_scaling_min_speed_; nav_msgs::msg::Path global_plan_; std::shared_ptr> global_pub_; diff --git a/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp b/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp index f58af35..ffe030b 100644 --- a/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp +++ b/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp @@ -49,6 +49,7 @@ 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_pure_pursuit_controller { @@ -124,11 +125,11 @@ void PurePursuitController::configure( 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_restricted_linear_velocity_scaling", rclcpp::ParameterValue(true)); + node, plugin_name_ + ".use_regulated_linear_velocity_scaling", rclcpp::ParameterValue(true)); declare_parameter_if_not_declared( - node, plugin_name_ + ".restricted_linear_scaling_min_radius", rclcpp::ParameterValue(0.90)); + node, plugin_name_ + ".regulated_linear_scaling_min_radius", rclcpp::ParameterValue(0.90)); declare_parameter_if_not_declared( - node, plugin_name_ + ".restricted_linear_scaling_min_speed", rclcpp::ParameterValue(0.25)); + node, plugin_name_ + ".regulated_linear_scaling_min_speed", rclcpp::ParameterValue(0.25)); node->get_parameter(plugin_name_ + ".desired_linear_vel", desired_linear_vel_); node->get_parameter(plugin_name_ + ".max_linear_accel", max_linear_accel_); @@ -141,12 +142,12 @@ void PurePursuitController::configure( 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_vel_scaling", min_approach_vel_scaling_); + node->get_parameter(plugin_name_ + ".min_approach_vel_scaling", min_approach_vel_scaling_); // TODO use an absolute velocity rather than a %? 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_restricted_linear_velocity_scaling", use_linear_velocity_scaling_); - node->get_parameter(plugin_name_ + ".restricted_linear_scaling_min_radius", restricted_linear_scaling_min_radius_); - node->get_parameter(plugin_name_ + ".restricted_linear_scaling_min_speed", restricted_linear_scaling_min_speed_); + node->get_parameter(plugin_name_ + ".use_regulated_linear_velocity_scaling", use_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("control_frequency", control_frequency); transform_tolerance_ = tf2::durationFromSec(transform_tolerance); @@ -193,7 +194,7 @@ void PurePursuitController::deactivate() double PurePursuitController::getLookAheadDistance(const geometry_msgs::msg::Twist & speed) { // If using velocity-scaled look ahead distances, find and clamp the dist - // Else, use the default look ahead distance + // 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_; @@ -243,7 +244,8 @@ geometry_msgs::msg::TwistStamped PurePursuitController::computeVelocityCommands( // Make sure we're in compliance with basic constraints applyConstraints( linear_vel, angular_vel, - fabs(lookahead_dist - sqrt(carrot_dist2)), lookahead_dist, curvature, speed); + 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)) { @@ -358,8 +360,6 @@ bool PurePursuitController::isCollisionImminent( bool PurePursuitController::inCollision(const double & x, const double & y) { - using namespace nav2_costmap_2d; // NOLINT - unsigned int mx, my; costmap_->worldToMap(x, y, mx, my); @@ -372,11 +372,48 @@ bool PurePursuitController::inCollision(const double & x, const double & y) } } +double PurePursuitController::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 PurePursuitController::applyConstraints( double & linear_vel, double & angular_vel, const double & dist_error, const double & lookahead_dist, - const double & curvature, const geometry_msgs::msg::Twist & curr_speed) + 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) { + std::cout << radius; + 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_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()) { @@ -387,30 +424,7 @@ void PurePursuitController::applyConstraints( linear_vel = linear_vel * velocity_scaling; } - // limit the linear velocity by curvature - const double radius = fabs(1.0 / curvature); - const double & min_rad = restricted_linear_scaling_min_radius_; - if (use_linear_velocity_scaling_ && radius < min_rad) { - std::cout << radius; - linear_vel *= 1.0 - (fabs(radius - min_rad) / min_rad); - if (linear_vel < restricted_linear_scaling_min_speed_) { - linear_vel = restricted_linear_scaling_min_speed_; - } - } - - // // limit the linear velocity by proximity to obstacles - // if (use_linear_velocity_scaling_ && cost != NO_INFORMATION) { - // // 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 - // if (cost > static_cast(128)) { - // linear_vel = restricted_linear_scaling_min_speed_; - // } else { - // linear_vel *= fabs(static_cast(cost) - 128.0) / 128.0; - // } - // } - - // make sure linear velocities are kinematically feasible, v = v0 + a * dt + // 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; @@ -425,7 +439,7 @@ void PurePursuitController::applyConstraints( // 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); - // make sure in range of generally valid velocities + // Limit range to generally valid velocities linear_vel = std::clamp(linear_vel, 0.0, desired_linear_vel_); angular_vel = std::clamp(angular_vel, -max_angular_vel_, max_angular_vel_); } From ccc9f827f322fe4f22f6d5a103095830035ab230 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 14 Dec 2020 18:31:37 -0800 Subject: [PATCH 18/32] adding changes on rayman's review --- nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp b/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp index ffe030b..d1ec736 100644 --- a/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp +++ b/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp @@ -255,8 +255,7 @@ geometry_msgs::msg::TwistStamped PurePursuitController::computeVelocityCommands( // populate and return message geometry_msgs::msg::TwistStamped cmd_vel; - cmd_vel.header.frame_id = pose.header.frame_id; - cmd_vel.header.stamp = clock_->now(); + cmd_vel.header = pose.header; cmd_vel.twist.linear.x = linear_vel; cmd_vel.twist.angular.z = angular_vel; return cmd_vel; From aaedde426c4090dfd3c9c03183d737ffb227ae71 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 14 Dec 2020 18:35:50 -0800 Subject: [PATCH 19/32] fixing typo --- nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp b/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp index d1ec736..a969444 100644 --- a/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp +++ b/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp @@ -404,7 +404,7 @@ void PurePursuitController::applyConstraints( // 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 + double max_non_collision = 128.0; if (pose_cost > max_non_collision) { linear_vel = regulated_linear_scaling_min_speed_; } else { From 35d64f214a153e6ee630918d9bf56f777d8902a6 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 14 Dec 2020 18:39:49 -0800 Subject: [PATCH 20/32] more readme color --- nav2_pure_pursuit_controller/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_pure_pursuit_controller/README.md b/nav2_pure_pursuit_controller/README.md index 8361f4c..25e736b 100644 --- a/nav2_pure_pursuit_controller/README.md +++ b/nav2_pure_pursuit_controller/README.md @@ -14,7 +14,7 @@ This lookahead point will be given to the pure pursuite algorithm to calculate a ## Features This implementation has a number of non-standard features. -- Collision avoidance in the computed velocity direction between the robot in the look ahead point to ensure safe path following. Uses a maximum collision time parameter to inform the amount of time to forward simulate for collisions. Set to a very large number to always forward simulate to the carrot location +- Collision detection in the computed velocity arc between the robot in the look ahead point to ensure safe path following. Uses a maximum collision time parameter to inform the amount of time to forward simulate for collisions. Set to a very large number to always forward simulate to the carrot location. Lookahead time scales the collision checking distance by the velocity so that it checks a consistent time `t` 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. - Optional dynamic scaling of the look ahead point distance proportional to velocity. This helps you have a more stable robot path tracking over a broader range of velocity inputs if your robot has a large band of operating velocities. There are parameters for the minimum and maximum distances as well. - Optional slow on approach to the goal. The default algorithm tracks a path at a given linear velocity. This feature allows you to slow the robot on its approach to a goal and also set the minimum percentage (from 0.0-1.0) on approach so it doesn't approach 0% before attaining the goal. - Kinematic speed limiting on linear velocities to make sure the resulting trajectories are kinematically feasible on getting up to speed and slowing to a stop. From 76df8a95a7bec26774f9faf99013e4d3098ea4f6 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 14 Dec 2020 18:42:00 -0800 Subject: [PATCH 21/32] more context --- nav2_pure_pursuit_controller/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_pure_pursuit_controller/README.md b/nav2_pure_pursuit_controller/README.md index 25e736b..6651c1b 100644 --- a/nav2_pure_pursuit_controller/README.md +++ b/nav2_pure_pursuit_controller/README.md @@ -18,7 +18,7 @@ This implementation has a number of non-standard features. - Optional dynamic scaling of the look ahead point distance proportional to velocity. This helps you have a more stable robot path tracking over a broader range of velocity inputs if your robot has a large band of operating velocities. There are parameters for the minimum and maximum distances as well. - Optional slow on approach to the goal. The default algorithm tracks a path at a given linear velocity. This feature allows you to slow the robot on its approach to a goal and also set the minimum percentage (from 0.0-1.0) on approach so it doesn't approach 0% before attaining the goal. - Kinematic speed limiting on linear velocities to make sure the resulting trajectories are kinematically feasible on getting up to speed and slowing to a stop. -- Optionally scaling linear velocity by its proximity to obstacles and curvature. 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. The major benefit of scaling by curvature is to create intuitive behavior of slowing the robot when making sharp turns and also de-sensitising the look ahead distances which would otherwise be more sensitive to overshoot in some such cases (e.g. average users require little tuning to get reasonable behavior). A secondary benefit of scaling by curvature is to all the robot to natively rotate to rough path heading when using holonomic planners (e.g. don't align starting pose orientation with current robot orientation or in preempted commands changing path directions) without explicit 'rotate to heading' commands. Pure Pursuit controllers otherwise would be unable to recover from this in confined spaces. +- Optionally scaling linear velocity by its proximity to obstacles and curvature. 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. The major benefit of scaling by curvature is to create intuitive behavior of slowing the robot when making sharp turns and also de-sensitising the look ahead distances which would otherwise be more sensitive to overshoot in some such cases (e.g. average users require little tuning to get reasonable behavior). A secondary benefit of scaling by curvature is to all the robot to natively rotate to rough path heading when using holonomic planners (e.g. don't align starting pose orientation with current robot orientation or in preempted commands changing path directions) without explicit 'rotate to heading' commands. Pure Pursuit controllers otherwise would be unable to recover from this in confined spaces. Mixed with the time-scaled collision checker, this makes a near-perfect combination to have the regulated pure pursuit algorithm handle high starting deviations from the path and navigate collision-free in tight spaces without overshoot. ## Topics and parameters From aed09f6e032a0c8f71b0de174a539252d3d3ef27 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 14 Dec 2020 18:52:46 -0800 Subject: [PATCH 22/32] remove printout --- nav2_pure_pursuit_controller/README.md | 1 + nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp | 1 - 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_pure_pursuit_controller/README.md b/nav2_pure_pursuit_controller/README.md index 6651c1b..87e5104 100644 --- a/nav2_pure_pursuit_controller/README.md +++ b/nav2_pure_pursuit_controller/README.md @@ -42,3 +42,4 @@ Parameters table - better intro / description / gif / images - fill in notes to users - cleanup features section +- my usual intro, "who uses it", etc diff --git a/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp b/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp index a969444..da78733 100644 --- a/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp +++ b/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp @@ -390,7 +390,6 @@ void PurePursuitController::applyConstraints( 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) { - std::cout << radius; 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_; From 035cca33858cf84f7edb38c0313c5730c51c3083 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 14 Dec 2020 19:21:39 -0800 Subject: [PATCH 23/32] change licenses with written approval from other contributors --- nav2_pure_pursuit_controller/README.md | 2 +- .../pure_pursuit_controller.hpp | 48 ++++++------------- nav2_pure_pursuit_controller/package.xml | 2 +- .../src/pure_pursuit_controller.cpp | 48 ++++++------------- 4 files changed, 30 insertions(+), 70 deletions(-) diff --git a/nav2_pure_pursuit_controller/README.md b/nav2_pure_pursuit_controller/README.md index 87e5104..91c24df 100644 --- a/nav2_pure_pursuit_controller/README.md +++ b/nav2_pure_pursuit_controller/README.md @@ -2,7 +2,7 @@ Tutorial code example is referenced in https://navigation.ros.org/plugin_tutorials/docs/writing_new_nav2controller_plugin.html -This controller 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. +This controller implements a variant on the pure pursuit algorithm to track a path. This variant we call the Regulated Adaptive Pure Pursuit Algorithm, due to its additional regulation terms on collision and linear speed. ## How the algorithm works The global path is continuously pruned to the closest point to the robot (see the figure below). 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 index 776f03c..4b5d06d 100644 --- 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 @@ -1,37 +1,17 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2020, Shrijit Singh - * Copyright (c) 2020, Samsung Research America - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ +// 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_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_ #define NAV2_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_ diff --git a/nav2_pure_pursuit_controller/package.xml b/nav2_pure_pursuit_controller/package.xml index 0a6f007..09c68be 100644 --- a/nav2_pure_pursuit_controller/package.xml +++ b/nav2_pure_pursuit_controller/package.xml @@ -6,7 +6,7 @@ Pure pursuit controller Steve Macenski Shrijit Singh - BSD-3-Clause + Apache-2.0 ament_cmake diff --git a/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp b/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp index da78733..42add66 100644 --- a/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp +++ b/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp @@ -1,37 +1,17 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2020, Shrijit Singh - * Copyright (c) 2020, Samsung Research America - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ +// 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 From 5f0f5dcfbd68db7e56902bf4cd250c0276ea1cf7 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 15 Dec 2020 12:42:16 -0800 Subject: [PATCH 24/32] adding rotate to path headings --- .../pure_pursuit_controller.hpp | 20 ++++- .../src/pure_pursuit_controller.cpp | 82 +++++++++++++++---- 2 files changed, 84 insertions(+), 18 deletions(-) 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 index 4b5d06d..6c874ff 100644 --- 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 @@ -35,6 +35,7 @@ class PurePursuitController : public nav2_core::Controller { public: PurePursuitController() = default; + ~PurePursuitController() override = default; void configure( @@ -44,7 +45,9 @@ class PurePursuitController : public nav2_core::Controller void cleanup() override; + void activate() override; + void deactivate() override; geometry_msgs::msg::TwistStamped computeVelocityCommands( @@ -63,11 +66,22 @@ class PurePursuitController : public nav2_core::Controller 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( @@ -78,13 +92,11 @@ class PurePursuitController : public nav2_core::Controller geometry_msgs::msg::PoseStamped getLookAheadMarker(const double &, const nav_msgs::msg::Path &); - rclcpp_lifecycle::LifecycleNode::WeakPtr node_; std::shared_ptr tf_; std::string plugin_name_; std::shared_ptr costmap_ros_; nav2_costmap_2d::Costmap2D * costmap_; rclcpp::Logger logger_ {rclcpp::get_logger("PurePursuitController")}; - rclcpp::Clock::SharedPtr clock_; double desired_linear_vel_; double lookahead_dist_; @@ -101,8 +113,12 @@ class PurePursuitController : public nav2_core::Controller 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_; diff --git a/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp b/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp index 42add66..533be2b 100644 --- a/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp +++ b/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp @@ -60,9 +60,7 @@ void PurePursuitController::configure( std::string name, const std::shared_ptr & tf, const std::shared_ptr & costmap_ros) { - node_ = parent; - - auto node = node_.lock(); + auto node = parent.lock(); if (!node) { throw std::runtime_error("Unable to lock node!"); } @@ -71,7 +69,6 @@ void PurePursuitController::configure( costmap_ = costmap_ros_->getCostmap(); tf_ = tf; plugin_name_ = name; - clock_ = node->get_clock(); logger_ = node->get_logger(); double transform_tolerance = 0.1; @@ -106,10 +103,18 @@ void PurePursuitController::configure( 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_); @@ -126,8 +131,12 @@ void PurePursuitController::configure( 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); @@ -186,6 +195,17 @@ double PurePursuitController::getLookAheadDistance(const geometry_msgs::msg::Twi return lookahead_dist; } +std::unique_ptr PurePursuitController::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 PurePursuitController::computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & speed) @@ -196,12 +216,7 @@ geometry_msgs::msg::TwistStamped PurePursuitController::computeVelocityCommands( // Find look ahead distance and point on path and publish const double lookahead_dist = getLookAheadDistance(speed); auto carrot_pose = getLookAheadMarker(lookahead_dist, transformed_plan); - 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 - carrot_pub_->publish(std::move(carrot_msg)); + carrot_pub_->publish(std::move(createCarrotMsg(carrot_pose))); double linear_vel, angular_vel; @@ -222,10 +237,15 @@ geometry_msgs::msg::TwistStamped PurePursuitController::computeVelocityCommands( angular_vel = desired_linear_vel_ * curvature; // Make sure we're in compliance with basic constraints - applyConstraints( - linear_vel, angular_vel, - fabs(lookahead_dist - sqrt(carrot_dist2)), lookahead_dist, curvature, speed, - costAtPose(pose.pose.position.x, pose.pose.position.y)); + double angle_to_path; + if (shouldRotateToPath(carrot_pose, angle_to_path)) { + rotateToHeading(linear_vel, angular_vel, angle_to_path, speed); + } else { + applyConstraints( + linear_vel, angular_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)) { @@ -241,6 +261,34 @@ geometry_msgs::msg::TwistStamped PurePursuitController::computeVelocityCommands( return cmd_vel; } +bool PurePursuitController::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); + if (use_rotate_to_heading_ && fabs(angle_to_path) > rotate_to_heading_min_angle_) { + return true; + } + + return false; +} + +void PurePursuitController::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 * max_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); + std::cout << "Rotating to path heading..." << std::endl; +} + geometry_msgs::msg::PoseStamped PurePursuitController::getLookAheadMarker( const double & lookahead_dist, const nav_msgs::msg::Path & transformed_plan) @@ -287,7 +335,7 @@ bool PurePursuitController::isCollisionImminent( // debug messages nav_msgs::msg::Path arc_pts_msg; arc_pts_msg.header.frame_id = costmap_ros_->getGlobalFrameID(); - arc_pts_msg.header.stamp = clock_->now(); + 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; @@ -377,7 +425,9 @@ void PurePursuitController::applyConstraints( } // limit the linear velocity by proximity to obstacles - if (use_regulated_linear_velocity_scaling_ && pose_cost != static_cast(NO_INFORMATION)) { + 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 From 5f2117e06c4dc81c1b4b11b81305ce856fba79c3 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 17 Dec 2020 15:01:03 -0800 Subject: [PATCH 25/32] update to minimum API --- .../pure_pursuit_controller.hpp | 4 ++-- .../src/pure_pursuit_controller.cpp | 13 ++++++------- 2 files changed, 8 insertions(+), 9 deletions(-) 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 index 6c874ff..6de951f 100644 --- 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 @@ -85,7 +85,7 @@ class PurePursuitController : public nav2_core::Controller double costAtPose(const double & x, const double & y); void applyConstraints( - double & linear_vel, double & angular_vel, + double & linear_vel, const double & dist_error, const double & lookahead_dist, const double & curvature, const geometry_msgs::msg::Twist & speed, const double & pose_cost); @@ -100,7 +100,7 @@ class PurePursuitController : public nav2_core::Controller double desired_linear_vel_; double lookahead_dist_; - double max_angular_vel_; + double rotate_to_heading_angular_vel_; double max_lookahead_dist_; double min_lookahead_dist_; double lookahead_time_; diff --git a/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp b/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp index 533be2b..595470b 100644 --- a/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp +++ b/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp @@ -89,7 +89,7 @@ void PurePursuitController::configure( declare_parameter_if_not_declared( node, plugin_name_ + ".lookahead_time", rclcpp::ParameterValue(1.5)); declare_parameter_if_not_declared( - node, plugin_name_ + ".max_angular_vel", rclcpp::ParameterValue(1.8)); + 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( @@ -123,7 +123,7 @@ void PurePursuitController::configure( 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_ + ".max_angular_vel", max_angular_vel_); + 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_); @@ -242,8 +242,8 @@ geometry_msgs::msg::TwistStamped PurePursuitController::computeVelocityCommands( rotateToHeading(linear_vel, angular_vel, angle_to_path, speed); } else { applyConstraints( - linear_vel, angular_vel, - fabs(lookahead_dist - sqrt(carrot_dist2)), lookahead_dist, curvature, speed, + linear_vel, fabs(lookahead_dist - sqrt(carrot_dist2)), + lookahead_dist, curvature, speed, costAtPose(pose.pose.position.x, pose.pose.position.y)); } @@ -280,7 +280,7 @@ void PurePursuitController::rotateToHeading( // 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 * max_angular_vel_; + 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; @@ -409,7 +409,7 @@ double PurePursuitController::costAtPose(const double & x, const double & y) } void PurePursuitController::applyConstraints( - double & linear_vel, double & angular_vel, + 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) @@ -469,7 +469,6 @@ void PurePursuitController::applyConstraints( // Limit range to generally valid velocities linear_vel = std::clamp(linear_vel, 0.0, desired_linear_vel_); - angular_vel = std::clamp(angular_vel, -max_angular_vel_, max_angular_vel_); } void PurePursuitController::setPlan(const nav_msgs::msg::Path & path) From a3245b40e00d15e5892dd5a2a395315dfc61564b Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 17 Dec 2020 15:07:46 -0800 Subject: [PATCH 26/32] change variable from scale to absolute value --- .../pure_pursuit_controller.hpp | 2 +- .../src/pure_pursuit_controller.cpp | 12 +++++++----- 2 files changed, 8 insertions(+), 6 deletions(-) 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 index 6de951f..539eedf 100644 --- 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 @@ -109,7 +109,7 @@ class PurePursuitController : public nav2_core::Controller bool use_velocity_scaled_lookahead_dist_; tf2::Duration transform_tolerance_; bool use_approach_vel_scaling_; - double min_approach_vel_scaling_; + double min_approach_linear_velocity_; double control_duration_; double max_allowed_time_to_collision_; bool use_regulated_linear_velocity_scaling_; diff --git a/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp b/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp index 595470b..6819357 100644 --- a/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp +++ b/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp @@ -96,7 +96,7 @@ void PurePursuitController::configure( node, plugin_name_ + ".use_velocity_scaled_lookahead_dist", rclcpp::ParameterValue(false)); declare_parameter_if_not_declared( - node, plugin_name_ + ".min_approach_vel_scaling", rclcpp::ParameterValue(0.10)); + 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( @@ -127,7 +127,7 @@ void PurePursuitController::configure( 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_vel_scaling", min_approach_vel_scaling_); // TODO use an absolute velocity rather than a %? + 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_); @@ -446,10 +446,12 @@ void PurePursuitController::applyConstraints( // 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); - if (velocity_scaling < min_approach_vel_scaling_) { - velocity_scaling = min_approach_vel_scaling_; + double unbounded_vel = linear_vel * velocity_scaling; + if (unbounded_vel < min_approach_linear_velocity_) { + linear_vel = min_approach_linear_velocity_; + } else { + linear_vel = linear_vel * velocity_scaling; } - linear_vel = linear_vel * velocity_scaling; } // Limit linear velocities to be kinematically feasible, v = v0 + a * dt From 2598dea49e6e8f0720385407f82cf34ea637cd82 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 17 Dec 2020 15:40:24 -0800 Subject: [PATCH 27/32] remove cout --- nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp b/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp index 6819357..80205a5 100644 --- a/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp +++ b/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp @@ -286,7 +286,6 @@ void PurePursuitController::rotateToHeading( 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); - std::cout << "Rotating to path heading..." << std::endl; } geometry_msgs::msg::PoseStamped PurePursuitController::getLookAheadMarker( From 35be757f93bd57066243d18db024c660d7e07103 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 17 Dec 2020 16:58:12 -0800 Subject: [PATCH 28/32] rename + readme --- nav2_pure_pursuit_controller/README.md | 45 ------ .../nav2_pure_pursuit_controller.xml | 10 -- .../CMakeLists.txt | 16 +-- .../README.md | 131 ++++++++++++++++++ .../doc/lookahead_algorithm.png | Bin .../regulated_pure_pursuit_controller.hpp | 18 +-- ...nav2_regulated_pure_pursuit_controller.xml | 10 ++ .../package.xml | 4 +- .../src/regulated_pure_pursuit_controller.cpp | 54 ++++---- 9 files changed, 189 insertions(+), 99 deletions(-) delete mode 100644 nav2_pure_pursuit_controller/README.md delete mode 100644 nav2_pure_pursuit_controller/nav2_pure_pursuit_controller.xml rename {nav2_pure_pursuit_controller => nav2_regulated_pure_pursuit_controller}/CMakeLists.txt (66%) create mode 100644 nav2_regulated_pure_pursuit_controller/README.md rename {nav2_pure_pursuit_controller => nav2_regulated_pure_pursuit_controller}/doc/lookahead_algorithm.png (100%) rename nav2_pure_pursuit_controller/include/nav2_pure_pursuit_controller/pure_pursuit_controller.hpp => nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp (87%) create mode 100644 nav2_regulated_pure_pursuit_controller/nav2_regulated_pure_pursuit_controller.xml rename {nav2_pure_pursuit_controller => nav2_regulated_pure_pursuit_controller}/package.xml (87%) rename nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp => nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp (92%) diff --git a/nav2_pure_pursuit_controller/README.md b/nav2_pure_pursuit_controller/README.md deleted file mode 100644 index 91c24df..0000000 --- a/nav2_pure_pursuit_controller/README.md +++ /dev/null @@ -1,45 +0,0 @@ -# Nav2 Pure pursuit controller - -Tutorial code example is referenced in https://navigation.ros.org/plugin_tutorials/docs/writing_new_nav2controller_plugin.html - -This controller implements a variant on the pure pursuit algorithm to track a path. This variant we call the Regulated Adaptive Pure Pursuit Algorithm, due to its additional regulation terms on collision and linear speed. - -## 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) - -## Features - -This implementation has a number of non-standard features. -- Collision detection in the computed velocity arc between the robot in the look ahead point to ensure safe path following. Uses a maximum collision time parameter to inform the amount of time to forward simulate for collisions. Set to a very large number to always forward simulate to the carrot location. Lookahead time scales the collision checking distance by the velocity so that it checks a consistent time `t` 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. -- Optional dynamic scaling of the look ahead point distance proportional to velocity. This helps you have a more stable robot path tracking over a broader range of velocity inputs if your robot has a large band of operating velocities. There are parameters for the minimum and maximum distances as well. -- Optional slow on approach to the goal. The default algorithm tracks a path at a given linear velocity. This feature allows you to slow the robot on its approach to a goal and also set the minimum percentage (from 0.0-1.0) on approach so it doesn't approach 0% before attaining the goal. -- Kinematic speed limiting on linear velocities to make sure the resulting trajectories are kinematically feasible on getting up to speed and slowing to a stop. -- Optionally scaling linear velocity by its proximity to obstacles and curvature. 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. The major benefit of scaling by curvature is to create intuitive behavior of slowing the robot when making sharp turns and also de-sensitising the look ahead distances which would otherwise be more sensitive to overshoot in some such cases (e.g. average users require little tuning to get reasonable behavior). A secondary benefit of scaling by curvature is to all the robot to natively rotate to rough path heading when using holonomic planners (e.g. don't align starting pose orientation with current robot orientation or in preempted commands changing path directions) without explicit 'rotate to heading' commands. Pure Pursuit controllers otherwise would be unable to recover from this in confined spaces. Mixed with the time-scaled collision checker, this makes a near-perfect combination to have the regulated pure pursuit algorithm handle high starting deviations from the path and navigate collision-free in tight spaces without overshoot. - -## Topics and parameters - -This package will also publish a few useful topics: -- lookahead point: the position at which the lookahead point is located the robot is attempting to follow -- lookahead arc: the arc produced by the pure pursuit algorithm for debug visualization while tuning. It is also the points on the path that the collision detector is running. In a collision state, the last published arc will be the points leading up to, and including, the first point in collision. It 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. - -Parameters table - -## Notes to users - -- On tuning look ahead point for desired behavior (wiggling / overshoot / cuttoff stuff) -- On using velocity based lookahead range rather than static -- On on-approach velocity scaling -- On linear velocity scaling by curvature / cost -- On rotate to heading / goal (?) - -## TODO -- param table -- topics table -- better intro / description / gif / images -- fill in notes to users -- cleanup features section -- my usual intro, "who uses it", etc 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/CMakeLists.txt b/nav2_regulated_pure_pursuit_controller/CMakeLists.txt similarity index 66% rename from nav2_pure_pursuit_controller/CMakeLists.txt rename to nav2_regulated_pure_pursuit_controller/CMakeLists.txt index af658d9..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) @@ -30,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 @@ -58,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..01c78b6 --- /dev/null +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -0,0 +1,131 @@ +# 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. + +TODO gif working + +## 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). + +TODO clean up this bullet list +This implementation has a number of non-standard features. +- Collision detection in the computed velocity arc between the robot in the look ahead point to ensure safe path following. Uses a maximum collision time parameter to inform the amount of time to forward simulate for collisions. Set to a very large number to always forward simulate to the carrot location. Lookahead time scales the collision checking distance by the velocity so that it checks a consistent time `t` 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. +- Optional dynamic scaling of the look ahead point distance proportional to velocity. This helps you have a more stable robot path tracking over a broader range of velocity inputs if your robot has a large band of operating velocities. There are parameters for the minimum and maximum distances as well. +- Optional slow on approach to the goal. The default algorithm tracks a path at a given linear velocity. This feature allows you to slow the robot on its approach to a goal and also set the minimum percentage (from 0.0-1.0) on approach so it doesn't approach 0% before attaining the goal. +- Kinematic speed limiting on linear velocities to make sure the resulting trajectories are kinematically feasible on getting up to speed and slowing to a stop. +- Optionally scaling linear velocity by its proximity to obstacles and curvature. 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. The major benefit of scaling by curvature is to create intuitive behavior of slowing the robot when making sharp turns and also de-sensitising the look ahead distances which would otherwise be more sensitive to overshoot in some such cases (e.g. average users require little tuning to get reasonable behavior). A secondary benefit of scaling by curvature is to all the robot to natively rotate to rough path heading when using holonomic planners (e.g. don't align starting pose orientation with current robot orientation or in preempted commands changing path directions) without explicit 'rotate to heading' commands. Pure Pursuit controllers otherwise would be unable to recover from this in confined spaces. Mixed with the time-scaled collision checker, this makes a near-perfect combination to have the regulated pure pursuit algorithm handle high starting deviations from the path and navigate collision-free in tight spaces without overshoot. + +TODO gif of it getting out of a constrained / slowing on high curavture features + +## 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_pure_pursuit_controller/include/nav2_pure_pursuit_controller/pure_pursuit_controller.hpp b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp similarity index 87% rename from nav2_pure_pursuit_controller/include/nav2_pure_pursuit_controller/pure_pursuit_controller.hpp rename to nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp index 539eedf..3fa6c80 100644 --- a/nav2_pure_pursuit_controller/include/nav2_pure_pursuit_controller/pure_pursuit_controller.hpp +++ b/nav2_regulated_pure_pursuit_controller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp @@ -13,8 +13,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_ -#define NAV2_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_ +#ifndef NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_ +#define NAV2_REGULATED_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_ #include #include @@ -28,15 +28,15 @@ #include "nav2_util/odometry_utils.hpp" #include "geometry_msgs/msg/pose2_d.hpp" -namespace nav2_pure_pursuit_controller +namespace nav2_regulated_pure_pursuit_controller { -class PurePursuitController : public nav2_core::Controller +class RegulatedPurePursuitController : public nav2_core::Controller { public: - PurePursuitController() = default; + RegulatedPurePursuitController() = default; - ~PurePursuitController() override = default; + ~RegulatedPurePursuitController() override = default; void configure( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, @@ -96,7 +96,7 @@ class PurePursuitController : public nav2_core::Controller std::string plugin_name_; std::shared_ptr costmap_ros_; nav2_costmap_2d::Costmap2D * costmap_; - rclcpp::Logger logger_ {rclcpp::get_logger("PurePursuitController")}; + rclcpp::Logger logger_ {rclcpp::get_logger("RegulatedPurePursuitController")}; double desired_linear_vel_; double lookahead_dist_; @@ -126,6 +126,6 @@ class PurePursuitController : public nav2_core::Controller std::shared_ptr> carrot_arc_pub_; }; -} // namespace nav2_pure_pursuit_controller +} // namespace nav2_regulated_pure_pursuit_controller -#endif // NAV2_PURE_PURSUIT_CONTROLLER__PURE_PURSUIT_CONTROLLER_HPP_ +#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 87% rename from nav2_pure_pursuit_controller/package.xml rename to nav2_regulated_pure_pursuit_controller/package.xml index 09c68be..2e47a8e 100644 --- a/nav2_pure_pursuit_controller/package.xml +++ b/nav2_regulated_pure_pursuit_controller/package.xml @@ -1,9 +1,9 @@ - nav2_pure_pursuit_controller + nav2_regulated_pure_pursuit_controller 1.0.0 - Pure pursuit controller + Regulated Pure Pursuit Controller Steve Macenski Shrijit Singh Apache-2.0 diff --git a/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp similarity index 92% rename from nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp rename to nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp index 80205a5..e9017bf 100644 --- a/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -18,7 +18,7 @@ #include #include -#include "nav2_pure_pursuit_controller/pure_pursuit_controller.hpp" +#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" @@ -31,7 +31,7 @@ using nav2_util::declare_parameter_if_not_declared; using nav2_util::geometry_utils::euclidean_distance; using namespace nav2_costmap_2d; // NOLINT -namespace nav2_pure_pursuit_controller +namespace nav2_regulated_pure_pursuit_controller { /** @@ -55,7 +55,7 @@ Iter min_by(Iter begin, Iter end, Getter getCompareVal) return lowest_it; } -void PurePursuitController::configure( +void RegulatedPurePursuitController::configure( const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, std::string name, const std::shared_ptr & tf, const std::shared_ptr & costmap_ros) @@ -147,40 +147,43 @@ void PurePursuitController::configure( carrot_arc_pub_ = node->create_publisher("lookahead_collision_arc", 1); } -void PurePursuitController::cleanup() +void RegulatedPurePursuitController::cleanup() { RCLCPP_INFO( logger_, - "Cleaning up controller: %s of type pure_pursuit_controller::PurePursuitController", + "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 PurePursuitController::activate() +void RegulatedPurePursuitController::activate() { RCLCPP_INFO( logger_, - "Activating controller: %s of type pure_pursuit_controller::PurePursuitController\" %s", + "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 PurePursuitController::deactivate() +void RegulatedPurePursuitController::deactivate() { RCLCPP_INFO( logger_, - "Deactivating controller: %s of type pure_pursuit_controller::PurePursuitController\" %s", + "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 PurePursuitController::getLookAheadDistance(const geometry_msgs::msg::Twist & speed) +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 @@ -195,7 +198,7 @@ double PurePursuitController::getLookAheadDistance(const geometry_msgs::msg::Twi return lookahead_dist; } -std::unique_ptr PurePursuitController::createCarrotMsg( +std::unique_ptr RegulatedPurePursuitController::createCarrotMsg( const geometry_msgs::msg::PoseStamped & carrot_pose) { auto carrot_msg = std::make_unique(); @@ -206,7 +209,7 @@ std::unique_ptr PurePursuitController::createC return carrot_msg; } -geometry_msgs::msg::TwistStamped PurePursuitController::computeVelocityCommands( +geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocityCommands( const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & speed) { @@ -250,7 +253,7 @@ geometry_msgs::msg::TwistStamped PurePursuitController::computeVelocityCommands( // 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("PurePursuitController detected collision ahead!"); + throw std::runtime_error("RegulatedPurePursuitController detected collision ahead!"); } // populate and return message @@ -261,7 +264,7 @@ geometry_msgs::msg::TwistStamped PurePursuitController::computeVelocityCommands( return cmd_vel; } -bool PurePursuitController::shouldRotateToPath( +bool RegulatedPurePursuitController::shouldRotateToPath( const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path) { // Whether we should rotate robot to rough path heading @@ -273,7 +276,7 @@ bool PurePursuitController::shouldRotateToPath( return false; } -void PurePursuitController::rotateToHeading( +void RegulatedPurePursuitController::rotateToHeading( double & linear_vel, double & angular_vel, const double & angle_to_path, const geometry_msgs::msg::Twist & curr_speed) { @@ -288,7 +291,7 @@ void PurePursuitController::rotateToHeading( angular_vel = std::clamp(angular_vel, min_feasible_angular_speed, max_feasible_angular_speed); } -geometry_msgs::msg::PoseStamped PurePursuitController::getLookAheadMarker( +geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadMarker( const double & lookahead_dist, const nav_msgs::msg::Path & transformed_plan) { @@ -306,7 +309,7 @@ geometry_msgs::msg::PoseStamped PurePursuitController::getLookAheadMarker( return *goal_pose_it; } -bool PurePursuitController::isCollisionImminent( +bool RegulatedPurePursuitController::isCollisionImminent( const geometry_msgs::msg::PoseStamped & robot_pose, const geometry_msgs::msg::PoseStamped & carrot_pose, const double & curvature, const double & linear_vel, @@ -384,7 +387,7 @@ bool PurePursuitController::isCollisionImminent( return false; } -bool PurePursuitController::inCollision(const double & x, const double & y) +bool RegulatedPurePursuitController::inCollision(const double & x, const double & y) { unsigned int mx, my; costmap_->worldToMap(x, y, mx, my); @@ -398,7 +401,7 @@ bool PurePursuitController::inCollision(const double & x, const double & y) } } -double PurePursuitController::costAtPose(const double & x, const double & y) +double RegulatedPurePursuitController::costAtPose(const double & x, const double & y) { unsigned int mx, my; costmap_->worldToMap(x, y, mx, my); @@ -407,7 +410,7 @@ double PurePursuitController::costAtPose(const double & x, const double & y) return static_cast(cost); } -void PurePursuitController::applyConstraints( +void RegulatedPurePursuitController::applyConstraints( double & linear_vel, const double & dist_error, const double & lookahead_dist, const double & curvature, const geometry_msgs::msg::Twist & curr_speed, @@ -472,13 +475,12 @@ void PurePursuitController::applyConstraints( linear_vel = std::clamp(linear_vel, 0.0, desired_linear_vel_); } -void PurePursuitController::setPlan(const nav_msgs::msg::Path & path) +void RegulatedPurePursuitController::setPlan(const nav_msgs::msg::Path & path) { global_plan_ = path; } -nav_msgs::msg::Path -PurePursuitController::transformGlobalPlan( +nav_msgs::msg::Path RegulatedPurePursuitController::transformGlobalPlan( const geometry_msgs::msg::PoseStamped & pose) { if (global_plan_.poses.empty()) { @@ -543,7 +545,7 @@ PurePursuitController::transformGlobalPlan( return transformed_plan; } -bool PurePursuitController::transformPose( +bool RegulatedPurePursuitController::transformPose( const std::string frame, const geometry_msgs::msg::PoseStamped & in_pose, geometry_msgs::msg::PoseStamped & out_pose) const @@ -566,4 +568,6 @@ bool PurePursuitController::transformPose( } // namespace nav2_pure_pursuit_controller // Register this controller as a nav2_core plugin -PLUGINLIB_EXPORT_CLASS(nav2_pure_pursuit_controller::PurePursuitController, nav2_core::Controller) +PLUGINLIB_EXPORT_CLASS( + nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController, + nav2_core::Controller) From 8a3be4dd6c64d3226a324745fd7670aa7fe473a1 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 17 Dec 2020 17:22:34 -0800 Subject: [PATCH 29/32] readme --- .../README.md | 26 ++++++++++++------- 1 file changed, 17 insertions(+), 9 deletions(-) diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md index 01c78b6..42f8a7d 100644 --- a/nav2_regulated_pure_pursuit_controller/README.md +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -24,15 +24,23 @@ Finally, the lookahead point will be given to the pure pursuit algorithm which f ## 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). - -TODO clean up this bullet list -This implementation has a number of non-standard features. -- Collision detection in the computed velocity arc between the robot in the look ahead point to ensure safe path following. Uses a maximum collision time parameter to inform the amount of time to forward simulate for collisions. Set to a very large number to always forward simulate to the carrot location. Lookahead time scales the collision checking distance by the velocity so that it checks a consistent time `t` 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. -- Optional dynamic scaling of the look ahead point distance proportional to velocity. This helps you have a more stable robot path tracking over a broader range of velocity inputs if your robot has a large band of operating velocities. There are parameters for the minimum and maximum distances as well. -- Optional slow on approach to the goal. The default algorithm tracks a path at a given linear velocity. This feature allows you to slow the robot on its approach to a goal and also set the minimum percentage (from 0.0-1.0) on approach so it doesn't approach 0% before attaining the goal. -- Kinematic speed limiting on linear velocities to make sure the resulting trajectories are kinematically feasible on getting up to speed and slowing to a stop. -- Optionally scaling linear velocity by its proximity to obstacles and curvature. 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. The major benefit of scaling by curvature is to create intuitive behavior of slowing the robot when making sharp turns and also de-sensitising the look ahead distances which would otherwise be more sensitive to overshoot in some such cases (e.g. average users require little tuning to get reasonable behavior). A secondary benefit of scaling by curvature is to all the robot to natively rotate to rough path heading when using holonomic planners (e.g. don't align starting pose orientation with current robot orientation or in preempted commands changing path directions) without explicit 'rotate to heading' commands. Pure Pursuit controllers otherwise would be unable to recover from this in confined spaces. Mixed with the time-scaled collision checker, this makes a near-perfect combination to have the regulated pure pursuit algorithm handle high starting deviations from the path and navigate collision-free in tight spaces without overshoot. +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. TODO gif of it getting out of a constrained / slowing on high curavture features From 605941e0b3f4679b16f685246ebb476417b69d77 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 17 Dec 2020 17:45:12 -0800 Subject: [PATCH 30/32] add gif --- nav2_regulated_pure_pursuit_controller/README.md | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md index 42f8a7d..e7c9d40 100644 --- a/nav2_regulated_pure_pursuit_controller/README.md +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -10,7 +10,7 @@ It builds on top of the ordinary pure pursuit algorithm in a number of ways. It This controller has been measured to run at well over 1 kHz on a modern intel processor. -TODO gif working +![test](https://user-images.githubusercontent.com/14944147/102563918-3cd49d80-408f-11eb-8e03-b472815a05d8.gif) ## Pure Pursuit Basics @@ -42,8 +42,6 @@ An unintended tertiary benefit of scaling the linear velocities by curvature is 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. -TODO gif of it getting out of a constrained / slowing on high curavture features - ## Configuration | Parameter | Description | From 0260329a674638143a64fa5640e8c8a1050d8c33 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Thu, 17 Dec 2020 17:48:38 -0800 Subject: [PATCH 31/32] attempt to center image --- nav2_regulated_pure_pursuit_controller/README.md | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/nav2_regulated_pure_pursuit_controller/README.md b/nav2_regulated_pure_pursuit_controller/README.md index e7c9d40..9ca3dc8 100644 --- a/nav2_regulated_pure_pursuit_controller/README.md +++ b/nav2_regulated_pure_pursuit_controller/README.md @@ -10,7 +10,9 @@ It builds on top of the ordinary pure pursuit algorithm in a number of ways. It This controller has been measured to run at well over 1 kHz on a modern intel processor. -![test](https://user-images.githubusercontent.com/14944147/102563918-3cd49d80-408f-11eb-8e03-b472815a05d8.gif) +

+ +

## Pure Pursuit Basics From 0e7abfbbc55f1cb3f0a22c2835d749f70365318a Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Mon, 4 Jan 2021 16:05:52 -0800 Subject: [PATCH 32/32] resolve review comments --- .../regulated_pure_pursuit_controller.hpp | 2 +- .../src/regulated_pure_pursuit_controller.cpp | 13 ++++--------- 2 files changed, 5 insertions(+), 10 deletions(-) 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 index 3fa6c80..7d04272 100644 --- 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 @@ -90,7 +90,7 @@ class RegulatedPurePursuitController : public nav2_core::Controller const double & curvature, const geometry_msgs::msg::Twist & speed, const double & pose_cost); - geometry_msgs::msg::PoseStamped getLookAheadMarker(const double &, const nav_msgs::msg::Path &); + geometry_msgs::msg::PoseStamped getLookAheadPoint(const double &, const nav_msgs::msg::Path &); std::shared_ptr tf_; std::string plugin_name_; 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 index e9017bf..20f246a 100644 --- a/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp +++ b/nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp @@ -16,7 +16,6 @@ #include #include #include -#include #include "nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp" #include "nav2_core/exceptions.hpp" @@ -218,7 +217,7 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity // Find look ahead distance and point on path and publish const double lookahead_dist = getLookAheadDistance(speed); - auto carrot_pose = getLookAheadMarker(lookahead_dist, transformed_plan); + auto carrot_pose = getLookAheadPoint(lookahead_dist, transformed_plan); carrot_pub_->publish(std::move(createCarrotMsg(carrot_pose))); double linear_vel, angular_vel; @@ -269,11 +268,7 @@ bool RegulatedPurePursuitController::shouldRotateToPath( { // Whether we should rotate robot to rough path heading angle_to_path = atan2(carrot_pose.pose.position.y, carrot_pose.pose.position.x); - if (use_rotate_to_heading_ && fabs(angle_to_path) > rotate_to_heading_min_angle_) { - return true; - } - - return false; + return use_rotate_to_heading_ && fabs(angle_to_path) > rotate_to_heading_min_angle_; } void RegulatedPurePursuitController::rotateToHeading( @@ -291,7 +286,7 @@ void RegulatedPurePursuitController::rotateToHeading( angular_vel = std::clamp(angular_vel, min_feasible_angular_speed, max_feasible_angular_speed); } -geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadMarker( +geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoint( const double & lookahead_dist, const nav_msgs::msg::Path & transformed_plan) { @@ -452,7 +447,7 @@ void RegulatedPurePursuitController::applyConstraints( if (unbounded_vel < min_approach_linear_velocity_) { linear_vel = min_approach_linear_velocity_; } else { - linear_vel = linear_vel * velocity_scaling; + linear_vel *= velocity_scaling; } }