源码学习：amr-lmpcc_05

本文最后更新于：2020年7月3日 上午

package: static_collision_avoidance

5.0 代办事项

StaticEnvironment::computeConstraint()分析

5.1 launch文件

1
2
3
4
\amr-lmpcc\static_collision_avoidance\launch
└── static_collision_avoidance.launch

file: 1

文件的开始是一些调试参数的设置

1
2
3
4
5
6
7
8
<!-- 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"/>
<arg name="config" default="$(find mobile_robot_state_publisher)"/>

然后是两个param参数

1
2
<param name="collision_avoidance/local_map" value="true" type="bool" />
<param name="simulation_mode" value="false" type="bool" />

为了可视化启动的costmap_2d_markers节点

1
2
3
4
<!-- Publishes the voxel grid to rviz for display -->
<node pkg="costmap_2d" type="costmap_2d_markers" name="voxel_visualizer">
	<remap from="voxel_grid" to="costmap/voxel_grid"/>
</node>

代价地图节点costmap_2d_node

1
2
3
4
<!-- Run the costmap node -->
<node name="costmap_node" pkg="costmap_2d" type="costmap_2d_node" >
	<rosparam file="$(find static_collision_avoidance)/config/local_map_params.yaml" command="load" ns="costmap" />
</node>

处理静态障碍物的static_collision_avoidance_node节点

1
2
3
<rosparam file="$(find static_collision_avoidance)/config/static_avoidance_config.yaml" command="load"/>

<node pkg="static_collision_avoidance" type="static_collision_avoidance_node" name="static_collision_avoidance_node" cwd="node" respawn="false" output="screen"/>

5.2 yaml文件

5.2.1 local_map_params.yaml

该文件主要包含地图，雷达等相关信息

5.2.2 static_avoidance_config.yaml

该文件主要包含机器人的描述信息，发布和订阅的话题名，避障的相关信息

5.3 头文件

1
2
3
4
\amr-lmpcc\static_collision_avoidance\include\static_collision_avoidance
└── static_environment.h

file: 1

在头文件中只定义了一个类StaticEnvironment

5.4 源文件

1
2
3
4
5
\amr-lmpcc\static_collision_avoidance\src
├── static_collision_avoidance_node.cpp
└── static_environment.cpp

file: 2

5.4.1 static_collision_avoidance_node.cpp

在该函数中定义着main函数

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

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

创建类StaticEnvironment的实例化对象node_

1
StaticEnvironment node_;

调用成员函数node_.initialize()进行初始化

1
2
3
4
5
6
7
8
9
10
11
12
// initialize predictive control node
if (!node_.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();
}

5.4.2 static_environment.cpp

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

订阅的话题

  • costmap_node/costmap/costmap：包costmap_2d发布
  • costmap_node/costmap/costmap_updates：包costmap_2d发布
  • predicted_trajectory：包lmpcc发布

发布的话题

  • collision_free_area：可视化工具rviz使用
  • collision_constraints：包lmpcc使用
  • my_map
1
2
3
4
5
6
7
local_map_sub_ = nh_.subscribe("costmap_node/costmap/costmap", 1, &StaticEnvironment::LocalMapCallBack, this);
local_map_updates_sub_ = nh_.subscribe("costmap_node/costmap/costmap_updates", 1, &StaticEnvironment::LocalMapUpdatesCallBack, this);
pred_traj_sub_ = nh_.subscribe("predicted_trajectory", 1, &StaticEnvironment::PredictedTrajectoryCallback, this);

collision_free_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("collision_free_area",1);
collision_constraint_pub_ = nh_.advertise<static_collision_avoidance::collision_free_polygon>("collision_constraints",1);
local_map_pub_ = nh_.advertise<nav_msgs::OccupancyGrid>("my_map",1);

启动的服务

1
2
/** Services **/
map_service_ = nh_.serviceClient<nav_msgs::GetMap>("static_map");

加载参数，这里仅展示部分

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

可视化设置

1
2
3
4
5
6
7
8
9
10
cube1.type = visualization_msgs::Marker::CUBE;
cube1.id = 60;
cube1.color.r = 0.5;
cube1.color.g = 0.5;
cube1.color.b = 0.0;
cube1.color.a = 0.1;
cube1.header.frame_id = planning_frame_;
cube1.ns = "trajectory";
cube1.action = visualization_msgs::Marker::ADD;
cube1.lifetime = ros::Duration(1);

无障碍区域初始化，注意到这里初始化的大小为prediction_steps_（25）

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

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

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

