源码学习：amr-lmpcc_02

本文最后更新于：2020年7月6日 下午

package: lmpcc

2.0 代办事项

`LMPCC::controlLoop()`中使用ACADO求解的过程
`reference_path.cpp`分析
`ACADO`求解器的使用方法

2.1 launch文件

1
2
3
4
5
6
\amr-lmpcc\lmpcc\launch
├── lmpcc.launch
├── lmpcc.launch.orig
└── robot_lmpcc.launch
    
file: 3

其中，lmpcc.launch.orig为一个备份文件，我们主要关注用于仿真环境的lmpcc.launch和用于实际机器人的robot_lmpcc.launch两个文件。

2.1.1 lmpcc.launch

首先是一些arg参数，主要是一些调试信息

1
2
3
4
5
6
7
<!-- Debug Info -->
<arg name="debug" default="false" />
<arg unless="$(arg debug)" name="launch_prefix" value="" />
<arg     if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />
<arg name="debug_config_parameter" default="false"/>
<arg name="debug_kinematic_cal" default="false"/>
<arg name="debug_collision_detection" default="false"/>

然后启动了用于发布机器人位姿的mobile_robot_state_publisher_node节点，注意到这里调用的包为【源码学习】amr-lmpcc_01中总结的8个单独的包中的其中一个，关于它的详细内容放在【源码学习】amr-lmpcc_03这篇文章中。

1
2
3
4
5
<arg name="config" default="$(find mobile_robot_state_publisher)"/>
<!--Publish the mobile robot pose -->
<node name="mobile_robot_state_publisher" pkg="mobile_robot_state_publisher" type="mobile_robot_state_publisher_node" respawn="false" output="screen">
	<rosparam command="load" file="$(arg config)/config/jackal.yaml" />
</node>

最后加载配置文件和启动lmpcc_node节点

1
2
<rosparam command="load" file="$(find lmpcc)/config/lmpcc_config_parameter.yaml"/>
<node pkg="lmpcc" type="lmpcc_node" name="lmpcc_node" cwd="node" respawn="false" output="screen"/>

文件的末尾是注释掉的用于可视化的rviz节点的启动

1
2
<!-- configured rviz -->
<!--node name="rviz" pkg="rviz" type="rviz" args="-d $(find lmpcc)/rviz_config/rviz_config.rviz" /-->

2.1.2 robot_lmpcc.launch

该文件和2.1.1 lmpcc.launch只有些许不同，这里省略相同部分，只列出不同的内容。

首先仍然是一些调试信息参数，不同的是多了雷达的话题名称scan_topic（可能是给amcl用的）

1
<arg name="scan_topic" value="velodyne_scan1" />

然后是注释掉的用于发布地图map_server和进行定位的amcl两个节点

1
2
3
4
5
6
<!--Launch localization node-->
<!--
<arg name="map_file" default="$(find jackal_navigation)/maps/cyberzoo.yaml"/>
<node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />
<include file="$(find jackal_navigation)/launch/include/amcl.launch" />
-->

最后在启动lmpcc_node节点时，设置了两个param参数。

1
2
3
<node pkg="lmpcc" type="lmpcc_node" name="lmpcc_node" cwd="node" respawn="false" output="screen"/>
<param name="collision_avoidance/local_map" value="true" type="bool" />
<param name="simulation_mode" value="false" type="bool" />

2.1.3 小结

使用时注意在robot_lmpcc.launch中的scan_topic的设置。

该launch文件一共启动了两个节点

  • mobile_robot_state_publisher_node
  • lmpcc_node

2.2 lmpcc_config_parameter.yaml

该文件主要设置了一些关于调试信息的标志位，控制频率，机器人的数学模型参数，订阅和发布的话题，关于Path的参数，全局路径，障碍物的数目和OCP相关的参数。

2.3 头文件

1
2
3
4
5
6
7
8
\amr-lmpcc\lmpcc\include\lmpcc
├── Clothoid.h
├── lmpcc_configuration.h
├── lmpcc_controller.h
├── ocp
└── reference_path.h

directory: 1 file: 4

