代码解析：DSVP: Dual-Stage Viewpoint Planner for Rapid Exploration by Dynamic Expansion

微鉴道长

分类：建图导航

发布时间 2022.09.20阅读数 3103 评论数 0

内容列表

写在前面
论文解析
代码注释

写在前面

这篇论文是CMU发表在IROS2021上的工作，并随着其整套开源工作一起发布出来。

作者是哈工大的Hongbiao Zhu博士，主页http://www.hongbiaoz.com/

论文地址：http://www.hongbiaoz.com/files/paper7.pdf

github地址：https://github.com/HongbiaoZ/dsv_planner

我因为看到了泡泡机器人公众号的首发【泡泡前沿专栏•重磅】全网首发，CMU - LOAM 团队讲述最新自研全套开源自主导航算法（一）自主系统

开始关注他们的整项工作，再加上高翔博士也开始参与宣传：

CMU团队开发的全套开源自主导航算法:cmu-exploration.com （二） - 半闲居士的文章 - 知乎

我对他们的工作产生了浓厚的兴趣。因此决定一读。

他们的这套工作，都是华人搞的，（通讯作者是LOAM的作者ZhangJi），总体分为三个工作：

第一个：TARE: A Hierarchical Framework for Efficiently Exploring Complex 3D Environments，这个是发表在RSS2021的工作，这篇文章获得了RSS的Best paper award和 Best system paper。能在RSS这种逼格的会议上发表还得了bestpaper，实在是强。

第二个：Exploring Large and Complex Environments Fast and Efficiently，这篇发表在ICRA2021上，其实和第一篇TARE基本思路上是差不多的工作，TARE应该算是这个的一些修修补补。

这两篇工作我之后可能会写一个博客介绍一下（毕竟原作者他们并没有写细节，那我就从一个学习者的角度学习一下），但是我看完了他们的这俩论文，感觉和我目前做的内容相关程度不是太大，从代码角度讲，对我来说没什么好复用的，所以优先级暂后。

我这里介绍的是这个第三篇工作：

就是本文DSVP: Dual-Stage Viewpoint Planner for Rapid Exploration by Dynamic Expansion，这篇发表在IROS2021上。

我们今年在IROS2021上也发表了一篇论文，但是我看了他们的这个系统的代码，说实话，我觉得比我们的代码写的要漂亮，所以决定仔细阅读一下他们的工作学习一下。（不过他们的代码风格我不太喜欢，大括号不换行，tab用两个空格，还有就是一行一句话明明没写完莫名其妙再来一个换行，读起来非常难受。所以我在写注释的时候都给换成我个人看着舒服的风格了。不知道他们这样写是个人写代码的习惯，还是CMU给他们配的电脑显示屏很小）

ps：这一系列工作的环境配置比较简单，我这里就不再提了。可见他们是真心在做开源，赞一个。提一嘴，有一些论文作者喜欢搞“假开源”，比如在投稿前宣称开源，中了以后当自己没说过；或者出于商业原因，开源一个只有自己会用，其他人谁也跑不起来的乱七八糟的版本，避免职务发明和知识产权的后续纠纷…… 这种很浪费读者的时间。

论文解析

光看完论文，有些地方理解的不知道对不对，所以这个先占个坑，等我看完代码以后再写进来。

代码注释

他们的仿真环境，对应的是另一个工作https://github.com/jizhang-cmu/ground_based_autonomy_basic。包括局部规划，地形重建，仿真模拟等等，这些工作我目前还没有细看，后续有空的话提及一下。

暂时本文只提这篇DSVP的对应代码。先看launch文件：
<launch>
<arg name="enable_bag_record" default="false"/>
<arg name="bag_name" default="forest"/>
<arg name="simulation" default="false"/>
<arg name="planner_param_file" default="$(find dsvp_launch)/config/exploration_forest.yaml" />
<arg name="octomap_param_file" default="$(find dsvp_launch)/config/octomap_forest.yaml" />

<rosparam command="load" file="$(arg planner_param_file)" />
<rosparam command="load" file="$(arg octomap_param_file)" />

<node name="dsvplanner" pkg="dsvplanner" type="dsvplanner" output="screen" />

<node name="exploration" pkg="dsvp_launch" type="exploration_with_graph_planner" output="screen" >
<param name="simulation" type="bool" value="$(arg simulation)" />
</node>

<include file="$(find graph_planner)/launch/graph_planner.launch"/>

<group if="$(arg enable_bag_record)">
<include file="$(find dsvp_launch)/launch/rosbag_record.launch">
<arg name="bag_name" value="$(arg bag_name)" />
</include>
</group>

<node pkg="rviz" type="rviz" name="dsvp_rviz" args="-d $(find dsvp_launch)/default.rviz"/>
</launch>

结合CmakeLists.txt可以看出，这里主要启动三个进程，一个是exploration_with_graph_planner.cpp，一个是graph_planner_node.cpp，一个是drrtp_node.cpp。

exploration_with_graph_planner.cpp

这三个进程中，exploration_with_graph_planner.cpp是一个总控的作用，向drrt部分调用服务，发送信号给graph_planner进程，由它负责对整体路径点的规划执行。这个进程由于是最外层的进程，内容比较高层，所以读起来基本比较简单。大致就是和RRT服务器drrtPlannerSrv进行交互，根据返回的数据，以及当前执行状态（是否探索完了处于最后回家的状态），发送一个/graph_planner_command 给graph_planner进程，让graph_planner负责实际的执行规划任务。

唯一有些奇怪的地方是在程序里有两个init_x，init_y变量，程序要把它们当成第一个waypoint，要在一开始发送出去，交给仿真部分对应的local_planner执行。也就是说最开始小车要二话不说向前走几步（init_x=4)，才会执行后续的探索任务。我尝试过把它赋值为0，发现程序会认为探索完成。这点存疑，因为涉及到服务器的应答，貌似如果小车开始如果在原地不动，那么服务器的返回值的planSrv.response.mode.data == 2，会认为探索完成。这个等我在之后看到对应位置的时候再补充为什么会这样，作者的目的是什么。暂时我确实不知道。

/*
exploration_with_graph_planner.cpp
the interface for drrt planner
Created and maintained by Hongbiao Zhu (hongbiaz@andrew.cmu.edu)
05/25/2020
*/

#include <chrono>
#include <math.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>

#include <geometry_msgs/PointStamped.h>
#include <nav_msgs/Odometry.h>
#include <ros/package.h>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Float32.h>
#include <std_srvs/Empty.h>

#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>

#include "dsvplanner/clean_frontier_srv.h"
#include "dsvplanner/dsvplanner_srv.h"
#include "graph_planner/GraphPlannerCommand.h"
#include "graph_planner/GraphPlannerStatus.h"

using namespace std::chrono;
#define cursup "\033[A"
#define cursclean "\033[2K"
#define curshome "\033[0;0H"

geometry_msgs::Point wayPoint;
geometry_msgs::Point wayPoint_pre;
graph_planner::GraphPlannerCommand graph_planner_command;
std_msgs::Float32 effective_time;
std_msgs::Float32 total_time;

bool simulation = false; // control whether use graph planner to follow path
bool begin_signal = false; // trigger the planner
bool gp_in_progress = false;
bool wp_state = false;
bool return_home = false;
double current_odom_x = 0;
double current_odom_y = 0;
double current_odom_z = 0;
double previous_odom_x = 0;
double previous_odom_y = 0;
double previous_odom_z = 0;
double dtime = 0.0;
double init_x = 2;
double init_y = 0;
double init_z = 2;
double return_home_threshold = 1.5;
double robot_moving_threshold = 6;
std::string map_frame = "map";

steady_clock::time_point plan_start;
steady_clock::time_point plan_over;
steady_clock::duration time_span;

void gp_status_callback(const graph_planner::GraphPlannerStatus::ConstPtr &msg) {
if (msg->status == graph_planner::GraphPlannerStatus::STATUS_IN_PROGRESS)
gp_in_progress = true;
else {
gp_in_progress = false;
}
}

void waypoint_callback(const geometry_msgs::PointStamped::ConstPtr &msg) {
wayPoint = msg->point;
wp_state = true;
}

void odom_callback(const nav_msgs::Odometry::ConstPtr &msg) {
current_odom_x = msg->pose.pose.position.x;
current_odom_y = msg->pose.pose.position.y;
current_odom_z = msg->pose.pose.position.z;
}

void begin_signal_callback(const std_msgs::Bool::ConstPtr &msg) {
begin_signal = msg->data;
}

bool robotPositionChange() {
double dist = sqrt(
(current_odom_x - previous_odom_x) * (current_odom_x - previous_odom_x) +
(current_odom_y - previous_odom_y) * (current_odom_y - previous_odom_y) +
(current_odom_z - previous_odom_z) * (current_odom_z - previous_odom_z));
if (dist < robot_moving_threshold)
return false;
previous_odom_x = current_odom_x;
previous_odom_y = current_odom_y;
previous_odom_z = current_odom_z;
return true;
}

void gotoxy(int x, int y) { printf("%c[%d;%df", 0x1B, y, x); }

int main(int argc, char **argv) {
ros::init(argc, argv, "exploration");
ros::NodeHandle nh;
ros::NodeHandle nhPrivate = ros::NodeHandle("~");
ros::Publisher waypoint_pub =nh.advertise<geometry_msgs::PointStamped>("/way_point", 5);
ros::Publisher gp_command_pub =nh.advertise<graph_planner::GraphPlannerCommand>("/graph_planner_command",1);
// ros::Publisher effective_plan_time_pub =
// nh.advertise<geometry_msgs::PointStamped>("/effective_plan_time", 1);
ros::Publisher effective_plan_time_pub =nh.advertise<std_msgs::Float32>("/runtime", 1);
ros::Publisher total_plan_time_pub =nh.advertise<std_msgs::Float32>("/totaltime", 1);
ros::Subscriber gp_status_sub =nh.subscribe<graph_planner::GraphPlannerStatus>("/graph_planner_status",1, gp_status_callback);
ros::Subscriber waypoint_sub = nh.subscribe<geometry_msgs::PointStamped>("/way_point", 1, waypoint_callback);
ros::Subscriber odom_sub =nh.subscribe<nav_msgs::Odometry>("/state_estimation", 1, odom_callback);
ros::Subscriber begin_signal_sub = nh.subscribe<std_msgs::Bool>("/start_exploring", 1, begin_signal_callback);
ros::Publisher stop_signal_pub =nh.advertise<std_msgs::Bool>("/stop_exploring", 1);

nhPrivate.getParam("simulation", simulation);
nhPrivate.getParam("/interface/dtime", dtime);
nhPrivate.getParam("/interface/initX", init_x);
nhPrivate.getParam("/interface/initY", init_y);
nhPrivate.getParam("/interface/initZ", init_z);
nhPrivate.getParam("/interface/returnHomeThres", return_home_threshold);
nhPrivate.getParam("/interface/robotMovingThres", robot_moving_threshold);
nhPrivate.getParam("/interface/tfFrame", map_frame);
nhPrivate.getParam("/interface/autoExp", begin_signal);

ros::Duration(1.0).sleep();
ros::spinOnce();
//默认配置文件中，写的是true，直接开始。如果配置文件里写的是false或者没写，可能需要通过外部发topic来决定这里什么时候开始
while (!begin_signal)
{
ros::Duration(0.5).sleep();
ros::spinOnce();
ROS_INFO("Waiting for Odometry");
}

ROS_INFO("Starting the planner: Performing initialization motion");
geometry_msgs::PointStamped wp;
wp.header.frame_id = map_frame;
wp.header.stamp = ros::Time::now();
wp.point.x = init_x + current_odom_x;
wp.point.y = init_y + current_odom_y;
wp.point.z = init_z + current_odom_z;

ros::Duration(0.5).sleep(); // wait for sometime to make sure waypoint can be published properly

waypoint_pub.publish(wp);
bool wp_ongoing = true;
//在这里程序一定要向前走几步，代码才能运行，否则会提示探索完成。
// 为什么会如此，暂时存疑
while (wp_ongoing)
// Keep publishing initial waypoint until the robot reaches that point
{
ros::Duration(0.1).sleep();
ros::spinOnce();
waypoint_pub.publish(wp);
double dist =sqrt((wp.point.x - current_odom_x) * (wp.point.x - current_odom_x) +(wp.point.y - current_odom_y) * (wp.point.y - current_odom_y));
if (dist < 0.5)
wp_ongoing = false;
}

ros::Duration(1.0).sleep();

std::cout << std::endl<< "\033[1;32mExploration Started\033[0m\n" << std::endl;
total_time.data = 0;
plan_start = steady_clock::now();
// Start planning: The planner is called and the computed goal point sent to
// the graph planner.
int iteration = 0;
while (ros::ok())
{
if (!return_home)
{
//下面这几个print没什么用，可以无视
//就是把历史的打印数据从终端显示给消点，起到一个光标上移的作用
if (iteration != 0)
{
for (int i = 0; i < 8; i++)
{
printf(cursup);
printf(cursclean);
}
}
std::cout << "Planning iteration " << iteration << std::endl;
dsvplanner::dsvplanner_srv planSrv;
dsvplanner::clean_frontier_srv cleanSrv;
planSrv.request.header.stamp = ros::Time::now();
planSrv.request.header.seq = iteration;
planSrv.request.header.frame_id = map_frame;
//可以看出这里调用了一个drrtPlannerSrv服务，服务器应答返回的goal的大小如果是0，就等待一下
if (ros::service::call("drrtPlannerSrv", planSrv))
{
if (planSrv.response.goal.size() ==0)
{ // usually the size should be 1 if planning successfully
ros::Duration(1.0).sleep();
continue;
}
//返回的应答的mode字段的data被赋值为2，则认为完成探索，进行返航
if (planSrv.response.mode.data == 2)
{
return_home = true;
std::cout << std::endl
<< "\033[1;32mExploration completed, returning home\033[0m"
<< std::endl
<< std::endl;
effective_time.data = 0;
effective_plan_time_pub.publish(effective_time);
}
//.response.mode.data != 2,认为还不应该返航,发布runtime
// mode包含的状态，1是属于探索阶段和relocation阶段，2就是都ok了返回出发点
//goal里面的内容，根据模式来决定怎么走, 是去探索当前边界,还是探索下一个边界，或者返航
else
{
return_home = false;
plan_over = steady_clock::now();
time_span = plan_over - plan_start;
effective_time.data = float(time_span.count()) *steady_clock::period::num /steady_clock::period::den;
effective_plan_time_pub.publish(effective_time);
}
//发布totaltime,实时更新，累加计数时间
total_time.data += effective_time.data;
total_plan_time_pub.publish(total_time);
// when not in simulation mode, the robot will go to the goal point according to graph planner
if (!simulation)
{
//从服务器返回依次读取目标，发布graph_planner的command
for (size_t i = 0; i < planSrv.response.goal.size(); i++)
{
graph_planner_command.command =graph_planner::GraphPlannerCommand::COMMAND_GO_TO_LOCATION;
graph_planner_command.location = planSrv.response.goal[i];
gp_command_pub.publish(graph_planner_command);
ros::Duration(dtime).sleep(); // give sometime to graph planner for
// searching path to goal point
ros::spinOnce(); // update gp_in_progree
int count = 200;
previous_odom_x = current_odom_x;
previous_odom_y = current_odom_y;
previous_odom_z = current_odom_z;
// if the waypoint keep the same for 20 (200*0.1)
while (gp_in_progress)
{
//这的意思有疑问，可能是在一个地方持续呆着，200开始计数，0.1s下降一个数，20秒内相对位置没超过10m,认为没动，那就清除这个边界点
ros::Duration(0.1).sleep(); // seconds, then give up the goal
wayPoint_pre = wayPoint;
ros::spinOnce();
bool robotMoving = robotPositionChange();
if (robotMoving)
{
count = 200;
}
else
{
count--;
}
//目标点不可达，就清除该边界点
if (count <= 0) //when the goal point cannot be reached, clean its correspoinding frontier if there is
{
cleanSrv.request.header.stamp = ros::Time::now();
cleanSrv.request.header.frame_id = map_frame;
ros::service::call("cleanFrontierSrv", cleanSrv);
ros::Duration(0.1).sleep();
break;
}
}
}
//目标点已经publish完了
graph_planner_command.command =graph_planner::GraphPlannerCommand::COMMAND_DISABLE;
gp_command_pub.publish(graph_planner_command);
}
//simulation 模式
// simulation mode is used when testing this planning algorithm with bagfiles where robot will not move to the planned goal.
// When in simulation mode, robot will keep replanning every two seconds
else
{
for (size_t i = 0; i < planSrv.response.goal.size(); i++)
{
graph_planner_command.command =graph_planner::GraphPlannerCommand::COMMAND_GO_TO_LOCATION;
graph_planner_command.location = planSrv.response.goal[i];
gp_command_pub.publish(graph_planner_command);
ros::Duration(2).sleep();
break;
}
}
plan_start = steady_clock::now();
}
//调用服务失败
else
{
std::cout << "Cannot call drrt planner." << std::flush;
ros::Duration(1.0).sleep();
}
iteration++;
}
//return_home=true,已经在返航了，并且当前位置和起点小于阈值1.5m，则输出已经到家的指令
else
{
ros::spinOnce();
if (fabs(current_odom_x) + fabs(current_odom_y) + fabs(current_odom_z) <=return_home_threshold)
{
printf(cursclean);
std::cout << "\033[1;32mReturn home completed\033[0m" << std::endl;
printf(cursup);

std_msgs::Bool stop_exploring;
//发送停止探索的指令
stop_exploring.data = true;
stop_signal_pub.publish(stop_exploring);
}
ros::Duration(0.1).sleep();
}
}
}

graph_planner.cpp

这个cpp是类graph_planner_ns的实现，根据graph_planner_node.cpp函数我们可以发现，进程里定义了一个类graph_planner_ns的对象，并且调用它的execute函数。

这个进程的作用是：

第一：监听exploration_with_graph_planner.cpp发来的command指令，具体指令分为三种，一种是graph_planner::GraphPlannerCommand::COMMAND_DISABLE，意思是指令取消，现在啥也不干。一种是graph_planner::GraphPlannerCommand::COMMAND_GO_TO_ORIGIN，意思是任务完成了，回远先最开始的位置。另一种是graph_planner::GraphPlannerCommand::COMMAND_GO_TO_LOCATION，主要是exploration_with_graph_planner.cpp中通过rrt服务器获取了之后要去的目标点，然后自己负责查询图中的节点，找一下看看该怎么去。这也是最重要的一个功能。

第二：监听地形消息，我理解为如果连续几个节点之间和附近都没有障碍物，就不用依次一个一个去，直接把最后一个点指定成waypoint来发送。地形消息代表的障碍物信息不是这篇工作的内容，是在仿真环境里面的一个功能包，我之后有空的话进行补充。

第三：监听"/local_graph "，这个"/local_graph "根据名字来看，是一个局部图，但其实在后续的代码中，如果是在全局规划阶段，会把全局图赋值给这个local_graph，也就是说，在探索阶段的话，它发布的就是局部图，如果在全局阶段它发布的就是全局图。而不是机器人附近的（轮为中的绿色方框内的局部地图，尽管话题名字有一个local）。

还有一些涉及到对图查找的工具函数，主要放在graph_utils，misc_utils下了，因为我也有自己的工作要做，不比读研阶段，只能抽空看看，所以这块我就不细看了，直接当黑盒子了，恳请读者谅解。
/**************************************************************************
graph_planner.cpp
Implementation of GraphPlanner class
Subscribes to local graph and search path to goal point on this graph
Created by Chao Cao (ccao1@andrew.cmu.edu)
6/3/19
Modified and maintained by Hongbiao Zhu (hongbiaz@andrew.cmu.edu)
5/25/2020
**************************************************************************/

#include <graph_utils.h>
#include <misc_utils/misc_utils.h>

#include "graph_planner.h"
#include <tf/transform_datatypes.h>

#include <string>

namespace graph_planner_ns {

GraphPlanner::GraphPlanner(const ros::NodeHandle &nh)
{
nh_ = nh;
initialize();
}

bool GraphPlanner::readParameters()
{
if (!nh_.getParam("world_frame_id", world_frame_id_))
{
ROS_ERROR("Cannot read parameter: world_frame_id_");
return false;
}
if (!nh_.getParam("graph_planner_command_topic",graph_planner_command_topic_))
{
ROS_ERROR("Cannot read parameter: graph_planner_command_topic_");
return false;
}
if (!nh_.getParam("graph_planner_status_topic", graph_planner_status_topic_)) {
ROS_ERROR("Cannot read parameter: graph_planner_status_topic_");
return false;
}
if (!nh_.getParam("pub_waypoint_topic", pub_waypoint_topic_))
{
ROS_ERROR("Cannot read parameter: pub_waypoint_topic_");
return false;
}
if (!nh_.getParam("pub_path_topic", pub_path_topic_))
{
ROS_ERROR("Cannot read parameter: pub_path_topic_");
return false;
}
if (!nh_.getParam("sub_odometry_topic", sub_odometry_topic_))
{
ROS_ERROR("Cannot read parameter: sub_odometry_topic_");
return false;
}
if (!nh_.getParam("sub_terrain_topic", sub_terrain_topic_))
{
ROS_ERROR("Cannot read parameter: sub_terrain_topic_");
return false;
}
if (!nh_.getParam("sub_graph_topic", sub_graph_topic_))
{
ROS_ERROR("Cannot read parameter: sub_graph_topic_");
return false;
}
if (!nh_.getParam("kLookAheadDist", kLookAheadDist))
{
ROS_ERROR("Cannot read parameter: kLookAheadDist");
return false;
}
if (!nh_.getParam("kWaypointProjectionDistance",kWaypointProjectionDistance))
{
ROS_ERROR("Cannot read parameter: kWaypointProjectionDistance");
return false;
}
if (!nh_.getParam("kDownsampleSize", kDownsampleSize))
{
ROS_ERROR("Cannot read parameter: kDownsampleSize");
return false;
}
if (!nh_.getParam("kObstacleHeightThres", kObstacleHeightThres))
{
ROS_ERROR("Cannot read parameter: kObstacleHeightThres");
return false;
}
if (!nh_.getParam("kOverheadObstacleHeightThres",kOverheadObstacleHeightThres))
{
ROS_ERROR("Cannot read parameter: kOverheadObstacleHeightThres");
return false;
}
if (!nh_.getParam("kCollisionCheckDistace", kCollisionCheckDistace))
{
ROS_ERROR("Cannot read parameter: kCollisionCheckDistace");
return false;
}
if (!nh_.getParam("kNextVertexMaintainTime", kNextVertexMaintainTime))
{
ROS_ERROR("Cannot read parameter: kNextVertexMaintainTime");
return false;
}
if (!nh_.getParam("kExecuteFrequency", kExecuteFrequency))
{
ROS_ERROR("Cannot read parameter: kExecuteFrequency");
return false;
}
return true;
}

//订阅local_graph的回调函数，这里我判断为应该是全局图
void GraphPlanner::graphCallback(const graph_utils::TopologicalGraph::ConstPtr &graph_msg)
{
planned_graph_ = *graph_msg;
}

//用来监听exploration_with_graph_planner.cpp发来的监听指令
void GraphPlanner::commandCallback( const graph_planner::GraphPlannerCommand::ConstPtr &msg)
{
//command中包含了目的地，和指令的模式是前进还是取消
graph_planner_command_ = *msg;
if (graph_planner_command_.command ==graph_planner::GraphPlannerCommand::COMMAND_GO_TO_LOCATION)
{
previous_shortest_path_size_ = 100000;
}
}

//定位算法发来的定位信息
void GraphPlanner::odometryCallback(const nav_msgs::Odometry::ConstPtr &odometry_msg)
{
double roll, pitch, yaw;
geometry_msgs::Quaternion geo_quat = odometry_msg->pose.pose.orientation;
tf::Matrix3x3(tf::Quaternion(geo_quat.x, geo_quat.y, geo_quat.z, geo_quat.w)).getRPY(roll, pitch, yaw);
robot_yaw_ = yaw;
robot_pos_ = odometry_msg->pose.pose.position;
}

//初始化各种定义，一般直接写在构造函数中，作者单独打包了一个函数
bool GraphPlanner::initialize()
{
if (!readParameters())
return false;

// Initialize target waypoint
waypoint_ = geometry_msgs::PointStamped();
waypoint_.point.x = -1.0;
waypoint_.point.y = -1.0;
waypoint_.point.z = 0.0;
waypoint_.header.frame_id = world_frame_id_;

// Initialize subscribers
//"/local_graph ",虽然名为local_graph，应该不是指机器人周围的那个graph
//参见goToVertex的注释
graph_sub_ =nh_.subscribe(sub_graph_topic_, 1, &GraphPlanner::graphCallback, this);
//"graph_planner_command"
graph_planner_command_sub_ = nh_.subscribe(graph_planner_command_topic_, 1, &GraphPlanner::commandCallback, this);
//"/state_estimation"
odometry_sub_ = nh_.subscribe(sub_odometry_topic_, 1,&GraphPlanner::odometryCallback, this);
//"/terrain_map_ext"
terrain_sub_ = nh_.subscribe(sub_terrain_topic_, 1,&GraphPlanner::terrainCallback, this);

// Initialize publishers
//"/way_point"
waypoint_pub_ =nh_.advertise<geometry_msgs::PointStamped>(pub_waypoint_topic_, 2);
//"/graph_planner_status"
graph_planner_status_pub_ = nh_.advertise<graph_planner::GraphPlannerStatus>(graph_planner_status_topic_, 2);
//" /graph_planner_path"
graph_planner_path_pub_ = nh_.advertise<nav_msgs::Path>(pub_path_topic_, 10);

ROS_INFO("Successfully launched graph_planner node");

return true;
}

//回调来监听地形消息，用terrain_point_crop_来记录会妨碍到机器人运动的地形？
void GraphPlanner::terrainCallback(const sensor_msgs::PointCloud2::ConstPtr &terrain_msg)
{
terrain_point_->clear();
terrain_point_crop_->clear();
pcl::fromROSMsg(*terrain_msg, *terrain_point_);

pcl::PointXYZI point;
int terrainCloudSize = terrain_point_->points.size();
for (int i = 0; i < terrainCloudSize; i++)
{
point = terrain_point_->points[i];

float pointX = point.x;
float pointY = point.y;
float pointZ = point.z;
//以配置文件default.yaml为例，高于0.2，低于1.2的障碍物点云判断为会对机器人行动造成妨碍
if (point.intensity > kObstacleHeightThres &&point.intensity < kOverheadObstacleHeightThres)
{
point.x = pointX;
point.y = pointY;
point.z = pointZ;
terrain_point_crop_->push_back(point);
}
}
pcl::VoxelGrid<pcl::PointXYZI> point_ds;
point_ds.setLeafSize(kDownsampleSize, kDownsampleSize, kDownsampleSize);
point_ds.setInputCloud(terrain_point_crop_);
point_ds.filter(*terrain_point_crop_);
}

//exploration_with_graph_planner.cpp发布的GraphPlannerCommand指令，
//在这里会决定graph_planner_status的状态，
//再次发送出去以后，又被exploration_with_graph_planner.cpp监听到，意思是正在执行运动操作。
//exploration_with_graph_planner.cpp中会根据这个的状态，来判断要不要监听运动情况（一直到不了则是要取消边界点的）
void GraphPlanner::publishInProgress(bool in_progress)
{
in_progress_ = in_progress; // used mainly for debugging

graph_planner::GraphPlannerStatus status;
if (in_progress) {
status.status = graph_planner::GraphPlannerStatus::STATUS_IN_PROGRESS;
} else {
status.status = graph_planner::GraphPlannerStatus::STATUS_DISABLED;
state_ = ALL_OTHER_STATES; // reset
}

graph_planner_status_pub_.publish(status);
}

//返回true，说明目前还在行进路上，返回false说明目前没有在走
bool GraphPlanner::goToPoint(geometry_msgs::Point point)
{
//订阅的是"/local_graph ",暂时不知里面存有什么,虽然名为local_graph，应该不是指机器人周围的那个graph
if (planned_graph_.vertices.size() <= 0)
{
return false;
}
// Find where I am on this graph
//robot_pos_在odom的callback中被保存有自己的位置姿态信息,找到最近的那个图的节点
int current_vertex_idx =graph_utils_ns::GetClosestVertexIdxToPoint(planned_graph_, robot_pos_);

// find closest goal vertex,找到距离目标最近的图节点
int goal_vertex_idx =graph_utils_ns::GetClosestVertexIdxToPoint(planned_graph_, point);

// Set the waypoint,负责执行操作.我推测执行应该是交由仿真环境对应的那个local_planner来执行
return goToVertex(current_vertex_idx, goal_vertex_idx);
}

//发布waypoint
void GraphPlanner::alterAndPublishWaypoint()
{
// Ship it!
waypoint_.header.frame_id = world_frame_id_; // added by Hong in 20191203
waypoint_.header.stamp = ros::Time::now(); // added by Hong in 20191203
waypoint_pub_.publish(waypoint_);
}

//机器人当前位置和形参中的end_vertex_idx之间是否可以无碰撞直达,碰撞为true，无碰撞为false
bool GraphPlanner::collisionCheckByTerrain(geometry_msgs::Point robot_position, int end_vertex_idx)
{
geometry_msgs::Point start_point = robot_position;
geometry_msgs::Point end_point =planned_graph_.vertices[end_vertex_idx].location;

int count = 0;
//distance是该函数从当前点到目标点的直线距离
double distance =sqrt((end_point.x - start_point.x) * (end_point.x - start_point.x) +(end_point.y - start_point.y) * (end_point.y - start_point.y));
//距离除以kCollisionCheckDistace，得到要碰撞检查的次数
double check_point_num = distance / (kCollisionCheckDistace);
for (int j = 0; j < check_point_num; j++)
{
geometry_msgs::Point p1;
//我理解为这个操作是沿着线去搜索，j从0到distance / (kCollisionCheckDistace)，分成很多段
//沿着段去搜索，如果有障碍物，count就不再是0，认为发生碰撞
p1.x =start_point.x + j * kCollisionCheckDistace / distance * (end_point.x - start_point.x);
p1.y =start_point.y + j * kCollisionCheckDistace / distance * (end_point.y - start_point.y);
for (int i = 0; i < terrain_point_crop_->points.size(); i++)
{
double dist = sqrt((p1.x - terrain_point_crop_->points[i].x) *(p1.x - terrain_point_crop_->points[i].x) +
p1.y - terrain_point_crop_->points[i].y) *(p1.y - terrain_point_crop_->points[i].y));
if (dist < kCollisionCheckDistace)
{
count++;
}
if (count > 0)
{
return true;
}
}
}
return false;
}

