ROS2 navigation2 controller

[soldierofhell]

Hi @croesmann, are there any plans for a ROS2 integration with nav2 stack similar like in teb? There's ros2-master branch, but seems like it doesn't implement nav2_core/controller.hpp interface

Regards,

[croesmann]

Hi @soldierofhell,

Yes, of course. But since the package is still in an early stage, I wanted to focus on bug fixing in ROS1 before porting to ROS2. I also expect some API changes in the integration with homotopy class planning (as it is done for the TEB).

However, the port to ROS2 should be straightforward as most of the code (planning backend) is ROS-free.

The only problem at the moment is my limited spare time for working on the project ;-)

[soldierofhell]

Thank you @croesmann. So I'll try to do it myself. Fortunatelly I noticed some similarities with teb code base, so should be easier.

Regards,

[SteveMacenski]

Make sure to PR it back here! @croesmann can you open a ros2 branch?

Also, maybe some documentation about the current status of this work and any positives / negatives people should know.

[soldierofhell]

@croesmann , while doing ROS2 port I found a copy-bug here:

mpc_local_planner/mpc_local_planner/src/controller.cpp

Line 894 in 0ad6111

double delta_rot = normalize_theta(fd_grid->getState(i)[i + 1] - fd_grid->getState(i)[0]);

[soldierofhell]

Can we safely replace g2o::normalize_theta() with normalize_theta() from math_utils.h?

mpc_local_planner/mpc_local_planner/src/mpc_local_planner_ros.cpp

Line 280 in 0ad6111

double delta_orient = g2o::normalize_theta(tf2::getYaw(global_goal.pose.orientation) - _robot_pose.theta());

mpc_local_planner/mpc_local_planner/src/controller.cpp

Line 906 in 0ad6111

intermediate_pose.theta() = g2o::normalize_theta(intermediate_pose.theta() + delta_rot / (n_additional_samples + 1.0));

[croesmann]

Thanks for the pointer to the bug. I‘ll check that later.

Yes, you can safely replace the g2o variant by the internal normalizing function.

[croesmann]

I fixed the bug and synced ros2-master with the ros1 master.

[dkuenster]

@soldierofhell I'm also thinking about porting this to ROS2. How far along are you with your port? Maybe we can avoid doing the same work twice.

[soldierofhell]

I've already done it, although didn't have time to test it before holidays. I'm back on Monday and start testing

[dkuenster]

Any estimation on when the PR will be available?

[ss26]

Is there any update on this PR? I would like to use mpc_local_planner in ros2.

[dkuenster]

I created a PR in this repo. Check out #35

[rzeeshan565]

Is there any update on this PR? I would like to use mpc_local_planner in ros-foxy.