How to Create an Initial Pose and Goal Publisher in ROS

darts_dart_board_game

In this tutorial, I will show you how to use ROS and Rviz to set the initial pose (i.e. position and orientation) of a robot. Once this pose is set, we can then give the robot a series of goal locations that it can navigate to. I’ll show you how to do all of this in this post.

Real-World Applications

This project has a number of real-world applications: 

  • Indoor Delivery Robots
  • Room Service Robots
  • Mapping of Underground Mines, Caves, and Hard-to-Reach Environments
  • Robot Vacuums
  • Order Fulfillment
  • Factories

Prerequisites

Create a Package

Let’s start by creating a new package.

Open a new terminal window, and type the following command (I assume you have a folder named jetson_nano_bot inside the catkin_ws/src folder):

cd ~/catkin_ws/src/jetson_nano_bot/

Create the package.

catkin_create_pkg localization_data_pub rospy roscpp std_msgs tf tf2_ros geometry_msgs sensor_msgs nav_msgs

Type dir.

dir

Here is what you should see:

2021-05-20-10.51.54

Now open a new terminal and move to your catkin workspace.

cd ~/catkin_ws/

Compile the package.

catkin_make --only-pkg-with-deps localization_data_pub 

Move inside the package.

cd ~/catkin_ws/src/jetson_nano_bot/localization_data_pub/

Go inside the CMakeLists.txt file.

gedit CMakeLists.txt

Remove the hashtag on line 5 to make sure that C++11 support is enabled.

Save and close the file.

Background Information

One of the most common ways to set the initial pose and desired goal destination of a robot using ROS is to use Rviz. 

You can see in this graphic below from the SLAM tutorial, for example, that we have two buttons at the top of rviz: 2D Pose Estimate and 2D Nav Goal. If we click these buttons, we can automatically publish an initial pose and a goal pose on ROS topics.

2021-04-09-14.33.20

2D Pose Estimate button

  • Published Topic: initialpose
  • Type: geometry_msgs/PoseWithCovarianceStamped
  • Description: Allows the user to initialize the localization system used by the navigation stack by setting the pose of the robot in the world.

2D Nav Goal button

  • Published Topic: move_base_simple/goal
  • Type: geometry_msgs/PoseStamped
  • Description: Allows the user to send a goal to the navigation by setting a desired pose for the robot to achieve.

However, a lot of the programs we write in ROS need the initial pose and goal destination in a specific format. THe RViz buttons I mentioned above publish the pose and goal destination using the following format:

  • /initialpose (geometry_msgs/PoseWithCovarianceStamped)
  • /move_base_simple/goal (geometry_msgs/PoseStamped)

For our system to work, we need to create a program called rviz_click_to_2d.cpp that subscribes to the two topics above and converts that data into a format that other programs in a ROS-based robotic system can use. In other words, we need to create a ROS node that can publish to the following topics:

  • /initial_2d topic (which the odometry publisher subscribes to) (geometry_msgs/PoseStamped)
  • /goal_2d topic (which our path planner node, which I’ll create in a later tutorial, will subscribe to). (geometry_msgs/PoseStamped)

We will name our ROS node, rviz_click_to_2d.cpp. When this program is running, you can click the 2D Pose Estimate button and the 2D Nav Goal button in RViz, and rviz_click_to_2d.cpp will convert the data to the appropriate format to publish on the /initial_2d and /goal_2d topics.

Write the Code

Open a terminal window in your Jetson Nano.

Move to the src folder of the localization package.

cd ~/catkin_ws/src/jetson_nano_bot/localization_data_pub/src

Open a new C++ file called rviz_click_to_2d.cpp.

gedit rviz_click_to_2d.cpp

Write the following code:

/*
 * Automatic Addison
 * Website: https://automaticaddison.com
 *   ROS node that converts the user's desired initial pose and goal location
 *   into a usable format.
 * Subscribe:
 *   initialpose : The initial position and orientation of the robot using 
 *                 quaternions. (geometry_msgs/PoseWithCovarianceStamped)
 *   move_base_simple/goal : Goal position and 
 *                           orientation (geometry_msgs::PoseStamped)
 * Publish: This node publishes to the following topics:   
 *   goal_2d : Goal position and orientation (geometry_msgs::PoseStamped)
 *   initial_2d : The initial position and orientation of the robot using 
 *                Euler angles. (geometry_msgs/PoseStamped)
 * From Practical Robotics in C++ book (ISBN-10 : 9389423465)
 *   by Lloyd Brombach
 */

// Include statements 
#include "ros/ros.h"
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include <tf/transform_broadcaster.h>
#include <iostream>

