前言：笔者研0学习中，理解有限，也在学习该算法的朋友可以共同交流学习。
1.bool GraphPlanner::readParameters
bool GraphPlanner::readParameters()该函数用来加载参数服务器参数并判断是否成功
2.void GraphPlanner::publishPath
该函数用来将planned_path_中的数据发布出去，其实就是wayPoint，根据作者描述是为了debug用的，对整体使用没有影响。
void GraphPlanner::publishPath()
{
nav_msgs::Path planned_path; //里程计消息
planned_path.header.frame_id = world_frame_id_;
planned_path.header.stamp = ros::Time::now();
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);//将添加后的路径发布出去
}3.void GraphPlanner::odometryCallback
该函数为回调函数在此处被调用
odometry_sub_ = nh_.subscribe(sub_odometry_topic_, 1, &GraphPlanner::odometryCallback, this);其作用为订阅sub_odometry_topic话题发送的消息，获取坐标和航向角yaw，由于无法直接通过四元数得到欧拉角所以通过tf::Matrix3x3(tf::Quaternion(geo_quat.x, geo_quat.y, geo_quat.z, geo_quat.w)).getRPY(roll, pitch, yaw);间接获取yaw.为了获取机器人当前位置使用。
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;
}4.void GraphPlanner::commandCallback
该函数用来处理3类command指令，当收到COMMAND_GO_TO_LOCATION后会将previous_shortest_path_size_设置为100000，为了防止机器人在探索过程中，在两个点之间不停的徘徊。
void GraphPlanner::commandCallback(const graph_planner::GraphPlannerCommand::ConstPtr& msg)
{
graph_planner_command_ = *msg;
if (graph_planner_command_.command == graph_planner::GraphPlannerCommand::COMMAND_GO_TO_LOCATION)
{
previous_shortest_path_size_ = 100000;
}
}5.void GraphPlanner::graphCallback
void GraphPlanner::graphCallback(const graph_utils::TopologicalGraph::ConstPtr& graph_msg)
{
planned_graph_ = *graph_msg;
}是用来订阅dsvplanner发来的局部图，其结构为：
TopologicalGraph
---Header header
---Vertex[] vertex #顶点
---geometry/Point location #每个顶点坐标
---int32 parent_idx #父顶点索引
---float32 information_gain
---int32 vertex_id #顶点索引
---Edge[] edge #每个顶点引出的边
---int32 vertex_id_end #每条边的终点
---bool keypose_edge
---float32 traversal_costs #每条边的损失
---float32 risks
6.void GraphPlanner::terrainCallback
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;
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_);
}点云还没了解后期补充
7.GraphPlanner::publishInProgress
发送给dsvplanner节点，来使能或者失能规划器
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);
}8.void GraphPlanner::alterAndPublishWaypoint
用来发送waypoint
void GraphPlanner::alterAndPublishWaypoint()
{
// Ship it!
waypoint_.header.frame_id = world_frame_id_; // added by Hong in 20191203 "/map"
waypoint_.header.stamp = ros::Time::now(); // added by Hong in 20191203
waypoint_pub_.publish(waypoint_);//"/way_point"
}9.bool GraphPlanner::goToVertex
该函数为本节点的核心逻辑，思想为找到当前点到目标点的最短路径，如果该路径大小为0说明无法到达目标点，如果该路径大小小于3则说明当前点或当前点的下一个点就是目标点，直接到达即可。
如果该路径大小大于2说明无法直接到达目标点，所以首先需要在最短路径中找到满足欧式距离和路径距离都大于移动阈值的第一个点，将这个点作为到达的顶点。但是还需要判断到达这个点的路径中是否与障碍物发生碰撞，如果发送就去碰撞点的前一个点，否则不用改变。
然后再将该点作为出发点寻找到达目标点的最小路径，判断此时路径大小是否大于上次路径大小，如果大于说明路径错误，之间将上一次的目标点作为当前目标点，否则就将当前的点存储起来。然后判断寻找到的waypoint是否为目标点，如果是，那么就到达该点并通知规划器开始新的一轮探索，否则就继续前进，重新构图。
bool GraphPlanner::goToVertex(int current_vertex_idx, int goal_vertex_idx)
{
std::vector<int> shortest_path;
//找到起点到目标点的最短路径，并传入shortest_path中
graph_utils_ns::ShortestPathBtwVertex(shortest_path, planned_graph_, current_vertex_idx, goal_vertex_idx);
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)//路径大小为2，也就是当前点即为目标点或者下一个点就为目标点
{
geometry_msgs::Point next_waypoint = planned_graph_.vertices[shortest_path.back()].location;//back是最后一个元素也就是目标点
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_;//"/map"
// clear the saved path
planned_path_.clear();
planned_path_.push_back(waypoint_.point);
alterAndPublishWaypoint();//发布waypoint
return false;
}
else//如果路径大小大于2
{
int next_vertex_id;
//找到大于阈值的那个点作为下一个移动点
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;
//判断是否碰撞
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;
}
}
std::vector<int> next_shortest_path;
//到达新的点后再次寻找新的点到目标点的最短路径，传入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
if (next_shortest_path.size() > previous_shortest_path_size_)
{
next_vertex_id = previous_vertex_id_;
backTraceCount_++;
if (backTraceCount_ > kNextVertexMaintainTime * kExecuteFrequency) // backTraceCount_>25
{
backTraceCount_ = 0;
previous_shortest_path_size_ = 100000;
}
}
else
{
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;
//这是两次无法到达的话让其快速到达
if (next_vertex_id != shortest_path.back())
{
next_waypoint = projectWayPoint(next_waypoint, robot_pos_);
}
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
alterAndPublishWaypoint();
return false;
}
else
{
// Keep moving along planned path
return true;
}
}
}10.bool GraphPlanner::goToPoint
获取距离机器人当前位置最近的点作为起点，将距离该点最近的点作为目标点（都是欧式距离）
bool GraphPlanner::goToPoint(geometry_msgs::Point point)
{
if (planned_graph_.vertices.size() <= 0)//判断顶点的size是否小于0
{
return false;
}
// Find where I am on this graph
//robot_pos_是通过里程计获取，获取到离机器人最近的那个点的id,也就是当前在定位机器人
int current_vertex_idx = graph_utils_ns::GetClosestVertexIdxToPoint(planned_graph_, robot_pos_);
// find closest goal vertex
//point为目标点（可能不在图中） 所以要找到图中离目标点最近的点(也可能就是自己)，所谓id就是数组的索引
int goal_vertex_idx = graph_utils_ns::GetClosestVertexIdxToPoint(planned_graph_, point);
// Set the waypoint
return goToVertex(current_vertex_idx, goal_vertex_idx);
}
11.bool GraphPlanner::collisionCheckByTerrain
障碍物碰撞检测
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;
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));
double check_point_num = distance / (kCollisionCheckDistace);
for (int j = 0; j < check_point_num; j++)
{
geometry_msgs::Point p1;
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;
}12.geometry_msgs::Point GraphPlanner::projectWayPoint
一种策略，为了更快的到达目标点。
geometry_msgs::Point GraphPlanner::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;
}13.bool GraphPlanner::initialize
该函数定义了目标点，以及一系列订阅者和发布者。
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
graph_sub_ = nh_.subscribe(sub_graph_topic_, 1, &GraphPlanner::graphCallback, this);
graph_planner_command_sub_ = nh_.subscribe(graph_planner_command_topic_, 1, &GraphPlanner::commandCallback, this);
odometry_sub_ = nh_.subscribe(sub_odometry_topic_, 1, &GraphPlanner::odometryCallback, this);
terrain_sub_ = nh_.subscribe(sub_terrain_topic_, 1, &GraphPlanner::terrainCallback, this);
// Initialize publishers
waypoint_pub_ = nh_.advertise<geometry_msgs::PointStamped>(pub_waypoint_topic_, 2);
graph_planner_status_pub_ = nh_.advertise<graph_planner::GraphPlannerStatus>(graph_planner_status_topic_, 2);
graph_planner_path_pub_ = nh_.advertise<nav_msgs::Path>(pub_path_topic_, 10);
ROS_INFO("Successfully launched graph_planner node");
return true;
}14.void GraphPlanner::executeGoToOrigin
该函数控制机器人回到起点
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);
}
}
15.void GraphPlanner::executeGoToLocation()
该函数控制机器人到达目标点
void GraphPlanner::executeGoToLocation()
{
// Compute waypoint
bool success = goToPoint(graph_planner_command_.location);
if (!success)
{
publishInProgress(false);
}
else
{
alterAndPublishWaypoint();
publishInProgress(true);
}
}
16.void GraphPlanner::executeCommand
对command的控制
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);
}
}
17.bool GraphPlanner::execute
该节点的总体逻辑，根据收到的command来进行不同的步骤实现对应功能
bool GraphPlanner::execute()
{
ros::Rate rate(kExecuteFrequency);//5hz
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;
}18.总结
该节点对局部图进行处理，找到并发布waypoint
2243
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
