ros中对move_base的调用

move_base包中自带costmap2d, global planner 等功能

也可以直接调用其中make_plan进行路径规划

#include "geometry_msgs/PoseStamped.h"
#includde "nav_msgs/GetPlan.h"
 
 
 
void fillPathRequest(nav_msgs::GetPlan::Request &request, float start_x, float start_y, float goal_x, float goal_y)
{
    request.start.header.frame_id ="map";
    request.start.pose.position.x = start_x;//初始位置x坐标
    request.start.pose.position.y = start_y;//初始位置y坐标
    request.start.pose.orientation.w = 1.0;//方向
    request.goal.header.frame_id = "map";
    request.goal.pose.position.x = goal_x;//终点坐标
    request.goal.pose.position.y = goal_y;
    request.goal.pose.orientation.w = 1.0;
    request.tolerance = 0.0;//如果不能到达目标，最近可到的约束
}
 
void myfunction(geometry_msgs::PoseStamped p)
{
    std::cout << "X = " << p.pose.position.x << "   Y = " << p.pose.position.y << std::endl;
}
 
//路线规划结果回调
void callPlanningService(ros::ServiceClient &serviceClient, nav_msgs::GetPlan &srv)
{
    // Perform the actual path planner call
    //执行实际路径规划器
    if (serviceClient.call(srv)) {
        //srv.response.plan.poses 为保存结果的容器，遍历取出
        if (!srv.response.plan.poses.empty()) {
//            std::for_each(srv.response.plan.poses.begin(),srv.response.plan.poses.end(),myfunction);
            ROS_INFO("make_plan success!");
        }
    }
    else {
        ROS_WARN("Got empty plan");
    }
}
 
int main(int argc, char **argv)
{
    ros::init(argc, argv, "system_manager_node");
    ros::NodeHandle nh("~");
    ros::NodeHandle nhandle;
 
    ros::Publisher markers_pub_ = nhandle.advertise("visualization_marker", 20);
 
    ros::ServiceClient make_plan_client = nhandle.serviceClient("move_base/NavfnROS/make_plan",true);
    if (!make_plan_client) {
        ROS_FATAL("Could not initialize get plan service from %s",
        make_plan_client.getService().c_str());
        return -1;
    }
 
    nav_msgs::GetPlan srv;
    fillPathRequest(srv.request,-7,-3.0,11.338,-5.8852);
    callPlanningService(make_plan_client,srv);
 
    visualization_msgs::Marker mk1;
    mk1.header.stamp = ros::Time::now();
    mk1.header.frame_id = "/map";
    mk1.id = 0;
    mk1.type = visualization_msgs::Marker::LINE_STRIP;
    mk1.scale.x = 0.05; mk1.scale.y = 0.05; mk1.scale.z = 0.05;
    mk1.color.r = 1.0; mk1.color.a = 1.0;
    for(int i = 100; i < srv.response.plan.poses.size(); i++)
    {
        mk1.points.push_back(srv.response.plan.poses[i].pose.position);
    }
    markers_pub_.publish(mk1);
}

注意其中的关键，topic应该是move_base/NavfnROS/make_plan否则会一直获取不到路径