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

主要是一个叫做make_plan的Service。

 

 

#include 
     
      #include 
      
       #include 
       
        #include 
        
         #include 
         
          #include 
          
           #define forEach BOOST_FOREACHnav_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
           
            ("/goal",10); plan_client=n.serviceClient
            
             ("/move_base/make_plan"); ros::Subscriber clickedpoint=n.subscribe("/clicked_point", 100, clicked_point); ros::spin(); return 0;}
            
           
          
         
        
       
      
     

 

 

 

参考资料：　　

Nav_msgs docs：

csdn的make_plan资料: