ROS中make_plan服务的使用

void fillPathRequest(nav_msgs::GetPlan::Request &request, float start_x, float start_y, float goal_x, float goal_y)
{
    request.start.header.frame_id ="map";
    request.start.pose.position.x = start_x;//初始位置x坐标
    request.start.pose.position.y = start_y;//初始位置y坐标
    request.start.pose.orientation.w = 1.0;//方向
    request.goal.header.frame_id = "map";
    request.goal.pose.position.x = goal_x;//终点坐标
    request.goal.pose.position.y = goal_y;
    request.goal.pose.orientation.w = 1.0;
    request.tolerance = 0.0;//如果不能到达目标，最近可到的约束
}

void myfunction(geometry_msgs::PoseStamped p)
{
    std::cout << "X = " << p.pose.position.x << "   Y = " << p.pose.position.y << std::endl;
}

//路线规划结果回调
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()) {
//            std::for_each(srv.response.plan.poses.begin(),srv.response.plan.poses.end(),myfunction);
            ROS_INFO("make_plan success!");
        }
    }
    else {
        ROS_WARN("Got empty plan");
    }
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "system_manager_node");
    ros::NodeHandle nh("~");
    ros::NodeHandle nhandle;

    ros::Publisher markers_pub_ = nhandle.advertise<visualization_msgs::Marker>("visualization_marker", 20);

    ros::ServiceClient make_plan_client = nhandle.serviceClient<nav_msgs::GetPlan>("move_base/make_plan",true);
    if (!make_plan_client) {
        ROS_FATAL("Could not initialize get plan service from %s",
        make_plan_client.getService().c_str());
        return -1;
    }

    nav_msgs::GetPlan srv;
    fillPathRequest(srv.request,-7,-3.0,11.338,-5.8852);
    callPlanningService(make_plan_client,srv);

    visualization_msgs::Marker mk1;
    mk1.header.stamp = ros::Time::now();
    mk1.header.frame_id = "/map";
    mk1.id = 0;
    mk1.type = visualization_msgs::Marker::LINE_STRIP;
    mk1.scale.x = 0.05; mk1.scale.y = 0.05; mk1.scale.z = 0.05;
    mk1.color.r = 1.0; mk1.color.a = 1.0;
    for(int i = 100; i < srv.response.plan.poses.size(); i++)
    {
        mk1.points.push_back(srv.response.plan.poses[i].pose.position);
    }
    markers_pub_.publish(mk1);
}