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);