在这些头文件中分别定义了三个类：

  • LMPCC：处理碰撞，获得机器人的位置和速度，发布速度控制指令
  • LMPCC_configuration：主要用于处理lmpcc_config_parameter.yaml文件中对应的参数
  • ReferencePath：全局参考路径和局部参考路径
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
class LMPCC{
	/** Managing execution of all classes of lmpcc
     * - Handle self collision avoidance
     * - Extract current position and velocity of manipulator joints
     * - Publish controlled joint velocity
     */
}
class LMPCC_configuration{
    /**
    *  @brief All neccessary configuration parameter of predictive control repository
    *         Read data from parameter server
    *         Updated old data with new data
    *  Note:  All data member name used like xyz_ and all parameter name is normal like xyz.
    */
}
class ReferencePath{
    /*
    *- 成员函数：ReferencePath::SetGlobalPath()
	*- 成员函数：ReferencePath::InitLocalRefPath()
	*- 成员函数：ReferencePath::UpdateLocalRefPath()
    */
}

2.4 源文件

1
2
3
4
5
6
7
8
9
10
11
\amr-lmpcc\lmpcc\src
├── Clothoid.cpp
├── generated_mpc
├── lmpcc_configuration.cpp
├── lmpcc_controller.cpp
├── lmpcc_node.cpp
├── ocp
├── reference_path.cpp
└── spline.cpp

directory: 2 file: 6

2.4.1 lmpcc_node.cpp

该文件中定义着主函数main。

文件的开头分别是ROS节点的初始化，

1
ros::init(argc, argv, ros::this_node::getName());

创建类LMPCC的实例化对象controller_，注意此时会调用构造函数LMPCC()，在构造函数中重启了reconfigure_server_

1
LMPCC controller_;

然后调用成员函数：controller_.initialize()进行初始化。

1
2
3
4
5
6
7
8
9
10
11
12
// initialize predictive control node
if (!controller_.initialize())
{
	ROS_ERROR_STREAM_NAMED("FAILED TO INITIALIZE %s", ros::this_node::getName().c_str());
	exit(1);
}
else
{
	// spin node, till ROS node is running on
	ROS_INFO_STREAM_NAMED("%s INITIALIZE SUCCESSFULLY!!", ros::this_node::getName().c_str());
	ros::spin();
}

2.4.2 lmpcc_controller.cpp

1. 成员函数：LMPCC::initialize()

1
bool LMPCC::initialize(){}

首先确保ROS节点仍在运行

1
if (ros::ok()){}

创建类LMPCC_configuration的实例化对象lmpcc_config_，注意此时会调用构造函数LMPCC_configuration()，在构造函数中将标志位initialize_success_设置为false。

1
2
// initialize parameter configuration class
lmpcc_config_.reset(new LMPCC_configuration());

调用类LMPCC_configuration的成员函数：lmpcc_config_.initialize()，该函数将在2.4.3 lmpcc_configuration.cpp中进行分析，如果初始化失败则打印错误信息

1
2
3
4
5
6
7
8
9
10
11
bool lmpcc_config_success = lmpcc_config_->initialize();

if (lmpcc_config_success == false)
{
	ROS_ERROR("LMPCC: FAILED TO INITIALIZE!!");
	std::cout << "States: \n"
		<< " pd_config: " << std::boolalpha << lmpcc_config_success << "\n"
		<< " pd config init success: " << std::boolalpha << lmpcc_config_->initialize_success_
        << std::endl;
	return false;
}

然后检查存储全局参考路径global_path的vector是否具有相同的长度

1
2
3
4
5
6
// Check if all reference vectors are of the same length
if (!( (lmpcc_config_->ref_x_.size() == lmpcc_config_->ref_y_.size()) && ( lmpcc_config_->ref_x_.size() == lmpcc_config_->ref_theta_.size() ) && (lmpcc_config_->ref_y_.size() == lmpcc_config_->ref_theta_.size()) ))
{
	ROS_ERROR("Reference path inputs should be of equal length");
	return false;
}

参数的检查和初始化

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
//Controller options
enable_output_ = lmpcc_config_->activate_output_;
n_iterations_ = lmpcc_config_->max_num_iteration_;

