enabling closed-loop control in spinning

[SteveMacenski]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#423
Primary OS tested on	Ubuntu
Robotic platform tested on	N/A -- Can I get help on that?

---

Description of contribution in a few bullet points

• enabling closed-loop control for the spin recovery
• closed-loop control is already in the backup recovery
• some other small tweeks

---

Future work that may be required in bullet points

• Can I get a hand in testing this in simulation for last minute validation? I do not yet have a 18.04 machine in my possession but soon to be true. It is however based on math that I verified against one of my (not today public) projects that uses the same math and that has been validated on hardware robots over the last year or so.

---

[codecov]

Codecov Report

Merging #723 into master will increase coverage by 6.41%.
The diff coverage is n/a.

@@            Coverage Diff             @@
##           master     #723      +/-   ##
==========================================
+ Coverage      24%   30.42%   +6.41%     
==========================================
  Files         233      235       +2     
  Lines        9359     9426      +67     
  Branches     2598     3105     +507     
==========================================
+ Hits         2247     2868     +621     
+ Misses       5481     4576     -905     
- Partials     1631     1982     +351

Impacted Files	Coverage Δ
...asks/include/nav2_tasks/goal_reached_condition.hpp	0% <0%> (-56%)	⬇️
..._tasks/include/nav2_tasks/rate_controller_node.hpp	6.45% <0%> (-35.49%)	⬇️
...ion2/nav2_tasks/include/nav2_tasks/task_client.hpp	49.01% <0%> (-1.97%)	⬇️
...vigation2/nav2_navfn_planner/src/navfn_planner.cpp	42.04% <0%> (-0.38%)	⬇️
...rc/navigation2/nav2_motion_primitives/src/spin.cpp	0% <0%> (ø)	⬆️
...on2/nav2_util/include/nav2_util/service_client.hpp	40% <0%> (ø)	⬆️
src/navigation2/nav2_amcl/src/amcl_node.cpp	0% <0%> (ø)	⬆️
...navigation2/nav2_motion_primitives/src/back_up.cpp	0% <0%> (ø)	⬆️
...on2/nav2_simple_navigator/src/simple_navigator.cpp	0% <0%> (ø)
src/navigation2/nav2_simple_navigator/src/main.cpp	0% <0%> (ø)
... and 56 more

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update 178c7c6...062cdf3. Read the comment docs.

[mhpanah]

@SteveMacenski For controlled spin task, I think we want to be able to either spin the robot CW or CCW by some angles based on the user input. For example: spin(-pi) should rotate the robot CCW by 180 degree whereas spin(pi) should rotate the robot CW by 180 degree.

[SteveMacenski]

@mhpanah Agreed, I'd rather not include this in this particular PR since it wasn't able to before.

My next steps here were actually going to be to change the spin to more of a wiggle of rotating by some angle, theta, back and forth. (so +theta, -2theta, +theta to get back to starting pose). I find that to be more useful and just stopping and doing a 180 deg rotation is a little weird

[mhpanah]

@mhpanah Agreed, I'd rather not include this in this particular PR since it wasn't able to before.

My next steps here were actually going to be to change the spin to more of a wiggle of rotating by some angle, theta, back and forth. (so +theta, -2theta, +theta to get back to starting pose). I find that to be more useful and just stopping and doing a 180 deg rotation is a little weird

@SteveMacenski I think we can make this PR similar to the backup one. What do you think of this code snippet below.
I just quickly wrote it and haven't compile it or test it. This is similar to the backup one and if we want to later add wiggle as you suggested we can do that through behavior tree.

nav2_tasks::TaskStatus Spin::onRun(const nav2_tasks::SpinCommand::SharedPtr command)
{
  double pitch, roll;
  tf2::getEulerYPR(command->quaternion, command_yaw_, pitch, roll);

  if (!robot_->getOdometry(initial_pose_)) {
    RCLCPP_ERROR(node_->get_logger(), "initial robot odom pose is not available.");
    return nav2_tasks::TaskStatus::FAILED;
  }
 
  tf2::getEulerYPR(initial_pose_, start_yaw_, pitch, roll);

  return nav2_tasks::TaskStatus::SUCCEEDED;
}

nav2_tasks::TaskStatus Spin::onCycleUpdate(nav2_tasks::SpinResult & result)
{
  TaskStatus status = controlledSpin();
  nav2_tasks::SpinResult empty_result;
  result = empty_result;
  return status;
}


nav2_tasks::TaskStatus Spin::controlledSpin()
{
  // Get current robot orientation
  auto current_pose = std::make_shared<geometry_msgs::msg::PoseWithCovarianceStamped>();
  if (!robot_->getCurrentPose(current_pose)) {
    RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available.");
    return TaskStatus::FAILED;
  }
   
  const double current_yaw = tf2::getYaw(current_pose->pose.pose.orientation);
  const double yaw_diff = start_yaw - current yaw;
  
  geometry_msgs::msg::Twist cmd_vel;
  cmd_vel.linear.x = 0.0;
  cmd_vel.linear.y = 0.0;
  
  if (yaw_diff >= abs(command_yaw_)) {
    cmd_vel.angular.z = 0.0;
    robot_->sendVelocity(cmd_vel);
    return TaskStatus::SUCCEEDED;
  }
  
  // We can send this cmd_vel
  // command_yaw_ < 0 ? cmd_vel.angular.z = -0.025 : cmd_vel.angular.z = 0.025;
  // But
  // In your PR I like it that you dynamically decrease the rotational velocity as it get's closer
  // To the goal. So lets try:

  double vel = sqrt(2 * rotational_acc_lim_ * yaw_diff);
  vel = std::min(std::max(vel, min_rotational_vel_), max_rotational_vel_);
  
  command_yaw_ < 0 ? cmd_vel.angular.z = -vel : cmd_vel.angular.z = vel;


  robot_->sendVelocity(cmd_vel);

  return TaskStatus::RUNNING;
}