获取global_map_地图信息，该服务由map_server提供

1
2
3
4
5
6
/** Request static environment map **/
if (map_service_.call(map_srv_))
{
	ROS_ERROR("Service GetMap succeeded.");
	global_map_ = map_srv_.response.map;
}

2. 成员函数：StaticEnvironment::LocalMapCallBack()

1
2
3
4
5
6
7
8
9
void StaticEnvironment::LocalMapCallBack(const nav_msgs::OccupancyGrid local_map)
{
    if (use_local_map_ ) {
        //ROS_WARN("StaticEnvironment::LocalMapCallBack");
        local_map_ = local_map;
        local_map_pub_.publish(local_map_);
        ROS_INFO("local map update received!");
    }
}

3. 成员函数：StaticEnvironment::LocalMapUpdatesCallBack()

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
void StaticEnvironment::LocalMapUpdatesCallBack(const map_msgs::OccupancyGridUpdate local_map_update)
{
	if (use_local_map_ ) {
		ROS_ERROR("StaticEnvironment::LocalMapUpdatesCallBack");
		int index = 0;
		geometry_msgs::Pose obs;
		for (int y = local_map_update.y; y < local_map_update.y + local_map_update.height; y++) {
			for (int x = local_map_update.x; x < local_map_update.x + local_map_update.width; x++) {
				local_map_.data[local_map_.info.width * y + x] = local_map_update.data[index++];
			}
		}
    	local_map_pub_.publish(local_map_);    
    }
//    ROS_INFO("local map update received!");
}

4. （重要）成员函数：StaticEnvironment::PredictedTrajectoryCallback()

1
void StaticEnvironment::PredictedTrajectoryCallback(const nav_msgs::Path msg){}

首先是传入参数的类型nav_msgs::Path

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
geometry_msgs/PoseStamped[] poses
  std_msgs/Header header
    uint32 seq
    time stamp
    string frame_id
  geometry_msgs/Pose pose
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w

并且结合LMPCC::publishPredictedTrajectory注意到传入参数中有ACADO_N个（15个）位置坐标，所含信息如下

1
2
3
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

函数的开始是局部变量的定义和参数的初始化

1
2
static_collision_avoidance::collision_free_polygon constraint_msg;
pred_traj_ = msg;

然后调用了成员函数StaticEnvironment::ComputeCollisionFreeArea()

1
ComputeCollisionFreeArea();

参数的传递和发布（这里仅展示部分）

1
2
3
4
5
6
constraint_msg.collision_free_a1x = collision_free_a1x;
constraint_msg.collision_free_a1y = collision_free_a1y;
constraint_msg.collision_free_a2x = collision_free_a2x;
constraint_msg.collision_free_a2y = collision_free_a2y;
/* ...... */
collision_constraint_pub_.publish(constraint_msg);

可视化

1
publishPosConstraint();

5. （重要）成员函数：StaticEnvironment::ComputeCollisionFreeArea()

1
void StaticEnvironment::ComputeCollisionFreeArea(){}

首先获取当前的时间

1
auto start = std::chrono::steady_clock::now();

局部变量的定义

1
2
3
4
5
int x_path_i, y_path_i;		//index
double x_path, y_path, psi_path, theta_search, r;	//r没有被使用
std::vector<double> C_N;	//C_N 没有被使用

int search_steps = 10;

地图的选择

1
2
3
4
5
6
if (use_local_map_ ){
	static_map_ = local_map_;
}
else{
	static_map_ = global_map_;
}

初始角度（collisionfree_delta_min = 2），但是没有被使用过

1
collision_free_delta_min_ = collision_free_delta_max_;

计算预测路径上的每个位置坐标处的限制条件

❗❗❗注意此处prediction_steps_为25，但是根据上面的分析，预测的路径中只有15个位置坐标。

1
2
3
// Iterate over points in prediction horizon to search for collision free circles
for (int i = 0; i < prediction_steps_; i++){
}

首先获取一个坐标位置

1
2
3
4
// Current search point of prediction horizon
x_path = pred_traj_.poses[i].pose.position.x;
y_path = pred_traj_.poses[i].pose.position.y;
psi_path = pred_traj_.poses[i].pose.orientation.z;

计算这个坐标 x 方向和 y 方向相对于原点的index

1
2
3
// Find corresponding index of the point in the occupancy grid map
x_path_i = (int) round((x_path - static_map_.info.origin.position.x)/static_map_.info.resolution);
y_path_i = (int) round((y_path - static_map_.info.origin.position.y)/static_map_.info.resolution);