/** Initialize reconfigurable parameters **/
cost_contour_weight_factors_ = transformStdVectorToEigenVector(lmpcc_config_ >contour_weight_factors_);
cost_control_weight_factors_ = transformStdVectorToEigenVector(lmpcc_config_->control_weight_factors_);

slack_weight_ = lmpcc_config_->slack_weight_;
repulsive_weight_ = lmpcc_config_->repulsive_weight_;
reference_velocity_ = lmpcc_config_->reference_velocity_;
reduced_reference_velocity_ = reference_velocity_;
n_search_points_ = lmpcc_config_->n_search_points_;
window_size_ = lmpcc_config_->search_window_size_;

/** Set task flags and counters **/
goal_reached_ = false;              // Flag for reaching the goal
segment_counter = 0;				// Initialize reference path segment counter

// DEBUG
if (lmpcc_config_->activate_debug_output_){
	ROS_WARN("===== DEBUG INFO ACTIVATED =====");
}
if (ACADO_N != lmpcc_config_->discretization_intervals_){
	ROS_WARN("Number of discretization steps differs from generated OCP");
}

根据是否是仿真环境，选择不同的速度控制指令话题

1
2
3
4
5
6
7
/** Control output topic **/
if (lmpcc_config_->simulation_mode_){
	cmd_topic_ = lmpcc_config_->cmd_sim_;
}
else{
	cmd_topic_ = lmpcc_config_->cmd_;
}

订阅的话题

  • robot_state：包mobile_robot_state_publisher发布，主要包括机器人的位姿和速度
  • ellipse_objects_feed：包lmpcc_obstacle_feed发布，主要是移动障碍物的信息
  • collision_constraints：包static_collision_avoidance发布，主要是无碰撞区域的多边形信息
1
2
3
4
/** Subscribers **/
robot_state_sub_ = nh.subscribe(lmpcc_config_->robot_state_, 1, &LMPCC::StateCallBack, this);
obstacle_feed_sub_ = nh.subscribe(lmpcc_config_->ellipse_objects_feed_, 1, &LMPCC::ObstacleCallBack, this);
collision_free_sub_ = nh.subscribe("collision_constraints", 1, &LMPCC::FreeAreaCallBack, this);

发布的话题

  • cmd_vel：机器人的速度控制指令
  • predicted_cmd
  • cost
  • contour_error
  • controller_feedback
1
2
3
4
5
6
/** Publishers **/
controlled_velocity_pub_ = nh.advertise<geometry_msgs::Twist>(cmd_topic_,1);
pred_cmd_pub_ = nh.advertise<nav_msgs::Path>("predicted_cmd",1);
cost_pub_ = nh.advertise<std_msgs::Float64>("cost",1);
contour_error_pub_ = nh.advertise<std_msgs::Float64MultiArray>("contour_error",1);
feedback_pub_ = nh.advertise<lmpcc_msgs::lmpcc_feedback>("controller_feedback",1);

提供的服务，第一个用于gazebo，第二个用于rviz（猜测）

1
2
reset_simulation_client_ = nh.serviceClient<std_srvs::Empty>("/gazebo/reset_world");
reset_ekf_client_ = nh.serviceClient<robot_localization::SetPose>("/set_pose");

定时启动控制循环（重要）

1
2
3
/** Set timer for control loop **/
timer_ = nh.createTimer(ros::Duration((double)1/lmpcc_config_->controller_frequency_), &LMPCC::controlLoop, this);
timer_.start();

设置动态参数服务器

1
2
3
4
/** Setting up dynamic_reconfigure server for the LmpccConfig parameters **/
ros::NodeHandle nh_lmpcc("lmpcc");
reconfigure_server_.reset(new dynamic_reconfigure::Server<lmpcc::LmpccConfig>(reconfig_mutex_, nh_lmpcc));
reconfigure_server_->setCallback(boost::bind(&LMPCC::reconfigureCallback, this, _1, _2));

初始化障碍物信息

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
/** Initialize obstacle positions over the time horizon **/
pred_traj_.poses.resize(ACADO_N);
pred_cmd_.poses.resize(ACADO_N);
obstacles_.lmpcc_obstacles.resize(lmpcc_config_->n_obstacles_);

