ROS 导航 :make_plan (路线规划)

最新推荐文章于 2025-01-13 08:04:28 发布

转载最新推荐文章于 2025-01-13 08:04:28 发布 · 2.2k 阅读

ROS 专栏收录该内容

75 篇文章

订阅专栏

本文深入讲解了在ROS中实现路径规划的方法，详细介绍了如何使用make_plan服务进行两点间最优路径的规划，包括代码示例和服务调用流程。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

以下是在学习ROS 导航时，按照自己的理解整理的资料，有不对的地方请指出。
路径规划：从一个点到另一个点，规划出最优的路线。用到service :make_plan (nav_msgs/GetPlan)
服务名为move_base_node/make_plan
nav_msgs/GetPlan api：

# Get a plan from the current position to the goal Pose

# The start pose for the plan
geometry_msgs/PoseStamped start

# The final pose of the goal position
geometry_msgs/PoseStamped goal

# If the goal is obstructed, how many meters the planner can
# relax the constraint in x and y before failing.
float32 tolerance
---
nav_msgs/Path plan

Compact Message Definition
geometry_msgs/PoseStamped start
geometry_msgs/PoseStamped goal
float32 tolerance
nav_msgs/Path plan

现在学习如何使用
在工作空间新建package navigation_example

cd ~/catkin_ws/src
catkin_create_pkg navigation_example std_msgs rospy roscpp tf actionlib

1
2

CMakeList.txt 中的find_package如下

find_package(catkin REQUIRED COMPONENTS
actionlib
roscpp
rospy
std_msgs
tf
)

在src目录下新建make_plan.cpp

/*
* make_plan.cpp
*
* Created on: Aug 10, 2016
* Author: unicorn
*/
//路线规划代码
#include <ros/ros.h>
#include <nav_msgs/GetPlan.h>
#include <geometry_msgs/PoseStamped.h>
#include <string>
#include <boost/foreach.hpp>
#define forEach BOOST_FOREACH
void fillPathRequest(nav_msgs::GetPlan::Request &request)
{
request.start.header.frame_id ="map";
request.start.pose.position.x = 12.378;//初始位置x坐标
request.start.pose.position.y = 28.638;//初始位置y坐标
request.start.pose.orientation.w = 1.0;//方向
request.goal.header.frame_id = "map";
request.goal.pose.position.x = 18.792;//终点坐标
request.goal.pose.position.y = 29.544;
request.goal.pose.orientation.w = 1.0;
request.tolerance = 0.5;//如果不能到达目标，最近可到的约束
}
//路线规划结果回调
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()) {
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_WARN("Got empty plan");
}
}
else {
ROS_ERROR("Failed to call service %s - is the robot moving?",
serviceClient.getService().c_str());
}
}

int main(int argc, char** argv)
{
ros::init(argc, argv, "make_plan_node");
ros::NodeHandle nh;
// Init service query for make plan
//初始化路径规划服务，服务名称为"move_base_node/make_plan"
std::string service_name = "move_base_node/make_plan";
//等待服务空闲，如果已经在运行这个服务，会等到运行结束。
while (!ros::service::waitForService(service_name, ros::Duration(3.0))) {
ROS_INFO("Waiting for service move_base/make_plan to become available");
}
/*初始化客户端，(nav_msgs/GetPlan)
Allows an external user to ask for a plan to a given pose from move_base without causing move_base to execute that plan.
允许用户从move_base 请求一个plan，并不会导致move_base 执行此plan
*/
ros::ServiceClient serviceClient = nh.serviceClient<nav_msgs::GetPlan>(service_name, true);
if (!serviceClient) {
ROS_FATAL("Could not initialize get plan service from %s",
serviceClient.getService().c_str());
return -1;
}
nav_msgs::GetPlan srv;
//请求服务：规划路线
fillPathRequest(srv.request);
if (!serviceClient) {
ROS_FATAL("Persistent service connection to %s failed",
serviceClient.getService().c_str());
return -1;
}
ROS_INFO("conntect to %s",serviceClient.getService().c_str());
callPlanningService(serviceClient, srv);
}