search

rst-tu-dortmund / mpc_local_planner

The mpc_local_planner package implements a plugin to the base_local_planner of the 2D navigation stack. It provides a generic and versatile model predictive control implementation with minimum-time and quadratic-form receding-horizon configurations.
http://wiki.ros.org/mpc_local_planner
GNU General Public License v3.0
545 stars 143 forks source link

Port to ROS2 #35

Open dkuenster opened 3 years ago

dkuenster commented 3 years ago

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 commented 3 years ago

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 commented 3 years ago

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?