for (int obst_it = 0; obst_it < lmpcc_config_->n_obstacles_; obst_it++)
{
	obstacles_.lmpcc_obstacles[obst_it].trajectory.poses.resize(ACADO_N);
	obstacles_.lmpcc_obstacles[obst_it].major_semiaxis.resize(ACADO_N);
	obstacles_.lmpcc_obstacles[obst_it].minor_semiaxis.resize(ACADO_N);
	for(int i=0;i < ACADO_N; i++)
    {
		obstacles_.lmpcc_obstacles[obst_it].trajectory.poses[i].pose.position.x = current_state_(0) - 100;
		obstacles_.lmpcc_obstacles[obst_it].trajectory.poses[i].pose.position.y = 0;
		obstacles_.lmpcc_obstacles[obst_it].trajectory.poses[i].pose.orientation.z = 0;
	}
}

pred_traj_.header.frame_id = lmpcc_config_->planning_frame_;
for(int i=0;i < ACADO_N; i++)
{
	pred_traj_.poses[i].header.frame_id = lmpcc_config_->planning_frame_;
}

计算在地图中用圆形表示机器人时的半径

1
computeEgoDiscs();

无碰撞区域初始化（ACADO_N= 15）

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
collision_free_C1.resize(ACADO_N);
collision_free_C2.resize(ACADO_N);
collision_free_C3.resize(ACADO_N);
collision_free_C4.resize(ACADO_N);

collision_free_a1x.resize(ACADO_N);
collision_free_a1y.resize(ACADO_N);
collision_free_a2x.resize(ACADO_N);
collision_free_a2y.resize(ACADO_N);
collision_free_a3x.resize(ACADO_N);
collision_free_a3y.resize(ACADO_N);
collision_free_a4x.resize(ACADO_N);
collision_free_a4y.resize(ACADO_N);

collision_free_xmin.resize(ACADO_N);
collision_free_xmax.resize(ACADO_N);
collision_free_ymin.resize(ACADO_N);
collision_free_ymax.resize(ACADO_N);

初始化全局参考路径，关于reference的成员函数将在2.4.4 reference_path.cpp中详细介绍。

1
referencePath.SetGlobalPath(lmpcc_config_->ref_x_, lmpcc_config_->ref_y_, lmpcc_config_->ref_theta_);

打印输出全局参考路径，初始化可视化信息

1
2
3
4
5
6
7
if (lmpcc_config_->activate_debug_output_) {
	referencePath.PrintGlobalPath();    // Print global reference path
}

if (lmpcc_config_->activate_visualization_) {
	initialize_visuals();
}

2. 成员函数：LMPCC::StateCallBack()

当订阅的话题robot_state接收到数据时的回调函数。

注意msg->position.z为速度信息，详情请参考[源码学习]amr-lmpcc_03

1
2
3
4
5
6
7
8
9
10
// read current position and velocity of robot joints
void LMPCC::StateCallBack(const geometry_msgs::Pose::ConstPtr& msg)
{
    last_state_ = current_state_;

    current_state_(0) =    msg->position.x;
    current_state_(1) =    msg->position.y;
    current_state_(2) =    msg->orientation.z;
    current_state_(3) =    msg->position.z;
}

3. 成员函数：LMPCC::ObstacleCallBack()

1
2
void LMPCC::ObstacleCallBack(const lmpcc_msgs::lmpcc_obstacle_array& received_obstacles){
}

当订阅的话题ellipse_objects_feed接收到数据时的回调函数。

首先初始化障碍物的数目，存储从话题ellipse_objects_feed中接收的障碍物信息

1
2
3
lmpcc_msgs::lmpcc_obstacle_array total_obstacles;
total_obstacles.lmpcc_obstacles.resize(lmpcc_config_->n_obstacles_);
total_obstacles.lmpcc_obstacles = received_obstacles.lmpcc_obstacles;

