rrt_exploration代码解析(二)—— local_rrt_detector.cpp

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

  • 1
    点赞
  • 4
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 0
    评论
rrt_exploration是一种基于快速随机树(Rapidly-exploring Random Trees,简称RRT)算法的探索路径规划代码RRT算法是一种高效的无人机航迹规划算法,常用于探索未知环境。 rrt_exploration代码的主要功能是在未知环境中生成一条接近最优的路径,使得无人机可以探索整个环境。代码的实现包含以下几个步骤: 1. 初始化:代码首先会获取环境地图和无人机初始位置信息。同时,会设置RRT算法的参数,如树的最大生长步长、最大迭代次数等。 2. 构建树:代码会根据无人机的初始位置作为树的起点,开始构建随机树。在每次迭代中,代码会生成一个随机点,并将该点与树上的最近节点进行连接,形成一条新的树枝。 3. 节点选择:代码会根据一定的策略,选择合适的节点进行生长。常用的策略有最近邻节点和最优路径代价节点选择。 4. 碰撞检测:在每次生成新节点时,代码会进行碰撞检测,确保新生成的路径不会与障碍物相交。 5. 收敛判断:当生成的树趋近于目标位置时,代码会判断是否达到收敛条件。如果达到收敛条件,则会停止迭代,树构建完成。 6. 最优路径回溯:在收敛时,代码会回溯树,找到从起点到目标位置的最优路径。最优路径一般是根据路径长度、代价函数等标准来评估的。 总的来说,rrt_exploration代码通过RRT算法在未知环境中生成一条最优路径,用于无人机的探索任务。通过树的构建和回溯,代码能够快速生成一条安全且高效的路径规划结果。
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

libenfan

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值