Robot Path Planning: A Guide to the ROS GetPlan Service in C++ and Python
Sometimes we want the ROS navigation stack to plan a path to a goal without moving the robot to it. In this short article, I’ll show you how to use the nav_msgs/GetPlan service to do exactly that in both C++ and Python.
What is the GetPlan Service?
The GetPlan service is a part of the Navigation stack in ROS, and it enables a robot to obtain a path between two points in a given map. The Navigation stack is a popular tool in mobile robotics, providing capabilities such as localization, mapping, and path planning. The service will use the same global costmap and global path planner as if it were being asked to move a robot, but you will specify a start location instead of the robot’s current location being presumed as the starting point.
Why is the GetPlan Service Useful?
The GetPlan service is useful in a variety of mobile robotics applications. Here are a few reasons why:
Decision making: Which robot can get to a given goal the fastest?
Optimizing travel: If a robot must visit a number of places, in which order should they be visited?
Integration with other ROS packages: By using the GetPlan service, we can integrate our own robot control code with these tools and benefit from their functionality.
Using the GetPlan Service in C++
To use the GetPlan service in C++, we need to create a ServiceClient and a service message of the type nav_msgs::GetPlan. The GetPlan message has a request component that includes the start, goal, and tolerance. When we call the service, it will return a response that includes the plan if possible.
#include <ros/ros.h>
#include <nav_msgs/GetPlan.h>
int main(int argc, char **argv)
{
// Initialize ROS node
ros::init(argc, argv, "make_plan_client");
ros::NodeHandle nh;
// Create service client
ros::ServiceClient client = nh.serviceClient<nav_msgs::GetPlan>("/move_base/make_plan");
// Create request message
nav_msgs::GetPlan srv;
srv.request.start.header.frame_id = "map";
srv.request.start.pose.position.x = 0.0;
srv.request.start.pose.position.y = 0.0;
srv.request.start.pose.orientation.w = 1.0;
srv.request.goal.header.frame_id = "map";
srv.request.goal.pose.position.x = 1.0;
srv.request.goal.pose.position.y = 1.0;
srv.request.goal.pose.orientation.w = 1.0;
srv.request.tolerance = 0.1;
// Call service
if (client.call(srv))
{
// Print response message
ROS_INFO_STREAM("Got plan with " << srv.response.plan.poses.size() << " waypoints.");
// Print the pose and orientation of the first waypoint
if (!srv.response.plan.poses.empty())
{
geometry_msgs::PoseStamped first_waypoint = srv.response.plan.poses.front();
ROS_INFO_STREAM("First waypoint pose: " << first_waypoint.pose.position.x
<< ", " << first_waypoint.pose.position.y
<< ", " << first_waypoint.pose.position.z);
ROS_INFO_STREAM("First waypoint orientation: " << first_waypoint.pose.orientation.w
<< ", " << first_waypoint.pose.orientation.x
<< ", " << first_waypoint.pose.orientation.y
<< ", " << first_waypoint.pose.orientation.z);
}
}
else
{
ROS_ERROR("Failed to call service /move_base/make_plan");
return 1;
}
return 0;
} Here we create a ROS node and a service client for the GetPlan service. We then create a GetPlan service request and set the start and goal positions of the robot in the map frame. Finally, we call the service and wait for the response.
Using the GetPlan Service in Python
Using the GetPlan service in Python is more or less the same but uses a ServiceProxy. Instead of constructing a service message object and adding start, goal, and tolerance to it, we just pass those to the proxy. Here's the Python code:
#!/usr/bin/env python
import rospy
from nav_msgs.srv import GetPlan
from geometry_msgs.msg import PoseStamped
if __name__ == '__main__':
# Initialize ROS node
rospy.init_node('make_plan_client')
# Create service client
client = rospy.ServiceProxy('/move_base/make_plan', GetPlan)
# Create request message
start = PoseStamped()
start.header.frame_id = "map"
start.pose.position.x = 0.0
start.pose.position.y = 0.0
start.pose.orientation.w = 1.0
goal = PoseStamped()
goal.header.frame_id = "map"
goal.pose.position.x = 1.0
goal.pose.position.y = 0.0
goal.pose.orientation.w = 1.0
tolerance = 0.1
# Call service
try:
response = client(start, goal, tolerance)
# Print response message
rospy.loginfo("Got plan with %d waypoints.", len(response.plan.poses))
# Print the pose and orientation of the first waypoint
if response.plan.poses:
first_waypoint = response.plan.poses[0]
rospy.loginfo("First waypoint pose: %f, %f, %f",
first_waypoint.pose.position.x,
first_waypoint.pose.position.y,
first_waypoint.pose.position.z)
rospy.loginfo("First waypoint orientation: %f, %f, %f, %f",
first_waypoint.pose.orientation.w,
first_waypoint.pose.orientation.x,
first_waypoint.pose.orientation.y,
first_waypoint.pose.orientation.z)
except rospy.ServiceException as e:
rospy.logerr("Failed to call service /move_base/make_plan: %s", e)That’s all there is to it. Don’t forget that orientations are in quaternions, so you will need to use the tf2 package to convert back and forth from Euler angles if they matter to you. If they don’t matter, you can just set the orientation.w values to 1 as I have in these examples. If you’re not sure how to convert back and forth from quaternions, I cover that in my book Practical Robotics in C++.
(Yes, this is an affiliate link and an Amazon Associate I earn from qualifying purchases. Clicking can help support my ability to provide tutorials even if you do not purchase)
Comments