ROS 导航 :make_plan (路线规划)

以下是在学习ROS 导航时，按照自己的理解整理的资料，有不对的地方请指出。
路径规划：从一个点到另一个点，规划出最优的路线。用到service :make_plan (nav_msgs/GetPlan)
服务名为move_base_node/make_plan
nav_msgs/GetPlan api：

 # Get a plan from the current position to the goal Pose 

# The start pose for the plan
geometry_msgs/PoseStamped start

# The final pose of the goal position
geometry_msgs/PoseStamped goal

# If the goal is obstructed, how many meters the planner can 
# relax the constraint in x and y before failing. 
float32 tolerance
---
nav_msgs/Path plan

Compact Message Definition
geometry_msgs/PoseStamped start
geometry_msgs/PoseStamped goal
float32 tolerance
nav_msgs/Path plan

现在学习如何使用
在工作空间新建package navigation_example

cd ~/catkin_ws/src
catkin_create_pkg navigation_example std_msgs rospy roscpp tf actionlib

CMakeList.txt 中的find_package如下

find_package(catkin REQUIRED COMPONENTS
  actionlib
  roscpp
  rospy
  std_msgs
  tf
)

在src目录下新建make_plan.cpp

/*
 * make_plan.cpp
 *
 *  Created on: Aug 10, 2016
 *      Author: unicorn
 */
//路线规划代码
#include <ros/ros.h>
#include <nav_msgs/GetPlan.h>
#include <geometry_msgs/PoseStamped.h>
#include <string>
#include <boost/foreach.hpp>
#define forEach BOOST_FOREACH
void fillPathRequest(nav_msgs::GetPlan::Request &request)
{
request.start.header.frame_id ="map";
request.start.pose.position.x = 12.378;//初始位置x坐标
request.start.pose.position.y = 28.638;//初始位置y坐标
request.start.pose.orientation.w = 1.0;//方向
request.goal.header.frame_id = "map";
request.goal.pose.position.x = 18.792;//终点坐标
request.goal.pose.position.y = 29.544;
request.goal.pose.orientation.w = 1.0;
request.tolerance = 0.5;//如果不能到达目标，最近可到的约束
}
//路线规划结果回调
void callPlanningService(ros::ServiceClient &serviceClient, nav_msgs::GetPlan &srv)
{
// Perform the actual path planner call
//执行实际路径规划器
if (serviceClient.call(srv)) {
//srv.response.plan.poses 为保存结果的容器，遍历取出
if (!srv.response.plan.poses.empty()) {
forEach(const geometry_msgs::PoseStamped &p, srv.response.plan.poses) {
ROS_INFO("x = %f, y = %f", p.pose.position.x, p.pose.position.y);
}
}
else {
ROS_WARN("Got empty plan");
}
}
else {
ROS_ERROR("Failed to call service %s - is the robot moving?",
serviceClient.getService().c_str());
}
}

int main(int argc, char** argv)
{
ros::init(argc, argv, "make_plan_node");
ros::NodeHandle nh;
// Init service query for make plan
//初始化路径规划服务，服务名称为"move_base_node/make_plan"
std::string service_name = "move_base_node/make_plan";
//等待服务空闲，如果已经在运行这个服务，会等到运行结束。
while (!ros::service::waitForService(service_name, ros::Duration(3.0))) {
ROS_INFO("Waiting for service move_base/make_plan to become available");
}
/*初始化客户端，(nav_msgs/GetPlan)
Allows an external user to ask for a plan to a given pose from move_base without causing move_base to execute that plan.
允许用户从move_base 请求一个plan，并不会导致move_base 执行此plan
*/ 
ros::ServiceClient serviceClient = nh.serviceClient<nav_msgs::GetPlan>(service_name, true);
if (!serviceClient) {
ROS_FATAL("Could not initialize get plan service from %s",
serviceClient.getService().c_str());
return -1;
}
nav_msgs::GetPlan srv;
//请求服务：规划路线
fillPathRequest(srv.request);
if (!serviceClient) {
ROS_FATAL("Persistent service connection to %s failed",
serviceClient.getService().c_str());
return -1;
}
ROS_INFO("conntect to %s",serviceClient.getService().c_str());
callPlanningService(serviceClient, srv);
}

CMakeList中添加

add_executable(make_plan src/make_plan.cpp)
 target_link_libraries(make_plan
   ${catkin_LIBRARIES}
 )

编译运行：

cd ~/catkin_ws
catkin_make
source devel/setup.bash
rosrun navigation_example make_plan

运行结果：