//判断是否在前往某个节点的路上，如果没有路径，或者到达路径，或者路径很短，则返回false
//具体内部的逻辑写的有点复杂
bool GraphPlanner::goToVertex(int current_vertex_idx, int goal_vertex_idx)
{
std::vector<int> shortest_path;
//给定起点id和终点id，根据图的点planned_graph_找到一条最短路径，，将结果保存在shortestpath中
//planned_graph_在graphCallback中被赋值，订阅的是“local_graph”这个话题,
//因此可看出local_graph这里指的绝对不是机器人附近的图，否则无法找到路径
graph_utils_ns::ShortestPathBtwVertex(shortest_path, planned_graph_,current_vertex_idx, goal_vertex_idx);
//没找到最短路径,说明目前没有处在前往某个点的路上，返回false
if (shortest_path.size() <= 0)
{
// ROS_WARN("Graph planner did not find path to selected goal!");

// clear the saved path
planned_path_.clear();

return false;
}
else if (shortest_path.size() <= 2)
{
//（误）注意这里有个shortest_path.back(),可以看出路径是从终点出发倒着找的，靠近起点的在末尾(这种理解错误！！)
// 看过函数构成，以及下一个else分支以后，则可以看出，shortest_path存放顺序为从起点出发到终点
geometry_msgs::Point next_waypoint =planned_graph_.vertices[shortest_path.back()].location;
waypoint_.header.stamp = ros::Time::now();
waypoint_.point.x = next_waypoint.x;
waypoint_.point.y = next_waypoint.y;
waypoint_.point.z = next_waypoint.z;
waypoint_.header.frame_id = world_frame_id_;

// clear the saved path
planned_path_.clear();
planned_path_.push_back(waypoint_.point);
//发布waypoint
alterAndPublishWaypoint();
return false;
}
else
{
int next_vertex_id;
//kLookAheadDist默认是5m，这个函数要找到超过5m的最近的节点
next_vertex_id = graph_utils_ns::GetFirstVertexBeyondThreshold(robot_pos_, shortest_path, planned_graph_, kLookAheadDist);
for (int i = 1; i < shortest_path.size(); i++)
{
// std::cout << "i = " << i << " id is " << shortest_path[i] <<
// std::endl;
//最短路径从从当前点出发开始搜索，
//如果不是超过5m的那个最近节点(next_vertex_id),
//并且从当前位置到达那个位置的直线上存在障碍物,
//next_vertex_id就会被更新为近的那一个
if (shortest_path[i] != next_vertex_id &&collisionCheckByTerrain(robot_pos_, shortest_path[i]))
{
next_vertex_id = shortest_path[i - 1];
break;
}
else if (shortest_path[i] == next_vertex_id)
{
//这块多写了，相等其实用不着赋值
next_vertex_id = shortest_path[i];
break;
}
}
// next_vertex_id到终点的距离
std::vector<int> next_shortest_path;
graph_utils_ns::ShortestPathBtwVertex(next_shortest_path, planned_graph_, next_vertex_id, goal_vertex_idx);

// prevent back and forth between two vertices
// 这段说实话写的让人很迷惑
// 刚开始第一轮直接进入else分支，因为 previous_shortest_path_size_是1000000
// 如果下一段的长度比之前保存的的大，，那么就暂时不执行，接着上一次的目标点执行（猜,先走短路再走长路？）
if (next_shortest_path.size() > previous_shortest_path_size_)
{
next_vertex_id = previous_vertex_id_;
backTraceCount_++;
//kNextVertexMaintainTime : 5 # when next vertex goal makes the path longer, keep the previous goal for this long time
//kExecuteFrequency : 5 # the frequency of the execute function
//backTraceCount_逐渐递增，大于25以后，则这个路径长短缓存机制失效。
//下次发现路径大于之前的路径时，则执行下次的目标点，不再“暂时不执行”
//这里感觉有点奇怪，怀疑这个trick到底起到作用没有？
//但是感觉这里并没有计时，25岂不是一下子就递增上去了？
//非也!!! 我们看到，在execute这个总函数中，设置了ros::Rate rate(kExecuteFrequency);，另外还有一个rate.sleep();
//因此每次最少休息0.2s，这里才+1。所以backTraceCount_从0递增到25，需要0.2*25=5s
//kNextVertexMaintainTime=5 直译为下一个节点的保持时间，25*0.2=5s，刚好对应
if (backTraceCount_ > kNextVertexMaintainTime * kExecuteFrequency)
{
backTraceCount_ = 0;
previous_shortest_path_size_ = 100000;
}
}
else
{
//用previous相关变量，记录了这一段的长度
//例如从1到终点10，现在找到了next_vertex_id是4，整个路径分成两段，1～4，4～10
//直接发4为waypoint，
//previous_vertex_id_记录为4，size记录为4～10
previous_vertex_id_ = next_vertex_id;
previous_shortest_path_size_ = next_shortest_path.size();
}

geometry_msgs::Point next_waypoint =planned_graph_.vertices[next_vertex_id].location;
//下一个节点不是终点，投影路径,使在运行过程中，waypoint不是固定的图节点值，而是随着机器人运动，投影到当前机器人到节点之间的路径上
if (next_vertex_id != shortest_path.back())
{
next_waypoint = projectWayPoint(next_waypoint, robot_pos_);
}
//注：waypoint_是一个类内的成员变量，这里保存到waypoint_里面以后，没有马上发布
//本函数叫goToVertex，本函数被goToPoint调用，
//而goToPoint又被executeGoToOrigin或executeGoToLocation来调用，这两者内部才通过alterAndPublishWaypoint发布waypoint执行
waypoint_.header.stamp = ros::Time::now();
waypoint_.point.x = next_waypoint.x;
waypoint_.point.y = next_waypoint.y;
waypoint_.point.z = next_waypoint.z;
waypoint_.header.frame_id = world_frame_id_;

// save path for debugging
planned_path_.clear();
for (const auto &v : shortest_path)
{
planned_path_.push_back(planned_graph_.vertices[v].location);
}

if (next_vertex_id == shortest_path.back())
{
// Assume reached vertex -- publish the last waypoint and then end
//到达了返回false
alterAndPublishWaypoint();
return false;
}
else
{
// Keep moving along planned path
return true;
}
}
}

//探索结束，返回原先位置
void GraphPlanner::executeGoToOrigin()
{
// Aim for (0,0,0)
geometry_msgs::Point origin;
origin.x = 0;
origin.y = 0;
origin.z = 0;
bool success = goToPoint(origin);

if (!success) {
publishInProgress(false);
} else {
alterAndPublishWaypoint();
publishInProgress(true);
}
}

//exploration_with_graph_planner.cpp在发送路径节点，
//在本cpp的commandCallback回调函数中被监听到，然后函数execute会根据这个状态下达指令，
//通过executeGoToLocation，具体来操作机器人到达各个目标点
void GraphPlanner::executeGoToLocation()
{
// Compute waypoint
bool success = goToPoint(graph_planner_command_.location);

if (!success)
{
publishInProgress(false);
}
else
{
alterAndPublishWaypoint();
publishInProgress(true);
}
}

void GraphPlanner::executeCommand()
{
// Choose from one of the graph_planner variants based on the command
//返回起点
if (graph_planner_command_.command ==graph_planner::GraphPlannerCommand::COMMAND_GO_TO_ORIGIN)
{
// COMMAND_GO_TO_ORIGIN
executeGoToOrigin();
}
//根据命令前往某个点
else if (graph_planner_command_.command ==graph_planner::GraphPlannerCommand::COMMAND_GO_TO_LOCATION)
{
// COMMAND_GO_TO_LOCATION
executeGoToLocation();
}
else
{
publishInProgress(false);
}
}

//这个函数可以理解为graph_planner进程的主函数，时刻监听来自exploration_with_graph_planner.cpp发来的指令信息，并交付执行
bool GraphPlanner::execute()
{
ros::Rate rate(kExecuteFrequency);
bool status = ros::ok();
while (status)
{
ros::spinOnce();

if (graph_planner_command_.command ==graph_planner::GraphPlannerCommand::COMMAND_DISABLE)
{
// Graph planner is disabled -- do nothing
publishInProgress(false);
}
else
{
// Graph planner is enabled -- select waypoint according to current command
executeCommand();
}

// Generate and publish the path
publishPath();

status = ros::ok();
rate.sleep();
}

return true;
}

//发布路径，给rviz订阅？或其他用途
void GraphPlanner::publishPath() {
nav_msgs::Path planned_path;
planned_path.header.frame_id = world_frame_id_;
planned_path.header.stamp = ros::Time::now();
//带下划线的path_是在goToVertex函数中被保存，这里用来发布，所以重新搞了一个不带下划线的path来缓存
if (in_progress_ && !planned_path_.empty())
{
planned_path.poses.resize(planned_path_.size() + 1);
planned_path.poses[0].pose.position = robot_pos_;
for (int i = 0; i < planned_path_.size(); i++)
{
planned_path.poses[i + 1].pose.position = planned_path_[i];
}
}
else
{
planned_path.poses.resize(1);
planned_path.poses[0].pose.position = robot_pos_;
}
graph_planner_path_pub_.publish(planned_path);
}

//这个函数的作用是，不断更新waypoint，使在运行过程中，waypoint不是固定的图节点值，而是随着机器人运动，投影到当前机器人到节点之间的路径上
geometry_msgs::PointGraphPlanner::projectWayPoint(geometry_msgs::Point next_vertex_pos, geometry_msgs::Point robot_pos)
{
double ratio =misc_utils_ns::PointXYDist<geometry_msgs::Point, geometry_msgs::Point>(robot_pos, next_vertex_pos) /kWaypointProjectionDistance;
double x = (next_vertex_pos.x - robot_pos.x) / ratio + robot_pos.x;
double y = (next_vertex_pos.y - robot_pos.y) / ratio + robot_pos.y;
double z = (next_vertex_pos.z - robot_pos.z) / ratio + robot_pos.z;
geometry_msgs::Point way_point = misc_utils_ns::GeoMsgPoint(x, y, z);
return way_point;
}

} // namespace graph_planner_ns

drrtp_node.cpp

先验

这个进程是算法的主要内容。在node中主要定义了一个drrtPlanner的对象。

这个drrtPlanner对象，是对内部算法的一个抽象的封装。

首先，定义了一大堆pub，实际上都没用，就是用到一个nextGoalPub_，把下一个要去的目标点给pub出去；

第二，根据回调函数odomCallback，把机器人位姿传入到drrt求解器中。

第三，定义服务/drrtPlannerSrv，这个服务在exploration_with_graph_planner.cpp中被调用，调用以后，这里返回一个目标点给它，然后它再打包成command，发送给global_graph来交付，打包成waypoint来执行……

第四，定义服务cleanFrontierSrv，这个服务也是在exploration_with_graph_planner.cpp中被调用，用来清空短时间内到不了的边界信息。

光看这个代码，不能理解其中的逻辑。原因在于，其中定义了如下几个内容，只看外层调用，难以理解含义，先占坑在这里，后续将一个一个补充进来：

1.dual_state_graph_：这个应该是一个全局图，包含局部节点和全局节点，这个进程中调用了如下几个函数：