调用成员函数computeConstraint()计算每个位置坐标的约束条件

1
2
// Compute the constraint
computeConstraint(x_path_i,y_path_i,x_path, y_path, psi_path, i);

获取结束时间，并计算时间间隔

1
2
3
auto end = std::chrono::steady_clock::now();

te_collision_free_ = double(std::chrono::duration_cast <std::chrono::milliseconds> (end-start).count());

输出提示信息

1
2
if (activate_timing_output_)
	ROS_INFO_STREAM("Free space solve time " << te_collision_free_  << " ms");

6. （重要）成员函数：StaticEnvironment::computeConstraint()

1
void StaticEnvironment::computeConstraint(int x_i, int y_i, double x_path, double y_path, double psi_path, int N){}

首先进行局部变量的定义和初始化

1
2
3
4
5
6
7
// Initialize linear constraint normal vectors
std::vector<double> t1(2, 0), t2(2, 0), t3(2, 0), t4(2, 0);

// Declare search iterators
int x_min, x_max, y_min, y_max;
int search_x, search_y;
int r_max_i_min, r_max_i_max;

计算搜索范围

1
2
3
4
5
6
7
8
9
// define maximum search distance in occupancy grid cells, based on discretization
r_max_i_min = (int) round(-collision_free_delta_max_ /static_map_.info.resolution);		//-40
r_max_i_max = (int) round(collision_free_delta_max_/static_map_.info.resolution);		//40

// Initialize found rectangle values with maxium search distance
x_min = r_max_i_min;
x_max = r_max_i_max;
y_min = r_max_i_min;
y_max = r_max_i_max;

初始化搜索的距离，注意这里的search_distance = 2可以理解为栅格地图上的两个格，而不是两米。

1
2
3
4
// Initialize search distance iterator
int search_distance = 2;
// Initialize boolean that indicates whether the region has been found
bool search_region = true;

分别在x的正方向和负方向，y的正方向和负方向上进行搜索，最终获得四个边界值x_min,x_max,y_min,y_max，四个方向的处理方式一样，这里仅展示一个。

在第一轮循环中

  • if语句对应x_min是否改变

  • for语句对应y方向上下2格的地方

  • 如果遇到了障碍物，x_min即为2，此时xmin的值发生改变，不会再进入if语句
  • 否则，递增search_distance，判断是否满足结束条件
  • 不满足的话，会再次进入if语句

注意以下程序是在一个while循环中

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
// Only search in x_min direction if no value has been found yet
if (x_min == r_max_i_min)
{
	search_x = -search_distance;
	for (int search_y_it = std::max(-search_distance,y_min); search_y_it < std::min(search_distance,y_max); search_y_it++)
	{
		// Assign value if occupied cell is found
		if (getRotatedOccupancy(x_i, search_x, y_i, search_y_it, psi_path) > occupied_threshold_)
		{
			x_min = search_x;
		}
	}
} //else {ROS_INFO_STREAM("Already found x_min = " << x_min);}

// Increase search distance
search_distance++;
// Determine whether the search is finished
search_region = (search_distance < r_max_i_max) && ( x_min == r_max_i_min || x_max == r_max_i_max || y_min == r_max_i_min || y_max == r_max_i_max );

❗❗❗重要

将边界值转换为地图上的坐标，猜测此处的0.35为用一个圆表示机器人时的机器人半径（计算值为：0.32），如果猜测正确，那么该值应该设置为变量。

1
2
3
4
5
// Assign the rectangle values
collision_free_xmin[N] = x_min*static_map_.info.resolution + 0.35;
collision_free_xmax[N] = x_max*static_map_.info.resolution - 0.35;
collision_free_ymin[N] = y_min*static_map_.info.resolution + 0.35;
collision_free_ymax[N] = y_max*static_map_.info.resolution - 0.35;

为了直观一些，我们可以假设x_path = 0，y_path = 0，psi_path = 30°进行分析，则以下代码为四个点的坐标

1
2
3
4
5
6
7
8
9
10
11
std::vector<double> sqx(4,0), sqy(4,0);

sqx[0] = x_path + cos(psi_path)*collision_free_xmin[N] - sin(psi_path)*collision_free_ymin[N];
sqx[1] = x_path + cos(psi_path)*collision_free_xmin[N] - sin(psi_path)*collision_free_ymax[N];
sqx[2] = x_path + cos(psi_path)*collision_free_xmax[N] - sin(psi_path)*collision_free_ymax[N];
sqx[3] = x_path + cos(psi_path)*collision_free_xmax[N] - sin(psi_path)*collision_free_ymin[N];

