mpc_local_planner 源码部分和以及在kinetic下使用

一、源码部分

首先在TestMpcOptimNode的main函数中，通过start函数配置相关参数。

    // configure obstacles
    // Add interactive markers
    // setup callback for custom obstacles
    // setup callback for clicked points (in rviz) that are considered as via-points
    // setup callback for via-points (callback overwrites previously set via-points)
    // Setup robot shape model
1
2
3
4
5
6

if (!controller.configure(nh, _obstacles, robot_model, _via_points))
{
    ROS_ERROR("Controller configuration failed.");
    return;
}
1
2
3
4
5

在controller.configure()中引入参数，并完成ocp初始化：

if (_ocp->initialize())
    ROS_INFO("OCP initialized.");
1
2

    teb_local_planner::PoseSE2 x0(0, 0, 0); //起始点
    teb_local_planner::PoseSE2 xf(5, 2, 0); //目标点
1
2

配置起始点和目标点传到controller.step()函数中，并且while (ros::ok())不停循环执行step()。

bool Controller::step(const Controller::PoseSE2& start, const Controller::PoseSE2& goal, const geometry_msgs::Twist& vel, double dt, ros::Time t,
                      corbo::TimeSeries::Ptr u_seq, corbo::TimeSeries::Ptr x_seq)
bool Controller::step(const std::vector<geometry_msgs::PoseStamped>& initial_plan, const geometry_msgs::Twist& vel, double dt, ros::Time t,
                      corbo::TimeSeries::Ptr u_seq, corbo::TimeSeries::Ptr x_seq)
1
2
3
4

（在测试中，首先将起始点和目标点存入到vector initial_plan中）
正常情况下，initial_plan中至少存在两个点，即起始点和目标点：

    PoseSE2 start(initial_plan.front().pose);
    PoseSE2 goal(initial_plan.back().pose);
1
2

检查新的测量值，即通过"state_feedback"话题得到的测量值，
如果下式成立的话即赋值给测量值x = _recent_x_feedback。

new_x = _recent_x_feedback.size() > 0 && (t - _recent_x_time).toSec() < 2.0 * dt;
1

否则将起始点位置赋值给测量值：

 _dynamics->getSteadyStateFromPoseSE2(start, x);
 _dynamics->mergeStateFeedbackAndOdomFeedback(start, vel, x);
1
2

接下来检查目标点，如果俩目标点之间的角度或者距离相差过大，则清空轨迹，重新计算。
在重新计算时，首先确定目标点是否在起始点后方：

bool backward = _guess_backwards_motion && (goal.position() -start.position()).dot(start.orientationUnitVec()) < 0;
generateInitialStateTrajectory(x, xf, initial_plan, backward);
1
2

在generateInitialStateTrajectory()函数中，假设所有pose均匀分布。

double dt_init = tf_ref / double(n_init - 1);
double t = dt_init
for (int i = 1; i < n_init - 1; ++i)
{
	double yaw;
	// get yaw from the orientation of the distance vector between pose_{i+1} and pose_{i}
	// ...
	ts->add(t, x);//中间点
	t += dt_init; //时间间隔
}
ts->add(tf_ref, xf);//目标点
1
2
3
4
5
6
7
8
9
10
11

如果目标点不在起始点后方，直接得到i时刻到i+1时刻的yaw：

double dx = initial_plan[i + 1].pose.position.x - initial_plan[i].pose.position.x;
double dy = initial_plan[i + 1].pose.position.y - initial_plan[i].pose.position.y;
yaw       = std::atan2(dy, dx);
1
2
3

如果在起始点后方的话，在计算yaw时：

if (backward) normalize_theta(yaw + M_PI);
1

以上部分时清空轨迹后初始轨迹步骤，如果未清空轨迹重新计算的话，则直接计算当前点到目标点轨迹：

PredictiveController::step(x, xref, uref);
1

其中x为当前位置，xref为目标点。

二、在kinetic下使用

Github：https://github.com/ltiselite/mpc_local_planner

1.将下载好的文件，放到ROS工作空间中，首先编译msg文件否则报错。

cd catkin_ws
catkin_make -DCATKIN_WHITELIST_PACKAGES="mpc_local_planner_msgs"
1
2

2.安装第三方库
control_box_rst：https://github.com/rst-tu-dortmund/control_box_rst
并不需要完全安装，只需要安装部分模块。