dual_state_graph_->getGain(robot_position)：即实现论文中的公式(4）和(5)
dual_state_graph_->best_vertex_id_：根据计算，得到gain最大的点作为best点，作为之后要前往的探索点。

dual_state_graph_->updateExploreDirection()：//根据计算得到的best_vertex_id_，得到从0节点到best_vertex_id_节点的探索方向的单位向量

dual_state_graph_->getBestGlobalVertexPosition() ：根据best_vertex_id_，在局部图中获取它的位置。（因为代码里有一个探索阶段把全局图赋值给局部图的操作）

dual_state_graph_->getBestLocalVertexPosition()：与getBestGlobalVertexPosition实现完全相同。反正是要返回best_vertex_id_的位置。

dual_state_graph_->updateGlobalGraph(); 更新全局图，因为根据论文描述，gain大于0的点，是要被加入到全局图中的。

dual_state_graph_->setCurrentPlannerStatus(drrt_->global_plan_pre_)：用于设置变量planner_status_的状态，这个变量在dual_state_graph.cpp中有两个作用，当它为false的时候，会对全局图进行修改剪枝操作（把有障碍物的点之间的边去掉），并且进行发布。第二个作用，当它为true时，在该cpp文件对应的pathCallback函数中进行的障碍物检查去除连接边的作用。

dual_state_graph_->clearLocalGraph(); 清空局部图，相当于是一个reset操作。

2.dual_state_frontier_：这应该是一个全局边界管理器，负责管理已知与未知之间的边界。这个进程中调用了如下几个函数：

dual_state_frontier_->setPlannerStatus(drrt_->global_plan_pre_);//设置边界处理器的状态，0是探索，1是relocation阶段
dual_state_frontier_->cleanAllUselessFrontiers();//把边界都清空掉

dual_state_frontier_->updateToCleanFrontier(drrt_->selectedGlobalFrontier_);//把形参加入到cleanedFrontier里面，当调用清除某个边界服务的时候，会调用这个函数，然后把它加入到cleanedFrontier，之后全局边界更新的时候不会再考虑到它

dual_state_frontier_->gloabalFrontierUpdate();//全局边界点的检查和更新

3.drrt_：这应该是一个drrt求解器，在定义对象时需要把全局图dual_state_graph_和全局边界管理器dual_state_frontier_传入进去。在这个进程中主要调用如下内容：

drrt_->setRootWithOdom(pose); 这个比较好理解，就是把机器人当前的pose给输入进去，让求解器知道机器人当前位姿。
drrt_->setTerrainVoxelElev(); 这个应该是用于设置地形的体素网格和对应的高度。具体操作是，调用dual_state_frontier_->getTerrainVoxelElev()来获取地形高度

drrt_->getNodeCounter() 这个用来统计全局节点的数目

drrt_->plannerInit(); 这个是rrt求解器的初始化函数，每次进程向服务器请求路径时，服务器用于初始化，具体是根据全局规划状态global_plan_来判断是在探索状态还是relocation状态，并且发布边界点

drrt_->normal_local_iteration_：normal_local_iteration_为false, 即当前局部窗口内没边界，但是系统想等待边界更新

drrt_->gainFound()：bestGain_大于0（默认），则返回true。bestGain_如何计算：

每次在图里加入一个节点的时候，这个全局变量会统计最大的gain。

drrt_->plannerIterate(); 功能：采样新的点，（靠近边界，或者机器人附近）,加入到图中

drrt_->publishNode(); 这个应该是用来发布计算好的rrt，包括候选的和选中的，用来酷炫的显示在rviz里

drrt_->global_plan_pre_ 我推测这个是一个中间变量，用来保存之前系统所处的状态，是属于全局规划状态还是局部窗口内的规划状态

drrt_->global_plan_：全局规划状态

drrt_->local_plan_：局部窗口内的规划状态

drrt_->nextNodeFound_ ：这个非常重要，多个分支逻辑需要靠它来判断，直译为“发现了下一个节点”，代表发现了下一个要去的全局节点。

drrt_->selectedGlobalFrontier_：//选择得到的边界位置（之后就是要去这个边界附近）

dual_state_graph.cpp

/**************************************************************************
dual_state_graph.cpp
Implementation of dual_state graph. Create local and global graph according
to the new node from dynamic rrt.
Hongbiao Zhu(hongbiaz@andrew.cmu.edu)
5/25/2020
**************************************************************************/

#include "dsvplanner/dual_state_graph.h"

#include <graph_utils.h>
#include <misc_utils/misc_utils.h>

#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>
namespace dsvplanner_ns
{

//参数服务器读取配置文件参数
bool DualStateGraph::readParameters()
{
nh_private_.getParam("/graph/world_frame_id", world_frame_id_);
nh_private_.getParam("/graph/pub_local_graph_topic", pub_local_graph_topic_);
nh_private_.getParam("/graph/pub_global_graph_topic",pub_global_graph_topic_);
nh_private_.getParam("/graph/pub_global_points_topic",pub_global_points_topic_);
nh_private_.getParam("/graph/sub_keypose_topic", sub_keypose_topic_);
nh_private_.getParam("/graph/sub_path_topic", sub_path_topic_);
nh_private_.getParam("/graph/sub_graph_planner_status_topic",sub_graph_planner_status_topic_);
nh_private_.getParam("/graph/kCropPathWithTerrain", kCropPathWithTerrain);
nh_private_.getParam("/graph/kConnectVertexDistMax", kConnectVertexDistMax);
nh_private_.getParam("/graph/kDistBadEdge", kDistBadEdge);
nh_private_.getParam("/graph/kDegressiveCoeff", kDegressiveCoeff);
nh_private_.getParam("/graph/kDirectionCoeff", kDirectionCoeff);
nh_private_.getParam("/graph/kExistingPathRatioThresholdGlobal",kExistingPathRatioThresholdGlobal);
nh_private_.getParam("/graph/kExistingPathRatioThreshold",kExistingPathRatioThreshold);
nh_private_.getParam("/graph/kLongEdgePenaltyMultiplier",kLongEdgePenaltyMultiplier);
nh_private_.getParam("/graph/kMaxLongEdgeDist", kMaxLongEdgeDist);
nh_private_.getParam("/graph/kMaxVertexAngleAlongZ", kMaxVertexAngleAlongZ);
nh_private_.getParam("/graph/kMaxVertexDiffAlongZ", kMaxVertexDiffAlongZ);
nh_private_.getParam("/graph/kMaxDistToPrunedRoot", kMaxDistToPrunedRoot);
nh_private_.getParam("/graph/kMaxPrunedNodeDist", kMaxPrunedNodeDist);
nh_private_.getParam("/graph/kMinVertexDist", kMinVertexDist);
nh_private_.getParam("/graph/kSurroundRange", kSurroundRange);
nh_private_.getParam("/graph/kMinGainRange", kMinGainRange);
nh_private_.getParam("/graph/kMinDistanceToRobotToCheck",kMinDistanceToRobotToCheck);
nh_private_.getParam("/rm/kBoundX", robot_bounding[0]);
nh_private_.getParam("/rm/kBoundY", robot_bounding[1]);
nh_private_.getParam("/rm/kBoundZ", robot_bounding[2]);
return true;
}

//发布局部图，具体是两种，
//local_graph_pub_发布的是graph_utils::TopologicalGraph形式的局部图，
//graph_points_pub_发布的是pointcloud2形式的局部图
void DualStateGraph::publishLocalGraph()
{
// Publish the current local graph
//以/local_graph的形式发布局部地图
local_graph_.header.stamp = ros::Time::now();
local_graph_.header.frame_id = world_frame_id_;
local_graph_pub_.publish(local_graph_);

// graph_point is used to detect frontiers
//以/global_points的形式，把局部图中的节点按点云的形式发布出去
graph_point_cloud_->points.clear();
for (int i = 0; i < local_graph_.vertices.size(); i++)
{
pcl::PointXYZ p1;
p1.x = local_graph_.vertices[i].location.x;
p1.y = local_graph_.vertices[i].location.y;
p1.z = local_graph_.vertices[i].location.z;
graph_point_cloud_->points.push_back(p1);
}
sensor_msgs::PointCloud2 graph_pc;
pcl::toROSMsg(*graph_point_cloud_, graph_pc);
graph_pc.header.frame_id = "map";
graph_points_pub_.publish(graph_pc);
}

//以/global_graph的形式发布全局图
void DualStateGraph::publishGlobalGraph()
{
// Publish the current global graph
global_graph_.header.stamp = ros::Time::now();
global_graph_.header.frame_id = world_frame_id_;
global_graph_pub_.publish(global_graph_);
}

//和addNewLocalVertex作用几乎一样，功能是加入一个节点
//多了一个“相似性检查”，如果离得太近了就不加入图中
void DualStateGraph::addNewLocalVertexWithoutDuplicates(geometry_msgs::Pose &vertex_msg, graph_utils::TopologicalGraph &graph)
{
// Same as addNewLocalVertex but only adds vertex if a similar vertex doesn't already exist

// Extract the point
geometry_msgs::Point new_vertex_location;
new_vertex_location = vertex_msg.position;

// Check if a similar vertex already exists
bool already_exists = false;
bool too_long = false;
double distance = -1;
int closest_vertex_idx = -1;
if (!graph.vertices.empty())
{
closest_vertex_idx =graph_utils_ns::GetClosestVertexIdxToPoint(graph, new_vertex_location);
auto &closest_vertex_location = graph.vertices[closest_vertex_idx].location;
distance = misc_utils_ns::PointXYZDist(new_vertex_location,closest_vertex_location);
if (distance < kMinVertexDist)
{
already_exists = true;
}
if (distance > kMaxLongEdgeDist ||abs(new_vertex_location.z - closest_vertex_location.z) >kMaxVertexDiffAlongZ)
{
too_long = true;
}
}

// If not, add a new one
if (!already_exists && !too_long)
{
prev_track_vertex_idx_ = closest_vertex_idx;
addNewLocalVertex(vertex_msg, graph);
}
}

//功能：加入一个新的节点，还要检查节点之间的欧式距离，不能超过阈值或低于阈值，还要保证不会发生碰撞
void DualStateGraph::addNewPrunedVertex(geometry_msgs::Pose &vertex_msg,graph_utils::TopologicalGraph &graph)
{
// Extract the point
geometry_msgs::Point new_vertex_location;
new_vertex_location = vertex_msg.position;

bool already_exists = false;
bool too_long = false;
double distance = -1;
int closest_vertex_idx = -1;
geometry_msgs::Point closest_vertex_location;
if (!graph.vertices.empty())
{
closest_vertex_idx =graph_utils_ns::GetClosestVertexIdxToPoint(graph, new_vertex_location);
closest_vertex_location = graph.vertices[closest_vertex_idx].location;
distance = misc_utils_ns::PointXYZDist(new_vertex_location,closest_vertex_location);
//如果新节点和图中的最近节点的欧式距离小于kMinVertexDist，不加入图中
if (distance < kMinVertexDist)
{
already_exists = true;
}
//距离太远也不加入图中
if (distance > kMaxLongEdgeDist ||fabs(new_vertex_location.z - closest_vertex_location.z) >kMaxVertexDiffAlongZ)
{
too_long = true;
}
}

// Check again if there is collision between the new node and its closest node. Although this has been done for local graph,
//but the octomap might be updated
if (!already_exists && !too_long)
{
Eigen::Vector3d origin;
Eigen::Vector3d end;
origin.x() = new_vertex_location.x;
origin.y() = new_vertex_location.y;
origin.z() = new_vertex_location.z;
end.x() = closest_vertex_location.x;
end.y() = closest_vertex_location.y;
end.z() = closest_vertex_location.z;
//确认连线没有碰撞
if (volumetric_mapping::OctomapManager::CellStatus::kFree ==manager_->getLineStatusBoundingBox(origin, end, robot_bounding))
{
prev_track_vertex_idx_ = closest_vertex_idx;
addNewLocalVertex(vertex_msg, graph);
}
}
}

//功能：给定一个pose，把它加入到图中
//直接连接最近的点的边，其他靠近的边进行公式(6)的约束检查，再进行连边
void DualStateGraph::addNewLocalVertex(geometry_msgs::Pose &vertex_msg,graph_utils::TopologicalGraph &graph)
{
// Add a new vertex to the graph associated with the input keypose
// Return the index of the vertex

// Extract the point
geometry_msgs::Point new_vertex_location;
new_vertex_location = vertex_msg.position;

// Create a new vertex
graph_utils::Vertex vertex;
vertex.location = new_vertex_location;
vertex.vertex_id = (int)graph.vertices.size();
vertex.information_gain = vertex_msg.orientation.y;

// Add this vertex to the graph
graph.vertices.push_back(vertex);
track_localvertex_idx_ = (int)graph.vertices.size() - 1;
auto &vertex_new = graph.vertices[track_localvertex_idx_];
// If there are already other vertices
if (graph.vertices.size() > 1)
{
// Add a parent backpointer to previous vertex
vertex_new.parent_idx = prev_track_vertex_idx_;

// Add an edge to previous vertex
//当前节点和最近的那个节点，已经在addNewPrunedVertex中进行过碰撞检查了，因此可以直接连线
addEdgeWithoutCheck(vertex_new.parent_idx, track_localvertex_idx_, graph);

// Also add edges to nearby vertices
for (auto &graph_vertex : graph.vertices)
{
// If within a distance
float dist_diff = misc_utils_ns::PointXYZDist(graph_vertex.location, vertex_new.location);
if (dist_diff < kConnectVertexDistMax)
{
// Add edge from this nearby vertex to current vertex
//欧式距离比较近的点，一概也进行连线（不过这次要进行碰撞检查了,另外还进行(6)的约束）
addEdge(graph_vertex.vertex_id, vertex_new.vertex_id, graph);
}
}
}
else
{
// This is the first vertex -- no edges
vertex.parent_idx = -1;
}
}

//直接加入一个图节点，不加入边
void DualStateGraph::addNewLocalVertexWithoutEdge(geometry_msgs::Pose &vertex_msg, graph_utils::TopologicalGraph &graph)
{
// Add a new vertex to the graph associated with the input keypose
// Return the index of the vertex

// Extract the point
geometry_msgs::Point new_vertex_location;
new_vertex_location = vertex_msg.position;

// Create a new vertex
graph_utils::Vertex vertex;
vertex.location = new_vertex_location;
vertex.vertex_id = (int)graph.vertices.size();
vertex.information_gain = vertex_msg.orientation.y;

// Add this vertex to the graph
graph.vertices.push_back(vertex);
track_localvertex_idx_ = (int)graph.vertices.size() - 1;
auto &vertex_new = graph.vertices[track_localvertex_idx_];
}

//对全局图操作，加入一个节点，多了一个距离和相似性检查，没问题才调用addNewGlobalVertex
//这个和addNewGlobalVertexWithKeypose几乎一样，
//有微小的区别在于，相似性检查上，addNewGlobalVertexWithKeypose要欧式距离小于kMinVertexDist/2
//本函数要求欧式距离小于于kMinVertexDist
void DualStateGraph::addNewGlobalVertexWithoutDuplicates(geometry_msgs::Pose &vertex_msg)
{
// Same as addNewLocalVertex but only adds vertex if a similar vertex doesn't
// already exist

// Extract the point
geometry_msgs::Point new_vertex_location;
new_vertex_location = vertex_msg.position;

// Check if a similar vertex already exists
bool already_exists = false;
bool too_long = false;
double distance = -1;
int closest_vertex_idx = -1;
if (!global_graph_.vertices.empty())
{
closest_vertex_idx = graph_utils_ns::GetClosestVertexIdxToPoint(global_graph_, new_vertex_location);
auto &closest_vertex_location =global_graph_.vertices[closest_vertex_idx].location;
distance = misc_utils_ns::PointXYZDist(new_vertex_location,closest_vertex_location);
if (distance < kMinVertexDist)
{
already_exists = true;
}
if (distance > kMaxLongEdgeDist ||fabs(new_vertex_location.z - closest_vertex_location.z) >kMaxVertexDiffAlongZ)
{
too_long = true;
}
}

// If not, add a new one
if (!already_exists && !too_long)
{
prev_track_vertex_idx_ = closest_vertex_idx;
addNewGlobalVertex(vertex_msg);
}
}

//对全局图操作，加入一个节点，多了一个距离和相似性检查，没问题才调用addNewGlobalVertex
//这个和addNewGlobalVertexWithoutDuplicates几乎一样，
//有微小的区别在于，相似性检查上，addNewGlobalVertexWithoutDuplicates要欧式距离小于kMinVertexDist
//本函数要求欧式距离小于于kMinVertexDist/2
void DualStateGraph::addNewGlobalVertexWithKeypose(geometry_msgs::Pose &vertex_msg)
{
// Same as addNewLocalVertex but only adds vertex if a similar vertex doesn't
// already exist

// Extract the point
geometry_msgs::Point new_vertex_location;
new_vertex_location = vertex_msg.position;

// Check if a similar vertex already exists
bool already_exists = false;
bool too_long = false;
double distance = -1;
int closest_vertex_idx = -1;
if (!global_graph_.vertices.empty())
{
closest_vertex_idx = graph_utils_ns::GetClosestVertexIdxToPoint(global_graph_, new_vertex_location);
auto &closest_vertex_location =global_graph_.vertices[closest_vertex_idx].location;
distance = misc_utils_ns::PointXYZDist(new_vertex_location,closest_vertex_location);
if (distance < kMinVertexDist / 2)
{
already_exists = true;
}
if (distance > kMaxLongEdgeDist ||fabs(new_vertex_location.z - closest_vertex_location.z) >kMaxVertexDiffAlongZ)
{
too_long = true;
}
}

// If not, add a new one
if (!already_exists && !too_long)
{
prev_track_vertex_idx_ = closest_vertex_idx;
addNewGlobalVertex(vertex_msg);
}
}

//功能：给定一个pose，把它加入到全局图中
//直接连接最近的点的边，其他靠近的边进行公式(6)的约束检查，再进行连边
void DualStateGraph::addNewGlobalVertex(geometry_msgs::Pose &vertex_msg)
{
// Extract the point
geometry_msgs::Point new_vertex_location;
new_vertex_location = vertex_msg.position;

// Create a new vertex
graph_utils::Vertex vertex;
vertex.location = new_vertex_location;
vertex.vertex_id = (int)global_graph_.vertices.size();
vertex.information_gain = vertex_msg.orientation.y;

// Add this vertex to the graph
global_graph_.vertices.push_back(vertex);
track_globalvertex_idx_ = (int)global_graph_.vertices.size() - 1;
auto &vertex_new = global_graph_.vertices[track_globalvertex_idx_];
// If there are already other vertices
if (global_graph_.vertices.size() > 1)
{
// Add a parent backpointer to previous vertex
vertex_new.parent_idx = prev_track_vertex_idx_;

// Add an edge to previous vertex
addGlobalEdgeWithoutCheck(prev_track_vertex_idx_, track_globalvertex_idx_);

// Also add edges to nearby vertices
for (auto &graph_vertex : global_graph_.vertices)
{
// If within a distance
float dist_diff = misc_utils_ns::PointXYZDist(graph_vertex.location,vertex_new.location);
if (dist_diff < kConnectVertexDistMax)
{
// Add edge from this nearby vertex to current vertex
addGlobalEdge(graph_vertex.vertex_id, vertex_new.vertex_id);
}
}
}
else
{
// This is the first vertex -- no edges
vertex.parent_idx = -1;
}
}

//功能：不进行任何碰撞检查，直接对两个节点之间加入边（边为欧式距离）
void DualStateGraph::addEdgeWithoutCheck(int start_vertex_idx, int end_vertex_idx, graph_utils::TopologicalGraph &graph)
{
// Add an edge in the graph from vertex start_vertex_idx to vertex end_vertex_idx

// Get the two vertices
auto &start_vertex = graph.vertices[start_vertex_idx];
auto &end_vertex = graph.vertices[end_vertex_idx];

// Check if edge already exists -- don't duplicate
//遍历开始节点的边，如果已经存在，就不进行操作
for (auto &edge : start_vertex.edges)
{
if (edge.vertex_id_end == end_vertex_idx)
{
// don't add duplicate edge
return;
}
}

// Compute the distance between the two points
float dist_diff =misc_utils_ns::PointXYZDist(start_vertex.location, end_vertex.location);

// Add an edge connecting the current node and the existing node on the graph

// Create two edge objects
graph_utils::Edge edge_to_start;
graph_utils::Edge edge_to_end;

// Join the two edges
edge_to_start.vertex_id_end = start_vertex_idx;
edge_to_end.vertex_id_end = end_vertex_idx;

// Compute the traversal cost
// For now, this is just Euclidean distance
edge_to_start.traversal_costs = dist_diff;
edge_to_end.traversal_costs = dist_diff;

// Add these two edges to the vertices
start_vertex.edges.push_back(edge_to_end);
end_vertex.edges.push_back(edge_to_start);

localEdgeSize_++;
}

//功能：进行碰撞检查和公式(6)定义的约束检查，对两个节点之间进行连线
void DualStateGraph::addEdge(int start_vertex_idx, int end_vertex_idx, graph_utils::TopologicalGraph &graph)
{
if (start_vertex_idx == end_vertex_idx)
{
// don't add edge to itself
return;
}

// Get the edge distance
float dist_edge =misc_utils_ns::PointXYZDist(graph.vertices[start_vertex_idx].location, graph.vertices[end_vertex_idx].location);
//直线距离大于了限制，就什么也不做（默认6m）
if (dist_edge > kMaxLongEdgeDist) { }
else
{
std::vector<int> path;
//找到两个点之间在图中的最短路径
graph_utils_ns::ShortestPathBtwVertex(path, graph, start_vertex_idx, end_vertex_idx);
bool path_exists = true;
float dist_path = 0;
//没有找到路径
if (path.empty())
{
path_exists = false;
}
else
{
//找到路径了，调轮子计算一下路径长度
dist_path = graph_utils_ns::PathLength(path, graph);
}
//即论文中的（6）式，如果欧式距离小，节点距离/欧式距离大于kExistingPathRatioThreshold，并且不发生碰撞，就进行一个连线
if ((!path_exists || (path_exists && ((dist_path / dist_edge) >=kExistingPathRatioThreshold)))
&&(!zCollisionCheck(start_vertex_idx, end_vertex_idx, graph))
&&(!grid_->collisionCheckByTerrain(graph.vertices[start_vertex_idx].location, graph.vertices[end_vertex_idx].location)))
{
Eigen::Vector3d origin;
Eigen::Vector3d end;
origin.x() = graph.vertices[start_vertex_idx].location.x;
origin.y() = graph.vertices[start_vertex_idx].location.y;
origin.z() = graph.vertices[start_vertex_idx].location.z;
end.x() = graph.vertices[end_vertex_idx].location.x;
end.y() = graph.vertices[end_vertex_idx].location.y;
end.z() = graph.vertices[end_vertex_idx].location.z;
if (volumetric_mapping::OctomapManager::CellStatus::kFree ==manager_->getLineStatusBoundingBox(origin, end, robot_bounding))
{
addEdgeWithoutCheck(start_vertex_idx, end_vertex_idx, graph);
}
}
}
}

//功能：不进行任何碰撞检查，直接对两个全局节点之间加入边（边为欧式距离）
//函数addEdgeWithoutCheck相当于可以传入图，而这个函数则是写死了对全局图进行操作。
void DualStateGraph::addGlobalEdgeWithoutCheck(int start_vertex_idx, int end_vertex_idx)
{
// Add an edge in the graph from vertex start_vertex_idx to vertex
// end_vertex_idx

// Get the two vertices
auto &start_vertex = global_graph_.vertices[start_vertex_idx];
auto &end_vertex = global_graph_.vertices[end_vertex_idx];

// Check if edge already exists -- don't duplicate
for (auto &edge : start_vertex.edges)
{
if (edge.vertex_id_end == end_vertex_idx)
{
// don't add duplicate edge
return;
}
}

// Compute the distance between the two points
float dist_diff =misc_utils_ns::PointXYZDist(start_vertex.location, end_vertex.location);

// Create two edge objects
graph_utils::Edge edge_to_start;
graph_utils::Edge edge_to_end;

// Join the two edges
edge_to_start.vertex_id_end = start_vertex_idx;
edge_to_end.vertex_id_end = end_vertex_idx;

// Compute the traversal cost
// For now, this is just Euclidean distance
edge_to_start.traversal_costs = dist_diff;
edge_to_end.traversal_costs = dist_diff;

// Add these two edges to the vertices
start_vertex.edges.push_back(edge_to_end);
end_vertex.edges.push_back(edge_to_start);

globalEdgeSize_++;
}

//功能：进行碰撞检查和公式(6)定义的约束检查，对对两个全局节点之间进行连线
//函数addEdge相当于可以传入图，而这个函数则是写死了对全局图进行操作。
void DualStateGraph::addGlobalEdge(int start_vertex_idx, int end_vertex_idx)
{
if (start_vertex_idx == end_vertex_idx)
{
return;
}

// Get the edge distance
float dist_edge = misc_utils_ns::PointXYZDist( global_graph_.vertices[start_vertex_idx].location, global_graph_.vertices[end_vertex_idx].location);

if (dist_edge > kMaxLongEdgeDist) { }
else
{
// Get the path distance
std::vector<int> path;
graph_utils_ns::ShortestPathBtwVertex(path, global_graph_, start_vertex_idx, end_vertex_idx);
bool path_exists = true;
float dist_path = 0;

// Check if there is an existing path
if (path.empty())
{
// No path exists
path_exists = false;
}
else
{
// Compute path length
dist_path = graph_utils_ns::PathLength(path, global_graph_);
// ROS_WARN("path exists of edge of length %f, where path exists of length
// %f", dist_edge, dist_path);
}

// Check if ratio is beyond threshold
// bool collision = false;

if ((!path_exists ||
(path_exists &&
((dist_path / dist_edge) >= kExistingPathRatioThresholdGlobal))) &&
(!zCollisionCheck(start_vertex_idx, end_vertex_idx, global_graph_)))
{
Eigen::Vector3d origin;
Eigen::Vector3d end;
origin.x() = global_graph_.vertices[start_vertex_idx].location.x;
origin.y() = global_graph_.vertices[start_vertex_idx].location.y;
origin.z() = global_graph_.vertices[start_vertex_idx].location.z;
end.x() = global_graph_.vertices[end_vertex_idx].location.x;
end.y() = global_graph_.vertices[end_vertex_idx].location.y;
end.z() = global_graph_.vertices[end_vertex_idx].location.z;
if (volumetric_mapping::OctomapManager::CellStatus::kFree ==manager_->getLineStatusBoundingBox(origin, end, robot_bounding))
{
addGlobalEdgeWithoutCheck(start_vertex_idx, end_vertex_idx);
}
}
}
}

//两个节点之间，俯仰角小于38度，并且z轴方向距离超过0.5m（默认）则认为两节点之间在z轴方向碰撞会发生
bool DualStateGraph::zCollisionCheck(int start_vertex_idx, int end_vertex_idx, graph_utils::TopologicalGraph graph)
{
auto &start_vertex_location = graph.vertices[start_vertex_idx].location;
auto &end_vertex_location = graph.vertices[end_vertex_idx].location;
float x_diff = start_vertex_location.x - end_vertex_location.x;
float y_diff = start_vertex_location.y - end_vertex_location.y;
float z_diff = start_vertex_location.z - end_vertex_location.z;
//计算俯仰角
float ang_diff = atan(fabs(z_diff) / sqrt(x_diff * x_diff + y_diff * y_diff)) * 180 / M_PI;
//两个节点之间，俯仰角小于38度，并且z轴方向距离超过0.5m（默认）则认为两节点之间在z轴方向碰撞会发生
if (fabs(ang_diff) > kMaxVertexAngleAlongZ ||fabs(z_diff) > kMaxVertexDiffAlongZ)
{
return true;
}
return false;
}

//获取局部图中节点数目
int DualStateGraph::getLocalVertexSize()
{
return (local_graph_.vertices.size());
}

//获取局部图的边的个数（不是长度），这个个数在加入节点的边的时候会被递增
int DualStateGraph::getLocalEdgeSize() { return localEdgeSize_; }

//获取全局节点的个数
int DualStateGraph::getGlobalVertexSize()
{
return (global_graph_.vertices.size());
}

//获取全局边的数目
int DualStateGraph::getGlobalEdgeSize() { return globalEdgeSize_; }

//功能：找到距离形参root最近的局部图节点，然后对其他每个节点，找到到达该节点的最短路径，存入pruned图中
void DualStateGraph::pruneGraph(geometry_msgs::Point root)
{
int closest_vertex_idx_to_root;
if (!local_graph_.vertices.empty())
{
closest_vertex_idx_to_root =graph_utils_ns::GetClosestVertexIdxToPoint(local_graph_, root);
auto &closest_vertex_location =local_graph_.vertices[closest_vertex_idx_to_root].location;
double distance =misc_utils_ns::PointXYZDist(root, closest_vertex_location);
Eigen::Vector3d origin(root.x, root.y, root.z);
Eigen::Vector3d end(closest_vertex_location.x, closest_vertex_location.y,closest_vertex_location.z);
//和root最近的图节点的直线距离如果大于kMaxDistToPrunedRoot（默认10），什么都不做，返回
//下面这些两个分支都是返回，意思是，如果距离最近的节点的欧式距离太大，或者有障碍物，就不加入pruned图中
if (distance > kMaxDistToPrunedRoot)
{
return;
}
else if (volumetric_mapping::OctomapManager::CellStatus::kFree !=manager_->getLineStatusBoundingBox(origin, end, robot_bounding))
{
return;
}
}

std::vector<int> path;
for (int path_id = 0; path_id < local_graph_.vertices.size(); path_id++)
{
path.clear();
//找到每一个节点，距离和root最近的图节点之间的路径，把路径存入path中
graph_utils_ns::ShortestPathBtwVertex(path, local_graph_, closest_vertex_idx_to_root, path_id);
if (path.size() == 0)
{
continue;
}
//根结点到每个节点之间的直线距离大于kMaxPrunedNodeDist（默认15）则跳过
if (misc_utils_ns::PointXYZDist( root, local_graph_.vertices[path_id].location) > kMaxPrunedNodeDist)
continue;
//遍历上面找到的路径，缓存信息，加入到pruned图中去
for (int j = 0; j < path.size(); j++)
{
tempvertex_.position = local_graph_.vertices[path[j]].location;
tempvertex_.position.z = MIN(tempvertex_.position.z, root.z);
tempvertex_.orientation.y =local_graph_.vertices[path[j]].information_gain;
addNewPrunedVertex(tempvertex_, pruned_graph_);
}
}
}

//功能：对全局图进行修建操作，凡是处于占据状态的，把连接的边给删除掉
void DualStateGraph::pruneGlobalGraph()
{
if (global_graph_.vertices.size() > 0)
{
for (int i = 0; i < global_graph_.vertices.size(); i++)
{
//对于全局图中的每一个点，生成一个位置向量
Eigen::Vector3d vertex_position(global_graph_.vertices[i].location.x, global_graph_.vertices[i].location.y,global_graph_.vertices[i].location.z);
//检查这些点的占据状态,如果被占据的，要进行剪枝操作
if (volumetric_mapping::OctomapManager::CellStatus::kFree !=manager_->getCellStatusPoint(vertex_position))
{
//依次遍历每个占据节点的边
for (int j = 0; j < global_graph_.vertices[i].edges.size(); j++)
{
int end_vertex_id = global_graph_.vertices[i].edges[j].vertex_id_end;
//擦除掉处于占据状态的边
for (int m = 0; m < global_graph_.vertices[end_vertex_id].edges.size(); m++)
{
if (global_graph_.vertices[end_vertex_id].edges[m].vertex_id_end ==i)
{
global_graph_.vertices[end_vertex_id].edges.erase(global_graph_.vertices[end_vertex_id].edges.begin() + m);
break;
}
}
}
}
}
}
}

//设置当前图管理器的状态
void DualStateGraph::setCurrentPlannerStatus(bool status)
{
planner_status_ = status;
}

//清空局部图
void DualStateGraph::clearLocalGraph()
{
local_graph_.vertices.clear();
track_localvertex_idx_ = 0;
robot_yaw_ = 0.0;
localEdgeSize_ = 0;
best_gain_ = 0;
best_vertex_id_ = 0;
gainID_.clear();
}

//这块内容实现的是公式4和5，得到当前区域朝着探索方向的最大的gain，以及节点号
double DualStateGraph::getGain(geometry_msgs::Point robot_position)
{
//遍历局部图的每一个顶点,目的应该是求出各个节点的gain，(5)式以及包括(4)式后面e的DTW次方等内容
for (auto &graph_vertex : local_graph_.vertices)
{
if (graph_vertex.vertex_id > 0)
{
// Get the path distance
std::vector<int> path;
//得到从局部图根结点出发到各个点的最短路径
graph_utils_ns::ShortestPathBtwVertex(path, local_graph_, 0, graph_vertex.vertex_id);
bool path_exists = true;
float dist_path = 0;

// Check if there is an existing path
if (path.empty())
{
// No path exists
path_exists = false;
graph_vertex.information_gain = 0;
continue;
}
else
{
// Compute path length
// 得到最短路径的长度
dist_path = graph_utils_ns::PathLength(path, local_graph_);
}
int NodeCountArround = 1;

//再次遍历，从当前节点，到局部图其他各个节点之间的欧式距离
for (auto &vertex : local_graph_.vertices)
{
float dist_edge =misc_utils_ns::PointXYZDist(graph_vertex.location, vertex.location);
//欧式距离小于kSurroundRange（默认4），则统计数目+1
if (dist_edge < kSurroundRange)
{
NodeCountArround++;
}
}
//图中各节点距离机器人位置的欧式距离小于kMinGainRange（默认4），则信息增益为0
if (misc_utils_ns::PointXYZDist(graph_vertex.location, robot_position) <kMinGainRange)
graph_vertex.information_gain = 0;

if (graph_vertex.information_gain > 0)
{
//explore_direction_未定时DTWvalue默认为0?
//需要关注explore_direction_从哪里冒出来的: updateExploreDirection函数中计算的
if (std::isnan(explore_direction_.x()) ||std::isnan(explore_direction_.y()))
DTWValue_ = exp(1);
else
{
//计算这条路径对应的DTW值，结果保存在全局变量DTWValue_ 中
DTW(path, robot_position);
//公式与论文中描述的不一致,不能理解原因，已发邮件请教
graph_vertex.information_gain =graph_vertex.information_gain / log(DTWValue_ * kDirectionCoeff);
}
}
graph_vertex.information_gain = graph_vertex.information_gain *kDegressiveCoeff / dist_path *exp(0.1 * NodeCountArround);
}
}
//求完了各个节点的gain，要累加出一条路径的gain,即公式（4）
for (auto &graph_vertex : local_graph_.vertices)
{
if (graph_vertex.vertex_id > 0)
{
double path_information_gain = 0;
std::vector<int> path;
graph_utils_ns::ShortestPathBtwVertex(path, local_graph_, 0,graph_vertex.vertex_id);
if (path.empty())
{
// No path exists
continue;
}
//pathgain为累加，即该式为公式(4),也可以看出节点的gain包含了DTW那一项
for (int i = 0; i < path.size(); i++)
{
path_information_gain +=local_graph_.vertices[path[i]].information_gain;
}

//gainID保存gain为正的顶点（根据论文内容来看，应该是要加入到全局节点中）
if (path_information_gain > 0)
{
gainID_.push_back(graph_vertex.vertex_id);
}

//找到最大的gain，保存最好的节点的节点号，以及gain值
if (path_information_gain > best_gain_)
{
best_vertex_id_ = graph_vertex.vertex_id;
best_gain_ = path_information_gain;
}
}
}
// ROS_INFO("Best gain is %f.\n Best vertex id is %d", best_gain_,
// best_vertex_id_);
return best_gain_;
}

//关于DTW的原理，这个文章写的最清晰明了https://zhuanlan.zhihu.com/p/43247215
//函数DTW被函数getGain调用，形参path为从局部图根结点出发，到各个点的最短路径
//这个函数的目的应该是计算这条path，距离各种“当前机器人位置出发到探索区域的路径”之间的差异，即公式4中的DTW函数
void DualStateGraph::DTW(std::vector<int> path, geometry_msgs::Point robot_position)
{
float dist_path = 0;
//node_num定义为路径长度
int node_num = path.size();
//dist_path理解为平均路径点之间的长度
dist_path = graph_utils_ns::PathLength(path, local_graph_) / node_num;
std::vector<geometry_msgs::Point> exp_node;
geometry_msgs::Point node_position;
//机器人的当前位置，沿着探索方向，以平均路径点长度distpath依次增长式递增
for (int i = 0; i < node_num; i++)
{
node_position.x =robot_position.x + explore_direction_.x() * dist_path * (i + 1);
node_position.y =robot_position.y + explore_direction_.y() * dist_path * (i + 1);
exp_node.push_back(node_position);
}

std::vector<std::vector<double>> DTWValue;
std::vector<double> sub_DTWValue;
//DTWvalue是DTW表，初始化为很大的值1000000，把第一个值赋为0
for (int i = 0; i < node_num + 1; i++)
{
for (int j = 0; j < node_num + 1; j++)
{
sub_DTWValue.push_back(1000000);
}
DTWValue.push_back(sub_DTWValue);
sub_DTWValue.clear();
}
DTWValue[0][0] = 0;

double dist = 0;
for (int i = 1; i < node_num + 1; i++)
{
for (int j = 1; j < node_num + 1; j++)
{
//DTW表中，行为局部图的点，列为探索方向的点，之间的距离dist为欧式距离
dist = misc_utils_ns::PointXYDist(local_graph_.vertices[path[i - 1]].location, exp_node[j - 1]);
//动态规划，更新求解，从i到j之间的最短距离
DTWValue[i][j] = dist + std::fmin(std::fmin(DTWValue[i - 1][j], DTWValue[i][j - 1]),DTWValue[i - 1][j - 1]);
}
}
DTWValue_ = DTWValue[node_num][node_num];
}

//局部图中的点加入到全局图中
void DualStateGraph::updateGlobalGraph()
{
std::vector<int> path;
//计算从0号点出发，到最好顶点的最短路径，存入path中
graph_utils_ns::ShortestPathBtwVertex(path, local_graph_, 0, best_vertex_id_);
//遍历这条路径，存入到全局图中
for (int i = 0; i < path.size(); i++)
{
tempvertex_.position = local_graph_.vertices[path[i]].location;
tempvertex_.orientation.y = local_graph_.vertices[path[i]].information_gain;
addNewGlobalVertexWithoutDuplicates(tempvertex_);
}
//gainID_是在函数getGain中得到的，具体表现为gain值为正的顶点，在这里根据论文内容，要加入到全局图中
if (gainID_.size() > 0)
{
for (int i = 0; i < gainID_.size(); i++)
{
path.clear();
graph_utils_ns::ShortestPathBtwVertex(path, local_graph_, 0, gainID_[i]);
for (int j = 0; j < path.size(); j++)
{
tempvertex_.position = local_graph_.vertices[path[j]].location;
tempvertex_.orientation.y =local_graph_.vertices[path[j]].information_gain;
addNewGlobalVertexWithoutDuplicates(tempvertex_);
}
}
}
//发布全局图
publishGlobalGraph();
}

//根据计算得到的best_vertex_id_，得到从0节点到best_vertex_id_节点的探索方向的单位向量
void DualStateGraph::updateExploreDirection()
{
std::vector<int> path;
//得到从local图的0节点到最佳探索节点的一条最短路径
graph_utils_ns::ShortestPathBtwVertex(path, local_graph_, 0, best_vertex_id_);
if (path.empty())
{
return;
}
//下面的这个if和else没有任何作用，因为完全一样，都是得到一个探索方向的向量
if (path.size() > 5)
{
explore_direction_[0] =local_graph_.vertices[path[path.size() - 1]].location.x -local_graph_.vertices[path[0]].location.x;
explore_direction_[1] =local_graph_.vertices[path[path.size() - 1]].location.y -local_graph_.vertices[path[0]].location.y;
}
else
{
explore_direction_[0] =local_graph_.vertices[path[path.size() - 1]].location.x -local_graph_.vertices[path[0]].location.x;
explore_direction_[1] =local_graph_.vertices[path[path.size() - 1]].location.y -local_graph_.vertices[path[0]].location.y;
}
//得到一个归一化的探索方向的向量
double length = sqrt(explore_direction_[0] * explore_direction_[0] + explore_direction_[1] * explore_direction_[1]);
explore_direction_[0] = explore_direction_[0] / length;
explore_direction_[1] = explore_direction_[1] / length;
}

//返回探索方向
Eigen::Vector3d DualStateGraph::getExploreDirection()
{
Eigen::Vector3d exploreDirection(explore_direction_[0], explore_direction_[1], explore_direction_[2]);
return exploreDirection;
}

//返回局部图中最好点的位置
geometry_msgs::Point DualStateGraph::getBestLocalVertexPosition()
{
geometry_msgs::Point best_vertex_location =local_graph_.vertices[best_vertex_id_].location;
return best_vertex_location;
}

//返回全局图中最好点的位置
geometry_msgs::Point DualStateGraph::getBestGlobalVertexPosition()
{
geometry_msgs::Point best_vertex_location =local_graph_.vertices[best_vertex_id_].location;
return best_vertex_location;
}

//监听/state_estimation_at_scan
//这个由仿真器附带的代码发布，作用暂时未知，监听到一次，就把它作为关键帧加入到全局图中
void DualStateGraph::keyposeCallback(const nav_msgs::Odometry::ConstPtr &msg)
{
geometry_msgs::Pose keypose;
keypose.position.x = msg->pose.pose.position.x;
keypose.position.y = msg->pose.pose.position.y;
keypose.position.z = msg->pose.pose.position.z;
//方位的y赋值为0，这个变量在创建的时候，会被赋值为图顶点的information_gain，可见这里不是代表真实意义，只是拿来暂存
keypose.orientation.y = 0;
addNewGlobalVertexWithKeypose(keypose);
}

//监听/graph_planner_path,在局部图中找到离路径点最近的顶点，进行碰撞检查，擦除掉不合适的顶点之间的边
//这个是被谁发布的？后续需要注意
void DualStateGraph::pathCallback(const nav_msgs::Path::ConstPtr &graph_path)
{
if (graph_path->poses.size() > 2)
{
//对路径中每一个元素进行遍历
for (int i = 1; i < graph_path->poses.size() - 1; i++)
{
int origin_vertex_id;
int end_vertex_id;
geometry_msgs::Point origin_position;
geometry_msgs::Point end_position;
//在局部图中找到第i个路径点最近的顶点
origin_vertex_id = graph_utils_ns::GetClosestVertexIdxToPoint(local_graph_, graph_path->poses[i].pose.position);
//找到第i个点相邻的第i+1个路径点在局部图中最近的顶点
end_vertex_id = graph_utils_ns::GetClosestVertexIdxToPoint(local_graph_, graph_path->poses[i + 1].pose.position);
//获取这两个顶点的位置，并用vector表示
origin_position = local_graph_.vertices[origin_vertex_id].location;
end_position = local_graph_.vertices[end_vertex_id].location;
Eigen::Vector3d origin(origin_position.x, origin_position.y,origin_position.z);
Eigen::Vector3d end(end_position.x, end_position.y, end_position.z);

//获取第i个点距离机器人此时位置的直线距离
double distance =misc_utils_ns::PointXYZDist(origin_position, robot_pos_);
//如果具有障碍物，要把局部图中的顶点之间的边给擦除掉
if (volumetric_mapping::OctomapManager::CellStatus::kFree !=manager_->getLineStatus(origin, end)
||(kCropPathWithTerrain && distance < kMinDistanceToRobotToCheck
&&grid_->collisionCheckByTerrain(origin_position, end_position)))
{
for (int j = 0;j < local_graph_.vertices[origin_vertex_id].edges.size(); j++)
{
//连接“离i最近的图节点”和“离i+1最近的图节点”之间的边进行擦除
if (local_graph_.vertices[origin_vertex_id].edges[j].vertex_id_end ==end_vertex_id)
{
local_graph_.vertices[origin_vertex_id].edges.erase(local_graph_.vertices[origin_vertex_id].edges.begin() + j);
if (planner_status_ == true)
{
global_graph_.vertices[origin_vertex_id].edges.erase(global_graph_.vertices[origin_vertex_id].edges.begin() + j);
}
break;
}
}
for (int j = 0; j < local_graph_.vertices[end_vertex_id].edges.size();j++)
{
//反向擦除，双向边
if (local_graph_.vertices[end_vertex_id].edges[j].vertex_id_end ==origin_vertex_id)
{
local_graph_.vertices[end_vertex_id].edges.erase(local_graph_.vertices[end_vertex_id].edges.begin() + j);
if (planner_status_ == true)
{
global_graph_.vertices[end_vertex_id].edges.erase(global_graph_.vertices[end_vertex_id].edges.begin() + j);
}
break;
}
}
//发布局部图
execute();
break;
}
}
}
//发布局部图
execute();
}

//订阅/graph_planner_status（由graph_planner节点发布），
//当状态处于false的时候，对全局图进行剪枝优化和优化，以及发布
void DualStateGraph::graphPlannerStatusCallback(const graph_planner::GraphPlannerStatusConstPtr &status)
{
graph_planner::GraphPlannerStatus prev_status = graph_planner_status_;
graph_planner_status_ = *status;
if (prev_status.status != graph_planner_status_.status &&graph_planner_status_.status ==graph_planner::GraphPlannerStatus::STATUS_IN_PROGRESS)
{
if (planner_status_ == false)
{
pruneGlobalGraph();
publishGlobalGraph();
}
}
}

//图管理器的构造函数定义
DualStateGraph::DualStateGraph(const ros::NodeHandle &nh, const ros::NodeHandle &nh_private,
volumetric_mapping::OctomapManager *manager,OccupancyGrid *grid)
: nh_(nh), nh_private_(nh_private)
{
manager_ = manager;
grid_ = grid;
initialize();
}

DualStateGraph::~DualStateGraph() {}

//变量定义，sub和pub定义
bool DualStateGraph::initialize()
{
best_gain_ = 0;
best_vertex_id_ = 0;
explore_direction_[0] = 0;
explore_direction_[1] = 0;
explore_direction_[2] = 0;
globalEdgeSize_ = 0;
localEdgeSize_ = 0;
robot_yaw_ = 0.0;
track_localvertex_idx_ = 0;
track_globalvertex_idx_ = 0;
DTWValue_ = 0;

// Read in parameters
if (!readParameters())
return false;

// Initialize subscriber
key_pose_sub_ = nh_.subscribe<nav_msgs::Odometry>(sub_keypose_topic_, 5, &DualStateGraph::keyposeCallback, this);
graph_planner_path_sub_ = nh_.subscribe<nav_msgs::Path>(sub_path_topic_, 1, &DualStateGraph::pathCallback, this);
graph_planner_status_sub_ = nh_.subscribe<graph_planner::GraphPlannerStatus>(sub_graph_planner_status_topic_, 1,
&DualStateGraph::graphPlannerStatusCallback, this);

// Initialize publishers
local_graph_pub_ =nh_.advertise<graph_utils::TopologicalGraph>(pub_local_graph_topic_, 2);
global_graph_pub_ =nh_.advertise<graph_utils::TopologicalGraph>(pub_global_graph_topic_, 2);
graph_points_pub_ =nh_.advertise<sensor_msgs::PointCloud2>(pub_global_points_topic_, 2);

ROS_INFO("Successfully launched DualStateGraph node");

return true;
}

//以两种形式来发布局部图
bool DualStateGraph::execute()
{
// Update the graph
publishLocalGraph();

return true;
}

}

// namespace dsvplanner_ns

drrtp.cpp

/*
drrtp.cpp
Implementation of drrt_planner class
Created by Hongbiao Zhu (hongbiaz@andrew.cmu.edu)
05/25/2020
*/

#include <eigen3/Eigen/Dense>

#include <visualization_msgs/Marker.h>

#include <dsvplanner/drrtp.h>

using namespace Eigen;

dsvplanner_ns::drrtPlanner::drrtPlanner(const ros::NodeHandle &nh, const ros::NodeHandle &nh_private) : nh_(nh), nh_private_(nh_private)
{
manager_ = new volumetric_mapping::OctomapManager(nh_, nh_private_);
grid_ = new OccupancyGrid(nh_, nh_private_);
//定义一个全局图
dual_state_graph_ = new DualStateGraph(nh_, nh_private_, manager_, grid_);
//定义一个全局边界求解器
dual_state_frontier_ =new DualStateFrontier(nh_, nh_private_, manager_, grid_);
//定义一个drrt求解器
drrt_ = new Drrt(manager_, dual_state_graph_, dual_state_frontier_, grid_);

init();
drrt_->setParams(params_);
drrt_->init();

ROS_INFO("Successfully launched DSVP node");
}

//析构操作
dsvplanner_ns::drrtPlanner::~drrtPlanner()
{
if (manager_) {
delete manager_;
}
if (dual_state_graph_) {
delete dual_state_graph_;
}
if (dual_state_frontier_) {
delete dual_state_frontier_;
}
if (grid_) {
delete grid_;
}
if (drrt_) {
delete drrt_;
}
}

bool dsvplanner_ns::drrtPlanner::init()
{
if (!setParams())
{
ROS_ERROR("Set parameters fail. Cannot start planning!");
}

odomSub_ = nh_.subscribe(odomSubTopic, 10,&dsvplanner_ns::drrtPlanner::odomCallback, this);

params_.newTreePathPub_ = nh_.advertise<visualization_msgs::Marker>(newTreePathPubTopic, 1000);
params_.remainingTreePathPub_ = nh_.advertise<visualization_msgs::Marker>(remainingTreePathPubTopic, 1000);
params_.boundaryPub_ =nh_.advertise<visualization_msgs::Marker>(boundaryPubTopic, 1000);
params_.globalSelectedFrontierPub_ = nh_.advertise<sensor_msgs::PointCloud2>(globalSelectedFrontierPubTopic, 1000);
params_.localSelectedFrontierPub_ = nh_.advertise<sensor_msgs::PointCloud2>(localSelectedFrontierPubTopic, 1000);
params_.randomSampledPointsPub_ = nh_.advertise<sensor_msgs::PointCloud2>(randomSampledPointsPubTopic, 1000);
params_.plantimePub_ =nh_.advertise<std_msgs::Float32>(plantimePubTopic, 1000);
params_.nextGoalPub_ =nh_.advertise<geometry_msgs::PointStamped>(nextGoalPubTopic, 1000);
params_.shutdownSignalPub =nh_.advertise<std_msgs::Bool>(shutDownTopic, 1000);

plannerService_ = nh_.advertiseService(plannerServiceName, &dsvplanner_ns::drrtPlanner::plannerServiceCallback,this);
cleanFrontierService_ = nh_.advertiseService(cleanFrontierServiceName,&dsvplanner_ns::drrtPlanner::cleanFrontierServiceCallback, this);
return true;
}

//读取配置参数
bool dsvplanner_ns::drrtPlanner::setParams() {
nh_private_.getParam("/rm/kSensorPitch", params_.sensorPitch);
nh_private_.getParam("/rm/kSensorHorizontal", params_.sensorHorizontalView);
nh_private_.getParam("/rm/kSensorVertical", params_.sensorVerticalView);
nh_private_.getParam("/rm/kVehicleHeight", params_.kVehicleHeight);
nh_private_.getParam("/rm/kBoundX", params_.boundingBox[0]);
nh_private_.getParam("/rm/kBoundY", params_.boundingBox[1]);
nh_private_.getParam("/rm/kBoundZ", params_.boundingBox[2]);
nh_private_.getParam("/drrt/gain/kFree", params_.kGainFree);
nh_private_.getParam("/drrt/gain/kOccupied", params_.kGainOccupied);
nh_private_.getParam("/drrt/gain/kUnknown", params_.kGainUnknown);
nh_private_.getParam("/drrt/gain/kMinEffectiveGain",params_.kMinEffectiveGain);
nh_private_.getParam("/drrt/gain/kRange", params_.kGainRange);
nh_private_.getParam("/drrt/gain/kRangeZMinus", params_.kGainRangeZMinus);
nh_private_.getParam("/drrt/gain/kRangeZPlus", params_.kGainRangeZPlus);
nh_private_.getParam("/drrt/gain/kZero", params_.kZeroGain);
nh_private_.getParam("/drrt/tree/kExtensionRange", params_.kExtensionRange);
nh_private_.getParam("/drrt/tree/kMinExtensionRange",params_.kMinextensionRange);
nh_private_.getParam("/drrt/tree/kMaxExtensionAlongZ",params_.kMaxExtensionAlongZ);
nh_private_.getParam("/rrt/tree/kExactRoot", params_.kExactRoot);
nh_private_.getParam("/drrt/tree/kCuttoffIterations",params_.kCuttoffIterations);
nh_private_.getParam("/drrt/tree/kGlobalExtraIterations",params_.kGlobalExtraIterations);
nh_private_.getParam("/drrt/tfFrame", params_.explorationFrame);
nh_private_.getParam("/drrt/vertexSize", params_.kVertexSize);
nh_private_.getParam("/drrt/keepTryingNum", params_.kKeepTryingNum);
nh_private_.getParam("/drrt/kLoopCountThres", params_.kLoopCountThres);
nh_private_.getParam("/lb/kMinXLocal", params_.kMinXLocalBound);
nh_private_.getParam("/lb/kMinYLocal", params_.kMinYLocalBound);
nh_private_.getParam("/lb/kMinZLocal", params_.kMinZLocalBound);
nh_private_.getParam("/lb/kMaxXLocal", params_.kMaxXLocalBound);
nh_private_.getParam("/lb/kMaxYLocal", params_.kMaxYLocalBound);
nh_private_.getParam("/lb/kMaxZLocal", params_.kMaxZLocalBound);
nh_private_.getParam("/gb/kMinXGlobal", params_.kMinXGlobalBound);
nh_private_.getParam("/gb/kMinYGlobal", params_.kMinYGlobalBound);
nh_private_.getParam("/gb/kMinZGlobal", params_.kMinZGlobalBound);
nh_private_.getParam("/gb/kMaxXGlobal", params_.kMaxXGlobalBound);
nh_private_.getParam("/gb/kMaxYGlobal", params_.kMaxYGlobalBound);
nh_private_.getParam("/gb/kMaxZGlobal", params_.kMaxZGlobalBound);
nh_private_.getParam("/elevation/kTerrainVoxelSize",params_.kTerrainVoxelSize);
nh_private_.getParam("/elevation/kTerrainVoxelWidth",params_.kTerrainVoxelWidth);
nh_private_.getParam("/elevation/kTerrainVoxelHalfWidth",params_.kTerrainVoxelHalfWidth);
nh_private_.getParam("/planner/odomSubTopic", odomSubTopic);
nh_private_.getParam("/planner/newTreePathPubTopic", newTreePathPubTopic);
nh_private_.getParam("/planner/remainingTreePathPubTopic",remainingTreePathPubTopic);
nh_private_.getParam("/planner/boundaryPubTopic", boundaryPubTopic);
nh_private_.getParam("/planner/globalSelectedFrontierPubTopic",globalSelectedFrontierPubTopic);
nh_private_.getParam("/planner/localSelectedFrontierPubTopic",localSelectedFrontierPubTopic);
nh_private_.getParam("/planner/plantimePubTopic", plantimePubTopic);
nh_private_.getParam("/planner/nextGoalPubTopic", nextGoalPubTopic);
nh_private_.getParam("/planner/randomSampledPointsPubTopic",randomSampledPointsPubTopic);
nh_private_.getParam("/planner/shutDownTopic", shutDownTopic);
nh_private_.getParam("/planner/plannerServiceName", plannerServiceName);
nh_private_.getParam("/planner/cleanFrontierServiceName",cleanFrontierServiceName);

return true;
}

//这个比较重要，是drrt求解器中，机器人位姿的来源，是由这个回调函数监听并传入drrt求解器中的
void dsvplanner_ns::drrtPlanner::odomCallback(const nav_msgs::Odometry &pose) {
drrt_->setRootWithOdom(pose);
// Planner is now ready to plan.
drrt_->plannerReady_ = true;
}

//服务器/drrtPlannerSrv，被exploration_with_graph_planner.cpp所调用
bool dsvplanner_ns::drrtPlanner::plannerServiceCallback(
dsvplanner::dsvplanner_srv::Request &req,
dsvplanner::dsvplanner_srv::Response &res)
{
plan_start_ = std::chrono::steady_clock::now();
// drrt_->gotoxy(0, 10); // Go to the specific line on the screen
// Check if the planner is ready.
//一些状态检查
if (!drrt_->plannerReady_)
{
std::cout << "No odometry. Planner is not ready!" << std::endl;
return true;
}
if (manager_ == NULL)
{
std::cout << "No octomap. Planner is not ready!" << std::endl;
return true;
}
if (manager_->getMapSize().norm() <= 0.0)
{
std::cout << "Octomap is empty. Planner is not set up!" << std::endl;
return true;
}

// set terrain points and terrain voxel elevation
//调用drrt求解器对应的函数，设置地形的体素网格和对应的高度
//根据dual_state_frontier_->getTerrainVoxelElev()，填充drrt_中的terrain_voxle_elev_变量
drrt_->setTerrainVoxelElev();

// Clear old tree and the last global frontier.
//清空并根据当前状态更新系统（已知与未知之间的）边界
cleanLastSelectedGlobalFrontier();
drrt_->clear();
// Reinitialize.
//每次进程向服务器请求路径时，服务器用于初始化，
//具体是根据全局规划状态global_plan_来判断是在探索状态还是relocation状态
//并且发布边界点
drrt_->plannerInit();

// Iterate the tree construction method.
int loopCount = 0;

//如果当前存在边界
while (ros::ok() && drrt_->remainingFrontier_ &&
//drrt求解器中的节点数小于kCuttoffIterations(默认200)
drrt_->getNodeCounter() < params_.kCuttoffIterations &&
//normal_local_iteration_为false, 即当前系统没边界，但是系统想等待边界更新
//或(全局节点数不大于kVertexSize默认120，或没有找到了某个节点获得的信息量增益大于kZeroGain默认0)
!(drrt_->normal_local_iteration_ && (drrt_->getNodeCounter() >= params_.kVertexSize && drrt_->gainFound() ) )
)
{
//限制迭代的次数
if (loopCount > drrt_->loopCount_ * (drrt_->getNodeCounter() + 1))
{
break;
}
//功能：采样新的点，（靠近边界，或者机器人附近）,加入到图中
drrt_->plannerIterate();
loopCount++;
}

// Publish rrt
//把rrt的结果给发布出去，用以rviz显示或其他用途，并计算规划时间
drrt_->publishNode();
std::cout << " New node number is " << drrt_->getNodeCounter() << "\n"
<< " Current local RRT size is "
<< dual_state_graph_->getLocalVertexSize() << "\n"
<< " Current global graph size is "
<< dual_state_graph_->getGlobalVertexSize() << std::endl;
RRT_generate_over_ = std::chrono::steady_clock::now();
time_span = RRT_generate_over_ - plan_start_;
double rrtGenerateTime = double(time_span.count()) *std::chrono::steady_clock::period::num /std::chrono::steady_clock::period::den;

// Reset planner state
//重置drrt求解器中的一些状态变量，均为bool型变量
//上一次是否处于全局规划状态
drrt_->global_plan_pre_ = drrt_->global_plan_;
//globalplan应该是全局规划状态
drrt_->global_plan_ = false;
//localplan应该是局部窗口内的规划状态
drrt_->local_plan_ = false;

// update planner state of last iteration
//设置边界规划器的状态
dual_state_frontier_->setPlannerStatus(drrt_->global_plan_pre_);

// Update planner state of next iteration
geometry_msgs::Point robot_position;
robot_position.x = drrt_->root_[0];
robot_position.y = drrt_->root_[1];
robot_position.z = drrt_->root_[2];
//drrt求解器没有发现下一个要去的全局节点，并且上一时刻处于全局规划状态，并且全局图中也没有一个点还能有新的信息增益
if (!drrt_->nextNodeFound_ && drrt_->global_plan_pre_ && drrt_->gainFound() <= 0)
{
//认为探索已经完成，可以返航了
drrt_->return_home_ = true;
geometry_msgs::Point home_position;
home_position.x = 0;
home_position.y = 0;
home_position.z = 0;
res.goal.push_back(home_position);
//把模式设为2
res.mode.data = 2; // mode 2 means returning home
//通知边界管理器，把无用的边界节点清除掉
dual_state_frontier_->cleanAllUselessFrontiers();
return true;
}
//drrt求解器没有发现下一个节点，并且上一时刻并不是处于全局规划阶段，当前位置没有信息增益
else if (!drrt_->nextNodeFound_ && !drrt_->global_plan_pre_ && dual_state_graph_->getGain(robot_position) <= 0)
{
//认为当前位置的边界已经探索完成，接下来进入relocation阶段，准备出发去下一个全局边界处
//当前处于全局规划阶段
drrt_->global_plan_ = true;
std::cout << " No Remaining local frontiers "
<< "\n"
<< " Switch to relocation stage "
<< "\n"
<< " Total plan lasted " << 0 << std::endl;
return true;
}
else
{
//当前局部窗口内还存在边界点，当前处于局部窗口探索规划阶段
drrt_->local_plan_ = true;
}
//计算一下时间，用以调试输出
gain_computation_over_ = std::chrono::steady_clock::now();
time_span = gain_computation_over_ - RRT_generate_over_;
double getGainTime = double(time_span.count()) * std::chrono::steady_clock::period::num / std::chrono::steady_clock::period::den;

// Extract next goal.
geometry_msgs::Point next_goal_position;
//nextNodeFound_表示发现了下一时刻应该去的全局节点位置
if (drrt_->nextNodeFound_)
{
dual_state_graph_->best_vertex_id_ = drrt_->NextBestNodeIdx_;
dual_state_graph_->updateExploreDirection();
next_goal_position = dual_state_graph_->getBestGlobalVertexPosition();
}
//未发现下一个节点，但全局规划之前处于true状态，并且有新的信息增益
else if (drrt_->global_plan_pre_ == true && drrt_->gainFound())
{
dual_state_graph_->best_vertex_id_ = drrt_->bestNodeId_;
dual_state_graph_->updateExploreDirection();
//这种情况下，下一个目标点是局部点
next_goal_position = dual_state_graph_->getBestLocalVertexPosition();
}
//未发现下一个该去的节点，之前处于探索阶段，就应该更新全局图
else
{
dual_state_graph_->updateGlobalGraph();
dual_state_graph_->updateExploreDirection();
next_goal_position = dual_state_graph_->getBestLocalVertexPosition();
}
dual_state_graph_->setCurrentPlannerStatus(drrt_->global_plan_pre_);
//将下一个目标点装入服务器应答，进行返回
//但是此处目测，只装了一次答复，
//因此我们可以得知，在exploration_with_graph_planner.cpp中，请求到的服务，只返回一个路径点(虽然里面写成了循环的形式)
res.goal.push_back(next_goal_position);
res.mode.data = 1; // mode 1 means exploration

geometry_msgs::PointStamped next_goal_point;
next_goal_point.header.frame_id = "/map";
next_goal_point.point = next_goal_position;
//"topic name: next_goal"
params_.nextGoalPub_.publish(next_goal_point);

plan_over_ = std::chrono::steady_clock::now();
time_span = plan_over_ - plan_start_;
double plantime = double(time_span.count()) * std::chrono::steady_clock::period::num / std::chrono::steady_clock::period::den;
std::cout << " RRT generation lasted " << rrtGenerateTime << "\n"
<< " Computiong gain lasted " << getGainTime << "\n"
<< " Total plan lasted " << plantime << std::endl;
return true;
}

//用来清除边界点的服务，如果nextNodeFound_，发现了下一个要去的节点
//（但是执行的时候发现去不了，所以才会请求这个服务把边界给清除掉)
//就把边界点给清除掉并及时更新边界管理器
bool dsvplanner_ns::drrtPlanner::cleanFrontierServiceCallback(
dsvplanner::clean_frontier_srv::Request &req,
dsvplanner::clean_frontier_srv::Response &res) {
if (drrt_->nextNodeFound_)
{
dual_state_frontier_->updateToCleanFrontier(drrt_->selectedGlobalFrontier_);
dual_state_frontier_->gloabalFrontierUpdate();
}
else
{
dual_state_graph_->clearLocalGraph();
}
res.success = true;

return true;
}

//检查drrt求解器是否发现了下一个节点，利用边界维护器更新边界，清空较老的边界，
void dsvplanner_ns::drrtPlanner::cleanLastSelectedGlobalFrontier() {
// only when last plan is global plan, this function will be executed to clear last selected global frontier.
if (drrt_->nextNodeFound_)
{
dual_state_frontier_->updateToCleanFrontier(drrt_->selectedGlobalFrontier_);
dual_state_frontier_->gloabalFrontierUpdate();
}
}

drrt.cpp

/*
drrt.cpp
Implementation of Drrt class. Dynamic tree is used to get random viewpoints in local area.
New rrt is generated based on the pruned tree of last time.
Created by Hongbiao Zhu (hongbiaz@andrew.cmu.edu)
05/25/2020
*/

#ifndef RRTTREE_HPP_
#define RRTTREE_HPP_

#include <cstdlib>
#include <dsvplanner/drrt.h>

dsvplanner_ns::Drrt::Drrt(volumetric_mapping::OctomapManager *manager,
DualStateGraph *graph, DualStateFrontier *frontier,
OccupancyGrid *grid)
{
manager_ = manager;
grid_ = grid;
dual_state_graph_ = graph;
dual_state_frontier_ = frontier;

ROS_INFO("Successfully launched Drrt node");
}

dsvplanner_ns::Drrt::~Drrt()
{
delete rootNode_;
kd_free(kdTree_);
}

void dsvplanner_ns::Drrt::setParams(Params params) { params_ = params; }

//初始化各个变量
void dsvplanner_ns::Drrt::init()
{
kdTree_ = kd_create(3);
iterationCount_ = 0;
bestGain_ = params_.kZeroGain;
bestNode_ = NULL;
rootNode_ = NULL;
nodeCounter_ = 0;
plannerReady_ = false;

global_plan_ = false;
global_plan_pre_ = true;
local_plan_ = true;
nextNodeFound_ = false;
remainingFrontier_ = true;
return_home_ = false;
global_vertex_size_ = 0;
NextBestNodeIdx_ = 0;
for (int i = 0; i < params_.kTerrainVoxelWidth * params_.kTerrainVoxelWidth;i++)
{
terrain_voxle_elev_.push_back(params_.kVehicleHeight);
}

srand((unsigned)time(NULL));
}

//工具函数，根据形参赋值root
void dsvplanner_ns::Drrt::setRootWithOdom(const nav_msgs::Odometry &pose) {
root_[0] = pose.pose.pose.position.x;
root_[1] = pose.pose.pose.position.y;
root_[2] = pose.pose.pose.position.z;
}

//根据dual_state_frontier_->getTerrainVoxelElev()，填充本类中的terrain_voxle_elev_变量
void dsvplanner_ns::Drrt::setTerrainVoxelElev()
{
if (dual_state_frontier_->getTerrainVoxelElev().size() > 0)
{
terrain_voxle_elev_.clear();
terrain_voxle_elev_ = dual_state_frontier_->getTerrainVoxelElev();
}
}

//工具函数，用来返回类的保护变量nodeCounter_,应该代表当前node的总数
//全局节点的数目
int dsvplanner_ns::Drrt::getNodeCounter() { return nodeCounter_; }

//工具函数，用来返回类的保护变量remainingNodeCount_
int dsvplanner_ns::Drrt::getRemainingNodeCounter()
{
return remainingNodeCount_;
}

//bestGain_大于0（默认），则返回true。bestGain_如何计算？
//每次在图里加入一个节点的时候，这个全局变量会统计最大的gain
bool dsvplanner_ns::Drrt::gainFound() { return bestGain_ > params_.kZeroGain; }

//根据余弦公式，计算两个方向向量之间的夹角，工具函数
double dsvplanner_ns::Drrt::angleDiff(StateVec direction1, StateVec direction2)
{
double degree;
degree =
acos(
(direction1[0] * direction2[0] + direction1[1] * direction2[1]) /
(sqrt(direction1[0] * direction1[0] + direction1[1] * direction1[1]) *
sqrt(direction2[0] * direction2[0] +direction2[1] * direction2[1]))) *
180 / M_PI;
return degree;
}

//清空系统缓存状态
void dsvplanner_ns::Drrt::clear()
{
delete rootNode_;
rootNode_ = NULL;

nodeCounter_ = 0;
bestGain_ = params_.kZeroGain;
bestNode_ = NULL;
if (nextNodeFound_)
{
dual_state_graph_->clearLocalGraph();
}
nextNodeFound_ = false;
remainingFrontier_ = false;
remainingNodeCount_ = 0;

sampledPoint_->points.clear();

kd_free(kdTree_);
}

//形参：一个向量（A点减去B点），函数内部计算A点相对于B点坐标系在一维坐标下的索引，
//根据这个索引得到terrain_voxel_elev_里保存的障碍物高度（加上机器人自身高度）
double dsvplanner_ns::Drrt::getZvalue(double x_position, double y_position)
{
int indX = int((x_position + params_.kTerrainVoxelSize / 2) / params_.kTerrainVoxelSize) + params_.kTerrainVoxelHalfWidth;
int indY = int((y_position + params_.kTerrainVoxelSize / 2) / params_.kTerrainVoxelSize) + params_.kTerrainVoxelHalfWidth;
if (x_position + params_.kTerrainVoxelSize / 2 < 0)
indX--;
if (y_position + params_.kTerrainVoxelSize / 2 < 0)
indY--;

//下面四个if，是要把indX和indY限制在0到kTerrainVoxelWidth（默认101）之间
if (indX > params_.kTerrainVoxelWidth - 1)
indX = params_.kTerrainVoxelWidth - 1;
if (indX < 0)
indX = 0;
if (indY > params_.kTerrainVoxelWidth - 1)
indY = params_.kTerrainVoxelWidth - 1;
if (indY < 0)
indY = 0;

//地形的表示terrain_voxle_elev_应该是一个方框，边长为kTerrainVoxelWidth
double z_position =terrain_voxle_elev_[params_.kTerrainVoxelWidth * indX + indY] +params_.kVehicleHeight;
return z_position;
}

//功能：传入传入一个方向，该方向和边界方向求和后的方向，判断延伸后的点是否能很好的观察到边界
//注意，这个函数会修改形参，传入一个方向，在边界附近查找它，然后会被更新为边界附近的点
bool dsvplanner_ns::Drrt::inSensorRange(StateVec &node)
{
//rootnode是机器人此时自身的位置
StateVec root_node(rootNode_->state_[0], rootNode_->state_[1],rootNode_->state_[2]);
//initnode是形参传入，某一个位置
StateVec init_node = node;
StateVec dir;
bool insideFieldOfView = false;
//localThreeFrontier_是局部图中的三个边界（方向）
for (int i = 0; i < localThreeFrontier_->points.size(); i++)
{
StateVec frontier_point(localThreeFrontier_->points[i].x,localThreeFrontier_->points[i].y,localThreeFrontier_->points[i].z);
//node现在更新为传入的位置沿着边界方向延伸后的位置
node[0] = init_node[0] + frontier_point[0];
node[1] = init_node[1] + frontier_point[1];
//再减去机器人自身位置，代表传入的位置沿着边界方向延伸后的位置，距离当前机器人位置的方向向量
double x_position = node[0] - root_node[0];
double y_position = node[1] - root_node[1];
node[2] = getZvalue(x_position, y_position);
if (!inPlanningBoundary(node))
continue;

//函数传入的位置沿边界扩展后的位置，距离边界的距离
dir = frontier_point - node;
// Skip if distance to sensor is too large
double rangeSq = pow(params_.kGainRange, 2.0);
//函数传入的位置沿边界扩展后的位置，距离边界的距离
//距离太大不要
if (dir.transpose().dot(dir) > rangeSq)
{
continue;
}
//这个要求在视野内，主要靠第一项判断，俯仰角小于 45度
if (fabs(dir[2] < sqrt(dir[0] * dir[0] + dir[1] * dir[1]) * tan(M_PI * params_.sensorVerticalView / 360)))
{
insideFieldOfView = true;
}

if (!insideFieldOfView)
{
continue;
}
//该节点处于空闲状态，且可以观察到边界点,返回true
if (manager_->getCellStatusPoint(node) ==volumetric_mapping::OctomapManager::CellStatus::kFree)
{
if (volumetric_mapping::OctomapManager::CellStatus::kOccupied !=this->manager_->getVisibility(node, frontier_point, false))
{
return true;
}
}
}
return false;
}

//判断点是否在规划区域内,探索阶段，则是机器人周围环境，relocation阶段，则为整个地图
//要求是在minX到maxX之间（再缩回来一点点boundingBox）
//在范围之内返回true
//否则返回false
bool dsvplanner_ns::Drrt::inPlanningBoundary(StateVec node)
{
if (node.x() < minX_ + 0.5 * params_.boundingBox.x())
{
return false;
}
else if (node.y() < minY_ + 0.5 * params_.boundingBox.y())
{
return false;
}
else if (node.z() < minZ_ + 0.5 * params_.boundingBox.z())
{
return false;
}
else if (node.x() > maxX_ - 0.5 * params_.boundingBox.x())
{
return false;
}
else if (node.y() > maxY_ - 0.5 * params_.boundingBox.y())
{
return false;
}
else if (node.z() > maxZ_ - 0.5 * params_.boundingBox.z())
{
return false;
}
else
{
return true;
}
}

//判断形参node是否在全局范围内
bool dsvplanner_ns::Drrt::inGlobalBoundary(StateVec node)
{
if (node.x() < params_.kMinXGlobalBound + 0.5 * params_.boundingBox.x())
{
return false;
}
else if (node.y() <params_.kMinYGlobalBound + 0.5 * params_.boundingBox.y())
{
return false;
}
else if (node.z() <params_.kMinZGlobalBound + 0.5 * params_.boundingBox.z())
{
return false;
}
else if (node.x() >params_.kMaxXGlobalBound - 0.5 * params_.boundingBox.x())
{
return false;
}
else if (node.y() >params_.kMaxYGlobalBound - 0.5 * params_.boundingBox.y())
{
return false;
}
else if (node.z() >params_.kMaxZGlobalBound - 0.5 * params_.boundingBox.z()) {
return false;
}
else
{
return true;
}
}

// 功能：在边界附近生成一个比较好观测的点，送入到形参newState中去
// 做法：随机延伸方向（x和y两个方向，-4到4之间），基于这个方向，判断在边界附近，这个方向能不能观察到边界
//将把边界附近观测点返回到形参中去
bool dsvplanner_ns::Drrt::generateRrtNodeToLocalFrontier(StateVec &newNode)
{
StateVec potentialNode;
bool nodeFound = false;
int count = 0;
double radius = sqrt(SQ(params_.kGainRange) + SQ(params_.kGainRange));
while (!nodeFound)
{
count++;
//迭代次数超过300次，返回false
if (count >= 300)
{
return false;
}
//rand()/ RAND_MAX 为0～1之间，再减去0.5，为-0.5～0.5之间，再乘2，为-1到1之间，
//再乘radius，为-radius到radius之间的随机数
potentialNode[0] =2.0 * radius * (((double)rand()) / ((double)RAND_MAX) - 0.5);
potentialNode[1] =2.0 * radius * (((double)rand()) / ((double)RAND_MAX) - 0.5);
potentialNode[2] = 0;
// -c<=a<=c
// -c<=b<=c
// 0<= a^2+b^2 <=2c^2
//那这里就是一半的生成直接丢掉
//相当于允许它在两个正负两个方向生长，但是模长半径不得超过radius
if ((SQ(potentialNode[0]) + SQ(potentialNode[1])) > pow(radius, 2.0))
continue;

//不能很好的观察到边界
//注意!!!这个函数会修改形参，生成的方向向量，会被改为边界附近一个合适的观测点
if (!inSensorRange(potentialNode))
{
continue;
}
//超出了规划的范围
if (!inPlanningBoundary(potentialNode))
{
continue;
}
//把新生成的点赋值给newnode
nodeFound = true;
newNode[0] = potentialNode[0];
newNode[1] = potentialNode[1];
newNode[2] = potentialNode[2];
return true;
}
}

//通过两次循环，找到距离边界最近的局部点
void dsvplanner_ns::Drrt::getNextNodeToClosestGlobalFrontier()
{
StateVec p1, p2;
pcl::PointXYZ p3;
double length1, length2;
pcl::PointCloud<pcl::PointXYZ>::Ptr globalSelectedFrontier =pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>());
nextNodeFound_ = false;
// Search from end to the begining. Nodes with large index means they are closer to the current position of the robot
for (int i = dual_state_graph_->local_graph_.vertices.size() - 1; i >= 0;i--)
{
//从大值开始倒着搜，获取顶点位置
p1.x() = dual_state_graph_->local_graph_.vertices[i].location.x;
p1.y() = dual_state_graph_->local_graph_.vertices[i].location.y;
p1.z() = dual_state_graph_->local_graph_.vertices[i].location.z;
//搜索遍历全局边界
for (int j = 0; j < dual_state_frontier_->global_frontier_->size(); j++)
{
p3 = dual_state_frontier_->global_frontier_->points[j];
p2.x() = dual_state_frontier_->global_frontier_->points[j].x;
p2.y() = dual_state_frontier_->global_frontier_->points[j].y;
p2.z() = dual_state_frontier_->global_frontier_->points[j].z;
//比较局部图的点距离边界的距离
length1 = sqrt(SQ(p1.x() - p2.x()) + SQ(p1.y() - p2.y()));
if (length1 >(this->manager_->getSensorMaxRange() + params_.kGainRange) || fabs(p1.z() - p2.z()) > params_.kMaxExtensionAlongZ)
// Not only consider the sensor range, also take the node's view range into consideration
{
continue;
// When the node is too far away from the frontier or the difference between the z of this node and frontier is too large,
//then skip to next frontier.
// No need to use FOV here.
}

if (volumetric_mapping::OctomapManager::CellStatus::kOccupied ==manager_->getVisibility(p1, p2, false))
{
continue;
// Only when there is no occupied voxels between the node and frontier, we consider the node can potentially see this frontier
}
else
{
//nextBesetNodeIdx代表下一个全局边界点附近的局部点
NextBestNodeIdx_ = i;
//nextnodefound代表找到了全局图的下一个边界点
nextNodeFound_ = true;
globalSelectedFrontier->points.push_back(p3);
//选择得到的边界位置（之后就是要去这个边界附近）
selectedGlobalFrontier_ = p3;
//找到一个就退出
break;
}
}
//找到就退出循环
if (nextNodeFound_)
break;
}
//这个比较迷惑，反正上面的循环是先找到距离当前位置最近的边界点
//然后根据这个边界点，再找到距离这个边界点最近的local节点
//即论文图3中的A、B点
if (nextNodeFound_)
{
for (int i = dual_state_graph_->local_graph_.vertices.size() - 1; i >= 0;i--)
{
p1.x() = dual_state_graph_->local_graph_.vertices[i].location.x;
p1.y() = dual_state_graph_->local_graph_.vertices[i].location.y;
p1.z() = dual_state_graph_->local_graph_.vertices[i].location.z;
length2 = sqrt(SQ(p1.x() - p2.x()) + SQ(p1.y() - p2.y()));
if (length2 > length1 ||fabs(p1.z() - p2.z()) > params_.kMaxExtensionAlongZ)
{
continue;
}
if (volumetric_mapping::OctomapManager::CellStatus::kOccupied ==manager_->getLineStatusBoundingBox(p1, p2, params_.boundingBox))
{
continue;
}
length1 = length2;
NextBestNodeIdx_ = i;
nextNodeFound_ = true;
p3.x = p1.x();
p3.y = p1.y();
p3.z = p1.z();
}
globalSelectedFrontier->points.push_back(p3);
}
sensor_msgs::PointCloud2 globalFrontier;
pcl::toROSMsg(*globalSelectedFrontier, globalFrontier);
//"/map"
globalFrontier.header.frame_id = params_.explorationFrame;
//" /globalSelectedfrontier"
params_.globalSelectedFrontierPub_.publish(globalFrontier); // publish the next goal node and corresponing frontier
}

