1、路径可视化发布

path_pub_ = nh.advertise<nav_msgs::Path>("path_view",10);    

    nav_msgs::Path path;

    path.header.stamp = ros::Time::now();

    path.header.frame_id = "/map";

    float j = 0;

    for(int i = 0; i < 300; ++i){

        j += 0.1;

        geometry_msgs::PoseStamped pose;

        pose.pose.position.x = j;

        pose.pose.position.y = j;

        pose.pose.position.z = 0;

        path.poses.push_back(pose);

    }

    path_pub_.publish(path);

2、marker发布

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

    visualization_msgs::Marker m;

    m.header.stamp = ros::Time::now();

    m.header.frame_id = "/map";

    m.id = 0;

    m.type = visualization_msgs::Marker::LINE_STRIP;;

    m.scale.x = .1;

    m.scale.y = .1;

    m.scale.z = .1;

    m.color.a = 1;

    m.lifetime = ros::Duration(0.5);

    m.color.r = 1;

    m.color.g = 1;

    float k = -1;

    for(int i = 0; i < 100; ++i)

    {

        k -= 0.1;

        geometry_msgs::Point p;

        p.x = k;

        p.y = k;

        p.z = 0;

        m.points.push_back(p);

    }

    markers_pub_.publish(m);

3、请求make_plan服务

#include <nav_msgs/GetPlan.h>

ros::ServiceClient serviceClient;

serviceClient = nh.serviceClient<nav_msgs::GetPlan>("move_base/make_plan", true);

int class::request_plan(void)

{

    nav_msgs::GetPlan srv;

    fillPathRequest(srv.request);

    if(serviceClient.call(srv))

    {

        callPlanningService(serviceClient, srv);

    }

    else

    {

        ROS_ERROR("Failed to call service!");

    }

    return 0;

}

void class::callPlanningService(ros::ServiceClient &serviceClient, nav_msgs::GetPlan &srv)

{

    if (!srv.response.plan.poses.empty()) {

        BOOST_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");

    }

}

void class::fillPathRequest(nav_msgs::GetPlan::Request &request)

{

    request.start.header.frame_id ="map";

    request.start.pose.position.x = -1;//初始位置x坐标

    request.start.pose.position.y = -1;//初始位置y坐标

    request.start.pose.orientation.w = 1.0;//方向

    request.goal.header.frame_id = "map";

    request.goal.pose.position.x = -10;//终点坐标

    request.goal.pose.position.y = -10;

    request.goal.pose.orientation.w = 1.0;

    request.tolerance = 0.5;//如果不能到达目标，最近可到的约束

}