# 探索算法：dsvp学习笔记（graph_planner.cpp）

## 2.void GraphPlanner::publishPath

void GraphPlanner::publishPath()
{
nav_msgs::Path planned_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);//将添加后的路径发布出去
}

nav_msgs消息包详细信息

## 3.void GraphPlanner::odometryCallback

odometry_sub_ = nh_.subscribe(sub_odometry_topic_, 1, &GraphPlanner::odometryCallback, this);

yaw.roll.pitch详解见该博主

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

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

TopologicalGraph

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

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

void GraphPlanner::alterAndPublishWaypoint()
{
// Ship it!
waypoint_pub_.publish(waypoint_);//"/way_point"
}

## 9.bool GraphPlanner::goToVertex

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_.point.x = next_waypoint.x;
waypoint_.point.y = next_waypoint.y;
waypoint_.point.z = next_waypoint.z;

// 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 =
//判断下一个目标点的路径中是否发生碰撞，如果碰撞就去碰撞点的前一个点
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_.point.x = next_waypoint.x;
waypoint_.point.y = next_waypoint.y;
waypoint_.point.z = next_waypoint.z;

// 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()
{
return false;

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

// 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

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

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

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.总结

• 0
点赞
• 2
收藏
觉得还不错? 一键收藏
• 0
评论
07-14 344
09-10 2734
12-23 997
12-07 1万+
01-07 5496
01-14 4980

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