//获取当前探索方向最近的三个边界
// Three local frontiers that are most close to the last exploration direciton will be selected.
void dsvplanner_ns::Drrt::getThreeLocalFrontierPoint()
{
StateVec exploreDirection, frontierDirection;
double firstDirection = 180, secondDirection = 180, thirdDirection = 180;
pcl::PointXYZ p1, p2, p3; // three points to save frontiers.
exploreDirection = dual_state_graph_->getExploreDirection();
int localFrontierSize = dual_state_frontier_->local_frontier_->size();

//遍历局部边界
for (int i = 0; i < localFrontierSize; i++)
{
// For ground robot, we only consider the direction along x-y plane
frontierDirection[0] = dual_state_frontier_->local_frontier_->points[i].x -root_[0];
frontierDirection[1] =dual_state_frontier_->local_frontier_->points[i].y - root_[1];
//计算探索方向和边界方向的夹角
double theta = angleDiff(frontierDirection, exploreDirection);
//夹角要和之前保存的三个方向的夹角进行对比
//小于之前保存的第一个方向夹角
if (theta < firstDirection)
{
//递推，丢掉第三个（夹角最大的那个），把theta赋给第一个
thirdDirection = secondDirection;
secondDirection = firstDirection;
firstDirection = theta;
//方向同理更新
frontier3_direction_ = frontier2_direction_;
frontier2_direction_ = frontier1_direction_;
frontier1_direction_ = frontierDirection;
//边界点同理更新
p3 = p2;
p2 = p1;
p1 = dual_state_frontier_->local_frontier_->points[i];
}
else if (theta < secondDirection)
{
//夹角大于第二个，但是比第一个小，所以存在2号位，1号位不动，三号位丢弃
thirdDirection = secondDirection;
secondDirection = theta;
frontier3_direction_ = frontier2_direction_;
frontier2_direction_ = frontierDirection;
p3 = p2;
p2 = dual_state_frontier_->local_frontier_->points[i];
}
else if (theta < thirdDirection)
{
//夹角排在第三，直接覆盖3号位置，1号2号不动
thirdDirection = theta;
frontier3_direction_ = frontierDirection;
p3 = dual_state_frontier_->local_frontier_->points[i];
}
}
// 把三个边界发布出去
localThreeFrontier_->clear();
localThreeFrontier_->points.push_back(p1);
localThreeFrontier_->points.push_back(p2);
localThreeFrontier_->points.push_back(p3);
sensor_msgs::PointCloud2 localThreeFrontier;
pcl::toROSMsg(*localThreeFrontier_, localThreeFrontier);
localThreeFrontier.header.frame_id = params_.explorationFrame;
params_.localSelectedFrontierPub_.publish(localThreeFrontier);
}

