What we are going to learn

Learn how to get the position and orientation of a robot, when someone asks through ROS Services.

List of resources used in this post

•  The ROSject of the project with the simulation and the ROS Packages: www.rosject.io/l/ee2768a/
•  Robot Ignite Academy courses: www.robotigniteacademy.com/en/ 
• The post in ROS Discourse where the challenge was posted: https://discourse.ros.org/t/rosject-of-the-week-challenge-6/11918

Opening the ROSject

Once you clicked in the ROSject link provided (https://buff.ly/2Pd5msM) you will get a copy of the ROSject. You can then click Open to have it open.

Where to find the code

To see the full code, open the IDE by going to the top menu and select Tools->IDE

Code Editor (IDE) – ROSDS

Launching the Simulation

Go to the top menu and select Simulations. On the menu that appears, click on the Choose launch file… button.

Choose lunch file to open simulation in ROSDS

Now, from the launch files list, select the launch file named rotw5.launch from the rosbot_gazebo package.

ROS Mini Challenged #6 – launch file

Finally, click on the Launch button to start the simulation. In the end, you should get something like this:

Husky Simulation in ROSDS

The problem to solve

source ~/catkin_ws/devel/setup.bash

rosrun rotw6_pkg get_pose_service.py

rosrun rotw6_pkg get_pose_client.py

Now, in the Shell where you started the Service Server, you should see the following:

Get robot pose service in ROSDS

NOTE: Here, it’s VERY IMPORTANT to note that it’s only retrieving the position and orientation of the robot, without any additional data.

Solving the ROS Mini Challenge

Ok, so… where’s the problem? If you have tried to reproduce the steps described above you have already seen that it DOES NOT WORK. When you run the Service Client, you are getting some errors that aren’t supposed to be there. So… what’s going on?

When we launch the service client, we get something like the error below:

user:~$ rosrun rotw6_pkg get_pose_client.py
Traceback (most recent call last):
  File "/home/user/catkin_ws/src/rotw6_pkg/src/get_pose_client.py", line 11, in <module>
    result = get_pose_client(get_pose_request_object)
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 435, in __call__
    return self.call(*args, **kwds)
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 495, in call
    service_uri = self._get_service_uri(request)
  File "/opt/ros/kinetic/lib/python2.7/dist-packages/rospy/impl/tcpros_service.py", line 444, in _get_service_uri
    raise TypeError("request object is not a valid request message instance")
TypeError: request object is not a valid request message instanc

The original content of the get_pose_client.py in the rotw6_pkg package is:

#! /usr/bin/env python

import rospy
from std_srvs.srv import Empty

rospy.init_node('rotw6_client')
rospy.wait_for_service('/get_pose_service')
get_pose_client = rospy.ServiceProxy('/get_pose_service', Empty)
get_pose_request_object = Empty()

result = get_pose_client(get_pose_request_object)

The problem is on the line get_pose_request_object = Empty(). We need to use EmptyRequest instead. The correct client, then, would be:

#! /usr/bin/env python

import rospy
from std_srvs.srv import Empty, EmptyRequest


rospy.init_node('rotw6_client')
rospy.wait_for_service('/get_pose_service')
get_pose_client = rospy.ServiceProxy('/get_pose_service', Empty)
get_pose_request_object = EmptyRequest()

result = get_pose_client(get_pose_request_object)

If we now run the client again, we should see no errors:

rosrun rotw6_pkg get_pose_client.py

and in the output of the server, we have:

$ rosrun rotw6_pkg get_pose_service.py
Robot Pose:
header:  seq: 86596  stamp:    secs: 3032    nsecs: 830000000
  frame_id: "odom"
child_frame_id: "base_link"
pose:
  pose:
    position:
      x: -0.0712665171853
      y: -0.00030466544589
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0      z: -0.493612905474      w: 0.869681723132  covariance: [86.67158253691281, -6.76509699953108e-05, 0.0, 0.0, 0.0, -0.010918187001924307, -6.765096999530603e-05, 86.68358224257844, 0.0, 0.0, 0.0, -0.18378969144765284, 0.0, 0.0, 4.997917968019755e-07
, -4.557105661285207e-23, 3.4006027312434455e-20, 0.0, 0.0, 0.0, -4.5571056612852066e-23, 4.99583853513
7798e-07, 2.1597386386459286e-32, 0.0, 0.0, 0.0, 3.4006027312434443e-20, -6.711057929105504e-33, 4.995838535137798e-07, 0.0, -0.010918187001923645, -0.18378969144767304, 0.0, 0.0, 0.0, 103.95483332789887]
twist:
  twist:
    linear:
      x: -1.972482338e-05
      y: 0.0
      z: 0.0
    angular:
      x: 0.0
      y: 0.0
      z: 0.0292241612329
  covariance: [0.0008855670954413368, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0008855670954413368, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 4.996877927028334e-07, -7.07772753671664e-33, 5.268413186594352e-30, 0.0, 0.0, 0.0, -7.077727536716642e-33, 4.987546680559569e-07, 1.3419297548549267e-41, 0.0, 0.0, 0.0, 5.268413186594355e-30, -4.178170793934322e-42, 4.987546680559569e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 9.592381167731944e-07]

as you can see, we have too much data in the output. We just need to show the position and orientation. The reason why we have so much data is that in the server code (get_pose_service.py) we print the message directly. The original server code is:

#! /usr/bin/env python

import rospy
from std_srvs.srv import Empty, EmptyResponse
from geometry_msgs.msg import PoseWithCovarianceStamped, Pose
from nav_msgs.msg import Odometry

robot_pose = Pose()

def service_callback(request):
    print "Robot Pose:"
    print robot_pose
    return EmptyResponse()

def sub_callback(msg):
    global robot_pose
    robot_pose = msg


rospy.init_node('rotw6_service')
my_service = rospy.Service('/get_pose_service', Empty , service_callback)
sub_pose = rospy.Subscriber('/odometry/filtered', Odometry, sub_callback)
rospy.spin()

The message type is Odometry. With rosmsg show Odometry  we can see this message type has a lot of fields:

$ rosmsg show Odometry
[nav_msgs/Odometry]:
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
string child_frame_id
geometry_msgs/PoseWithCovariance pose
  geometry_msgs/Pose pose
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w
  float64[36] covariance
geometry_msgs/TwistWithCovariance twist
  geometry_msgs/Twist twist
    geometry_msgs/Vector3 linear
      float64 x
      float64 y
      float64 z
    geometry_msgs/Vector3 angular
      float64 x
      float64 y
      float64 z
  float64[36] covariance

In the message structure above, we are only interested in the position and orientation. To achieve that, in the server we can just change the sub_callback function from:

def sub_callback(msg):
    global robot_pose
    robot_pose = msg

to:

def sub_callback(msg):
    global robot_pose
    robot_pose = msg.pose.pose

as easy as that. Pretty easy, right?

If for any reason you are still struggling to understand or memorize the steps reproduced here, I highly recommend you taking our ROS courses at www.robotigniteacademy.com/en/ .

Youtube video

So this is the post for today. Remember that we have the live version of this post on YouTube. If you liked the content, please consider subscribing to our youtube channel. We are publishing new content ~every day.

Keep pushing your ROS Learning.