popt, EPL 1.0, https://github.com/coin-or/Ipopt
参考文章：Ipopt安装
Ubuntu 18.04 安装CppAD 与 Ipopt避坑指南
ROS版本自带安装！

sudo apt-get install ros-kinetic-ifopt 
1

安装完成后重新编译control_box_rst

安装过程中出现的问题：

1. Eigen和Ceres版本冲突

CMake Error at /usr/local/lib/cmake/Ceres/CeresConfig.cmake:88 (message):
  Failed to find Ceres - Found Eigen dependency, but the version of Eigen
  found (3.3.7) does not exactly match the version of Eigen Ceres was
  compiled with (3.2.10).  This can cause subtle bugs by triggering
  violations of the One Definition Rule.  See the Wikipedia article
  http://en.wikipedia.org/wiki/One_Definition_Rule for more details
Call Stack (most recent call first):
  /usr/local/lib/cmake/Ceres/CeresConfig.cmake:217 (ceres_report_not_found)
  CMakeLists.txt:19 (find_package)
1
2
3
4
5
6
7
8
9

重新下载Eigen3.2版本编译安装！

2. 缺少依赖包：costmap_converter，mbf_costmap_core，mbf_msgs，teb_local_planner

  Could not find a package configuration file provided by "costmap_converter"
  with any of the following names:
    costmap_converterConfig.cmake
    costmap_converter-config.cmake
1
2
3
4

直接安装即可，例如：sudo apt-get install ros-kinetic-costmap-converter ，其他包相同。

3. 由于在ros-kinetic版本下使用，而mpc local在ROS Melodic下。

/home/ilook/catkin_ws/src/mpc_local_planner/src/mpc_local_planner_ros.cpp:247:42: error: no matching function for call to ‘costmap_2d::Costmap2DROS::getRobotPose(geometry_msgs::PoseStamped&)’
     _costmap_ros->getRobotPose(robot_pose);

1
2
3

// costmap_2d_ros.h     line 123
 bool getRobotPose(tf::Stamped<tf::Pose>& global_pose) const;
 
 // odometry_helper_ros.h    line 66
 void getRobotVel(tf::Stamped<tf::Pose>& robot_vel);
1
2
3
4
5

使用transformStampedTFToMsg函数将PoseStamped转成tf::Pose传给costmap_2d，同理对于getRobotVel()函数：

    // Get robot pose
    geometry_msgs::PoseStamped robot_pose;

    tf::Stamped<tf::Pose> robot_pose_tf;
    _costmap_ros->getRobotPose(robot_pose_tf);
    tf::poseStampedTFToMsg(robot_pose_tf,robot_pose);

    //_costmap_ros->getRobotPose(robot_pose);
    _robot_pose = PoseSE2(robot_pose.pose);

    // Get robot velocity
    geometry_msgs::PoseStamped robot_vel_tf;

    tf::Stamped<tf::Pose> robot_vel_tf_temp;
    _odom_helper.getRobotVel(robot_vel_tf_temp);
    tf::poseStampedTFToMsg(robot_vel_tf_temp,robot_vel_tf);

    //_odom_helper.getRobotVel(robot_vel_tf);
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

4. new 了一个抽象类出错

/home/like/catkin_ws/src/mpc_local_planner/mpc_local_planner/src/mpc_local_planner_ros.cpp:1073:1:   required from here
/opt/ros/kinetic/include/class_loader/meta_object.hpp:198:16: error: invalid new-expression of abstract class type ‘mpc_local_planner::MpcLocalPlannerROS’
     return new C;

1
2
3
4

这是因为在kinetic下版本的问题，以下两个函数为纯虚函数并没有被实现：