//通过调用边界管理器，查看当前是否存在局部边界点
bool dsvplanner_ns::Drrt::remainingLocalFrontier()
{
int localFrontierSize = dual_state_frontier_->local_frontier_->points.size();
if (localFrontierSize > 0)
return true;
return false;
}

//功能：采样新的点，（靠近边界，或者机器人附近）,加入到图中
void dsvplanner_ns::Drrt::plannerIterate()
{
// In this function a new configuration is sampled and added to the tree.
StateVec newState;
bool generateNodeArroundFrontierSuccess = false;

double radius = 0.5 * sqrt(SQ(minX_ - maxX_) + SQ(minY_ - maxY_));
bool solutionFound = false;
int count = 0;
while (!solutionFound)
{
count++;
//最多进行1000次迭代
if (count > 1000)
return; // Plan fail if cannot find a required node in 1000 iterations

//采样一个随机数，0.25的概率
if (((double)rand()) / ((double)RAND_MAX) > 0.75 &&localThreeFrontier_->size() > 0)
{
//处于局部规划阶段
if (local_plan_ == true)
{
//在边界附近生成一个比较好观测的点，送入到形参newState中去
generateNodeArroundFrontierSuccess =generateRrtNodeToLocalFrontier(newState);
}
// Generate node near local frontier fail
// 未能生成一个比较好观测的点
if (!generateNodeArroundFrontierSuccess)
{
//生成-radius到radius之间的向量
newState[0] =2.0 * radius * (((double)rand()) / ((double)RAND_MAX) - 0.5);
newState[1] =2.0 * radius * (((double)rand()) / ((double)RAND_MAX) - 0.5);
newState[2] = 0; // Do not consider z value because ground robot cannot move along z
//要求向量模长不得超过半径radius
if (SQ(newState[0]) + SQ(newState[1]) > pow(radius, 2.0))
continue;
//既然当前窗口没有合适观测边界的点，那就胡乱走吧，，把方向加到机器人自身位置处
newState[0] += root_[0];
newState[1] += root_[1];
newState[2] += root_[2];
//要求在局部窗口或全局内
if ((!inPlanningBoundary(newState)) && (!inGlobalBoundary(newState)))
{
continue;
}
}
}
//有0.75的概率随便选一个点，随便去走
else
{
newState[0] =2.0 * radius * (((double)rand()) / ((double)RAND_MAX) - 0.5);
newState[1] =2.0 * radius * (((double)rand()) / ((double)RAND_MAX) - 0.5);
newState[2] = 0;
if (SQ(newState[0]) + SQ(newState[1]) > pow(radius, 2.0))
continue;
newState[0] += root_[0];
newState[1] += root_[1];
newState[2] += root_[2];
if ((!inPlanningBoundary(newState)) && (!inGlobalBoundary(newState)))
{
continue;
}
}
solutionFound = true;
}

//把新选好的点加入到samplepoint中
pcl::PointXYZI sampledPoint;
sampledPoint.x = newState[0];
sampledPoint.y = newState[1];
sampledPoint.z = newState[2];
sampledPoint_->points.push_back(sampledPoint);

//kd树中找到最近的点
// Find nearest neighbour
kdres *nearest =kd_nearest3(kdTree_, newState.x(), newState.y(), newState.z());
if (kd_res_size(nearest) <= 0)
{
kd_res_free(nearest);
return;
}
//取出这个最近的点
dsvplanner_ns::Node *newParent =(dsvplanner_ns::Node *)kd_res_item_data(nearest);
kd_res_free(nearest);

// Check for collision of new connection.
//通过之前的操作，采样出一个点，然后在kd树中找到最近的点,最近的点叫newparent,也就是origin
StateVec origin(newParent->state_[0], newParent->state_[1], newParent->state_[2]);
//采样点和kd树中最近点的方向向量
StateVec direction(newState[0] - origin[0], newState[1] - origin[1], 0);
//距离太短就算了
if (direction.norm() < params_.kMinextensionRange)
{
return;
}
else if (direction.norm() > params_.kExtensionRange)
{
//沿着这个方向，但是距离给予一个限制kExtensionRange
direction = params_.kExtensionRange * direction.normalized();
}
//origin是kd树中现存的最近点，加上修正后的方向向量，得到一个延伸后的新点endpoint
StateVec endPoint = origin + direction;
//覆盖newstate变量
newState[0] = endPoint[0];
newState[1] = endPoint[1];

//延伸后的新点现在是newState，得到一个距离当前机器人位置的向量
double x_position = newState[0] - root_[0];
double y_position = newState[1] - root_[1];
newState[2] = getZvalue(x_position, y_position);

if (newState[2] >= 1000) // the sampled position is above the untraversed area
{
return;
}
// Check if the new node is too close to any existing nodes after extension
//再次进行操作，得到kd树中最近的点（迷惑行为？？）
kdres *nearest_node =kd_nearest3(kdTree_, newState.x(), newState.y(), newState.z());
if (kd_res_size(nearest_node) <= 0)
{
kd_res_free(nearest_node);
return;
}
dsvplanner_ns::Node *nearestNode =(dsvplanner_ns::Node *)kd_res_item_data(nearest_node);
kd_res_free(nearest_node);

//现在的origin是newParent，也就是最开始的采样点在kd树中最近的那个点
origin[0] = newParent->state_[0];
origin[1] = newParent->state_[1];
origin[2] = newParent->state_[2];

//现在的newstate，是延伸过的新点（最开始的采样点，经过距离等限制修正以后的新点）
//得到新点和最开始最近的那个点之间的方向
direction[0] = newState[0] - newParent->state_[0];
direction[1] = newState[1] - newParent->state_[1];
direction[2] = newState[2] - newParent->state_[2];
if (direction.norm() < params_.kMinextensionRange ||direction[2] > params_.kMaxExtensionAlongZ)
{
return;
}
// check collision if the new node is in the planning boundary
//不在局部规划范围内，就算了
if (!inPlanningBoundary(newState))
{
return;
}
else
{
//确保点的连线没有障碍物
if (volumetric_mapping::OctomapManager::CellStatus::kFree ==manager_->getLineStatusBoundingBox(origin, newState,params_.boundingBox)
&&(!grid_->collisionCheckByTerrainWithVector(origin,newState))) // connection is free
{
// Create new node and insert into tree
//把新的点插入到树中
dsvplanner_ns::Node *newNode = new dsvplanner_ns::Node;
newNode->state_ = newState;
newNode->parent_ = newParent;
newNode->distance_ = newParent->distance_ + direction.norm();
newParent->children_.push_back(newNode);
newNode->gain_ = gain(newNode->state_);

kd_insert3(kdTree_, newState.x(), newState.y(), newState.z(), newNode);

geometry_msgs::Pose p1;
p1.position.x = newState.x();
p1.position.y = newState.y();
p1.position.z = newState.z();
p1.orientation.y = newNode->gain_;
dual_state_graph_->addNewLocalVertexWithoutDuplicates(p1, dual_state_graph_->local_graph_);
dual_state_graph_->execute();

// Display new node
node_array.push_back(newNode);
// Update best IG and node if applicable
if (newNode->gain_ > bestGain_)
{
bestGain_ = newNode->gain_;
}
nodeCounter_++;
}
}
}

