rrt_exploration代码解析(一)—— global_rrt_detector.cpp
rrt_exploration代码解析(二)—— local_rrt_detector.cpp
rrt_exploration代码解析(三)—— filter.py
rrt_exploration代码解析(四)—— assigner.py
第二个核心文件是local_rrt_detector.cpp,是rrt_exploration的局部探测器。与全局检测器类似,局部探测器的作用也是在于找到地图上未知区域的边界点,两个探测器最主要的区别在于,局部探测器的根节点会随着无人车的移动发生改变。下面我们来详细看看源码。
local_rrt_detector.cpp
前半部分功能与全局探测器类似,大家可以参考上一篇文章。
std::vector<float> temp1;
temp1.push_back(points.points[0].x);
temp1.push_back(points.points[0].y);
std::vector<float> temp2;
temp2.push_back(points.points[2].x);
temp2.push_back(points.points[0].y);
init_map_x=Norm(temp1,temp2);
temp1.clear(); temp2.clear();
temp1.push_back(points.points[0].x);
temp1.push_back(points.points[0].y);
temp2.push_back(points.points[0].x);
temp2.push_back(points.points[2].y);
init_map_y=Norm(temp1,temp2);
temp1.clear(); temp2.clear();
Xstartx=(points.points[0].x+points.points[2].x)*.5;
Xstarty=(points.points[0].y+points.points[2].y)*.5;
geometry_msgs::Point trans;
trans=points.points[4];
std::vector< std::vector<float> > V;
std::vector<float> xnew;
xnew.push_back( trans.x);xnew.push_back( trans.y);
V.push_back(xnew);
points.points.clear();
pub.publish(points) ;
std::vector<float> frontiers;
int i=0;
float xr,yr;
std::vector<float> x_rand,x_nearest,x_new;
tf::TransformListener listener;
// Main loop
while (ros::ok()){
// Sample free
x_rand.clear();
xr=(drand()*init_map_x)-(init_map_x*0.5)+Xstartx;
yr=(drand()*init_map_y)-(init_map_y*0.5)+Xstarty;
x_rand.push_back( xr ); x_rand.push_back( yr );
// Nearest
x_nearest=Nearest(V,x_rand);
// Steer
x_new=Steer(x_nearest,x_rand,eta);
// ObstacleFree 1:free -1:unkown (frontier region) 0:obstacle
char checking=ObstacleFree(x_nearest,x_new,mapData);
区别在于检测到路径上存在未知区域时,局部探测器将新的节点认为是边界点,并通过"/detected_points"话题将边界点数据exploration_goal发送出去,同时通过"local_rrt_frontier_detector_shapes"话题将边界点发送到rviz用于可视化。发送完成后,局部规划器会清空有效点集V,将无人车位置设置为新的根节点,在下次生长时由该根节点开始。清空line保存的点的数据,搜索树从新的根节点开始生长。
if (checking==-1){
exploration_goal.header.stamp=ros::Time(0);
exploration_goal.header.frame_id=mapData.header.frame_id;
exploration_goal.point.x=x_new[0];
exploration_goal.point.y=x_new[1];
exploration_goal.point.z=0.0;
p.x=x_new[0];
p.y=x_new[1];
p.z=0.0;
points.points.push_back(p);
pub.publish(points) ;
targetspub.publish(exploration_goal);
// 清空点集
points.points.clear();
V.clear();
tf::StampedTransform transform;
int temp=0;
while (temp==0){
try{
temp=1;
listener.lookupTransform(map_topic, base_frame_topic , ros::Time(0), transform);
}
catch (tf::TransformException ex){
temp=0;
ros::Duration(0.1).sleep();
}}
x_new[0]=transform.getOrigin().x();
x_new[1]=transform.getOrigin().y();
V.push_back(x_new);
line.points.clear();
}
若连线上没有障碍物或未知区域,则将得到的新节点和近邻点都保存在line中。
else if (checking==1){
V.push_back(x_new);
p.x=x_new[0];
p.y=x_new[1];
p.z=0.0;
line.points.push_back(p);
p.x=x_nearest[0];
p.y=x_nearest[1];
p.z=0.0;
line.points.push_back(p);
}
每次循环都会将line上的数据通过"local_rrt_frontier_detector_shapes"话题发送到rviz上显示。
pub.publish(line);