Robot Path Planning: A Guide to the ROS GetPlan Service in C++ and Python

A* (Pronounced "A star") is just one of the path planning algorithms available in the ROS navigation stack

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:

1. Decision making: Which robot can get to a given goal the fastest?

2. Optimizing travel: If a robot must visit a number of places, in which order should they be visited?

3. 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)