//每次进程向服务器请求路径时，服务器用于初始化，
//具体是根据全局规划状态global_plan_来判断是在探索状态还是relocation状态
//并且发布边界点
void dsvplanner_ns::Drrt::plannerInit()
{
// This function is to initialize the tree
//创建一个3维kd树
kdTree_ = kd_create(3);

node_array.clear();
rootNode_ = new Node;
rootNode_->distance_ = 0.0;
//根结点获取的信息量被初始化为0
rootNode_->gain_ = params_.kZeroGain;
rootNode_->parent_ = NULL;

global_vertex_size_ = 0;
geometry_msgs::Pose p1;
//全局规划状态处于false时，那么当前是在局部探索阶段
if (global_plan_ == false)
{
std::cout << "Exploration Stage" << std::endl;
//根结点被设置为机器人的初始位置
rootNode_->state_ = root_;
//把根节点加入到kd树中
kd_insert3(kdTree_, rootNode_->state_.x(), rootNode_->state_.y(),rootNode_->state_.z(), rootNode_);
iterationCount_++;
//如果目前还存在局部边界点
if (remainingLocalFrontier())
{
localPlanOnceMore_ = true;
//这个loopCount一共出现两次，一次被赋值成kLoopCountThres，一次被赋值成kLoopCountThres*3
//用处是在外部的drrtp.cpp中，限制循环次数（其实我感觉没什么用）
loopCount_ = params_.kLoopCountThres;
//这个normal_local_iteration_出现三次，
//如果当前处于探索阶段，还有边界点，它就是true，
//如果处于探索阶段并且没有边界点，系统也不打算等地图更新了，它也是true。
//只有没有边界，但是系统想等待一下地图边界更新的时候，它才是false
normal_local_iteration_ = true;
keepTryingNum_ =params_.kKeepTryingNum; // Try 1 or 2 more times even if there is no local frontier
remainingFrontier_ = true;
//获取当前探索方向最近的三个边界
getThreeLocalFrontierPoint();
//构建prune图
pruneTree(root_);
//清空局部图的节点
dual_state_graph_->clearLocalGraph();
//这一步很重要，VoxelGain获取的信息量，是通过本cpp的gain获取的，
//然后构造了prune图，pruned图又赋值给local图，说明prune图是相当于一个中间变量的作用，用来辅助构建局部图
dual_state_graph_->local_graph_ = dual_state_graph_->pruned_graph_;
//发布局部图
dual_state_graph_->execute();
}
//目前不存在边界点的情况
else
{
//暂时没边界点，不打算再尝试了
if (!localPlanOnceMore_)
{
dual_state_graph_->clearLocalGraph();
dual_state_graph_->pruned_graph_.vertices.clear();
remainingFrontier_ = false;
localPlanOnceMore_ = true;
normal_local_iteration_ = true;
}
//暂时没边界点，再尝试一下
else
{
remainingFrontier_ = true;
loopCount_ = params_.kLoopCountThres * 3;
normal_local_iteration_ = false;
localThreeFrontier_->clear();
// pruneTree(root_);
dual_state_graph_->clearLocalGraph();
dual_state_graph_->pruned_graph_.vertices.clear();
// dual_state_graph_->local_graph_ =
// dual_state_graph_->pruned_graph_;
dual_state_graph_->execute();
//这个keepTryingNum_是配置文件里给的，逐步递减，减到为0，重新赋值让它递减
//有的配置文件里这里是0
keepTryingNum_--;
if (keepTryingNum_ <= 0)
{
localPlanOnceMore_ = false;
//这个注释比较关键，在切换到relocation阶段的时候，多等等，等边界及时更新
// After switching to relocation stage, give another more chance in case that some frontiers are not updated
keepTryingNum_ = params_.kKeepTryingNum +1;
}
}
}
//kMaxXLocal在配置文件中定义，xy方向为15，z方向为5，
//会被用在gain中
//minX和maxX感觉是当前局部窗口的各个角的坐标，
//rootNode_->state_.x()是自己的位置，kMaxXLocalBound是边界框的大小（+15，-15）之类的
maxX_ = rootNode_->state_.x() + params_.kMaxXLocalBound;
maxY_ = rootNode_->state_.y() + params_.kMaxYLocalBound;
maxZ_ = rootNode_->state_.z() + params_.kMaxZLocalBound;
minX_ = rootNode_->state_.x() + params_.kMinXLocalBound;
minY_ = rootNode_->state_.y() + params_.kMinYLocalBound;
minZ_ = rootNode_->state_.z() + params_.kMinZLocalBound;
}
else
{
std::cout << "Relocation Stage" << std::endl;
localPlanOnceMore_ = true;
StateVec node1;
double gain1;
//全局图节点个数大于0
if (dual_state_graph_->global_graph_.vertices.size() > 0)
{
node1[0] = dual_state_graph_->global_graph_.vertices[0].location.x;
node1[1] = dual_state_graph_->global_graph_.vertices[0].location.y;
node1[2] = dual_state_graph_->global_graph_.vertices[0].location.z;
rootNode_->state_ = node1;
//全局图的0号节点位置加入到kd树中?
kd_insert3(kdTree_, rootNode_->state_.x(), rootNode_->state_.y(), rootNode_->state_.z(), rootNode_);

dual_state_graph_->clearLocalGraph();
//这里比较重要，把全局图赋值给了局部图
dual_state_graph_->local_graph_ = dual_state_graph_->global_graph_;
dual_state_graph_->local_graph_.vertices[0].information_gain =rootNode_->gain_;
dual_state_graph_->execute();
//寻找距离全局边界最近的局部点(其实这时候的局部图是全局图)
getNextNodeToClosestGlobalFrontier();
//找到了下一个要去的全局节点
if (nextNodeFound_)
{
dual_state_graph_->local_graph_.vertices[NextBestNodeIdx_] .information_gain = 300000; // set a large enough value as the best gain
bestGain_ = 300000;
nodeCounter_ = dual_state_graph_->global_graph_.vertices.size();
global_vertex_size_ = nodeCounter_;
dual_state_graph_->publishGlobalGraph();
}
//没有找到全局边界，重建rrt
else
{
// Rebuild the rrt accordingt to current graph and then extend in plannerIterate.
// This only happens when no global frontiers can be seen.
// Mostly used at the end of the exploration in case that there are some narrow areas are ignored.
for (int i = 1; i < dual_state_graph_->global_graph_.vertices.size();i++)
{
p1.position = dual_state_graph_->global_graph_.vertices[i].location;
node1[0] = p1.position.x;
node1[1] = p1.position.y;
node1[2] = p1.position.z;
//找到kd树中最近的点
kdres *nearest =kd_nearest3(kdTree_, node1.x(), node1.y(), node1.z());
if (kd_res_size(nearest) <= 0)
{
kd_res_free(nearest);
continue;
}
//取出这个节点,nearest中只装一个值
dsvplanner_ns::Node *newParent =(dsvplanner_ns::Node *)kd_res_item_data(nearest);
kd_res_free(nearest);

StateVec origin(newParent->state_[0], newParent->state_[1], newParent->state_[2]);
//global图里每一个点，到它在kd树中查找得到的最近点的方向向量
StateVec direction(node1[0] - origin[0], node1[1] - origin[1],node1[2] - origin[2]);

//对于Vector，norm返回的是向量的二范数
//对于Matrix，norm返回的是矩阵的弗罗贝尼乌斯范数（Frobenius Norm)
if (direction.norm() > params_.kExtensionRange)
{
//normalized()其实就是把自身的各元素除以它的范数
//沿着这个方向，但是距离给予一个限制kExtensionRange
direction = params_.kExtensionRange * direction.normalized();
}
node1[0] = origin[0] + direction[0];
node1[1] = origin[1] + direction[1];
node1[2] = origin[2] + direction[2];
global_vertex_size_++;
// Create new node and insert into tree
//提取相关信息，打包成一个新的点
dsvplanner_ns::Node *newNode = new dsvplanner_ns::Node;
newNode->state_ = node1;
newNode->parent_ = newParent;
newNode->distance_ = newParent->distance_ + direction.norm();
newParent->children_.push_back(newNode);
newNode->gain_ = gain(newNode->state_);
//把这个提取到的点插入到kd树中
kd_insert3(kdTree_, node1.x(), node1.y(), node1.z(), newNode);

// save new node to node_array
dual_state_graph_->local_graph_.vertices[i].information_gain =newNode->gain_;
node_array.push_back(newNode);
//不断比较记录最大的gain
if (newNode->gain_ > bestGain_)
{
//用executedBestNodeList_来保存已经记录过的全局bestgain号
if (std::find(executedBestNodeList_.begin(),executedBestNodeList_.end(), i) != executedBestNodeList_.end())
{
bestGain_ = newNode->gain_;
bestNodeId_ = i;
}
}
// nodeCounter_++;
}
executedBestNodeList_.push_back(bestNodeId_);
nodeCounter_ = dual_state_graph_->global_graph_.vertices.size();
}
}
//暂时没有全局图节点，把当前位置加入到kd树中
else
{
rootNode_->state_ = root_;
kd_insert3(kdTree_, rootNode_->state_.x(), rootNode_->state_.y(), rootNode_->state_.z(), rootNode_);
iterationCount_++;
}

//如果处于探索阶段，这里是maxX是rootNode_->state_.x() + params_.kMaxXLocalBound;（其他同理），xy方向默认为15
//而全局relocation阶段则这里是用的globalbound, xy方向默认300
//全局阶段，maxX_则是整个地图的最大范围
maxX_ = params_.kMaxXGlobalBound;
maxY_ = params_.kMaxYGlobalBound;
maxZ_ = params_.kMaxZGlobalBound;
minX_ = params_.kMinXGlobalBound;
minY_ = params_.kMinYGlobalBound;
minZ_ = params_.kMinZGlobalBound;
}

//发布边界内容，用于rviz可视化
// Publish visualization of total current planning boundary
visualization_msgs::Marker p;
p.header.stamp = ros::Time::now();
//"/map"
p.header.frame_id = params_.explorationFrame;
p.id = 0;
p.ns = "boundary";
//立方体
p.type = visualization_msgs::Marker::CUBE;
p.action = visualization_msgs::Marker::ADD;
p.pose.position.x = 0.5 * (minX_ + maxX_);
p.pose.position.y = 0.5 * (minY_ + maxY_);
p.pose.position.z = 0.5 * (minZ_ + maxZ_);
tf::Quaternion quat;
quat.setEuler(0.0, 0.0, 0.0);
p.pose.orientation.x = quat.x();
p.pose.orientation.y = quat.y();
p.pose.orientation.z = quat.z();
p.pose.orientation.w = quat.w();
p.scale.x = maxX_ - minX_;
p.scale.y = maxY_ - minY_;
p.scale.z = maxZ_ - minZ_;
p.color.r = 252.0 / 255.0;
p.color.g = 145.0 / 255.0;
p.color.b = 37.0 / 255.0;
p.color.a = 0.3;
p.lifetime = ros::Duration(0.0);
p.frame_locked = false;
params_.boundaryPub_.publish(p);
}

//功能：传入一个位置，得到一个pruned图，包含local图中的各个最短路径，
//插入到kd树中，获取bestgain，对pruned图的gain重新赋值
void dsvplanner_ns::Drrt::pruneTree(StateVec root)
{
//清空pruned图的节点
dual_state_graph_->pruned_graph_.vertices.clear();
geometry_msgs::Pose p1;
p1.position.x = root[0];
p1.position.y = root[1];
p1.position.z = root[2];
p1.orientation.y = params_.kZeroGain;
//把节点root加入到图pruned_graph_中
dual_state_graph_->addNewLocalVertexWithoutDuplicates(p1, dual_state_graph_->pruned_graph_);

geometry_msgs::Point root_point;
root_point.x = root[0];
root_point.y = root[1];
root_point.z = root[2];
//提取一个prune图,我理解为一个包含有最短路径的图
dual_state_graph_->pruneGraph(root_point);

StateVec node;
for (int i = 1; i < dual_state_graph_->pruned_graph_.vertices.size(); i++)
{
node[0] = dual_state_graph_->pruned_graph_.vertices[i].location.x;
node[1] = dual_state_graph_->pruned_graph_.vertices[i].location.y;
node[2] = dual_state_graph_->pruned_graph_.vertices[i].location.z;
//遍历prune图得到各个node,从kd树中找到距离node最近的点
kdres *nearest = kd_nearest3(kdTree_, node.x(), node.y(), node.z());
//得到最近node的数目
if (kd_res_size(nearest) <= 0)
{
//没找到就把内存清空掉
kd_res_free(nearest);
continue;
}
//取出这个节点,nearest中只装一个值
dsvplanner_ns::Node *newParent =(dsvplanner_ns::Node *)kd_res_item_data(nearest);
//把内存清空掉
kd_res_free(nearest);

// Check for collision
StateVec origin(newParent->state_[0], newParent->state_[1], newParent->state_[2]);
//pruned图里每一个点，到它在kd树中查找得到的最近点的方向向量
StateVec direction(node[0] - origin[0], node[1] - origin[1], node[2] - origin[2]);

//提取相关信息，打包成一个新的点
dsvplanner_ns::Node *newNode = new dsvplanner_ns::Node;
newNode->state_ = node;
newNode->parent_ = newParent;
newNode->distance_ = newParent->distance_ + direction.norm();
newParent->children_.push_back(newNode);

//新的点在gain字段的赋值
if (dual_state_graph_->pruned_graph_.vertices[i].information_gain > 0)
newNode->gain_ = gain(newNode->state_);
else .
{
newNode->gain_ = 0;
}

//把这个提取到的点插入到kd树中
kd_insert3(kdTree_, node.x(), node.y(), node.z(), newNode);
node_array.push_back(newNode);
//不断比较记录最大的gain
if (newNode->gain_ > bestGain_)
{
bestGain_ = newNode->gain_;
}
//把kd树的节点的gain赋值到pruned图中
dual_state_graph_->pruned_graph_.vertices[i].information_gain =newNode->gain_;
}

remainingNodeCount_ = node_array.size();
}

//统计增益,形参位置处的VoxelGain（论文公式5中内容）
//这个内容比较重要，在dual_State_graph.cpp中计算用到的information_gain应该就是来自此处计算，并通过pruneTree函数传入
double dsvplanner_ns::Drrt::gain(StateVec state)
{
// This function computes the gain
double gain = 0.0;
//dist是网格分辨率
const double disc = manager_->getResolution();
StateVec origin(state[0], state[1], state[2]);
StateVec vec;
double rangeSq = pow(params_.kGainRange, 2.0);

// Iterate over all nodes within the allowed distance
//根据循环来看，应该是机器人周围的一个正方体？（之类的内容）在里面进行以分辨率为单位的遍历
for (vec[0] = std::max(state[0] - params_.kGainRange, minX_); vec[0] < std::min(state[0] + params_.kGainRange, maxX_); vec[0] += disc)
{
for (vec[1] = std::max(state[1] - params_.kGainRange, minY_); vec[1] < std::min(state[1] + params_.kGainRange, maxY_); vec[1] += disc)
{
for (vec[2] = std::max(state[2] - params_.kGainRangeZMinus, minZ_); vec[2] < std::min(state[2] + params_.kGainRangeZPlus, maxZ_); vec[2] += disc)
{
//周围各个点到自己中心的位置
StateVec dir = vec - origin;
// Skip if distance is too large
//转置点乘其实就是欧式距离
if (dir.transpose().dot(dir) > rangeSq)
{
continue;
}
bool insideAFieldOfView = false;
// Check that voxel center is inside the field of view. This check is for velodyne.
//params_.sensorVerticalView指的激光雷达上下垂直分辨率一共是30度，
//这块的trick不太理解，根据注释，意思是判断各个视点在机器人的视野中？
//第一项意味着机器人对这个点的观测俯仰角在45度内，第二项和传感器有关，本来也不是0，可以无视它
if (fabs(dir[2] < sqrt(dir[0] * dir[0] + dir[1] * dir[1]) * tan(M_PI * params_.sensorVerticalView / 360)))
{
insideAFieldOfView = true;
}
if (!insideAFieldOfView)
{
continue;
}
// Check cell status and add to the gain considering the corresponding factor.
double probability;
volumetric_mapping::OctomapManager::CellStatus node =manager_->getCellProbabilityPoint(vec, &probability);
if (node == volumetric_mapping::OctomapManager::CellStatus::kUnknown)
{
if (volumetric_mapping::OctomapManager::CellStatus::kOccupied !=this->manager_->getVisibility(origin, vec, false))
{
//处于未知状态，则则算做一个信息增益,把gain加起来,配置文件中为1
gain += params_.kGainUnknown;
}
}
else if (node ==volumetric_mapping::OctomapManager::CellStatus::kOccupied)
{
if (volumetric_mapping::OctomapManager::CellStatus::kOccupied !=this->manager_->getVisibility(origin, vec, false))
{
//配置文件中为0，可以无视，这个其实也不算信息量的增益
gain += params_.kGainOccupied;
}
}
else
{
if (volumetric_mapping::OctomapManager::CellStatus::kOccupied !=this->manager_->getVisibility(origin, vec, false))
{
//配置文件中为0，可以无视，这个其实也不算信息量的增益
gain += params_.kGainFree;
}
}
}
}
}

// Scale with volume
if (gain < params_.kMinEffectiveGain)
gain = 0;
gain *= pow(disc, 3.0);

return gain;
}

void dsvplanner_ns::Drrt::publishNode()
{
sensor_msgs::PointCloud2 random_sampled_points_pc;
pcl::toROSMsg(*sampledPoint_, random_sampled_points_pc);
random_sampled_points_pc.header.frame_id = params_.explorationFrame;
params_.randomSampledPointsPub_.publish(random_sampled_points_pc);

visualization_msgs::Marker node;
visualization_msgs::Marker branch;
node.header.stamp = ros::Time::now();
node.header.frame_id = params_.explorationFrame;
node.ns = "drrt_node";
node.type = visualization_msgs::Marker::POINTS;
node.action = visualization_msgs::Marker::ADD;
node.scale.x = 0.4;
node.color.r = 167.0 / 255.0;
node.color.g = 167.0 / 255.0;
node.color.b = 0.0;
node.color.a = 1.0;
node.frame_locked = false;

branch.ns = "drrt_branches";
branch.header.stamp = ros::Time::now();
branch.header.frame_id = params_.explorationFrame;
branch.type = visualization_msgs::Marker::LINE_LIST;
branch.action = visualization_msgs::Marker::ADD;
branch.scale.x = 0.08;
branch.color.r = 167.0 / 255.0;
branch.color.g = 167.0 / 255.0;
branch.color.b = 0.0;
branch.color.a = 1.0;
branch.frame_locked = false;

geometry_msgs::Point node_position;
geometry_msgs::Point parent_position;
//node_array记录了kd树中的节点
//这里需要关注一下remainingNodeCount_和node_array的区别在哪里？
//每次往kd树里面加入值，同时就会加入到这个node_array数据结构中
//而remainingNodeCount_只是在函数pruneTree中被按node_array的大小赋值了一次
if (remainingNodeCount_ > 0 && remainingNodeCount_ <= node_array.size())
{
for (int i = 0; i < remainingNodeCount_; i++)
{
node_position.x = node_array[i]->state_[0];
node_position.y = node_array[i]->state_[1];
node_position.z = node_array[i]->state_[2];
node.points.push_back(node_position);
//连上kd树的边
if (node_array[i]->parent_)
{
parent_position.x = node_array[i]->parent_->state_[0];
parent_position.y = node_array[i]->parent_->state_[1];
parent_position.z = node_array[i]->parent_->state_[2];

branch.points.push_back(parent_position);
branch.points.push_back(node_position);
}
}
params_.remainingTreePathPub_.publish(node);
params_.remainingTreePathPub_.publish(branch);
node.points.clear();
branch.points.clear();
node.color.r = 167.0 / 255.0;
node.color.g = 0.0 / 255.0;
node.color.b = 167.0 / 255.0;
node.color.a = 1.0;
branch.color.r = 167.0 / 255.0;
branch.color.g = 0.0 / 255.0;
branch.color.b = 167.0 / 255.0;
branch.color.a = 1.0;
for (int i = remainingNodeCount_; i < node_array.size(); i++)
{
node_position.x = node_array[i]->state_[0];
node_position.y = node_array[i]->state_[1];
node_position.z = node_array[i]->state_[2];
node.points.push_back(node_position);

if (node_array[i]->parent_)
{
parent_position.x = node_array[i]->parent_->state_[0];
parent_position.y = node_array[i]->parent_->state_[1];
parent_position.z = node_array[i]->parent_->state_[2];

branch.points.push_back(parent_position);
branch.points.push_back(node_position);
}
}
params_.newTreePathPub_.publish(node);
params_.newTreePathPub_.publish(branch);
}
else
{
for (int i = 0; i < node_array.size(); i++)
{
node_position.x = node_array[i]->state_[0];
node_position.y = node_array[i]->state_[1];
node_position.z = node_array[i]->state_[2];
node.points.push_back(node_position);

if (node_array[i]->parent_)
{
parent_position.x = node_array[i]->parent_->state_[0];
parent_position.y = node_array[i]->parent_->state_[1];
parent_position.z = node_array[i]->parent_->state_[2];

branch.points.push_back(parent_position);
branch.points.push_back(node_position);
}
}
params_.newTreePathPub_.publish(node);
params_.newTreePathPub_.publish(branch);

// When there is no remaining node, publish an empty one
node.points.clear();
branch.points.clear();
params_.remainingTreePathPub_.publish(node);
params_.remainingTreePathPub_.publish(branch);
}
}

