ROS入门（八） make_plan的Server连接

在move_base里面，将起始点和终点规划路径并且离散化。

主要是一个叫做make_plan的Service。

#include <ros/ros.h>
#include <nav_msgs/GetPlan.h>
#include <move_base_msgs/MoveBaseActionGoal.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PointStamped.h>
#include <boost/foreach.hpp>
#define forEach BOOST_FOREACH

nav_msgs::GetPlan srv;
ros::Publisher goal_pub;
ros::ServiceClient plan_client;

void callPlanningService(ros::ServiceClient plan_client, nav_msgs::GetPlan &srv)
{
  if(plan_client.call(srv)){
    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_INFO("EMPTY PLAN.");
    }
  }
  else
  {
    ROS_ERROR("Failed to call service.");
  }
}


void clicked_point(const geometry_msgs::PointStamped& msg)
{
  //ROS_INFO("Got clicked point:[%f,%f,%f]", msg.pose.x, msg.pose.y, msg.pose.z);

  nav_msgs::GetPlan srv;
  move_base_msgs::MoveBaseActionGoal goal;

  geometry_msgs::PoseStamped goalPos;
  goalPos.header.frame_id = "map";
  goalPos.header.stamp = ros::Time::now();
  goalPos.pose.position.x = 1.63;
  goalPos.pose.position.y = 2.9;
  goalPos.pose.position.z = 0;
  goalPos.pose.orientation.x = 0;
  goalPos.pose.orientation.y = 0;
  goalPos.pose.orientation.z = 0.115;
  goalPos.pose.orientation.w = 0.99;

  srv.request.start.header.stamp = ros::Time::now();
  srv.request.start.header.frame_id = "map";
  srv.request.start.pose.position.x  = msg.point.x;
  srv.request.start.pose.position.y  = msg.point.y;
  srv.request.start.pose.position.z  = 0;
  srv.request.start.pose.orientation.x  = 0;
  srv.request.start.pose.orientation.y  = 0;
  srv.request.start.pose.orientation.z  = 0;
  srv.request.start.pose.orientation.w  = 1;
  //srv.request.goal.header.frame_id = "map";
  srv.request.goal = goalPos;
  srv.request.tolerance = 0.5;

  goal.goal.target_pose = goalPos;
  goal_pub.publish(goal);
  callPlanningService(plan_client, srv);
}



int main(int argc, char** argv)
{
  ros::init(argc, argv, "send_goal");
  ros::NodeHandle n;
  goal_pub=n.advertise<move_base_msgs::MoveBaseActionGoal>("/goal",10);
  plan_client=n.serviceClient<nav_msgs::GetPlan>("/move_base/make_plan");
  ros::Subscriber clickedpoint=n.subscribe("/clicked_point", 100, clicked_point);

  ros::spin();
  return 0;
}

参考资料：　　

Nav_msgs docs：

http://docs.ros.org/api/nav_msgs/html/srv/GetPlan.html

csdn的make_plan资料:

http://blog.csdn.net/yiranhaiziqi/article/details/52891312