[request for input] Move pure persuit to nav2 repo?

SteveMacenski commented 3 years ago

Is this generally useful enough that it should be a full fledged controller?

If so, then the following optimizations must be made:

acceleration parameter for ramping up speed and ramping down near goal (proportional to carrot shorted distance?), rather than straight velocity, using kinematic acceleration / deceleration parameters.
rotate to goal after achieving tolerance
collision checking robot -> carrot

Rayman commented 3 years ago

Cool, I would like to contribute to this. What do you think about:

doing some simple forward simulation and check if the trajectory is in collision, if so brake
for pure pursuit controllers, an alignment state is very useful. I'm now really familiar with the behavior tree, is there a node for that?

SteveMacenski commented 3 years ago

ooooh yeah this does no collision checking, absolutely. I totally just assumed that was happening. Yes. We need to do collision checking between the carrot we're driving to at bare minimum.

I'm not sure what you mean by the second point, can you elaborate?

SteveMacenski commented 3 years ago

I agree at the end of paths its missing the 'rotate to pose' thing at the end. Added to list above

Rayman commented 3 years ago

I meant this piece of code: https://github.com/ros-planning/navigation2_tutorials/blob/126902457c5c646b136569886d6325f070c1073d/nav2_pure_pursuit_controller/src/pure_pursuit_controller.cpp#L134-L136 Would it make sense to extract that into a behavior tree node. I can image kind of code ends up in a lot of different controllers.

Some kind of 'rotate to align with the path'

SteveMacenski commented 3 years ago

Maybe not as a BT node, but as a controller plugin, that has crossed my mind. https://github.com/ros-planning/navigation2/issues/2064 this ticket highlights that as a "rotate to path orientation" to start a controller following task. This is kind of the opposite with rotating to goal once in tolerance. I think the prior could be a BT node, but the latter couldn't be since the BT couldn't know that this particular algorithm requires rotating to goal, that's usually done in the controller server.

This topic has come up pretty much everywhere I've ever worked or done open-source relating to. I agree this is worth some further discussion in that ticket about the best way to handle it, please comment in there.

For now though, I think it would be fair to have the pure pursuit controller have a rotate to goal action in the controller itself to make it complete.

SteveMacenski commented 3 years ago

@Rayman I started looking this to clean some things up, and I think there are some bigs in the core algorithm.

  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_;
  }

This is the core algorithm. The algorithm is described on page 18 of this doc. What i see is that curvature's denominator should be a distance from current pose to the carrot. This goal_pose.position.x * goal_pose.position.x + goal_pose.position.y * goal_pose.position.y is not a distance (no carrot!). So that should actually be hypot(pose.pose.position.x - carrot_pose.position.x, pose.pose.position.y - carrot_pose.position.y).

Moreover, the numerator is similarly suspect, why is it the absolute carrot position? That means the curvature is a function of the position on the carrot distance, which seems... about right, but not quite right. It looks like curvature = 2 * sin(alpha) / L. L is the hypot() dist, and alpha = arctan(diff Y / diff X) - theta, so I don't see how those values align correctly.

This is by block of code for the changes, can you sanity check this / test it? I'm surprised since you were using the pure pursuit controller that you didn't run into more issues.


  double carrot_dist = hypot(
    pose.pose.position.x - carrot_pose.position.x,
    pose.pose.position.y - carrot_pose.position.y);

  double alpha = atan2(
    pose.pose.position.y - carrot_pose.position.y,
    pose.pose.position.x - carrot_pose.position.x) - tf2::getYaw(pose.pose.orientation);
  double curvature = 0.0;
  if (carrot_dist > 0.001) {
    curvature = 2.0 * sin(alpha) / carrot_dist;
  }

  linear_vel = desired_linear_vel_;
  angular_vel = desired_linear_vel_ * curvature;

CC @shrijitsingh99

SteveMacenski commented 3 years ago

Edit: maybe actually the numerator is correct, but the denominator was just a copy paste issue? Page 10 of this makes it seem like you can write it as a function of the lateral offsets since we're in robot base frame.

My checklist of things to do here

[x] adding acceleration ramp up from current speed and ramp down proportional to carrot distance (so if approaching goal, the carrots will be closer so we should scale down speeds by carrot distance)
[ ] deal with rotation towards path at start and rotation to orientation of goal
[x] add collision checking
[x] (optional) adding scale for look ahead distance by speed. Papers linked above both recommend this for stability at variable speeds.