void dsvplanner_ns::Drrt::gotoxy(int x, int y) {
printf("%c[%d;%df", 0x1B, y, x);
}

#endif

dual_state_frontier.cpp

/**************************************************************************
dual_state_fontier.cpp
Implementation of dual_state frontier detection. Detect local and global
frontiers
to guide the extension of local and global graph.
Hongbiao Zhu(hongbiaz@andrew.cmu.edu)
5/25/2020
**************************************************************************/

#include "dsvplanner/dual_state_frontier.h"

#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>

namespace dsvplanner_ns {

DualStateFrontier::DualStateFrontier(const ros::NodeHandle &nh, const ros::NodeHandle &nh_private,
volumetric_mapping::OctomapManager *manager, OccupancyGrid *grid)
: nh_(nh), nh_private_(nh_private)
{
manager_ = manager;
grid_ = grid;
initialize();
}

DualStateFrontier::~DualStateFrontier() {}

bool DualStateFrontier::readParameters()
{
nh_private_.getParam("/frontier/world_frame_id", world_frame_id_);
nh_private_.getParam("/planner/odomSubTopic", sub_odom_topic_);
nh_private_.getParam("/planner/terrainCloudSubTopic", sub_terrain_point_cloud_topic_);
nh_private_.getParam("/frontier/sub_graph_points_topic", sub_graph_points_topic_);
nh_private_.getParam("/frontier/pub_unknown_points_topic", pub_unknown_points_topic_);
nh_private_.getParam("/frontier/pub_global_frontier_points_topic", pub_global_frontier_points_topic_);
nh_private_.getParam("/frontier/pub_local_frontier_points_topic", pub_local_frontier_points_topic_);
nh_private_.getParam("/frontier/kExecuteFrequency", kExecuteFrequency_);
nh_private_.getParam("/frontier/kFrontierResolution", kFrontierResolution);
nh_private_.getParam("/frontier/kFrontierFilterSize", kFrontierFilterSize);
nh_private_.getParam("/frontier/kSearchRadius", kSearchRadius);
nh_private_.getParam("/frontier/kSearchBoundingX", search_bounding[0]);
nh_private_.getParam("/frontier/kSearchBoundingY", search_bounding[1]);
nh_private_.getParam("/frontier/kSearchBoundingZ", search_bounding[2]);
nh_private_.getParam("/frontier/kEffectiveUnknownNumAroundFrontier", kEffectiveUnknownNumAroundFrontier);
nh_private_.getParam("/frontier/kFrontierNeighboutSearchRadius",kFrontierNeighboutSearchRadius);
nh_private_.getParam("/gb/kMaxXGlobal", kGlobalMaxX);
nh_private_.getParam("/gb/kMaxYGlobal", kGlobalMaxY);
nh_private_.getParam("/gb/kMaxZGlobal", kGlobalMaxZ);
nh_private_.getParam("/gb/kMinXGlobal", kGlobalMinX);
nh_private_.getParam("/gb/kMinYGlobal", kGlobalMinY);
nh_private_.getParam("/gb/kMinZGlobal", kGlobalMinZ);
nh_private_.getParam("/rm/kBoundX", robot_bounding[0]);
nh_private_.getParam("/rm/kBoundY", robot_bounding[1]);
nh_private_.getParam("/rm/kBoundZ", robot_bounding[2]);
nh_private_.getParam("/rm/kSensorVertical", kSensorVerticalView);
nh_private_.getParam("/rm/kSensorHorizontal", kSensorHorizontalView);
nh_private_.getParam("/rm/kVehicleHeight", kVehicleHeight);
nh_private_.getParam("/elevation/kTerrainVoxelSize", kTerrainVoxelSize);
nh_private_.getParam("/elevation/kTerrainVoxelHalfWidth",kTerrainVoxelHalfWidth);
nh_private_.getParam("/elevation/kTerrainVoxelWidth", kTerrainVoxelWidth);
return true;
}

//给定一个范围，和位置中心，得到它周围的属于未知状态的点云
void DualStateFrontier::getUnknowPointcloudInBoundingBox (const StateVec &center, const StateVec &bounding_box_size)
{
unknown_points_->clear();
local_frontier_->clear();
pcl::PointCloud<pcl::PointXYZ>::Ptr local_frontier_temp =pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>());

const double epsilon = 0.001; // Small offset to not hit boundary of nodes.
StateVec epsilon_3d;
epsilon_3d.setConstant(epsilon);

//这个操作的细节目的不是很清楚
//为什么要对中心进行修正调整呢？我认为作用不大
// Determine correct center of voxel.
const StateVec center_corrected(kFrontierResolution * std::floor(center.x() / kFrontierResolution) +kFrontierResolution / 2.0,
kFrontierResolution * std::floor(center.y() / kFrontierResolution) +kFrontierResolution / 2.0,
center.z());

StateVec bbx_min = -bounding_box_size / 2 - epsilon_3d;
StateVec bbx_max = bounding_box_size / 2 + epsilon_3d;

for (double x_position = bbx_min.x(); x_position <= bbx_max.x(); x_position += kFrontierResolution)
{
for (double y_position = bbx_min.y(); y_position <= bbx_max.y();y_position += kFrontierResolution)
{
double x = center_corrected[0] + x_position;
double y = center_corrected[1] + y_position;
double z = getZvalue(x_position, y_position);
//当高程差超过0.4这里会被赋值为1000
if (z >= 1000)
continue;
//按照分辨率，依次遍历得到当前位置周围各个位置的xyz
octomap::point3d point = octomap::point3d(x, y, z);
//调用octomap，根据点坐标得到索引值
octomap::OcTreeKey key = manager_->octree_->coordToKey(point);
//根据索引值，得到具体的节点
octomap::OcTreeNode *node = manager_->octree_->search(key);
//如果没有这样的点
if (node == NULL)
{
//就加入到未知点集合中
unknown_points_->push_back(pcl::PointXYZ(point.x(), point.y(), point.z()));
//在给定的范围内，并且确实是一个边界点
if (FrontierInBoundry(point) && frontierDetect(point))
{
//是边界点就加入到local_frontier_temp中
local_frontier_temp->push_back(pcl::PointXYZ(point.x(), point.y(), point.z()));
}
}
}
}
pcl::VoxelGrid<pcl::PointXYZ> point_ds;
point_ds.setLeafSize(2, 2, 2);
point_ds.setInputCloud(local_frontier_temp);
//下采样滤波至变量local_frontier_中
point_ds.filter(*local_frontier_);
}

//功能：给定一个点，检查它是不是边界点
bool DualStateFrontier::frontierDetect(octomap::point3d point) const
{
//给定一个点，在x和y两个方向的正负方向，分别在octomap里检查索引，得到坐标
//node_outside相关的内容没用到，可以一律无视

const double resolution = manager_->octree_->getResolution();
bool xPositive = false, xNegative = false, yPositive = false, yNegative = false;
bool effectiveFree = false;
bool effectiveUnknown = false;
int unknowCount = 0;
octomap::OcTreeNode *node_inside;
octomap::OcTreeNode *node_outside;
octomap::OcTreeKey key_inside, key_outside;
octomap::point3d surround_point_inside, surround_point_outside;

surround_point_inside.x() = point.x();
surround_point_inside.y() = point.y() - resolution;
surround_point_inside.z() = point.z();
key_inside = manager_->octree_->coordToKey(surround_point_inside);
node_inside = manager_->octree_->search(key_inside);

surround_point_outside.x() = point.x();
surround_point_outside.y() = point.y() - 2 * resolution;
surround_point_outside.z() = point.z();
key_outside = manager_->octree_->coordToKey(surround_point_outside);
node_outside = manager_->octree_->search(key_outside);

//node_inside存在，没被占据
if (node_inside != NULL &&(!manager_->octree_->isNodeOccupied(node_inside)))
{
yNegative = true;
}
else if ((node_inside != NULL &&manager_->octree_->isNodeOccupied(node_inside)))
{
// node_inside存在，被占据
return false;
}
else if (node_inside == NULL)
{
//node_inside存在，不存在，未知节点统计变量+1
unknowCount++;
}

//后面的和前面的同理
surround_point_inside.x() = point.x();
surround_point_inside.y() = point.y() + resolution;
surround_point_inside.z() = point.z();
key_inside = manager_->octree_->coordToKey(surround_point_inside);
node_inside = manager_->octree_->search(key_inside);
surround_point_outside.x() = point.x();
surround_point_outside.y() = point.y() + 2 * resolution;
surround_point_outside.z() = point.z();
key_outside = manager_->octree_->coordToKey(surround_point_outside);
node_outside = manager_->octree_->search(key_outside);
if (node_inside != NULL &&(!manager_->octree_->isNodeOccupied(node_inside)))
{
yPositive = true;
}
else if ((node_inside != NULL &&manager_->octree_->isNodeOccupied(node_inside)))
{
return false;
}
else if (node_inside == NULL)
{
unknowCount++;
}

surround_point_inside.x() = point.x() - resolution;
surround_point_inside.y() = point.y();
surround_point_inside.z() = point.z();
key_inside = manager_->octree_->coordToKey(surround_point_inside);
node_inside = manager_->octree_->search(key_inside);
surround_point_outside.x() = point.x() - 2 * resolution;
surround_point_outside.y() = point.y();
surround_point_outside.z() = point.z();
key_outside = manager_->octree_->coordToKey(surround_point_outside);
node_outside = manager_->octree_->search(key_outside);
if (node_inside != NULL &&(!manager_->octree_->isNodeOccupied(node_inside)))
{
xNegative = true;
}
else if ((node_inside != NULL &&manager_->octree_->isNodeOccupied(node_inside)))
{
return false;
}
else if (node_inside == NULL)
{
unknowCount++;
}

surround_point_inside.x() = point.x() + resolution;
surround_point_inside.y() = point.y();
surround_point_inside.z() = point.z();
key_inside = manager_->octree_->coordToKey(surround_point_inside);
node_inside = manager_->octree_->search(key_inside);
surround_point_outside.x() = point.x() + 2 * resolution;
surround_point_outside.y() = point.y();
surround_point_outside.z() = point.z();
key_outside = manager_->octree_->coordToKey(surround_point_outside);
node_outside = manager_->octree_->search(key_outside);

if (node_inside != NULL &&(!manager_->octree_->isNodeOccupied(node_inside)))
{
xPositive = true;
}
else if ((node_inside != NULL &&manager_->octree_->isNodeOccupied(node_inside)))
{
return false;
}
else if (node_inside == NULL)
{
unknowCount++;
}

//x和y的正负方向，有一个是属于free状态
effectiveFree = xPositive || xNegative || yPositive || yNegative;
//有一个是未知状态(kEffectiveUnknownNumAroundFrontier默认为1)
effectiveUnknown = unknowCount >= kEffectiveUnknownNumAroundFrontier;
//一个点周围，有空闲，也有未知，那么这个点作为边界点
return (effectiveFree && effectiveUnknown);
}

//判断边界点是否在全局给定的范围内
bool DualStateFrontier::FrontierInBoundry(octomap::point3d point) const
{
if (point.x() > kGlobalMaxX)
return false;
else if (point.y() > kGlobalMaxY)
return false;
else if (point.z() > kGlobalMaxZ)
return false;
else if (point.x() < kGlobalMinX)
return false;
else if (point.y() < kGlobalMinY)
return false;
else if (point.z() < kGlobalMinZ)
return false;
else
return true;
}

//把形参加入到cleanedFrontier里面
void DualStateFrontier::updateToCleanFrontier(pcl::PointXYZ point)
{
cleanedFrontier_->points.push_back(point);
}

//判断一个点是否是cleanFrontier中的：
//形参输入进去，遍历cleanFrontier，如果和任何一个cleanFrontier中的点的距离小于3米，返回true
bool DualStateFrontier::isCleanedFrontier(pcl::PointXYZ point)
{
if (cleanedFrontier_->points.size() > 0)
{
for (int i = 0; i < cleanedFrontier_->points.size(); i++)
{
double dist = sqrt((point.x - cleanedFrontier_->points[i].x) *(point.x - cleanedFrontier_->points[i].x) +
(point.y - cleanedFrontier_->points[i].y) *(point.y - cleanedFrontier_->points[i].y) +
(point.z - cleanedFrontier_->points[i].z) *(point.x - cleanedFrontier_->points[i].z));
if (dist < 3)
{
return true;
}
}
}
return false;
}

//功能：输入一个点,利用kd树，在全局中找到最近的点的集合，
//只要有一个可以被形参位置正常观测，并且没有障碍物，则认为是true
bool DualStateFrontier::inSensorRangeofGraphPoints(StateVec point)
{
pcl::PointXYZ check_point;
check_point.x = point[0];
check_point.y = point[1];
check_point.z = point[2];
std::vector<int> pointSearchInd;
std::vector<float> pointSearchDist;
//"/global_points"
if (graphPoints_->points.size() > 0)
{
//创建Kd树然后搜索半径在配置文件中
//指定半径范围查找近邻
//球状固定距离半径近邻搜索
//kSearchRadius是搜索半径，pointSearchInd应该是返回的index，pointSearchSqDis应该是依次距离中心点的距离
kdtree_->setInputCloud(graphPoints_);
kdtree_->radiusSearch(check_point, kSearchRadius, pointSearchInd, pointSearchDist);
//遍历kd树中找到的近的点
for (int i = 0; i < pointSearchInd.size(); i++)
{
//得到和当前形参点的距离
StateVec node_point(graphPoints_->points[pointSearchInd[i]].x, graphPoints_->points[pointSearchInd[i]].y,graphPoints_->points[pointSearchInd[i]].z);
StateVec dir = point - node_point;
// Skip if distance is too large
//这一步有点迷惑，感觉用不着这样，kd树找到的本来也不会大于这个半径
double rangeSq = pow(kSearchRadius, 2.0);
if (dir.transpose().dot(dir) > rangeSq)
{
continue;
}
bool insideAFieldOfView = false;
//俯仰角小于45度, insideAFieldOfView为true
if (fabs(dir[2] < sqrt(dir[0] * dir[0] + dir[1] * dir[1]) * tan(M_PI * kSensorVerticalView / 360)))
{
insideAFieldOfView = true;
}
if (!insideAFieldOfView)
{
continue;
}
if (manager_->CellStatus::kOccupied !=manager_->getVisibility(node_point, point, false) &&
!grid_->collisionCheckByTerrainWithVector(node_point, point))
{
return true;
}
}
return false;
}
return false;
}

//局部边界点的检查和更新（改变状态的不再是，另外加入到全局边界global_frontier_中去）
void DualStateFrontier::localFrontierUpdate(StateVec &center)
{
local_frontier_pcl_->clear();
StateVec checkedPoint;
//做一次边界的更新，local_frontier_保存的是一路下来的局部边界，从中遍历，检查每个点的占据和观测状态
//把相对形参位置有没有被占据，没有障碍物，等等，就加入到全局边界中
for (int i = 0; i < local_frontier_->size(); i++)
{
checkedPoint.x() = local_frontier_->points[i].x;
checkedPoint.y() = local_frontier_->points[i].y;
checkedPoint.z() = local_frontier_->points[i].z;
if ((manager_->CellStatus::kOccupied !=manager_->getVisibility(center, checkedPoint, false) &&
!grid_->collisionCheckByTerrainWithVector(center, checkedPoint)) ||(!planner_status_ && inSensorRangeofGraphPoints(checkedPoint)))
{
local_frontier_pcl_->points.push_back(local_frontier_->points[i]);
global_frontier_->points.push_back(local_frontier_->points[i]);
}
}

local_frontier_->clear();
pcl::VoxelGrid<pcl::PointXYZ> point_ds;
point_ds.setLeafSize(kFrontierFilterSize, kFrontierFilterSize, kFrontierFilterSize);
point_ds.setInputCloud(local_frontier_pcl_);
//把更新后的点重新保存到local_frontier_里面
point_ds.filter(*local_frontier_);
}

//全局边界点的检查和更新
void DualStateFrontier::gloabalFrontierUpdate()
{
global_frontier_pcl_->clear();

int size = global_frontier_->points.size();
octomap::OcTreeNode *node;
octomap::OcTreeKey key;
octomap::point3d point;
StateVec checkedPoint;
//遍历全局边界点
for (int i = 0; i < size; i++)
{
point.x() = global_frontier_->points[i].x;
point.y() = global_frontier_->points[i].y;
point.z() = global_frontier_->points[i].z;
//如果属于被清空过的边界（到不了），则不考虑
if (isCleanedFrontier(global_frontier_->points[i]))
{
continue;
}
checkedPoint.x() = point.x();
checkedPoint.y() = point.y();
checkedPoint.z() = point.z();
key = manager_->octree_->coordToKey(point);
node = manager_->octree_->search(key);
//再次搜索octomap，检查是否存在这个点
//（但我感觉肯定存在的，否则怎么通过检查并且加入进来的呢？不过也可能是octomap更新导致节点状态变化）
//再检查目前还是不是边界了
if (node == NULL && frontierDetect(point))
{
global_frontier_pcl_->points.push_back(global_frontier_->points[i]);
}
}
global_frontier_->clear();
pcl::VoxelGrid<pcl::PointXYZ> point_ds_;
point_ds_.setLeafSize(kFrontierFilterSize, kFrontierFilterSize,kFrontierFilterSize);
point_ds_.setInputCloud(global_frontier_pcl_);
//更新全局边界
point_ds_.filter(*global_frontier_);
}

//对全局边界点的一个下采样
void DualStateFrontier::globalFrontiersNeighbourCheck()
{
global_frontier_pcl_->clear();

int size = global_frontier_->points.size();
pcl::PointXYZ p1;
std::vector<int> pointSearchInd;
std::vector<float> pointSearchDist;
if (size > 0)
{
global_frontiers_kdtree_->setInputCloud(global_frontier_);
for (int i = 0; i < size; i++)
{
p1 = global_frontier_->points[i];
pointSearchInd.clear();
pointSearchDist.clear();
//遍历全局边界点，然后在kFrontierNeighboutSearchRadius（默认5米）
global_frontiers_kdtree_->radiusSearch(p1, kFrontierNeighboutSearchRadius,pointSearchInd, pointSearchDist);
//内进行搜索最近的点，如果最近也存在其他全局点，就只保存这一个
if (pointSearchInd.size() > 1)
global_frontier_pcl_->points.push_back(p1);
}
}
global_frontier_->clear();
//相当于对全局点的一个下采样
*global_frontier_ = *global_frontier_pcl_;
}

//把边界都清空掉
void DualStateFrontier::cleanAllUselessFrontiers()
{
global_frontier_->clear();
local_frontier_->clear();
publishFrontiers();
}

//一直在执行的execute函数中真正在调用的函数
void DualStateFrontier::getFrontiers()
{
//根据机器人的位置，在给定搜索范围内search_bounding（例40，40，0.5）范围内确定边界点
getUnknowPointcloudInBoundingBox(robot_position_, search_bounding);
//更新当前位置的边界点，加入到全局边界点中
localFrontierUpdate(robot_position_);
//再更新全局边界点
gloabalFrontierUpdate();
//对全局点的一个下采样
globalFrontiersNeighbourCheck();
//发布边界信息
publishFrontiers();
}

//对各个内容的发布，包括局部边界点，全局边界点，处于未知状态的点云
void DualStateFrontier::publishFrontiers()
{
sensor_msgs::PointCloud2 unknown_pcl, local_frontier_pcl, global_frontier_pcl;
pcl::toROSMsg(*unknown_points_, unknown_pcl);
pcl::toROSMsg(*global_frontier_, global_frontier_pcl);
pcl::toROSMsg(*local_frontier_, local_frontier_pcl);
unknown_pcl.header.frame_id = world_frame_id_;
global_frontier_pcl.header.frame_id = world_frame_id_;
local_frontier_pcl.header.frame_id = world_frame_id_;
unknown_points_pub_.publish(unknown_pcl);
global_frontier_points_pub_.publish(global_frontier_pcl);
local_frontier_points_pub_.publish(local_frontier_pcl);
}

//监听里程计信息,以及地形信息
void DualStateFrontier::terrainCloudAndOdomCallback( const nav_msgs::Odometry::ConstPtr &odom_msg,
const sensor_msgs::PointCloud2::ConstPtr &terrain_msg)
{
robot_position_[0] = odom_msg->pose.pose.position.x;
robot_position_[1] = odom_msg->pose.pose.position.y;
robot_position_[2] = odom_msg->pose.pose.position.z;
terrain_cloud_->clear();
terrain_cloud_ds->clear();
terrain_elev_cloud_->clear();
terrain_voxel_points_num_.clear();
terrain_voxel_min_elev_.clear();
terrain_voxel_max_elev_.clear();
terrain_voxel_points_num_.resize(kTerrainVoxelWidth * kTerrainVoxelWidth);
terrain_voxel_min_elev_.resize(kTerrainVoxelWidth * kTerrainVoxelWidth, 1000);
terrain_voxel_max_elev_.resize(kTerrainVoxelWidth * kTerrainVoxelWidth,-1000);

pcl::fromROSMsg(*terrain_msg, *terrain_cloud_);

pcl::VoxelGrid<pcl::PointXYZI> point_ds;
point_ds.setLeafSize(0.3, 0.3, 0.3);
point_ds.setInputCloud(terrain_cloud_);
//对地形信息进行滤波
point_ds.filter(*terrain_cloud_ds);

//根据地形消息点云，构建相关地形数组,terrain_elev_cloud_
updateTerrainMinElevation();
updateTerrainElevationForKnown();
updateTerrainElevationForUnknow();

//发布高程地图,但是不晓得它这是要发布给谁
sensor_msgs::PointCloud2 elevVoxel2;
pcl::toROSMsg(*terrain_elev_cloud_, elevVoxel2);
elevVoxel2.header.stamp = ros::Time::now();
elevVoxel2.header.frame_id = "/map";
terrain_elev_cloud_pub_.publish(elevVoxel2);
}

//根据地形消息修改存有地形数目、低值和高值的一维数组
void DualStateFrontier::updateTerrainMinElevation()
{
pcl::PointXYZI point;
int terrainCloudSize = terrain_cloud_ds->points.size();
//遍历地形点
for (int i = 0; i < terrainCloudSize; i++)
{
point.x = terrain_cloud_ds->points[i].x;
point.y = terrain_cloud_ds->points[i].y;
point.z = terrain_cloud_ds->points[i].z;
point.intensity = terrain_cloud_ds->points[i].intensity;
//point.x - robot_position_[0]是当前点到地形点的向量
//kTerrainVoxelSize默认为0.4，0.4/2为0.2
//地形向量+0.2，再一起比0.4
//化简为地形向量比分辨率kTerrainVoxelSize，再加0.5，再转int，应该是四舍五入
//也就是说，假如障碍物点x坐标和当前差了5m，比分辨率0.4为8个格子
//再加上半个kTerrainVoxelHalfWidth（50），即这个点应该在58处
//机器人是中心，一个点在机器人右边5米，那么在整个地形表示里面，它应该是从左边开始58个格子（机器人左边有50个格子，50*0.4=20米）
int indX = int((point.x - robot_position_[0] + kTerrainVoxelSize / 2) /kTerrainVoxelSize) +kTerrainVoxelHalfWidth;
int indY = int((point.y - robot_position_[1] + kTerrainVoxelSize / 2) /kTerrainVoxelSize) +kTerrainVoxelHalfWidth;

//应对四舍五入为负的时候，例如x本来是-0.8，再加上0.5就变成了-0.3，求int就变成了0，但应该是-1
if (point.x - robot_position_[0] + kTerrainVoxelSize / 2 < 0)
indX--;
if (point.y - robot_position_[1] + kTerrainVoxelSize / 2 < 0)
indY--;

//限制大小不要超出边界
if (indX > kTerrainVoxelWidth - 1)
indX = kTerrainVoxelWidth - 1;
if (indX < 0)
indX = 0;
if (indY > kTerrainVoxelWidth - 1)
indY = kTerrainVoxelWidth - 1;
if (indY < 0)
indY = 0;
int indVoxel = kTerrainVoxelWidth * indX + indY;

//terrain_voxel_points_num_应该是一个一维数组，统计落在该位置的点的数目
terrain_voxel_points_num_[indVoxel]++;
//terrain_voxel_min_elev_也是一维数组，统计地形最低值
//terrain_voxel_max_elev_同理
if (point.z < terrain_voxel_min_elev_[indVoxel])
terrain_voxel_min_elev_[indVoxel] = point.z;
if (point.z > terrain_voxel_max_elev_[indVoxel])
terrain_voxel_max_elev_[indVoxel] = point.z;

for (int dX = -1; dX <= 1; dX = dX + 2)
{
for (int dY = -1; dY <= 1; dY = dY + 2)
{
if (indX + dX >= 0 && indX + dX < kTerrainVoxelWidth && indY + dY >= 0 && indY + dY < kTerrainVoxelWidth)
{
//我理解为这里在做一个对周围元素的滤波，如果当前位置有点，就对上下左右的数目也给加一下
terrain_voxel_points_num_[kTerrainVoxelWidth * (indX + dX) + indY +dY]++;
//最低值数组也会被统计，不过不知道为什么没有再修改最高值
if (point.z <terrain_voxel_min_elev_[kTerrainVoxelWidth * (indX + dX) + indY +dY])
terrain_voxel_min_elev_[kTerrainVoxelWidth * (indX + dX) + indY +dY] = point.z;
}
}
}
}
}

