Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Better management of RPP path pruning when cusp points are detected #4763

Open
wants to merge 1 commit into
base: humble
Choose a base branch
from
Open

Better management of RPP path pruning when cusp points are detected #4763

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
113 changes: 108 additions & 5 deletions ...ller/include/nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,7 @@

namespace nav2_regulated_pure_pursuit_controller
{
using PathIterator = std::vector<geometry_msgs::msg::PoseStamped>::iterator;

/**
* @class nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController
Expand Down Expand Up @@ -244,20 +245,118 @@ class RegulatedPurePursuitController : public nav2_core::Controller
const geometry_msgs::msg::Point & p2,
double r);

/**
* @brief normalize
* Normalizes the angle to be -M_PI circle to +M_PI circle
* It takes and returns radians.
* @param angles Angles to normalize
* @return normalized angles
*/
template<typename T>
auto normalize_angles(const T & angles)
{
auto && theta = fmod(angles + M_PI, 2.0 * M_PI);
if (theta < 0)
theta += 360;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Isn't this in radians?

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

yes radians, I translated a function from degree to radians but it looks like I forgot to update theta += 360 to theta += 2.0 * M_PI

return theta - M_PI;
}


/**
* @brief shortest_angular_distance
*
* Given 2 angles, this returns the shortest angular
* difference. The inputs and ouputs are of course radians.
*
* The result
* would always be -pi <= result <= pi. Adding the result
* to "from" will always get you an equivelent angle to "to".
* @param from Start angle
* @param to End angle
* @return Shortest distance between angles
*/
template<typename F, typename T>
auto shortest_angular_distance(
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I believe both of these functions can be used from angles rather than reimplemented https://github.com/ros/angles

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I suppose so. I follow MPPI controller utils.hpp implementation in humble. I see Nav2 Rolling switched to angles dependency

const F & from,
const T & to)
{
return normalize_angles(to - from);
}

/**
* @brief Get lookahead point
* @param lookahead_dist Optimal lookahead distance
* @param path Current global path
* @return Lookahead point
*/
geometry_msgs::msg::PoseStamped getLookAheadPoint(const double &, const nav_msgs::msg::Path &);


/**
* @brief Find the iterator of the first pose at which there is an inversion on the path,
* @param path to check for inversion
* @return the first point after the inversion found in the path
*/
inline unsigned int findFirstPathInversion(nav_msgs::msg::Path & path)
{
// At least 3 poses for a possible inversion
if (path.poses.size() < 3) {
return path.poses.size();
}

// Iterating through the path to determine the position of the path inversion
for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) {
// We have two vectors for the dot product OA and AB. Determining the vectors.
float oa_x = path.poses[idx].pose.position.x -
path.poses[idx - 1].pose.position.x;
float oa_y = path.poses[idx].pose.position.y -
path.poses[idx - 1].pose.position.y;
float ab_x = path.poses[idx + 1].pose.position.x -
path.poses[idx].pose.position.x;
float ab_y = path.poses[idx + 1].pose.position.y -
path.poses[idx].pose.position.y;

// Checking for the existance of cusp, in the path, using the dot product.
float dot_product = (oa_x * ab_x) + (oa_y * ab_y);
if (dot_product < 0.0) {
return idx + 1;
}
}

return path.poses.size();
}

/**
* @brief checks for the cusp position
* @param pose Pose input to determine the cusp position
* @return robot distance from the cusp
* @brief Find and remove poses after the first inversion in the path
* @param path to check for inversion
* @return The location of the inversion, return 0 if none exist
*/
double findVelocitySignChange(const nav_msgs::msg::Path & transformed_plan);
inline unsigned int removePosesAfterFirstInversion(nav_msgs::msg::Path & path)
{
nav_msgs::msg::Path cropped_path = path;
const unsigned int first_after_inversion = findFirstPathInversion(cropped_path);
if (first_after_inversion == path.poses.size()) {
return 0u;
}

cropped_path.poses.erase(
cropped_path.poses.begin() + first_after_inversion, cropped_path.poses.end());
path = cropped_path;
return first_after_inversion;
}

/**
* @brief Prune a path to only interesting portions
* @param plan Plan to prune
* @param end Final path iterator
*/
void prunePlan(nav_msgs::msg::Path & plan, const PathIterator end);

/**
* @brief Check if the robot pose is within the set inversion tolerances
* @param robot_pose Robot's current pose to check
* @return bool If the robot pose is within the set inversion tolerances
*/
bool isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose);

