Skip to content

Commit

Permalink
Remove formatting changes from conflict with upstream rebase
Browse files Browse the repository at this point in the history 
Signed-off-by: James Ward <[email protected]>
  • Loading branch information
@james-ward
james-ward committed Feb 21, 2024
1 parent 15d4446 commit 4296c84
Showing 1 changed file with 63 additions and 93 deletions.
156 changes: 63 additions & 93 deletions nav2_regulated_pure_pursuit_controller/src/regulated_pure_pursuit_controller.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,30 +14,29 @@
// limitations under the License.

#include <algorithm>
#include <string>
#include <limits>
#include <memory>
#include <string>
#include <utility>
#include <vector>
#include <utility>

#include "nav2_core/controller_exceptions.hpp"
#include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
#include "nav2_regulated_pure_pursuit_controller/regulated_pure_pursuit_controller.hpp"
#include "nav2_util/geometry_utils.hpp"
#include "nav2_core/controller_exceptions.hpp"
#include "nav2_util/node_utils.hpp"
#include "nav2_util/geometry_utils.hpp"
#include "nav2_costmap_2d/costmap_filters/filter_values.hpp"

using std::abs;
using std::hypot;
using std::max;
using std::min;
using namespace nav2_costmap_2d; // NOLINT
using std::abs;
using namespace nav2_costmap_2d; // NOLINT

