1、路徑視覺化釋出

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

nav_msgs::Pathpath;

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

path.header.frame_id="/map";

floatj=0;

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

j+=0.1;

geometry_msgs::PoseStampedpose;

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::Markerm;

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;

floatk=-1;

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

{

k-=0.1;

geometry_msgs::Pointp;

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::ServiceClientserviceClient;

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

intclass::request_plan(void)

{

nav_msgs::GetPlansrv;

fillPathRequest(srv.request);

if(serviceClient.call(srv))

{

callPlanningService(serviceClient,srv);

}

else

{

ROS_ERROR("Failedtocallservice!");

}

return0;

}

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

{

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

BOOST_FOREACH(constgeometry_msgs::PoseStamped&p,srv.response.plan.poses){

ROS_INFO("x=%f,y=%f",p.pose.position.x,p.pose.position.y);

}

}

else{

ROS_WARN("Gotemptyplan");

}

}

voidclass::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;//如果不能到達目標，最近可到的約束

}