/**
* Get the greatest extent of the costmap in meters from the center.
Expand Down Expand Up @@ -307,8 +406,12 @@ class RegulatedPurePursuitController : public nav2_core::Controller
bool allow_reversing_;
double max_robot_pose_search_dist_;
bool use_interpolation_;
float inversion_xy_tolerance_{0.2};
float inversion_yaw_tolerance_{0.4};
unsigned int inversion_locale_{0u};

nav_msgs::msg::Path global_plan_;
nav_msgs::msg::Path global_plan_up_to_inversion_;
std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> global_path_pub_;
std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PointStamped>>
carrot_pub_;
Expand Down
118 changes: 63 additions & 55 deletions nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@
#include <memory>
#include <vector>
#include <utility>
#include <tuple>

#include "nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp"
#include "nav2_core/exceptions.hpp"
Expand Down Expand Up @@ -111,6 +112,10 @@ void RegulatedPurePursuitController::configure(
node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(3.2));
declare_parameter_if_not_declared(
node, plugin_name_ + ".allow_reversing", rclcpp::ParameterValue(false));
declare_parameter_if_not_declared(
node, plugin_name_ + ".inversion_xy_tolerance", rclcpp::ParameterValue(0.2));
declare_parameter_if_not_declared(
node, plugin_name_ + ".inversion_yaw_tolerance", rclcpp::ParameterValue(0.4));
declare_parameter_if_not_declared(
node, plugin_name_ + ".max_robot_pose_search_dist",
rclcpp::ParameterValue(getCostmapMaxExtent()));
Expand Down Expand Up @@ -169,6 +174,9 @@ void RegulatedPurePursuitController::configure(
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(plugin_name_ + ".allow_reversing", allow_reversing_);
node->get_parameter(plugin_name_ + "inversion_xy_tolerance", inversion_xy_tolerance_);
node->get_parameter(plugin_name_ + "inversion_yaw_tolerance", inversion_yaw_tolerance_);
inversion_locale_ = 0u;
node->get_parameter("controller_frequency", control_frequency);
node->get_parameter(
plugin_name_ + ".max_robot_pose_search_dist",
Expand Down Expand Up @@ -293,25 +301,16 @@ geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocity
} else {
goal_dist_tol_ = pose_tolerance.position.x;
}

// Transform path to robot base frame
auto transformed_plan = transformGlobalPlan(pose);

// Find look ahead distance and point on path and publish
double lookahead_dist = getLookAheadDistance(speed);

// Check for reverse driving
if (allow_reversing_) {
// Cusp check
double dist_to_cusp = findVelocitySignChange(transformed_plan);

// if the lookahead distance is further than the cusp, use the cusp distance instead
if (dist_to_cusp < lookahead_dist) {
lookahead_dist = dist_to_cusp;
}
}

auto carrot_pose = getLookAheadPoint(lookahead_dist, transformed_plan);

geometry_msgs::msg::PoseStamped carrot_pose;
carrot_pose = getLookAheadPoint(lookahead_dist, transformed_plan);

carrot_pub_->publish(createCarrotMsg(carrot_pose));

double linear_vel, angular_vel;
Expand Down Expand Up @@ -669,6 +668,10 @@ void RegulatedPurePursuitController::applyConstraints(
void RegulatedPurePursuitController::setPlan(const nav_msgs::msg::Path & path)
{
global_plan_ = path;
global_plan_up_to_inversion_ = global_plan_;
if (allow_reversing_) {
inversion_locale_ = removePosesAfterFirstInversion(global_plan_up_to_inversion_);
}
}

void RegulatedPurePursuitController::setSpeedLimit(
Expand All @@ -692,40 +695,42 @@ void RegulatedPurePursuitController::setSpeedLimit(
nav_msgs::msg::Path RegulatedPurePursuitController::transformGlobalPlan(
const geometry_msgs::msg::PoseStamped & pose)
{
if (global_plan_.poses.empty()) {
if (global_plan_up_to_inversion_.poses.empty()) {
throw nav2_core::PlannerException("Received plan with zero length");
}

// let's get the pose of the robot in the frame of the plan
geometry_msgs::msg::PoseStamped robot_pose;
if (!transformPose(global_plan_.header.frame_id, pose, robot_pose)) {
if (!transformPose(global_plan_up_to_inversion_.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
double max_costmap_extent = getCostmapMaxExtent();


auto begin = global_plan_up_to_inversion_.poses.begin();

auto closest_pose_upper_bound =
nav2_util::geometry_utils::first_after_integrated_distance(
global_plan_.poses.begin(), global_plan_.poses.end(), max_robot_pose_search_dist_);
global_plan_up_to_inversion_.poses.begin(), global_plan_up_to_inversion_.poses.end(), max_robot_pose_search_dist_);

// First find the closest pose on the path to the robot
// bounded by when the path turns around (if it does) so we don't get a pose from a later
// portion of the path
auto transformation_begin =
nav2_util::geometry_utils::min_by(
global_plan_.poses.begin(), closest_pose_upper_bound,
begin, closest_pose_upper_bound,
[&robot_pose](const geometry_msgs::msg::PoseStamped & ps) {
return euclidean_distance(robot_pose, ps);
});

// Find points up to max_transform_dist so we only transform them.
auto transformation_end = std::find_if(
transformation_begin, global_plan_.poses.end(),
transformation_begin, global_plan_up_to_inversion_.poses.end(),
[&](const auto & pose) {
return euclidean_distance(pose, robot_pose) > max_costmap_extent;
});

// Lambda to transform a PoseStamped from global frame to local
auto transformGlobalPoseToLocal = [&](const auto & global_plan_pose) {
geometry_msgs::msg::PoseStamped stamped_pose, transformed_pose;
Expand All @@ -745,10 +750,18 @@ nav_msgs::msg::Path RegulatedPurePursuitController::transformGlobalPlan(
transformGlobalPoseToLocal);
transformed_plan.header.frame_id = costmap_ros_->getBaseFrameID();
transformed_plan.header.stamp = robot_pose.header.stamp;

// Remove the portion of the global plan that we've already passed so we don't
// process it on the next iteration (this is called path pruning)
global_plan_.poses.erase(begin(global_plan_.poses), transformation_begin);

prunePlan(global_plan_up_to_inversion_, transformation_begin);

if (allow_reversing_ && inversion_locale_ != 0u) {
if (isWithinInversionTolerances(robot_pose)) {
// Remove the portion of the global plan from starting point up to inversion_locale_ (index)
prunePlan(global_plan_, global_plan_.poses.begin() + inversion_locale_);
global_plan_up_to_inversion_ = global_plan_;
inversion_locale_ = removePosesAfterFirstInversion(global_plan_up_to_inversion_);
}
}

global_path_pub_->publish(transformed_plan);

if (transformed_plan.poses.empty()) {
Expand All @@ -758,36 +771,6 @@ nav_msgs::msg::Path RegulatedPurePursuitController::transformGlobalPlan(
return transformed_plan;
}

double RegulatedPurePursuitController::findVelocitySignChange(
const nav_msgs::msg::Path & transformed_plan)
{
// Iterating through the transformed global path to determine the position of the cusp
for (unsigned int pose_id = 1; pose_id < transformed_plan.poses.size() - 1; ++pose_id) {
// We have two vectors for the dot product OA and AB. Determining the vectors.
double oa_x = transformed_plan.poses[pose_id].pose.position.x -
transformed_plan.poses[pose_id - 1].pose.position.x;
double oa_y = transformed_plan.poses[pose_id].pose.position.y -
transformed_plan.poses[pose_id - 1].pose.position.y;
double ab_x = transformed_plan.poses[pose_id + 1].pose.position.x -
transformed_plan.poses[pose_id].pose.position.x;
double ab_y = transformed_plan.poses[pose_id + 1].pose.position.y -
transformed_plan.poses[pose_id].pose.position.y;

/* Checking for the existance of cusp, in the path, using the dot product
and determine it's distance from the robot. If there is no cusp in the path,
then just determine the distance to the goal location. */
if ( (oa_x * ab_x) + (oa_y * ab_y) < 0.0) {
// returning the distance if there is a cusp
// The transformed path is in the robots frame, so robot is at the origin
return hypot(
transformed_plan.poses[pose_id].pose.position.x,
transformed_plan.poses[pose_id].pose.position.y);
}
}

