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_msgs::Marker>("visualization_marker", 20);ros::ServiceClient make_plan_client = nhandle.serviceClient<nav_msgs::GetPlan>("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否则会一直获取不到路径