【⾃主探索】CMUAutonomousExploration系列笔记
链接:
CMU机器⼈研究所于2021年7⽉开源全套移动机器⼈⾃主导航和探索框架，其主要算法都出⾃近两年CMU发出的顶会论⽂.该框架主要分为⾃主探索环境和探索planner两部分:
1.⾃主探索开发环境
github截图如下：探索环境部分主要包括移动双轮差速机器⼈模拟器，传感器模拟器，局部路径规划器，可视化⼯具，路点发布example，路点发布rviz插件，地形分析程序，扩展地形分析程序等.
⼏个模块介绍如下:
vehicle_simulator 该ros包主要功能是实现了⼀个双轮差动底盘的模拟，接收cmd_vel速度信息,⾃⼰⼿写的机器⼈运动学微分⽅程来对机器⼈位姿进⾏推算，从⽽输出⾥程计odometry,⽽gazebo只是⽤来做模型显⽰.该包中包含了CMU制作的⼏个探索gazebo world，分别包含了不同的环境类型.
velodyne_simulator velodyne激光雷达模拟器,velodyne开源gazebo插件
local_planner 局部路径规划planner，输⼊机器⼈⾥程计和⽬标路点，输出控制指令cmd_vel.⽤于局部路径规划导航，其中包括了localPlanner和pathFoller两部分,localplanner作⽤是规划局部路径,pathFollower作⽤是根据规划的局部轨迹进⾏轨迹跟踪，核⼼思想和pid差不多.
loam_interface loam的接⼝，作⽤是将loam输出的⾥程计进⾏坐标系转换，同时将单帧点云装换到全局坐标系map下⾯输⼊给localPlanner.
terrain_analysis 地形分析，叠加多帧点云，将机器⼈周围区域的地⾯进⾏可⾏驶区域分析，不能通⾏的区域会以点云的形式传输给localPlanner,⽤于localPlanner避障.
terrain_analysis_ext扩展地形分析，从rviz结果来看是对更⼤区域的地形进⾏了可⾏驶区域进⾏分析，具体代码内容还没有看. waypoint_example路点发布example程序，将设置好的路点写到data⽬录下
⾯，运⾏就能安装顺序发布路点，从⽽进⾏逐个路点导航. waypoint_rviz_plugin waypoint发布的rviz插件，运⾏此插件后,rviz就能通过waypoint按钮⿏标点击发布路点，直接进⾏导航.
visualization_tools可视化⼯具，将探索过程中的三个实验指标曲线进⾏可视化，通过matplotlib绘图显⽰出来，包括探索体积 探索路程每次规划计算时间.
运⾏⼀个vehiclesimulator 的launch⽂件后
roslaunch vehicle_simulator system_garage.launch
rqt_graph显⽰如上图,⼏个模块中最核⼼的是localPlanner模块，实现了⾃主导航避障的功能.其代码出⾃cmu zhangji2019 IROS 和2020JFR的论⽂.
J. Zhang, C. Hu, R. Gupta Chadha, and S. Singh. Falco: Fast Likelihood-bad Collision Avoidance with Extension to Human-guided Navigation. Journal of Field Robotics. vol. 37, no. 8, pp. 1300–1313, 2020. []
2.tare planner ⾃主探索算法
论⽂解析参考博客
(翻译的的真清楚)
代码模块:
grid_world
keypo_graph
lidar_model雷达模型，应该是⽤来计算viewpoint的回报的时候⽤的.
local_coverage_planner
planning_env
pointcloud_manager
rolling_grid
rolling_occupancy_grid
nsor_coverage_planner SensorCoveragePlanner3D类函数的实现
tare_planner_node Tare planner 主程序⼊⼝
tare_visualizer发布⼀些可视化信息给rviz
tsp_solver实现了tsp求解器的封装，在tsp规划的时候调⽤.
utils杂项功能函数
viewpoint
viewpoint_manager
exploration_path ExplorationPath结构体以及成员函数的实现,⽤于存储探索路径信息
(这个表可能要慢慢完善了)
部分代码理解
代码运⾏⼊⼝在nsor_coverage_planner_ground.cpp 这个cpp⽂件实现了 SensorCoveragePlanner3D类的成员函数.
该类构造函数实现了各个成员的初始化，同时在初始化函数
bool SensorCoveragePlanner3D::initialize(ros::NodeHandle& nh, ros::NodeHandle& nh_p)
开启了⼀个定时器，见代码:
execution_timer_ = nh.createTimer(ros::Duration(1.0), &SensorCoveragePlanner3D::execute, this); //每秒执⾏⼀次execute函数
该定时器的回调函数execute才是全部代码的核⼼循环执⾏部分:
有趣的发现作文
void SensorCoveragePlanner3D::execute(const ros::TimerEvent&)
{
static int a=0;
std::string counter_str="execute count:"+std::to_string(a++);
讲不完的故事// PrintExplorationStatus(counter_str,true);
if (!pp_.kAutoStart && !start_exploration_)
尼莫地平片{
ROS_INFO("Waiting for start signal");
return;
}
Timer overall_processing_timer("overall processing"); //这⾥的timer是⾃⼰写的，⽤来计时但是没有回调函数，与ros的timer不同
update_reprentation_runtime_ = 0;
local_viewpoint_sampling_runtime_ = 0;
local_path_finding_runtime_ = 0;
global_planning_runtime_ = 0;
trajectory_optimization_runtime_ = 0;
overall_runtime_ = 0;
if (!initialized_)
{
SendInitialWaypoint();
start_time_ = ros::Time::now();
繁文缛节什么意思啊return;
化疗是什么意思
}
overall_processing_timer.Start();
if (keypo_cloud_update_)
{
keypo_cloud_update_ = fal;
/*----------------------------------------------更新环境的表⽰------------------------------------------------------------------------------------*/
misc_utils_ns::Timer update_reprentation_timer("update reprentation");
update_reprentation_timer.Start();
// Update grid world
UpdateGlobalReprentation(); //更新全局环境表⽰
int viewpoint_candidate_count = UpdateViewPoints(); //更新viewpoints
if (viewpoint_candidate_count == 0)
if (viewpoint_candidate_count == 0)
{
ROS_WARN("Cannot get candidate viewpoints, skipping this round");
return;
}
UpdateKeypoGraph(); //更新关键位姿图
int uncovered_point_num = 0;
int uncovered_frontier_point_num = 0;
if (!exploration_finished_)
{
UpdateViewPointCoverage(); //更新viewpoint的覆盖
UpdateCoveredAreas(uncovered_point_num, uncovered_frontier_point_num); //更新覆盖的⾯积
}
el
{
pd_.viewpoint_manager_->RetViewPointCoverage();
}
update_reprentation_timer.Stop(fal);
update_reprentation_runtime_ += update_reprentation_timer.GetDuration("ms");
/*--------------------------------------------更新环境表⽰结束----------------------------------------------------------------------------*/
// Global TSP ------------------全局tsp规划
std::vector<int> global_cell_tsp_order;
exploration_path_ns::ExplorationPath global_path;
GlobalPlanning(global_cell_tsp_order, global_path);
// Local TSP ---------------------局部TSP规划
exploration_path_ns::ExplorationPath local_path;
LocalPlanning(uncovered_point_num, uncovered_frontier_point_num, global_path, local_path);
near_home_ = GetRobotToHomeDistance() < pp_.kRushHomeDist;
at_home_ = GetRobotToHomeDistance() < pp_.kAtHomeDistThreshold;
if (pd_.grid_world_->IsReturningHome() && pd_.local_coverage_planner_->IsLocalCoverageComplete() &&
(ros::Time::now() - start_time_).toSec() > 5)
{
if (!exploration_finished_)
{
PrintExplorationStatus("Exploration completed, returning home", fal);
}
exploration_finished_ = true;
}
if (exploration_finished_ && at_home_ && !stopped_)
{
PrintExplorationStatus("Return home completed", fal);
stopped_ = true;
}
pd_.exploration_path_ = ConcatenateGlobalLocalPath(global_path, local_path); //连接全局和局部的路径
连消带打PublishExplorationState();
lookahead_point_update_ = GetLookAheadPoint(pd_.exploration_path_, global_path, pd_.lookahead_point_);
PublishWaypoint();
overall_processing_timer.Stop(fal);
overall_runtime_ = overall_processing_timer.GetDuration("ms");
游戏著作权pd_.visualizer_->GetGlobalSubspaceMarker(pd_.grid_world_, global_cell_tsp_order); //下⾯是发布可视化信息到rviz进⾏显⽰ Eigen::Vector3d viewpoint_origin = pd_.viewpoint_manager_->GetOrigin();
pd_.visualizer_->GetLocalPlanningHorizonMarker(viewpoint_origin.x(), viewpoint_origin.y(), pd_.robot_position_.z);
经纬仪pd_.visualizer_->PublishMarkers();
PublishLocalPlanningVisualization(local_path);//局部规划可视化
PublishGlobalPlanningVisualization(global_path, local_path);//全局规划可视化
PublishRuntime();
}
// return true;
}
先写到这，等下⼀步更新viewpoint采样和viewpoint计算⽅式.