This would be a BT for wiggle

<SequenceStar name="wiggle">
    <Spin theta="180" />
    <Spin theta="-360" />
    <Spin theta="180" />
</SequenceStar>

[SteveMacenski]

@mhpanah I like your snippet, but then aren't you changing the definition of the input command? As it exists, the command is the current pose. I'm actually not totally sure where that input is defined to make sure we change appropriately

Fixed the squared issue.

[mhpanah]

@SteveMacenski The command is not current pose. For backup, command is defined in back_up_task.hpp and in back_up_action.hpp we set it to command_x-> = -0.15. For spin, the command is defined in spin_task.hpp but we haven't set it to anything in spin_action.hpp yet. For now you can set it to any theta in spin_action.hpp just like the backup. However, eventually, both the backup and spin commands should be coming from BT.

[SteveMacenski]

Ok, I can handle this then tomorrow 👍

[orduno]

Some things to consider (for this or future PRs):

• Requirements: CW, CCW, multiple turns, speed. This is tied to how we are using the primitive, for example, for recovery only moving on a given direction and a set amount?
• Some behavior or primitive based on another should probably be defined inside the primitives package. Not sure if it's worth the overhead of defining it on the behavior tree. For example, wiggle should probably be a separate primitive and internally use the spin controller.

I have other comments about the implementation of the controller but will add those next to the code.

In addition, I was also working on a spin controller #599, but put that on-hold before my leave.

[SteveMacenski]

I don't know if sub-trees are a thing in the BT library (e.i. rather than defining a wiggle recovery, we have a spin recovery, and a wiggle tree, that is used by the larger tree). Given the similar overlaps, we could use the spin recovery in the wiggle recovery, but that would not look very clean. @mhpanah recommendation for the tree implementation of a wiggle I thought was very clever

[orduno]

Yeah, I agree it is clever, however, each BT node involves some ROS action call which I'm not sure if it's good.

On the code, the only issue I see here is handling the pi,-pi discontinuity. Not sure what the code will do if the starting orientation is pi and is requested a delta rotation of pi. I think it just stops at the discontinuity. The code is based on ROS1 spin recovery, which always rotates pi, I believe.

#599 does some angle wrapping there, also handles both CW & CCW movements.

[SteveMacenski]

Ok, well we can deal with the "wiggle" if we feel its necessary in another PR, nothing about this really covers that, just leaves the opportunity.

Let me think about the angle bit and put something together

[mhpanah]

@orduno For the requirements, I agree with CW, CCW and speed, multiple turns can be handled with for example spin(-720) or rotate CCW twice.
IMO primitives should be just simple actions such as rotate or move forward/backward. More complex actions/behaviors such as wiggle which is composed of simpler actions can be made with BT.

@SteveMacenski We are actually still figuring out on how to include simpler BT's into a larger one. But for now within the larger BT that we have, you can add the wiggle by a sequence star node with three children.

<SequenceStar name="wiggle">
    <Spin theta="180" />
    <Spin theta="-360" />
    <Spin theta="180" />
</SequenceStar>

In the navigate_w_recovery_retry.xml file you can replace the <spin/> with the above code. But as of now the Spin is not ready to accept parameters. You would have to look at the rate_controller_node.hpp as an example to see how it passes parameter.

If you like I can add the wiggle later.

[mkhansenbot]

@SteveMacenski - I just noticed you're creating a lot of branches in the repo. Please create branches in your fork instead and do a PR from there. We are trying to keep branches to a minimum. The only ones we should have right now are master, crystal-devel and lifecycle. lifecycle is being used as a temporary staging branch and the plan is to rebase, merge and delete it ASAP.

[SteveMacenski]

@mkhansen-intel will do

[SteveMacenski]

@orduno @mhpanah please review again - added handling of directions and true distances vs. normalized distances.

This handles all sets of signed angles 0->180 where the sign of the quaternion rotation defines if its CW or CCW. This limitation does also make it so that then the maximum valid rotation is 180 deg, else it will rotate the complementary angle in the opposite direction, but that's a limitation imposed from the inputs as only a quaternion, not something I can pull out in math without more information given. The quaternion getYaw's are all normalized (-pi, pi].

[SteveMacenski]

There's a bunch of problems here now that i think about it. I would like to generally just redo this plugin, I'm closing this PR.

I'm going to wait until the lifecycle branch lands so that we can merge in the recovery stuff before its worth the refactor necessary. I dont think a quaternion is the right input and more information should be added