return std::numeric_limits<double>::max();
}

bool RegulatedPurePursuitController::transformPose(
const std::string frame,
const geometry_msgs::msg::PoseStamped & in_pose,
Expand Down Expand Up @@ -868,6 +851,10 @@ RegulatedPurePursuitController::dynamicParametersCallback(
max_angular_accel_ = parameter.as_double();
} else if (name == plugin_name_ + ".rotate_to_heading_min_angle") {
rotate_to_heading_min_angle_ = parameter.as_double();
} else if (name == plugin_name_ + ".inversion_xy_tolerance") {
inversion_xy_tolerance_ = parameter.as_double();
} else if (name == plugin_name_ + ".inversion_yaw_tolerance") {
inversion_yaw_tolerance_ = parameter.as_double();
}
} else if (type == ParameterType::PARAMETER_BOOL) {
if (name == plugin_name_ + ".use_velocity_scaled_lookahead_dist") {
Expand Down Expand Up @@ -899,6 +886,27 @@ RegulatedPurePursuitController::dynamicParametersCallback(
result.successful = true;
return result;
}

bool RegulatedPurePursuitController::isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose)
{
// Keep full path if we are within tolerance of the inversion pose
const auto last_pose = global_plan_up_to_inversion_.poses.back();
float distance = hypotf(
robot_pose.pose.position.x - last_pose.pose.position.x,
robot_pose.pose.position.y - last_pose.pose.position.y);

float angle_distance = shortest_angular_distance(
tf2::getYaw(robot_pose.pose.orientation),
tf2::getYaw(last_pose.pose.orientation));


return distance <= inversion_xy_tolerance_ && fabs(angle_distance) <= inversion_yaw_tolerance_;
}

void RegulatedPurePursuitController::prunePlan(nav_msgs::msg::Path & plan, const PathIterator end)
{
plan.poses.erase(plan.poses.begin(), end);
}

} // namespace nav2_regulated_pure_pursuit_controller

Expand Down
Loading