/opt/ros/kinetic/include/mbf_costmap_core/costmap_controller.h:119:20: note: 	virtual void mbf_costmap_core::CostmapController::initialize(std::__cxx11::string, TF*, costmap_2d::Costmap2DROS*)
       virtual void initialize(std::string name, TF *tf, costmap_2d::Costmap2DRO

/opt/ros/kinetic/include/nav_core/base_local_planner.h:78:20: note: 	virtual void nav_core::BaseLocalPlanner::initialize(std::__cxx11::string, tf::TransformListener*, costmap_2d::Costmap2DROS*)
       virtual void initialize(std::string name, tf::TransformListener* tf, cost
1
2
3
4
5

分别找到对应的头文件，做以下修改：

//virtual void initialize(std::string name, TF *tf, costmap_2d::Costmap2DROS *costmap_ros) = 0;
virtual void initialize(std::string name, TF *tf, costmap_2d::Costmap2DROS *costmap_ros) const;

//virtual void initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros) = 0;
virtual void initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros) const;
1
2
3
4
5

但是此处编译虽然通过，但是动态库中未定义。

/home/like/catkin_ws/devel/lib/libmpc_local_planner.so: undefined reference to `nav_core::BaseLocalPlanner::initialize(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, tf::TransformListener*, costmap_2d::Costmap2DROS*)'
/home/like/catkin_ws/devel/lib/libmpc_local_planner.so: undefined reference to `mbf_costmap_core::CostmapController::initialize(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, tf::TransformListener*, costmap_2d::Costmap2DROS*)'
/home/like/catkin_ws/devel/lib/libmpc_local_planner.so: undefined reference to `typeinfo for mbf_costmap_core::CostmapController'
collect2: error: ld returned 1 exit status
1
2
3
4

提示错误纯虚函数未被实现，发现在继承接口的时候，参数列表不同，因此可能导致纯虚函数并没有被实现，在继承的两个接口中，参数列表都不一样，如下所示：

//继承时
void initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros);
//接口参数
virtual void initialize(std::string name, tf::TransformListener* tf, costmap_2d::Costmap2DROS* costmap_ros) = 0;
virtual void initialize(std::string name, TF *tf, costmap_2d::Costmap2DROS *costmap_ros) = 0;
1
2
3
4
5

因此将接口中的参数都更改为tf2_ros::Buffer* tf，编译通过，感觉在运行时会出现问题。

5. boost::placeholders 未声明，因为boost版本问题，估计要在1.59后才定义了placeholders

/home/like/catkin_ws/src/mpc_local_planner/mpc_local_planner/src/test_mpc_optim_node.cpp:184:111: error: ‘boost::placeholders’ has not been declared
 er->setCallback(i_marker.name, boost::bind(&TestMpcOptimNode::CB_obstacle_marker, this, boost::placeholder  
1
2

参考问题：https://github.com/CopernicaMarketingSoftware/AMQP-CPP/pull/163
因此用boost::arg<1>()来替代 boost::placeholders

// marker_server->setCallback(i_marker.name, boost::bind(&TestMpcOptimNode::CB_obstacle_marker, this, boost::placeholders::_1));
    marker_server->setCallback(i_marker.name, boost::bind(&TestMpcOptimNode::CB_obstacle_marker, this, boost::arg<1>()));
1
2

6. 在运行carlike仿真时出现错误（1）

[FATAL] [1602552568.957154870, 0.900000000]: Failed to create the global_planner/GlobalPlanner planner, are you sure it is properly registered and that the containing library is built? Exception: According to the loaded plugin descriptions the class global_planner/GlobalPlanner with base class type nav_core::BaseGlobalPlanner does not exist. Declared types are  navfn/NavfnROS
[move_base-4] process has died [pid 10476, exit code 1, cmd /opt/ros/kinetic/lib/move_base/move_base __name:=move_base __log:=/home/like/.ros/log/887d8712-0cf3-11eb-b8f9-9cb6d0c8eb4f/move_base-4.log].
log file: /home/like/.ros/log/887d8712-0cf3-11eb-b8f9-9cb6d0c8eb4f/move_base-4*.log

1
2
3
4

应该是缺少相关包，安装即可。

sudo apt-get install ros-kinetic-goal-passer ros-kinetic-move-slow-and-clear ros-kinetic-navigation

1
2

7. 在运行carlike仿真时出现错误（2）：

move_base: ../nptl/pthread_mutex_lock.c:352: __pthread_mutex_lock_full: Assertion `INTERNAL_SYSCALL_ERRNO (e, __err) != ESRCH || !robust' failed.
[move_base-2] process has died [pid 30225, exit code -6, cmd /opt/ros/kinetic/lib/move_base/move_base __name:=move_base __log:=/home/like/.ros/log/cd3e9412-0cf4-11eb-b8f9-9cb6d0c8eb4f/move_base-2.log].
log file: /home/like/.ros/log/cd3e9412-0cf4-11eb-b8f9-9cb6d0c8eb4f/move_base-2*.log

1
2
3
4

此时，路径已经成功规划，但是在计算车体控制时崩溃，即在函数computeVelocityCommands中，
经过查找源码发现在pruneGlobalPlan和transformGlobalPlan两个函数中，使用tf.lookupTransform来获取当前Pose和规划出来的global_plan之间的TF关系，也就是在这时候崩掉，未找到原因…（希望有大佬能提供思路）
不过在测试时发现，global plan中的pose和当前pose仅仅是转向不同，而且在后面似乎并没有用到，因此尝试直接使用当前Pose和global pose进行对比，不进行TF变换，注释掉相关代码。

        //transform robot pose into the plan frame (we do not wait here, since pruning not crucial, if missed a few times)
        // geometry_msgs::TransformStamped global_to_plan_transform =
        //     tf.lookupTransform(global_plan.front().header.frame_id, global_pose.header.frame_id, ros::Time(0));

        // geometry_msgs::PoseStamped robot;
        // tf2::doTransform(global_pose, robot, global_to_plan_transform);
      
        double dist_thresh_sq = dist_behind_robot * dist_behind_robot;
        // iterate plan until a pose close the robot is found
        std::vector<geometry_msgs::PoseStamped>::iterator it        = global_plan.begin();
        std::vector<geometry_msgs::PoseStamped>::iterator erase_end = it;

        while (it != global_plan.end())
        {  
            double dx      = global_pose.pose.position.x - it->pose.position.x;
            double dy      = global_pose.pose.position.y - it->pose.position.y;
            double dist_sq = dx * dx + dy * dy;
            if (dist_sq < dist_thresh_sq)
            {
                erase_end = it;

                break;
            }
            ++it;
        }
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25

在transformGlobalPlan函数中同理，修改的地方较多，只要是通过plan_to_global_transform变换的，都将使用变换前的pose替代，最终在carlike上可以跑通，其他问题还有待测试。

8. 经过以上更改之后虽然能运行，但是感觉效果不好， 后来又把问题7.中的两个函数用Teb local planner中的替换，即将tf2_ros::Buffer* tf使用tf::TransformListener* tf替代，感觉效果有明显的提升，直接不用的话还是有问题，两个版本下的函数仅仅是pose类型不一致，转换之后再直接调用。

// Get robot pose
    //ROS_INFO("Get robot pose");
    geometry_msgs::PoseStamped robot_pose;

    tf::Stamped<tf::Pose> robot_pose_tf;
    _costmap_ros->getRobotPose(robot_pose_tf);

    tf::poseStampedTFToMsg(robot_pose_tf,robot_pose);

    //_costmap_ros->getRobotPose(robot_pose);
    _robot_pose = PoseSE2(robot_pose.pose);

    // Get robot velocity
    //ROS_INFO("Get robot velocity");
    geometry_msgs::PoseStamped robot_vel_tf;

    tf::Stamped<tf::Pose> robot_vel_tf_temp;
    _odom_helper.getRobotVel(robot_vel_tf_temp);
    tf::poseStampedTFToMsg(robot_vel_tf_temp,robot_vel_tf);

    //_odom_helper.getRobotVel(robot_vel_tf);

    _robot_vel.linear.x  = robot_vel_tf.pose.position.x;
    _robot_vel.linear.y  = robot_vel_tf.pose.position.y;
    _robot_vel.angular.z = tf2::getYaw(robot_vel_tf.pose.orientation);

    //std::cout<<robot_pose<<std::endl;

    // prune global plan to cut off parts of the past (spatially before the robot)
    //pruneGlobalPlan(*_tf, robot_pose, _global_plan, _params.global_plan_prune_distance);
    pruneGlobalPlan(*tf_, robot_pose_tf, _global_plan, _params.global_plan_prune_distance);    


    // Transform global plan to the frame of interest (w.r.t. the local costmap)
    std::vector<geometry_msgs::PoseStamped> transformed_plan;
    int goal_idx;
    //geometry_msgs::TransformStamped tf_plan_to_global;
    tf::StampedTransform tf_plan_to_global;
    if (!transformGlobalPlan(*tf_, _global_plan, robot_pose_tf, *_costmap, _global_frame, _params.max_global_plan_lookahead_dist, transformed_plan,
                             &goal_idx, &tf_plan_to_global))
    {
        ROS_WARN("Could not transform the global plan to the frame of the controller");
        message = "Could not transform the global plan to the frame of the controller";
        return mbf_msgs::ExePathResult::INTERNAL_ERROR;
    }
    tf::poseStampedTFToMsg(robot_pose_tf,robot_pose);
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46

此时tf_plan_to_global类型也被更改，后面都需要转换一下。