namespace nav2_regulated_pure_pursuit_controller
{

void RegulatedPurePursuitController::configure(
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent, std::string name,
std::shared_ptr<tf2_ros::Buffer> tf,
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
{
auto node = parent.lock();
Expand All @@ -63,19 +62,16 @@ void RegulatedPurePursuitController::configure(
tf2::durationFromSec(params_->transform_tolerance), tf_, costmap_ros_);

// Checks for imminent collisions
collision_checker_ =
std::make_unique<CollisionChecker>(node, costmap_ros_, params_);
collision_checker_ = std::make_unique<CollisionChecker>(node, costmap_ros_, params_);

double control_frequency = 20.0;
goal_dist_tol_ = 0.25; // reasonable default before first update

node->get_parameter("controller_frequency", control_frequency);
control_duration_ = 1.0 / control_frequency;

global_path_pub_ =
node->create_publisher<nav_msgs::msg::Path>("received_global_plan", 1);
carrot_pub_ = node->create_publisher<geometry_msgs::msg::PointStamped>(
"lookahead_point", 1);
global_path_pub_ = node->create_publisher<nav_msgs::msg::Path>("received_global_plan", 1);
carrot_pub_ = node->create_publisher<geometry_msgs::msg::PointStamped>("lookahead_point", 1);
}

void RegulatedPurePursuitController::cleanup()
Expand Down Expand Up @@ -111,8 +107,7 @@ void RegulatedPurePursuitController::deactivate()
carrot_pub_->on_deactivate();
}

std::unique_ptr<geometry_msgs::msg::PointStamped>
RegulatedPurePursuitController::createCarrotMsg(
std::unique_ptr<geometry_msgs::msg::PointStamped> RegulatedPurePursuitController::createCarrotMsg(
const geometry_msgs::msg::PoseStamped & carrot_pose)
{
auto carrot_msg = std::make_unique<geometry_msgs::msg::PointStamped>();
Expand All @@ -132,8 +127,7 @@ double RegulatedPurePursuitController::getLookAheadDistance(
if (params_->use_velocity_scaled_lookahead_dist) {
lookahead_dist = fabs(speed.linear.x) * params_->lookahead_time;
lookahead_dist = std::clamp(
lookahead_dist, params_->min_lookahead_dist,
params_->max_lookahead_dist);
lookahead_dist, params_->min_lookahead_dist, params_->max_lookahead_dist);
}

return lookahead_dist;
Expand All @@ -143,7 +137,8 @@ double calculateCurvature(geometry_msgs::msg::Point lookahead_point)
{
// Find distance^2 to look ahead point (carrot) in robot base frame
// This is the chord length of the circle
const double carrot_dist2 = (lookahead_point.x * lookahead_point.x) +
const double carrot_dist2 =
(lookahead_point.x * lookahead_point.x) +
(lookahead_point.y * lookahead_point.y);

// Find curvature of circle (k = 1 / R)
Expand All @@ -154,17 +149,15 @@ double calculateCurvature(geometry_msgs::msg::Point lookahead_point)
}
}

geometry_msgs::msg::TwistStamped
RegulatedPurePursuitController::computeVelocityCommands(
geometry_msgs::msg::TwistStamped RegulatedPurePursuitController::computeVelocityCommands(
const geometry_msgs::msg::PoseStamped & pose,
const geometry_msgs::msg::Twist & speed,
nav2_core::GoalChecker * goal_checker)
{
std::lock_guard<std::mutex> lock_reinit(param_handler_->getMutex());

nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap();
std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(
*(costmap->getMutex()));
std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> lock(*(costmap->getMutex()));

// Update for the current goal checker's state
geometry_msgs::msg::Pose pose_tolerance;
Expand All @@ -188,8 +181,7 @@ RegulatedPurePursuitController::computeVelocityCommands(
// Cusp check
const double dist_to_cusp = findVelocitySignChange(transformed_plan);

// if the lookahead distance is further than the cusp, use the cusp distance
// instead
// 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;
}
Expand All @@ -205,10 +197,10 @@ RegulatedPurePursuitController::computeVelocityCommands(

double regulation_curvature = lookahead_curvature;
if (params_->use_fixed_curvature_lookahead) {
auto curvature_lookahead_pose =
getLookAheadPoint(params_->curvature_lookahead_dist, transformed_plan);
regulation_curvature =
calculateCurvature(curvature_lookahead_pose.pose.position);
auto curvature_lookahead_pose = getLookAheadPoint(
params_->curvature_lookahead_dist,
transformed_plan);
regulation_curvature = calculateCurvature(curvature_lookahead_pose.pose.position);
}

// Setting the velocity direction
Expand All @@ -222,33 +214,26 @@ RegulatedPurePursuitController::computeVelocityCommands(
// Make sure we're in compliance with basic constraints
double angle_to_heading;
if (shouldRotateToGoalHeading(carrot_pose)) {
double angle_to_goal =
tf2::getYaw(transformed_plan.poses.back().pose.orientation);
double angle_to_goal = tf2::getYaw(transformed_plan.poses.back().pose.orientation);
rotateToHeading(linear_vel, angular_vel, angle_to_goal, speed);
} else if (shouldRotateToPath(carrot_pose, angle_to_heading)) {
rotateToHeading(linear_vel, angular_vel, angle_to_heading, speed);
} else {
applyConstraints(
regulation_curvature, speed,
collision_checker_->costAtPose(
pose.pose.position.x,
pose.pose.position.y),
transformed_plan, linear_vel, sign);
collision_checker_->costAtPose(pose.pose.position.x, pose.pose.position.y), transformed_plan,
linear_vel, sign);

// Apply curvature to angular velocity after constraining linear velocity
angular_vel = linear_vel * lookahead_curvature;
}

// Collision checking on this velocity heading
const double & carrot_dist =
hypot(carrot_pose.pose.position.x, carrot_pose.pose.position.y);
const double & carrot_dist = hypot(carrot_pose.pose.position.x, carrot_pose.pose.position.y);
if (params_->use_collision_detection &&
collision_checker_->isCollisionImminent(
pose, linear_vel, angular_vel,
carrot_dist))
collision_checker_->isCollisionImminent(pose, linear_vel, angular_vel, carrot_dist))
{
throw nav2_core::NoValidControl(
"RegulatedPurePursuitController detected collision ahead!");
throw nav2_core::NoValidControl("RegulatedPurePursuitController detected collision ahead!");
}

// populate and return message
Expand All @@ -263,8 +248,7 @@ bool RegulatedPurePursuitController::shouldRotateToPath(
const geometry_msgs::msg::PoseStamped & carrot_pose, double & angle_to_path)
{
// Whether we should rotate robot to rough path heading
angle_to_path =
atan2(carrot_pose.pose.position.y, carrot_pose.pose.position.x);
angle_to_path = atan2(carrot_pose.pose.position.y, carrot_pose.pose.position.x);
return params_->use_rotate_to_heading &&
fabs(angle_to_path) > params_->rotate_to_heading_min_angle;
}
Expand All @@ -273,43 +257,38 @@ bool RegulatedPurePursuitController::shouldRotateToGoalHeading(
const geometry_msgs::msg::PoseStamped & carrot_pose)
{
// Whether we should rotate robot to goal heading
double dist_to_goal =
std::hypot(carrot_pose.pose.position.x, carrot_pose.pose.position.y);
double dist_to_goal = std::hypot(carrot_pose.pose.position.x, carrot_pose.pose.position.y);
return params_->use_rotate_to_heading && dist_to_goal < goal_dist_tol_;
}

void RegulatedPurePursuitController::rotateToHeading(
double & linear_vel, double & angular_vel, const double & angle_to_path,
const geometry_msgs::msg::Twist & curr_speed)
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 * params_->rotate_to_heading_angular_vel;

const double & dt = control_duration_;
const double min_feasible_angular_speed =
curr_speed.angular.z - params_->max_angular_accel * dt;
const double max_feasible_angular_speed =
curr_speed.angular.z + params_->max_angular_accel * dt;
angular_vel = std::clamp(
angular_vel, min_feasible_angular_speed,
max_feasible_angular_speed);
const double min_feasible_angular_speed = curr_speed.angular.z - params_->max_angular_accel * dt;
const double max_feasible_angular_speed = curr_speed.angular.z + params_->max_angular_accel * dt;
angular_vel = std::clamp(angular_vel, min_feasible_angular_speed, max_feasible_angular_speed);
}

geometry_msgs::msg::Point
RegulatedPurePursuitController::circleSegmentIntersection(
const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2,
geometry_msgs::msg::Point RegulatedPurePursuitController::circleSegmentIntersection(
const geometry_msgs::msg::Point & p1,
const geometry_msgs::msg::Point & p2,
double r)
{
// Formula for intersection of a line with a circle centered at the origin,
// modified to always return the point that is on the segment between the two
// points. https://mathworld.wolfram.com/Circle-LineIntersection.html This
// works because the poses are transformed into the robot frame. This can be
// derived from solving the system of equations of a line and a circle which
// results in something that is just a reformulation of the quadratic formula.
// Interactive illustration in doc/circle-segment-intersection.ipynb as well
// as at https://www.desmos.com/calculator/td5cwbuocd
// modified to always return the point that is on the segment between the two points.
// https://mathworld.wolfram.com/Circle-LineIntersection.html
// This works because the poses are transformed into the robot frame.
// This can be derived from solving the system of equations of a line and a circle
// which results in something that is just a reformulation of the quadratic formula.
// Interactive illustration in doc/circle-segment-intersection.ipynb as well as at
// https://www.desmos.com/calculator/td5cwbuocd
double x1 = p1.x;
double x2 = p2.x;
double y1 = p1.y;
Expand All @@ -332,15 +311,13 @@ RegulatedPurePursuitController::circleSegmentIntersection(
return p;
}

geometry_msgs::msg::PoseStamped
RegulatedPurePursuitController::getLookAheadPoint(
const double & lookahead_dist, const nav_msgs::msg::Path & transformed_plan)
geometry_msgs::msg::PoseStamped RegulatedPurePursuitController::getLookAheadPoint(
const double & lookahead_dist,
const nav_msgs::msg::Path & transformed_plan)
{
// Find the first pose which is at a distance greater than the lookahead
// distance
// 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) {
transformed_plan.poses.begin(), transformed_plan.poses.end(), [&](const auto & ps) {
return hypot(ps.pose.position.x, ps.pose.position.y) >= lookahead_dist;
});

Expand All @@ -353,14 +330,12 @@ RegulatedPurePursuitController::getLookAheadPoint(
}
} else if (goal_pose_it != transformed_plan.poses.begin()) {
// Find the point on the line segment between the two poses
// that is exactly the lookahead distance away from the robot pose (the
// origin) This can be found with a closed form for the intersection of a
// segment and a circle Because of the way we did the std::find_if,
// prev_pose is guaranteed to be inside the circle, and goal_pose is
// guaranteed to be outside the circle.
// that is exactly the lookahead distance away from the robot pose (the origin)
// This can be found with a closed form for the intersection of a segment and a circle
// Because of the way we did the std::find_if, prev_pose is guaranteed to be inside the circle,
// and goal_pose is guaranteed to be outside the circle.
auto prev_pose_it = std::prev(goal_pose_it);
auto point =
circleSegmentIntersection(
auto point = circleSegmentIntersection(
prev_pose_it->pose.position,
goal_pose_it->pose.position, lookahead_dist);
geometry_msgs::msg::PoseStamped pose;
Expand Down Expand Up @@ -412,8 +387,7 @@ RegulatedPurePursuitController::projectCarrotPastGoal(

void RegulatedPurePursuitController::applyConstraints(
const double & curvature, const geometry_msgs::msg::Twist & /*curr_speed*/,
const double & pose_cost, const nav_msgs::msg::Path & path,
double & linear_vel, double & sign)
const double & pose_cost, const nav_msgs::msg::Path & path, double & linear_vel, double & sign)
{
double curvature_vel = linear_vel, cost_vel = linear_vel;

Expand All @@ -425,16 +399,12 @@ void RegulatedPurePursuitController::applyConstraints(

// limit the linear velocity by proximity to obstacles
if (params_->use_cost_regulated_linear_velocity_scaling) {
cost_vel = heuristics::costConstraint(
linear_vel, pose_cost, costmap_ros_,
params_);
cost_vel = heuristics::costConstraint(linear_vel, pose_cost, costmap_ros_, params_);
}

// Use the lowest of the 2 constraints, but above the minimum translational
// speed
// Use the lowest of the 2 constraints, but above the minimum translational speed
linear_vel = std::min(cost_vel, curvature_vel);
linear_vel =
std::max(linear_vel, params_->regulated_linear_scaling_min_speed);
linear_vel = std::max(linear_vel, params_->regulated_linear_scaling_min_speed);

// Apply constraint to reduce speed on approach to the final goal pose
linear_vel = heuristics::approachVelocityConstraint(
Expand Down Expand Up @@ -519,8 +489,7 @@ void RegulatedPurePursuitController::setSpeedLimit(
} else {
if (percentage) {
// Speed limit is expressed in % from maximum speed of robot
params_->desired_linear_vel =
params_->base_desired_linear_vel * speed_limit / 100.0;
params_->desired_linear_vel = params_->base_desired_linear_vel * speed_limit / 100.0;
} else {
// Speed limit is expressed in absolute value
params_->desired_linear_vel = speed_limit;
Expand Down Expand Up @@ -558,7 +527,8 @@ unsigned int RegulatedPurePursuitController::getIndexOfNextCusp(
}
if ((hypot(oa_x, oa_y) == 0.0 &&
transformed_plan.poses[pose_id - 1].pose.orientation !=
transformed_plan.poses[pose_id].pose.orientation) ||
transformed_plan.poses[pose_id].pose.orientation)
||
(hypot(ab_x, ab_y) == 0.0 &&
transformed_plan.poses[pose_id].pose.orientation !=
transformed_plan.poses[pose_id + 1].pose.orientation))
Expand Down

0 comments on commit 4296c84

Please sign in to comment.