Branch: https://github.com/ros-planning/navigation2_tutorials/tree/pure_updates anyone interested in helping with any of these points is welcome, just let me know. I have basic ideas of what I want to do for all of these except the pure rotate to path / goal.

Rayman commented 3 years ago

Using formula V4

alpha = arctan((yref-y)/(xref-x))-theta

We transformed the goal to base_link, so that means our current position is 0, 0, 0. With this we can simplify the formula.

alpha = arctan(yref/xref)

The heading speed is:

omega = 2*v*sin(alpha)/L = 2*v/L*sin(arctan(yref/xref))

Simplify that with the following rule sin(arctan(y/x))=y/sqrt(x^2+y^2)

omega = 2*v/L*yref/sqrt(xref^2+yref^2)

L is the distance to the robot, and that is L=sqrt(xref^2+yref^2). Multiplying two square roots, we can simplify further.

omega = 2*v*yref/(xref^2+yref^2)

In the end, I'm getting the same formula as @shrijitsingh99. Did I make a mistake somewhere?

shrijitsingh99 commented 3 years ago

I believe what @Rayman & I are getting is correct. The confusion might be arising due to the fact that in the code everything in the code is in the frame of base_link while literature does not assume the reference frame to be base_link.

I think the paper linked in the tutorial provides one of the best explanation of the maths for pure pursuit and the code seems to match it.

Regarding, reviewing/helping out with the remaining feature set. I have some cycles available towards the end of the week and can help out. Would it possible to open a PR for this @SteveMacenski. Will be easier to discuss code there. Though I feel a PR should just be opened on the nav2 repo and not here. One of the primary reasons I didn't add extra feature to pure pursuit originally was that this was meant to be a tutorial for beginners. So maybe lets just keep the tutorial as is for simplicity and just add the new stuff directly to the main repo.

The idea @Rayman suggested having a BT for rotate-to-goal seems quite tempting, since I to have had to implement that functionality in the past, and adding it as a BT module might be handy. Though like mentioned above, let's move that to a discussion of its own, I am sure there might be some cons to it that we might be missing.

SteveMacenski commented 3 years ago

I agree with the numerator after looking into it more. The denominator was still in question to me, but I did forget that the pose is in robot base frame, which will always be 0,0. I thought there was a situation where it would not be, but I forgot that DWB / local costmap are going to share the same robot base frame (so one wouldn't use a different frame for there to be a difference).

For now, I want to submit the PR in this repo so that it can be reviewed with a diff of my specific changes for review. Once that's in, then we can merge the completed thing back into Nav2 with a fresh review based on the final state of it. So for now, we should work in this repo on it and once we're happy with it, moving it over to Nav2.

shrijitsingh99 commented 3 years ago

For now, I want to submit the PR in this repo so that it can be reviewed with a diff of my specific changes for review. Makes sense. Will put in a review and contribute once the PR up on this repo.

SteveMacenski commented 3 years ago

I'm hoping to have the collision checking stuff done today and I'll open a PR with it and the velocity scaling / kinematics I already did. I've tested none of this so far though.

SteveMacenski commented 3 years ago

I have things done-ish for a first look. I'm struggling getting my ros2 raw builds running so that I can build fully and test locally, but it's mostly there if you wanted to start looking https://github.com/ros-planning/navigation2_tutorials/pull/17

I didn't do any of the rotate to heading / goal items. I think the best bet there is to make a generic class library that we can apply to make whatever we want (shim nodes, in this one, BT nodes, etc)

SteveMacenski commented 3 years ago

@Rayman @shrijitsingh99 its ready for you to review / test. There's some documentation I still need to make and probably some defaults tweaking to make sure the performance is good out of the box, but its ready for you to start playing with it.

The only major thing missing is the turn to heading part isnt there yet, so make sure to create goals in the same orientation of the robot so you can fairly compare.

Edit: As an aside, its interesting to look at a prototype to a more rugged solution and see why Navigation2 has so. many. parameters. Even something as simple as a pure pursuit planner will end up with something like 20 parameters.

SteveMacenski commented 3 years ago

https://github.com/ros-planning/navigation2/pull/2152#pullrequestreview-574395605

Moved into Nav2 stack!

ros-navigation / navigation2_tutorials

[request for input] Move pure persuit to nav2 repo? #15