Pure Pursuit updates

[SteveMacenski]

Added

• Kinematic acceleration limits
• Slow down on approach
• Collision checking
• Dynamically scaling look ahead distance by speed optionally
• breaking out functions for more easy testing
• Simplify transformations
• carrot position publisher
• collision arc publisher
• constraints on linear velocity on path curvature and costmap costs
• rotate to headings

Tested working...

• collision checking
• dynamic scaling of look ahead point
• kinematic limiting
• slow down on approach
• test generally working after all done: follow path well, doesnt veer off, defaults, can get on paths OK
• potentially look into linear velocity scaling based on curvature (e.g. slow down if lots of curves so we can follow well)
• potentially slow linear vel when close to obstacles
• rotate to path headings
• readme with context and params

Future...

• unit test coverage
• rotate to goal at end
• interpolated lookahead distances

[SteveMacenski]

Nice little visualization of the collision checker points, projecting the velocities forward in time til the carrot (carrot also published as a point). Also just a good little visualization tool for the pure pursuit planner to see what's going on.

[shrijitsingh99]

The visualization look awesome! WIll lookup other implementations/literature of pure pursuit, to see how they are handling some of the issues we are looking to resolve.

[Rayman]

Nice work! I might be able to test this tomorrow.

Do we want to keep a simplified version of the pure pursuit controller for tutorial purposes?

[SteveMacenski]

@Rayman @shrijitsingh99 I'm not 100% sure about the velocity scaling based on cost https://github.com/ros-planning/navigation2_tutorials/pull/17/files#diff-897735e06a9135ba25ccaf49336bea674e13ef232a5097224f65868723e5fbe6R402-R415. I think it works (?) but its hard to tell how beneficial it is given the TB3 world has so many obstacles so close together it mostly goes to minimum. The idea if that if you're in a really tight area, you probably want to be going slower. I think its a helpful feature, though open to suggestions or removal.

The linear velocity scaling based on curvature definitely has a positive improvement in performance. Combined with the time-variable collision checking arc, it actually makes for a uniquely good setup. It helps rotate to the path orientation itself without needing the 'rotate to pose' part (we'll still want it but makes it less sensitive since the controller can handle some of it itself) because its a high curvature path, so angular velocity skyrockets and linear velocity drops, so you get an almost rotate to heading behavior. Since the collision checker distance on the arc is proportional to speed, then since this is low, we don't collision check too far head to artificially call for a "collision!" in tight spaces knowing the velocities will drive away from targets. Its not perfect and when highly constrained it doesn't quite get to the path angle, so we'll still need a rotate to heading object. Though it could probably handle itself if not in a constrained space.

They can both be disabled by setting use regulated velocity scaling to false.

[SteveMacenski]

Fun working :-)

[shrijitsingh99]

Suggested change

	// First find the closest pose on the path to the robot
	auto transformation_begin =
	min_by(
	global_plan_.poses.begin(), global_plan_.poses.end(),
	[&robot_pose](const geometry_msgs::msg::PoseStamped & ps) {
	return euclidean_distance(robot_pose, ps);
	});
	// Find points definitely outside of the costmap so we won't transform them.
	auto transformation_end = std::find_if(
	transformation_begin, end(global_plan_.poses),
	[&](const auto & global_plan_pose) {
	return euclidean_distance(robot_pose, global_plan_pose) > max_transform_dist;
	});
	// Compute euclidean distance of each pose of global plan from robot
	std::vector<double> euclidean_dists(global_plan_.poses.size());
	std::transform(global_plan_.poses.begin(), global_plan_.poses.end(), euclidean_dists.begin(),
	[&robot_pose](const geometry_msgs::msg::PoseStamped & ps) {
	return euclidean_distance(robot_pose, ps);
	});
	// First find the closest pose on the path to the robot
	auto min_euclidean_dist = std::min_element(euclidean_dists.begin(), euclidean_dists.end());
	auto transformation_begin = global_plan_.poses.begin() + std::distance(
	euclidean_dists.begin(), min_euclidean_dist);
	// Find points definitely outside of the costmap so we won't transform them.
	auto transformation_end = global_plan_.poses.begin() + std::distance(min_euclidean_dist, std::find_if(
	min_euclidean_dist, euclidean_dists.end(),
	[&max_transform_dist](const auto & euclidean_dist) {
	return euclidean_dist > max_transform_dist;
	}));

Cache the euclidean distances for use while finding transformation_end and transformation_end. Also removes the need for min_by which feels extraneous since it is used only once.

[Rayman]

Nice refactor!

[SteveMacenski]

This looks like its actually more computation than what we have now. Right now its :

• 1 full iteration and returns the iterator to the closest point
• from the closest point to the robot to the first point that is outside of the lookahead distance.

So that's 1 full iteration and a closest -> lookahead distance sub-iteration.

---

This new one is:

• 1 full iteration
• 1 search to find the minimum element (more or less the same implementation as @Rayman's min_by except since it wasn't combined with the euclidean, it's an extra bit of CPU O(N-1).
• Then something to turn min_euclidean_dist -> transformation_begin which I don't understand, https://en.cppreference.com/w/cpp/algorithm/min_element min_element doesn't return the distance, it returns the iterator to the item of the lowest element. So that seems... unnecessary
• a find_if which the other one owns as well already counted in the 1 full iteration and a closest -> lookahead distance sub-iteration.

I think what's there right now is faster, since the second run through of euclidean distances is a pretty short sub-set of the path just from the robot pose to the carrot distance. I would think that would be faster than the min_element search alone.

[shrijitsingh99]

I think for each of the cost-based scaling methods i.e. collision, near goal, and curvature, we should independently compute the scaled linear velocities and then take a min of it in the end.
Applying the scaling factors one after another could lead to undesirable linear velocities.

[SteveMacenski]

We want them all to count against, that's why we have a minimum set velocity so that it doesn't dip below something reasonable with each of these cost functions applied (and kinematic checking shown below to make sure any rapid changes are still feasible). If we're in a high curvature and high cost space, wouldn't you want to be the conservative-ness of both these heuristics? I agree the multiplication of each is a little aggressive. Maybe this is something we can play with before merging this into Nav2 main but for the tutorial stuff here I think this is OK.

Gaming this out:

• On normal paths not near the goal, the only 2 that can be applied are the curvature and cost. Curvature is a multiplier on the base max linear speed and the cost is a negative linear function to reduce it. So there isn't compounding multiplier here.
• On approaching the goal, the other functions basically don't matter. The curvature will naturally be very low since we're driving straight into it. The cost might impact things, but the closer we get, the more likely that the velocity scaling on approach cost function will take the bulk of the work.

It might be good to do either-or on the cost / curvature, but there's a regulated_linear_scaling_min_speed_ min set so that it can never get too low. I'll add a TODO to test this and play around before a PR is submitted into the main repo. I did actually turn off the cost based one for testing because of how aggressive it is so that's might be a good signal 😆

  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);

[SteveMacenski]

@shrijitsingh99 here's a patch for doing the max of the heuristics rather than just the compounding of them https://drive.google.com/file/d/1MqOrdVz-ZQcqbjoJ_CaXACdIPJVJ2LWN/view?usp=sharing you can apply it to this branch with git apply <path name>.patch.

Let me know what you think. It's better-ish but hard to really tell in the turtlebot3 sandbox world

[SteveMacenski]

@shrijitsingh99 I'm going to merge this in now to get it building and we can address the heuristics question in a Nav2 PR to follow. I need to keep the ball rolling and there's still another gateway before this is merged into a usable repo

[SteveMacenski]

ros-navigation/navigation2#2152 follow up in Nav2