using namespace std;

// Initialize ROS publishers
ros::Publisher pub;
ros::Publisher pub2;

// Take move_base_simple/goal as input and publish goal_2d
void handle_goal(const geometry_msgs::PoseStamped &goal) {
  geometry_msgs::PoseStamped rpyGoal;
  rpyGoal.header.frame_id = "map";
  rpyGoal.header.stamp = goal.header.stamp;
  rpyGoal.pose.position.x = goal.pose.position.x;
  rpyGoal.pose.position.y = goal.pose.position.y;
  rpyGoal.pose.position.z = 0;
  tf::Quaternion q(0, 0, goal.pose.orientation.z, goal.pose.orientation.w);
  tf::Matrix3x3 m(q);
  double roll, pitch, yaw;
  m.getRPY(roll, pitch, yaw);
  rpyGoal.pose.orientation.x = 0;
  rpyGoal.pose.orientation.y = 0;
  rpyGoal.pose.orientation.z = yaw;
  rpyGoal.pose.orientation.w = 0;
  pub.publish(rpyGoal);
}

// Take initialpose as input and publish initial_2d
void handle_initial_pose(const geometry_msgs::PoseWithCovarianceStamped &pose) {
  geometry_msgs::PoseStamped rpyPose;
  rpyPose.header.frame_id = "map";
  rpyPose.header.stamp = pose.header.stamp;
  rpyPose.pose.position.x = pose.pose.pose.position.x;
  rpyPose.pose.position.y = pose.pose.pose.position.y;
  rpyPose.pose.position.z = 0;
  tf::Quaternion q(0, 0, pose.pose.pose.orientation.z, pose.pose.pose.orientation.w);
  tf::Matrix3x3 m(q);
  double roll, pitch, yaw;
  m.getRPY(roll, pitch, yaw);
  rpyPose.pose.orientation.x = 0;
  rpyPose.pose.orientation.y = 0;
  rpyPose.pose.orientation.z = yaw;
  rpyPose.pose.orientation.w = 0;
  pub2.publish(rpyPose);
}

int main(int argc, char **argv) {
  ros::init(argc, argv, "rviz_click_to_2d");
  ros::NodeHandle node;
  pub = node.advertise<geometry_msgs::PoseStamped>("goal_2d", 0);
  pub2 = node.advertise<geometry_msgs::PoseStamped>("initial_2d", 0);
  ros::Subscriber sub = node.subscribe("move_base_simple/goal", 0, handle_goal);
  ros::Subscriber sub2 = node.subscribe("initialpose", 0, handle_initial_pose);
  ros::Rate loop_rate(10);
  while (ros::ok()) {
        ros::spinOnce();
        loop_rate.sleep();
  }
  return 0;
}

Save the file and close it.

Now open a new terminal window, and type the following command:

cd ~/catkin_ws/src/jetson_nano_bot/localization_data_pub/

gedit CMakeLists.txt

Go to the bottom of the file.

Add the following lines.

INCLUDE_DIRECTORIES(/usr/local/lib)
LINK_DIRECTORIES(/usr/local/lib)

add_executable(rviz_click_to_2d src/rviz_click_to_2d.cpp)
target_link_libraries(rviz_click_to_2d ${catkin_LIBRARIES})

Save the file, and close it.

Go to the root of the workspace.

cd ~/catkin_ws

Compile the code.

catkin_make --only-pkg-with-deps localization_data_pub

To run the code, you would type the following commands:

roscore

In another terminal window, type:

rosrun localization_data_pub rviz_click_to_2d

Then open another terminal, and launch RViz.

rviz
rviz

In another terminal type:

rostopic echo /initial_2d

In another terminal type:

rostopic echo /goal_2d

Then on Rviz, you can click the 2D Pose Estimate button to set the pose. You click on the button and then click on somewhere in the environment to set the pose.

Then click the 2D Nav Goal button to set the goal destination. The output will print out to the terminal windows.

Here is what you should see in the terminal windows:

initial-pose
goal-2d
setting_pose

Add to a Launch File

Here is what you can add to your launch file. The first piece of code will launch Rviz, and the second piece of code will start our node.

  <!-- Initial Pose and Goal Publisher -->
  <!-- Publish: /initialpose, /move_base_simple/goal -->
  <node pkg="rviz" type="rviz" name="rviz">
  </node>
  <!-- Subscribe: /initialpose, /move_base_simple/goal -->
  <!-- Publish: /initial_2d, /goal_2d --> 
  <node pkg="localization_data_pub" type="rviz_click_to_2d" name="rviz_click_to_2d">
  </node>

That’s it! Keep building!