//功能：根据高程差，对已知区域赋值terrain_voxel_elev_数组，反解点云位置，保存terrain_elev_cloud_
void DualStateFrontier::updateTerrainElevationForKnown()
{
pcl::PointXYZI point;
for (int i = 0; i < kTerrainVoxelWidth * kTerrainVoxelWidth; i++)
{
//这里terrain_voxel_points_num_如果大于0，说明是已知区域
if (terrain_voxel_points_num_[i] > 0)
{
//最高点和最低点相差0.4
if (terrain_voxel_max_elev_[i] - terrain_voxel_min_elev_[i] >= 0.4)
//地形赋值1000
terrain_voxel_elev_[i] = 1000;
else
//否则按最低点来赋值
terrain_voxel_elev_[i] = terrain_voxel_min_elev_[i];

//得到当前i位置的xy索引
int indX = int(i / kTerrainVoxelWidth);
int indY = i % kTerrainVoxelWidth;

//
if (indX - kTerrainVoxelHalfWidth < 0)
{
indX++;
}
if (indY - kTerrainVoxelWidth < 0)
{
indY++;
}
// 就像updateTerrainMinElevation函数中的内容，根据索引，反解出机器人坐标系下的点云值:
// int indX = int((point.x - robot_position_[0] + kTerrainVoxelSize / 2) /kTerrainVoxelSize) +kTerrainVoxelHalfWidth;
point.x = (indX - kTerrainVoxelHalfWidth) * kTerrainVoxelSize -kTerrainVoxelSize / 2 + robot_position_[0];
point.y = (indY - kTerrainVoxelHalfWidth) * kTerrainVoxelSize -kTerrainVoxelSize / 2 + robot_position_[1];
point.z = 0;
point.intensity = terrain_voxel_elev_[i];
terrain_elev_cloud_->push_back(point);
}
}
}

//功能：根据周围的状态,对未知区域赋值terrain_voxel_elev_数组，反解点云位置，保存terrain_elev_cloud_
void DualStateFrontier::updateTerrainElevationForUnknow()
{
pcl::KdTreeFLANN<pcl::PointXYZI> kdtree;
std::vector<int> pointIdxNKNSearch(1);
std::vector<float> pointNKNSquaredDistance(1);
kdtree.setInputCloud(terrain_elev_cloud_);

pcl::PointXYZI point;
//主要靠terrain_voxel_points_num_来判断这个地方已知还是未知，如果没点落在这里，那么这里就是未知（地面也应该有点的）
//0号为坐上角首个位置
if (terrain_voxel_points_num_[0] <= 0)
{
//地形高度为机器人位置减去机器人高度（考虑到有楼层的情况）
terrain_voxel_elev_[0] = robot_position_[2] - kVehicleHeight;
}

for (int i = 1; i < kTerrainVoxelWidth * kTerrainVoxelWidth; i++)
{
if (terrain_voxel_points_num_[i] <= 0)
{
//反解得到点云位置
int indX = int(i / kTerrainVoxelWidth);
int indY = i % kTerrainVoxelWidth;
point.x = (indX - kTerrainVoxelHalfWidth) * kTerrainVoxelSize -kTerrainVoxelSize / 2 + robot_position_[0];
point.y = (indY - kTerrainVoxelHalfWidth) * kTerrainVoxelSize -kTerrainVoxelSize / 2 + robot_position_[1];
point.z = 0;

if (terrain_elev_cloud_->points.size() > 0)
{
//未知点的强度，用周围的点来代替
if (kdtree.nearestKSearch(point, 1, pointIdxNKNSearch, pointNKNSquaredDistance) > 0)
{
point.intensity =terrain_elev_cloud_->points[pointIdxNKNSearch[0]].intensity;
terrain_voxel_elev_[i] = point.intensity;
}
terrain_elev_cloud_->push_back(point);
}
}
}
}

//监听"/global_points"
void DualStateFrontier::graphPointsCallback(const sensor_msgs::PointCloud2 &graph_msg)
{
graphPoints_->clear();
pcl::fromROSMsg(graph_msg, *graphPoints_);
}

//形参：一个向量（A点减去B点），函数内部计算A点相对于B点坐标系在一维坐标下的索引，
//根据这个索引得到terrain_voxel_elev_里保存的障碍物高度（加上机器人自身高度）
double DualStateFrontier::getZvalue(double x_position, double y_position)
{
int indX = int((x_position + kTerrainVoxelSize / 2) / kTerrainVoxelSize) +kTerrainVoxelHalfWidth;
int indY = int((y_position + kTerrainVoxelSize / 2) / kTerrainVoxelSize) +kTerrainVoxelHalfWidth;
if (x_position + kTerrainVoxelSize / 2 < 0)
indX--;
if (y_position + kTerrainVoxelSize / 2 < 0)
indY--;

//下面四个if，是要把indX和indY限制在0到kTerrainVoxelWidth（默认101）之间
if (indX > kTerrainVoxelWidth - 1)
indX = kTerrainVoxelWidth - 1;
if (indX < 0)
indX = 0;
if (indY > kTerrainVoxelWidth - 1)
indY = kTerrainVoxelWidth - 1;
if (indY < 0)
indY = 0;
return terrain_voxel_elev_[kTerrainVoxelWidth * indX + indY] + kVehicleHeight;
}

//返回变量terrain_voxel_elev_，这个要被drrt.cpp中调用
//它调用以后，是要用在它的getZvalue函数中
std::vector<double> DualStateFrontier::getTerrainVoxelElev()
{
return terrain_voxel_elev_;
}

//设置边界处理器的状态
void DualStateFrontier::setPlannerStatus(bool status)
{
planner_status_ = status;// 0 means exploration and 1 means relocation
}

bool DualStateFrontier::initialize()
{
// Read in parameters
if (!readParameters())
return false;

// Initialize subscriber
//"/global_points",由dual_state_graph.cpp发布，发布的是局部图中的节点
graph_points_sub_ =nh_.subscribe(sub_graph_points_topic_, 1, &DualStateFrontier::graphPointsCallback, this);
// "/state_estimation"
odom_sub_.subscribe(nh_, sub_odom_topic_, 1);
//" /terrain_map_ext"
terrain_point_cloud_sub_.subscribe(nh_, sub_terrain_point_cloud_topic_, 1);
//同时获取两个信息的写法，值得学习
//获取里程计订阅和地形形式订阅
sync_.reset(new Sync(syncPolicy(10), odom_sub_, terrain_point_cloud_sub_));
sync_->registerCallback(boost::bind(&DualStateFrontier::terrainCloudAndOdomCallback, this, _1, _2));

//"/octomap_unknown"
unknown_points_pub_ =nh_.advertise<sensor_msgs::PointCloud2>(pub_unknown_points_topic_, 1);
//"/global_frontier"
global_frontier_points_pub_ = nh_.advertise<sensor_msgs::PointCloud2>(pub_global_frontier_points_topic_, 1);
//"local_frontiers"
local_frontier_points_pub_ = nh_.advertise<sensor_msgs::PointCloud2>(pub_local_frontier_points_topic_, 1);
terrain_elev_cloud_pub_ =nh_.advertise<sensor_msgs::PointCloud2>("/elevation_map", 1);

//每隔一定频率执行一次execute函数
if (kExecuteFrequency_ > 0.0)
{
executeTimer_ =nh_private_.createTimer(ros::Duration(1.0 / kExecuteFrequency_),&DualStateFrontier::execute, this);
}
for (int i = 0; i < kTerrainVoxelWidth * kTerrainVoxelWidth; i++)
{
terrain_voxel_elev_.push_back(robot_position_.z());
terrain_voxel_points_num_.push_back(0);
terrain_voxel_min_elev_.push_back(1000);
terrain_voxel_max_elev_.push_back(-1000);
}

ROS_INFO("Successfully launched DualStateFrontier node");
return true;
}

void DualStateFrontier::execute(const ros::TimerEvent &e) { getFrontiers(); }

}

grid.cpp

/*
grid.cpp
Implementation of OccupancyGrid class. Occupancy grid is used to get do the
collision check
based on terrain points in
local area.
Created by Hongbiao Zhu (hongbiaz@andrew.cmu.edu)
05/25/2020
*/

#ifndef GRID_HPP_
#define GRID_HPP_

#include "dsvplanner/grid.h"

namespace dsvplanner_ns {
OccupancyGrid::OccupancyGrid(const ros::NodeHandle &nh, const ros::NodeHandle &nh_private)
: nh_(nh), nh_private_(nh_private)
{
initialize();
}

OccupancyGrid::~OccupancyGrid() {}

bool OccupancyGrid::readParameters()
{
nh_private_.getParam("/grid/world_frame_id", world_frame_id_);
nh_private_.getParam("/grid/odomSubTopic", sub_odom_topic_);
nh_private_.getParam("/grid/terrainCloudSubTopic",sub_terrain_point_cloud_topic_);
nh_private_.getParam("/grid/pubGridPointsTopic", pub_grid_points_topic_);
nh_private_.getParam("/grid/kMapWidth", kMapWidth);
nh_private_.getParam("/grid/kGridSize", kGridSize);
nh_private_.getParam("/grid/kDownsampleSize", kDownsampleSize);
nh_private_.getParam("/grid/kObstacleHeightThre", kObstacleHeightThre);
nh_private_.getParam("/grid/kFlyingObstacleHeightThre",kFlyingObstacleHeightThre);
nh_private_.getParam("/rm/kBoundX", kCollisionCheckX);
nh_private_.getParam("/rm/kBoundY", kCollisionCheckY);
return true;
}

bool OccupancyGrid::initialize()
{
// Read in parameters
if (!readParameters())
return false;
// Initialize subscriber
//"/state_estimation"
odom_sub_.subscribe(nh_, sub_odom_topic_, 1);
//"terrain_map_ext"
terrain_point_cloud_sub_.subscribe(nh_, sub_terrain_point_cloud_topic_, 1);
sync_.reset(new Sync(syncPolicy(100), odom_sub_, terrain_point_cloud_sub_));
sync_->registerCallback(boost::bind(&OccupancyGrid::terrainCloudAndOdomCallback, this, _1, _2));
//"/occpancy_grid_map"
grid_cloud_pub_ =nh_.advertise<sensor_msgs::PointCloud2>(pub_grid_points_topic_, 1);
//机器人周围的网格大小/2，再比kGridSize网格大小0.1，得到格子数目
map_half_width_grid_num_ = int(kMapWidth / 2 / kGridSize);
//加1应该是指自己正中间那一行/列
map_width_grid_num_ = map_half_width_grid_num_ * 2 + 1;

clearGrid();

ROS_INFO("Successfully launched OccupancyGrid node");

return true;
}

//回调函数，监听地形信息，里程计信息，更新并发布gridmap
void OccupancyGrid::terrainCloudAndOdomCallback(const nav_msgs::Odometry::ConstPtr &odom_msg,
const sensor_msgs::PointCloud2::ConstPtr &terrain_msg)
{
terrain_time_ = terrain_msg->header.stamp;
robot_position_[0] = odom_msg->pose.pose.position.x;
robot_position_[1] = odom_msg->pose.pose.position.y;
robot_position_[2] = odom_msg->pose.pose.position.z;
terrain_cloud_->clear();
terrain_cloud_ds->clear();
terrain_cloud_traversable_->clear();
terrain_cloud_obstacle_->clear();
pcl::fromROSMsg(*terrain_msg, *terrain_cloud_);

pcl::VoxelGrid<pcl::PointXYZI> point_ds;
point_ds.setLeafSize(kDownsampleSize, kDownsampleSize, kDownsampleSize);
//地形消息下采样
point_ds.setInputCloud(terrain_cloud_);
point_ds.filter(*terrain_cloud_ds);

pcl::PointXYZI point;
int terrainCloudSize = terrain_cloud_ds->points.size();
for (int i = 0; i < terrainCloudSize; i++)
{
point.x = terrain_cloud_ds->points[i].x;
point.y = terrain_cloud_ds->points[i].y;
point.z = terrain_cloud_ds->points[i].z;
point.intensity = terrain_cloud_ds->points[i].intensity;
// crop all ground points
//0.2m到1m之间的障碍物认为是obstacle（kObstacleHeightThre默认0.2，感觉有点偏高了）
if (point.intensity > kObstacleHeightThre &&point.intensity < kFlyingObstacleHeightThre)
{
terrain_cloud_obstacle_->push_back(point);
}
else if (point.intensity <= kObstacleHeightThre)
{
terrain_cloud_traversable_->push_back(point);
}
}

clearGrid();
updateGrid();
publishGridMap();
}

//功能：给定一个网格索引，返回一个点，x和y为机器人坐标系下该索引处的位置，z为机器人的高度
geometry_msgs::Point OccupancyGrid::getPoint(GridIndex p)
{
int indX = p[0];
int indY = p[1];
double x = kGridSize * (indX - map_half_width_grid_num_) + robot_position_[0];
double y = kGridSize * (indY - map_half_width_grid_num_) + robot_position_[1];
geometry_msgs::Point point;
point.x = x;
point.y = y;
point.z = robot_position_[2];
return point;
}

//给定一个点，返回gridindex格式的索引（Vector2i ）
GridIndex OccupancyGrid::getIndex(StateVec point)
{
int indX = int((point.x() - robot_position_[0] + kGridSize / 2) / kGridSize) +map_half_width_grid_num_;
int indY = int((point.y() - robot_position_[1] + kGridSize / 2) / kGridSize) +map_half_width_grid_num_;
if (point.x() - robot_position_[0] + kGridSize / 2 < 0)
indX--;
if (point.y() - robot_position_[1] + kGridSize / 2 < 0)
indY--;
if (indX < 0)
indX = 0;
if (indY < 0)
indY = 0;
if (indX > map_width_grid_num_ - 1)
indX = map_width_grid_num_ - 1;
if (indY > map_width_grid_num_ - 1)
indY = map_width_grid_num_ - 1;
GridIndex grid_index(indX, indY);
return grid_index;
}

//把grideState全部填充为unknown状态
void OccupancyGrid::clearGrid()
{
gridState_.clear();
std::vector<int> y_vector;
for (int i = 0; i < map_width_grid_num_; i++)
{
y_vector.clear();
for (int j = 0; j < map_width_grid_num_; j++)
{
gridStatus grid_state = unknown;
y_vector.push_back(grid_state);
}
gridState_.push_back(y_vector);
}
}

//根据terrain_cloud_obstacle_和terrain_cloud_traversable_保存grideState状态
//terrain_cloud_obstacle_等在回调函数中被处理和填充
void OccupancyGrid::updateGrid()
{
pcl::PointXYZI point;
//遍历的是障碍物的点
for (int i = 0; i < terrain_cloud_obstacle_->points.size(); i++)
{
point = terrain_cloud_obstacle_->points[i];
//将障碍物点换算成索引
int indX = int((point.x - robot_position_[0] + kGridSize / 2) / kGridSize) +map_half_width_grid_num_;
int indY = int((point.y - robot_position_[1] + kGridSize / 2) / kGridSize) +map_half_width_grid_num_;
//应对负数四舍五入情况
if (point.x - robot_position_[0] + kGridSize / 2 < 0)
indX--;
if (point.y - robot_position_[1] + kGridSize / 2 < 0)
indY--;

//限制索引范围
if (indX < 0)
indX = 0;
if (indY < 0)
indY = 0;
if (indX > map_width_grid_num_ - 1)
indX = map_width_grid_num_ - 1;
if (indY > map_width_grid_num_ - 1)
indY = map_width_grid_num_ - 1;

if (indX >= 0 && indX < map_width_grid_num_ && indY >= 0 && indY < map_width_grid_num_)
{
//赋值占据状态
gridStatus grid_state = occupied;
gridState_[indX][indY] = grid_state;
}
}

for (int i = 0; i < terrain_cloud_traversable_->points.size(); i++)
{
//将空闲点换算成索引
point = terrain_cloud_traversable_->points[i];
int indX = int((point.x - robot_position_[0] + kGridSize / 2) / kGridSize) +map_half_width_grid_num_;
int indY = int((point.y - robot_position_[1] + kGridSize / 2) / kGridSize) +map_half_width_grid_num_;
//应对负数四舍五入情况
if (point.x - robot_position_[0] + kGridSize / 2 < 0)
indX--;
if (point.y - robot_position_[1] + kGridSize / 2 < 0)
indY--;

if (indX < 0)
indX = 0;
if (indY < 0)
indY = 0;
if (indX > map_width_grid_num_ - 1)
indX = map_width_grid_num_ - 1;
if (indY > map_width_grid_num_ - 1)
indY = map_width_grid_num_ - 1;

if (indX >= 0 && indX < map_width_grid_num_ && indY >= 0 &&indY < map_width_grid_num_)
{
//unknown = 0, free = 1, occupied = 2, near_occupied = 3
if (gridState_[indX][indY] == 2)
{
continue;
}
//返回true，代表周围没有障碍物
if (updateFreeGridWithSurroundingGrids(indX, indY) == false)
{
gridStatus grid_state = free;
gridState_[indX][indY] = grid_state;
}
else
{
//周围如果也有障碍物，这里也弄成near_occupied
gridStatus grid_state = near_occupied;
gridState_[indX][indY] = grid_state;
}
}
}
}

//发布"/occpancy_grid_map"
void OccupancyGrid::publishGridMap()
{
grid_cloud_->clear();
pcl::PointXYZI p1;
geometry_msgs::Point p2;
GridIndex p3;
for (int i = 0; i < map_width_grid_num_; i++)
{
for (int j = 0; j < map_width_grid_num_; j++)
{
p3[0] = i;
p3[1] = j;
p2 = getPoint(p3);
p1.x = p2.x;
p1.y = p2.y;
p1.z = p2.z;
//点云的强度字段被填入状态
//unknown = 0, free = 1, occupied = 2, near_occupied = 3
p1.intensity = gridState_[i][j];
grid_cloud_->points.push_back(p1);
}
}

//发布"/occpancy_grid_map"
sensor_msgs::PointCloud2 gridCloud2;
pcl::toROSMsg(*grid_cloud_, gridCloud2);
gridCloud2.header.stamp = terrain_time_;
gridCloud2.header.frame_id = world_frame_id_;
grid_cloud_pub_.publish(gridCloud2);
}

//周围kBoundX范围内如果有障碍物，这里也会认为有障碍物,返回true为有障碍物
bool OccupancyGrid::updateFreeGridWithSurroundingGrids(int indx, int indy)
{
//kCollisionCheckX是kBoundX，碰撞半径
//碰撞半径的一半，除以栅格大小，代表碰撞半径的栅格数目，
//下面在循环里要进行负到正，检查当前索引周围有没有障碍物
int count_x = ceil(0.5 * kCollisionCheckX / kGridSize);
int count_y = ceil(0.5 * kCollisionCheckY / kGridSize);
int indX;
int indY;
for (int i = -count_x; i <= count_x; i++)
{
for (int j = -count_y; j <= count_y; j++)
{
indX = indx + i;
indY = indy + j;
if (indX >= 0 && indX < map_width_grid_num_ && indY >= 0 &&indY < map_width_grid_num_)
{
if (gridState_[indX][indY] == 2)
{
return true;
}
}
}
}
return false;
}

//给定一个起点，一个终点，判断二者连线上有没有障碍物
bool OccupancyGrid::collisionCheckByTerrainWithVector(StateVec origin_point,StateVec goal_point)
{
// ROS_INFO("Start Check Collision");
//获取起点的索引
GridIndex origin_grid_index = getIndex(origin_point);
//获取目标点索引
GridIndex goal_grid_index = getIndex(goal_point);
//获取整个gridmap的索引
GridIndex max_grid_index(map_width_grid_num_ - 1, map_width_grid_num_ - 1);
GridIndex min_grid_index(0, 0);

GridIndex grid_index;
//得到从起点到终点的沿途索引点
std::vector<GridIndex> ray_tracing_grids = rayCast(origin_grid_index, goal_grid_index, max_grid_index, min_grid_index);
int length = ray_tracing_grids.size();
for (int i = 0; i < length; i++)
{
grid_index = ray_tracing_grids[i];
//依次遍历，如果发现处于2状态或3状态，都是被占据了，则返回true
if (gridState_[grid_index[0]][grid_index[1]] == 2 ||gridState_[grid_index[0]][grid_index[1]] == 3)
{
return true;
}
}
return false;
}

//给定一个起点，一个终点，判断二者连线上有没有障碍物
bool OccupancyGrid::collisionCheckByTerrain(geometry_msgs::Point origin,geometry_msgs::Point goal)
{
StateVec origin_point(origin.x, origin.y, origin.z);
StateVec goal_point(goal.x, goal.y, goal.z);

return collisionCheckByTerrainWithVector(origin_point, goal_point);
}

//功能：传入三个参数，判定第一个参数的x和y是否在第二个和第三个之间
bool OccupancyGrid::InRange(const GridIndex sub, const GridIndex max_sub,const GridIndex min_sub)
{
return sub.x() >= min_sub.x() && sub.x() <= max_sub.x() &&sub.y() >= min_sub.y() && sub.y() <= max_sub.y();
}

//符号函数，把形参根据正负极性转化为-1，0，1三个值
int OccupancyGrid::signum(int x) { return x == 0 ? 0 : x < 0 ? -1 : 1; }

//求最小的正t，使s+t*ds为整数。
double OccupancyGrid::intbound(double s, double ds)
{
// Find the smallest positive t such that s+t*ds is an integer.
if (ds < 0)
{
return intbound(-s, -ds);
}
else
{
s = mod(s, 1);
// problem is now s+t*ds = 1
return (1 - s) / ds;
}
}

//返回value除以modulus的余数
double OccupancyGrid::mod(double value, double modulus)
{
//fmod（x，y）返回x除以y的余数
//例如value=10.1,moudulus=3,
//fmod(10.1, 3)=1.1
//fmod(10.1, 3)+moudulus=4.1
//fmod(4.1,3)=1.1
//为什么要求两次fmod？
return fmod(fmod(value, modulus) + modulus, modulus);
}

//功能：给定一个起点，一个终点，把沿途连线的索引坐标找到返回
std::vector<GridIndex> OccupancyGrid::rayCast(GridIndex origin, GridIndex goal, GridIndex max_grid, GridIndex min_grid)
{
std::vector<GridIndex> grid_pairs;
//起点和终点索引位置相同，就返回起点
if (origin == goal)
{
grid_pairs.push_back(origin);
return grid_pairs;
}
GridIndex diff = goal - origin;
//距离的平方范数
double max_dist = diff.squaredNorm();
//取起点到终点向量在两个方向的正负
int step_x = signum(diff.x());
int step_y = signum(diff.y());
//如果是0的话，设tmax_x为double变量的最大值
//否则调用intbound函数，例如origin.x=5，diff.x=3,送入以后，5先对1求余数，得0，然后t_max_x就是1/diff.x()=0.333
//迷惑：这是在干什么
double t_max_x = step_x == 0 ? DBL_MAX : intbound(origin.x(), diff.x());
double t_max_y = step_y == 0 ? DBL_MAX : intbound(origin.y(), diff.y());
//然后t_delta_x也=1/3=0.333
double t_delta_x = step_x == 0 ? DBL_MAX : (double)step_x / (double)diff.x();
double t_delta_y = step_y == 0 ? DBL_MAX : (double)step_y / (double)diff.y();
double dist = 0;
GridIndex cur_sub = origin;

while (true)
{
//一般都会满足这个if，因为调用时当前位置肯定在min到max之间
if (InRange(cur_sub, max_grid, min_grid))
{
grid_pairs.push_back(cur_sub);
dist = (cur_sub - origin).squaredNorm();

if (cur_sub == goal || dist > max_dist)
{
return grid_pairs;
}
if (t_max_x < t_max_y)
{
cur_sub.x() += step_x;
t_max_x += t_delta_x;
}
else
{
cur_sub.y() += step_y;
t_max_y += t_delta_y;
}
}
else
{
return grid_pairs;
}
}
}

}
#endif

SLAM 建模仿真建图导航 DSVP

打赏 0

上一篇：Realsense相机在linux下的配置使用，RGB与depth图像对齐

下一篇：SLAM就业问题汇总复习

代码解析：DSVP: Dual-Stage Viewpoint Planner for Rapid Exploration by Dynamic Expansion

微鉴道长

内容列表

写在前面

论文解析

代码注释

exploration_with_graph_planner.cpp

graph_planner.cpp

drrtp_node.cpp

先验

dual_state_graph.cpp

drrtp.cpp

drrt.cpp

dual_state_frontier.cpp

grid.cpp

为你推荐

win平台使用惯导模块：WHEELTEC N100 模块

Udacity机器人软件工程师课程笔记（五）-样本搜索和找回-基于漫游者号模拟器-自主驾驶

机器人学习--从路径规划侧面看栅格地图

基础卡尔曼定位-多维情况

精选AGV导航中的最短路径算法比较

今天开始学vSLAM啦—— （一）综述

评论（0）

关于作者

微鉴道长

28

0

0

5

SLAM14讲学习笔记（十五）卡尔曼滤波器的直观理解

SLAM14讲学习笔记（二）相机模型总结

SLAM学习笔记（十六）快速了解EPnP算法原理

相关推荐

视觉slam学习之——ch6 非线性曲线拟合（centos系统）

激光SLAM | 基于固态雷达的里程计：Loam Livox

CBS多机器人路径规划

精选三大后端优化方法(Ceres&G2O&>SAM)

视觉SLAM学习【1】-----基于ubuntu16.04的SLAM中ORB特征点提取及暴力匹配

A星算法优化（一）启发函数

热门泡泡

30积分 失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

TA的专栏

SLAM学习笔记

给作者打赏

忘记密码

修改头像

添加你感兴趣的标签

举报类型（必选）

举报详情（选填）

30积分失眠，聊聊自己搞ROS的心得体会吧

5积分想买能用ROS2的开发套件。或者开发板