如果接受的障碍物数目小于lmpcc_config_->n_obstacles_(4)，默认初始化剩余的障碍物信息

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
if (received_obstacles.lmpcc_obstacles.size() < lmpcc_config_->n_obstacles_)
{
	for (int obst_it = received_obstacles.lmpcc_obstacles.size(); obst_it < lmpcc_config_->n_obstacles_; obst_it++)
	{
		total_obstacles.lmpcc_obstacles[obst_it].pose.position.x = current_state_(0) - 100;
		total_obstacles.lmpcc_obstacles[obst_it].pose.position.y = 0;
		total_obstacles.lmpcc_obstacles[obst_it].pose.orientation.z = 0;

		for (int traj_it = 0; traj_it < ACADO_N; traj_it++)
		{
			total_obstacles.lmpcc_obstacles[obst_it].trajectory.poses[traj_it].pose.position.x = current_state_(0) - 100;
			total_obstacles.lmpcc_obstacles[obst_it].trajectory.poses[traj_it].pose.position.y = 0;
			total_obstacles.lmpcc_obstacles[obst_it].trajectory.poses[traj_it].pose.orientation.z = 0;
			total_obstacles.lmpcc_obstacles[obst_it].major_semiaxis[traj_it] = 0.001;
            total_obstacles.lmpcc_obstacles[obst_it].minor_semiaxis[traj_it] = 0.001;
		}
	}
}

将所有的障碍物信息存放在obstacles_中

1
2
3
4
for (int total_obst_it = 0; total_obst_it < lmpcc_config_->n_obstacles_; total_obst_it++)
{
	obstacles_.lmpcc_obstacles[total_obst_it] = total_obstacles.lmpcc_obstacles[total_obst_it];
}

4. 成员函数：LMPCC::FreeAreaCallBack()

当订阅的话题collision_constraints接收到数据时的回调函数。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
void LMPCC::FreeAreaCallBack(const static_collision_avoidance::collision_free_polygon& msg){
    //ROS_INFO("LMPCC::FreeAreaCallBack");
    collision_free_a1x = msg.collision_free_a1x;
    collision_free_a1y = msg.collision_free_a1y;
    collision_free_a2x = msg.collision_free_a2x;
    collision_free_a2y = msg.collision_free_a2y;
    collision_free_a3x = msg.collision_free_a3x;
    collision_free_a3y = msg.collision_free_a3y;
    collision_free_a4x = msg.collision_free_a4x;
    collision_free_a4y = msg.collision_free_a4y;

    collision_free_C1 = msg.collision_free_C1;
    collision_free_C2 = msg.collision_free_C2;
    collision_free_C3 = msg.collision_free_C3;
    collision_free_C4 = msg.collision_free_C4;

    collision_free_xmin = msg.collision_free_xmin;
    collision_free_xmax = msg.collision_free_xmax;
    collision_free_ymin = msg.collision_free_ymin;
    collision_free_ymax = msg.collision_free_ymax;
}

5. （重要）成员函数：LMPCC::controlLoop()

1
2
// This function is called each 1/controller_frequency
void LMPCC::controlLoop(const ros::TimerEvent &event){}

首先是局部变量初始化以及初始化ACADO求解器

1
2
3
4
5
int N_iter;
acado_timer t;
acado_tic( &t );

acado_initializeSolver( );

如果plan_标志位为真，则执行以下操作

首先是ACADO相关变量的赋值

1
2
3
4
5
6
7
8
9
10
acadoVariables.x[0] = current_state_(0);
acadoVariables.x[1] = current_state_(1);
acadoVariables.x[2] = current_state_(2);
acadoVariables.x[4] = 0.0000001;          //dummy state
acadoVariables.x[5] = 0.0000001;          //dummy state

acadoVariables.u[0] = controlled_velocity_.linear.x;
acadoVariables.u[1] = controlled_velocity_.angular.z;
acadoVariables.u[2] = 0.0000001;           //slack variable
acadoVariables.u[3] = 0.0000001;           //slack variable

❓❓❓没有看懂的判断条件

1
if(acadoVariables.x[3] > ss[2]) {}

然后判断是否到达目标点，如果当前位置的 x 坐标值与15米相差不超过1米，或者当前位置的x坐标已超过15。

注意：程序在此处将终点设置在了固定不变的位置。

1
if((std::sqrt(std::pow(current_state_(0)-15,2)+std::pow(current_state_(1),2))<1) || (current_state_(0)>15)){}