sqy[0] = y_path + sin(psi_path)*collision_free_xmin[N] + cos(psi_path)*collision_free_ymin[N];
sqy[1] = y_path + sin(psi_path)*collision_free_xmin[N] + cos(psi_path)*collision_free_ymax[N];
sqy[2] = y_path + sin(psi_path)*collision_free_xmax[N] + cos(psi_path)*collision_free_ymax[N];
sqy[3] = y_path + sin(psi_path)*collision_free_xmax[N] + cos(psi_path)*collision_free_ymin[N];

将坐标进行标准化？

1
2
3
4
5
6
7
8
9
t1[0] = (sqx[1] - sqx[0])/sqrt((sqx[1] - sqx[0])*(sqx[1] - sqx[0]) + (sqy[1] - sqy[0])*(sqy[1] - sqy[0]));
t2[0] = (sqx[2] - sqx[1])/sqrt((sqx[2] - sqx[1])*(sqx[2] - sqx[1]) + (sqy[2] - sqy[1])*(sqy[2] - sqy[1]));
t3[0] = (sqx[3] - sqx[2])/sqrt((sqx[3] - sqx[2])*(sqx[3] - sqx[2]) + (sqy[3] - sqy[2])*(sqy[3] - sqy[2]));
t4[0] = (sqx[0] - sqx[3])/sqrt((sqx[0] - sqx[3])*(sqx[0] - sqx[3]) + (sqy[0] - sqy[3])*(sqy[0] - sqy[3]));

t1[1] = (sqy[1] - sqy[0])/sqrt((sqx[1] - sqx[0])*(sqx[1] - sqx[0]) + (sqy[1] - sqy[0])*(sqy[1] - sqy[0]));
t2[1] = (sqy[2] - sqy[1])/sqrt((sqx[2] - sqx[1])*(sqx[2] - sqx[1]) + (sqy[2] - sqy[1])*(sqy[2] - sqy[1]));
t3[1] = (sqy[3] - sqy[2])/sqrt((sqx[3] - sqx[2])*(sqx[3] - sqx[2]) + (sqy[3] - sqy[2])*(sqy[3] - sqy[2]));
t4[1] = (sqy[0] - sqy[3])/sqrt((sqx[0] - sqx[3])*(sqx[0] - sqx[3]) + (sqy[0] - sqy[3])*(sqy[0] - sqy[3]));

结果如下图所示：

得到约束条件

1
2
3
4
5
6
7
8
9
10
11
12
13
14
collision_free_a1x[N] = t1[1];
collision_free_a2x[N] = t2[1];
collision_free_a3x[N] = t3[1];
collision_free_a4x[N] = t4[1];

collision_free_a1y[N] = -t1[0];
collision_free_a2y[N] = -t2[0];
collision_free_a3y[N] = -t3[0];
collision_free_a4y[N] = -t4[0];

collision_free_C1[N] = sqx[0]*collision_free_a1x[N] + sqy[0]*collision_free_a1y[N];
collision_free_C2[N] = sqx[1]*collision_free_a2x[N] + sqy[1]*collision_free_a2y[N];
collision_free_C3[N] = sqx[2]*collision_free_a3x[N] + sqy[2]*collision_free_a3y[N];
collision_free_C4[N] = sqx[3]*collision_free_a4x[N] + sqy[3]*collision_free_a4y[N];

7. 成员函数：StaticEnvironment::getRotatedOccupancy()

1
2
3
4
5
6
7
8
9
10
11
12
13
int StaticEnvironment::getRotatedOccupancy(int x_i, int search_x, int y_i, int search_y, double psi) {
    //ROS_INFO_STREAM("getRotatedOccupancy");
    int x_search_rotated = (int) round(cos(psi) * search_x - sin(psi) * search_y);
    int y_search_rotated = (int) round(sin(psi) * search_x + cos(psi) * search_y);

    if ((x_i + x_search_rotated) > static_map_.info.width || (y_i + y_search_rotated) > static_map_.info.height ||
        (x_i + x_search_rotated) < 0 || (y_i + y_search_rotated) < 0) {
        return (int) 100;
    }
    else {
        return static_map_.data[static_map_.info.width * (y_i + y_search_rotated) + (x_i + x_search_rotated)];
    }
}
编程
MPC ROS 源码学习

本博客所有文章除特别声明外，均采用 CC BY-SA 3.0协议 。转载请注明出处！