利用 make_plan 规划起点到目标点的路径，并且发布出去

 

geometry_msgs::PoseStamped Start;
    Start.header.seq = 0;
    Start.header.stamp = Time(0);
    Start.header.frame_id = "map";
    Start.pose.position.x = x1;
    Start.pose.position.y = y1;
    Start.pose.position.z = 0.0;
    Start.pose.orientation.x = 0.0;
    Start.pose.orientation.y = 0.0;
    Start.pose.orientation.w = 1.0;

geometry_msgs::PoseStamped Goal;
    Goal.header.seq = 0;
    Goal.header.stamp = Time(0);
    Goal.header.frame_id = "map";
    Goal.pose.position.x = x2;
    Goal.pose.position.y = y2;
    Goal.pose.position.z = 0.0;
    Goal.pose.orientation.x = 0.0;
    Goal.pose.orientation.y = 0.0;
    Goal.pose.orientation.w = 1.0;

ServiceClient check_path = nh_.serviceClient<nav_msgs::GetPlan>("make_plan");
    nav_msgs::GetPlan srv;
    srv.request.start = Start;
    srv.request.goal = Goal;
    srv.request.tolerance = 1.5;

    ROS_INFO("Make plan: %d", (check_path.call(srv) ? 1 : 0));
    ROS_INFO("Plan size: %d", srv.response.plan.poses.size());



move_base_msgs::MoveBaseGoal move_goal;
move_goal.target_pose.header.frame_id = "map";
move_goal.target_pose.header.stamp = Time(0);

move_goal.target_pose.pose.position.x = x;
move_goal.target_pose.pose.position.y = y;
move_goal.target_pose.pose.position.z = 0.0;
move_goal.target_pose.pose.orientation.x = 0.0;
move_goal.target_pose.pose.orientation.y = 0.0;
move_goal.target_pose.pose.orientation.w = 1.0;

ROS_INFO("Sending goal");
ac_.sendGoal(move_goal,boost::bind(&ExploreAction::doneCb, this, _1, _2));

 

参考：

https://answers.ros.org/question/264369/move_base-make_plan-service-is-returning-an-empty-path/

 

转载于:https://www.cnblogs.com/sea-stream/p/11129980.html