则认为已经到达目标

1
2
goal_reached_ = true;
ROS_ERROR_STREAM("GOAL REACHED");

在达到目标后，如果启用循环模式loop_mode_，则重新初始化以下信息，对应成员函数LMPCC::initialize()中的两个服务

1
2
reset_simulation_client_.call(reset_msg_);		//gazebo/reset_world
reset_ekf_client_.call(reset_pose_msg_);		//set_pose

重新初始化本地参考路径

1
2
3
4
/** Re-initialize local reference path **/
referencePath.InitLocalRefPath(lmpcc_config_->n_local_,lmpcc_config_->n_poly_per_clothoid_,ss,xx,yy,vv);
reset_solver();
acadoVariables.x[3] = referencePath.GetS0();

如果没有到达目标位置，则更新本地路径

❗❗❗注：猜测acadoVariables.x[3]为论文中的 $\theta_0$

1
2
3
segment_counter++;
referencePath.UpdateLocalRefPath(segment_counter, ss, xx, yy, vv);
acadoVariables.x[3] = referencePath.GetS0();

关于reference的成员函数将在2.4.4 reference_path.cpp中详细介绍。

如果使能输出enable_output_

1
2
3
4
5
if(enable_output_) {
	double smin;
	smin = referencePath.ClosestPointOnPath(current_state_, ss[1], 100, acadoVariables.x[3], window_size_, n_search_points_);
	acadoVariables.x[3] = smin;
}

接下来计算每一步的参考速度，并以reference_velocity_为上限

1
2
3
4
5
6
for (N_iter = 0; N_iter < ACADO_N; N_iter++) {
	reduced_reference_velocity_ = current_state_(3) + 1 * 0.25 * (N_iter+1);
	if (reduced_reference_velocity_ > reference_velocity_)
		reduced_reference_velocity_ = reference_velocity_;
	acadoVariables.od[(ACADO_NOD * N_iter) + 40] = reduced_reference_velocity_;
}

然后是ACADO的参数设置，这里仅展示部分

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
for (N_iter = 0; N_iter < ACADO_N; N_iter++) {
	acadoVariables.od[(ACADO_NOD * N_iter) + 0 ] = referencePath.ref_path_x.m_a[1];        // spline coefficients
	acadoVariables.od[(ACADO_NOD * N_iter) + 1 ] = referencePath.ref_path_x.m_b[1];
	acadoVariables.od[(ACADO_NOD * N_iter) + 2 ] = referencePath.ref_path_x.m_c[1];        // spline coefficients
	acadoVariables.od[(ACADO_NOD * N_iter) + 3 ] = referencePath.ref_path_x.m_d[1];
	acadoVariables.od[(ACADO_NOD * N_iter) + 4 ] = referencePath.ref_path_y.m_a[1];        // spline coefficients
	acadoVariables.od[(ACADO_NOD * N_iter) + 5 ] = referencePath.ref_path_y.m_b[1];
	acadoVariables.od[(ACADO_NOD * N_iter) + 6 ] = referencePath.ref_path_y.m_c[1];        // spline coefficients
	acadoVariables.od[(ACADO_NOD * N_iter) + 7 ] = referencePath.ref_path_y.m_d[1];
    
    /* ...... */
    
    for (int obst_it = 0; obst_it < lmpcc_config_->n_obstacles_; obst_it++) {
		//ROS_INFO_STREAM("Next agent" << obst_it <<"at time "<< N_iter <<" pos: " << obstacles_.lmpcc_obstacles[obst_it].trajectory.poses[N_iter].pose.position.x);
		acadoVariables.od[(ACADO_NOD * N_iter) +
                                  55+obst_it*3] = obstacles_.lmpcc_obstacles[obst_it].trajectory.poses[N_iter].pose.position.x;      // x position of obstacle 1
		acadoVariables.od[(ACADO_NOD * N_iter) +
                                  56+obst_it*3] = obstacles_.lmpcc_obstacles[obst_it].trajectory.poses[N_iter].pose.position.y;      // y position of obstacle 1
		acadoVariables.od[(ACADO_NOD * N_iter) +
                                  57+obst_it*3] = obstacles_.lmpcc_obstacles[obst_it].trajectory.poses[N_iter].pose.orientation.z;   // heading of obstacle 1
	}
}

ACADO的参数设置

1
2
3
4
5
6
acadoVariables.x0[ 0 ] = current_state_(0);
acadoVariables.x0[ 1 ] = current_state_(1);
acadoVariables.x0[ 2 ] = current_state_(2);
acadoVariables.x0[ 3 ] = acadoVariables.x[3];
acadoVariables.x0[ 4 ] = 0.0000001;             //dummy state
acadoVariables.x0[ 5 ] = 0.0000001;             //dummy state

ACADO

1
2
acado_preparationStep();
acado_feedbackStep();

如果acado_getKKT()没有达到最小限制且没有达到最大的迭代次数

1
2
3
4
5
int j=1;
while (acado_getKKT() > lmpcc_config_->kkt_tolerance_ && j < n_iterations_){
    acado_preparationStep();
	acado_feedbackStep();
}

当迭代次数大于6时，参考速度设置为减速

1
2
3
4
5
6
7
8
if(j >6){
	for (N_iter = 0; N_iter < ACADO_N; N_iter++) {
		reduced_reference_velocity_ = current_state_(3) - 0.2 * 0.2 * (N_iter+1);
		if(reduced_reference_velocity_ < 0.05)
			reduced_reference_velocity_=0;
		acadoVariables.od[(ACADO_NOD * N_iter) + 40] = reduced_reference_velocity_;
	}
}

当机器人的当前速度小于参考速度时，参考速度设置为加速并以reference_velocity_为上限，以0.1为下限。

1
2
3
4
5
6
7
8
9
10
11
if (current_state_(3) < reference_velocity_) {
	for (N_iter = 0; N_iter < ACADO_N; N_iter++) {
		reduced_reference_velocity_ = current_state_(3) + 0.2 * 0.2* (N_iter+1);
		if (reduced_reference_velocity_ > reference_velocity_)
			reduced_reference_velocity_ = reference_velocity_;
		if(reduced_reference_velocity_ < 0.1)
			reduced_reference_velocity_ = 0.1;
		acadoVariables.od[(ACADO_NOD * N_iter) + 40] = reduced_reference_velocity_;
	}
}
j++;

获得当前的速度控制指令

1
2
controlled_velocity_.linear.x = acadoVariables.u[0];
controlled_velocity_.angular.z = acadoVariables.u[1];

在满足kkt_tolerance_的条件下，如果设置了可视化，则发布以下信息

  • publishPredictedTrajectory()：给static_obstacle_feed使用
  • publishPredictedCollisionSpace()：可视化
  • publishLocalRefPath()：可视化
1
2
3
4
5
6
7
if(acado_getKKT() < lmpcc_config_->kkt_tolerance_) {
	if (lmpcc_config_->activate_visualization_) {
		publishPredictedTrajectory();
		publishPredictedCollisionSpace();
		publishLocalRefPath();
	}
}

求解时间和反馈信息

1
2
3
4
5
6
7
te_ = acado_toc(&t);
if (lmpcc_config_->activate_timing_output_)
	ROS_INFO_STREAM("Solve time " << te_ * 1e6 << " us");

if (lmpcc_config_->activate_feedback_message_){
	publishFeedback(j,te_);
}

如果没有使能输出或者超过kkt_tolerance_限制，输出零速度指令，否则发布控制速度

1
2
3
4
5
6
if(!enable_output_ || acado_getKKT() > lmpcc_config_->kkt_tolerance_) {
	publishZeroJointVelocity();
}
else {
	controlled_velocity_pub_.publish(controlled_velocity_);
}

6. 成员函数：LMPCC::publishPredictedTrajectory()

1
2
3
4
5
6
7
8
9
10
11
12
void LMPCC::publishPredictedTrajectory(void)
{
    //ROS_INFO("LMPCC::publishPredictedTrajectory");
    for (int i = 0; i < ACADO_N; i++)
    {
        pred_traj_.poses[i].pose.position.x = acadoVariables.x[i * ACADO_NX + 0]; //x
        pred_traj_.poses[i].pose.position.y = acadoVariables.x[i * ACADO_NX + 1]; //y
		pred_traj_.poses[i].pose.orientation.z = acadoVariables.x[i * ACADO_NX + 2]; //theta
    }

	pred_traj_pub_.publish(pred_traj_);
}

