Port to ROS2

[dkuenster]

Ports mpc_local_planner to ros2 so it can be used as a nav2_core::Controller with the navigation 2 stack.
Verified that the carlike_minimum_time example works, but it needs a adapted version of the stage simulator wrapper for ros2.
(https://github.com/ymd-stella/stage_ros2)

[mkolodziejczyk-piap]

Hi @dkuenster,
Your PR is prepared for Rolling distribution (unfortunately I don't have one installed so I can't build it), and after merge I think it would be good to prepare foxy branch.
As for the g2o I think you can drop this dependency, because this repo don't use g2o graph optimization. Only g2o::normalize_theta() is used which can be replaced by normalize_theta() from math_utils.h.
In transformGlobalPlan() it's better to replace tf.transform() with https://github.com/ros-planning/navigation2/blob/b91f1ccc3188a9192b0783cdd816bb44badb5ff6/nav2_dwb_controller/nav_2d_utils/src/tf_help.cpp#L42. It has exeption handling in case tf doesn't exists, which is common case on production environments
I like that you moved all the parameters to separate file like in teb
@croesmann , @amakarow , can we merge this PR and start working on other branch/features?

[dkuenster]

Hi @dkuenster,
Your PR is prepared for Rolling distribution (unfortunately I don't have one installed so I can't build it), and after merge I think it would be good to prepare foxy branch.
As for the g2o I think you can drop this dependency, because this repo don't use g2o graph optimization. Only g2o::normalize_theta() is used which can be replaced by normalize_theta() from math_utils.h.
In transformGlobalPlan() it's better to replace tf.transform() with https://github.com/ros-planning/navigation2/blob/b91f1ccc3188a9192b0783cdd816bb44badb5ff6/nav2_dwb_controller/nav_2d_utils/src/tf_help.cpp#L42. It has exeption handling in case tf doesn't exists, which is common case on production environments
I like that you moved all the parameters to separate file like in teb
@croesmann , @amakarow , can we merge this PR and start working on other branch/features?

Good Points. I switched to the transformPose method. It adds an dependency on nav_2d_utils, but I guess the exception handling is worth it.
Getting rid of the g2o dependency definitely makes sense, but requires some more changes, as mpc_local_planner includes pose_se2.hfrom teb_local_planner, which also uses the g2o functions. Did you change the pose_se2.h in teb or did you create your own pose_se2 for mpc_local_planner to get rid of those functions?