7. 成员函数：LMPCC::reconfigureCallback()

1
void LMPCC::reconfigureCallback(lmpcc::LmpccConfig& config, uint32_t level){}

首先是变量的初始化

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
cost_contour_weight_factors_(0) = config.Wcontour;
cost_contour_weight_factors_(1) = config.Wlag;
cost_control_weight_factors_(0) = config.Kv;
cost_control_weight_factors_(1) = config.Kw;

slack_weight_= config.Ws;
repulsive_weight_ = config.WR;

reference_velocity_ = config.vRef;

enable_output_ = config.enable_output;
loop_mode_ = config.loop_mode;
n_iterations_ = config.n_iterations;

//Search window parameters
window_size_ = config.window_size;
n_search_points_ = config.n_search_points;
plan_ = config.plan;

如果plan_为真

1
if(plan_){}

初始化求解器

1
2
3
4
5
/** Initialize constant Online Data Variables **/
reset_solver();
/** Set task flags and counters **/
segment_counter = 0;
goal_reached_ = false;

初始化局部参考路径

1
referencePath.InitLocalRefPath(lmpcc_config_->n_local_,lmpcc_config_->n_poly_per_clothoid_,ss,xx,yy,vv);

打印输出

1
2
3
4
5
6
7
if (lmpcc_config_->activate_debug_output_)
	referencePath.PrintLocalPath(ss,xx,yy);     // Print local reference path
if (lmpcc_config_->activate_visualization_)
{
	publishLocalRefPath();                      // Publish local reference path for visualization
	publishGlobalPlan();                        // Publish global reference path for visualization
}

重置plan_

1
config.plan = false;

2.4.3 lmpcc_configuration.cpp

1. 成员函数：LMPCC_configuration::initialize()

该函数主要用于处理lmpcc_config_parameter.yaml中的参数

对于没有默认值的参数，按照如下方式处理

1
2
3
4
5
if (!nh.getParam ("simulation_mode", simulation_mode_) )
{
	ROS_WARN(" Parameter 'simulation_mode' not set on %s node " , ros::this_node::getName().c_str());
	return false;
}

对于有默认值的参数，按照如下方式处理

1
2
/** Debug modes **/
nh.param("activate_output", activate_output_, bool(true));

2.4.4 reference_path.cpp

1. 成员函数：ReferencePath::SetGlobalPath

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
void ReferencePath::SetGlobalPath(std::vector<double> X, std::vector<double> Y, std::vector<double> Theta)
{
    // Check if all reference vectors are of the same length
    if (!( (X.size() == Y.size()) && (X.size() == Theta.size()) && (Y.size() == Theta.size()) ) )
    {
        ROS_ERROR("Reference path inputs should be of equal length");
    }

    // Resize global path vectors
    X_global.resize(X.size());
    Y_global.resize(Y.size());
    Theta_global.resize(Theta.size());

    // Set global path
    X_global = X;
    Y_global = Y;
    Theta_global = Theta;

    globalPathSet = true;

    ROS_INFO_STREAM("Initialized global path of length " << X_global.size());
}

2. 成员函数：ReferencePath::InitLocalRefPath

1
void ReferencePath::InitLocalRefPath(int N_local, int N_seg_per_cloth, std::vector<double> &s_local, std::vector<double> &x_local, std::vector<double> &y_local, std::vector<double> &v_local){}

3. 成员函数：ReferencePath::UpdateLocalRefPath

1
void ReferencePath::UpdateLocalRefPath(int traj_i, std::vector<double> &s_local, std::vector<double> &x_local, std::vector<double> &y_local, std::vector<double> &v_local){}

4. 成员函数：ReferencePath::ClosestPointOnPath

1
double ReferencePath::ClosestPointOnPath(Eigen::Vector4d current_state, double s_min, double s_max, double s_guess, double window, int n_tries){}