ROS Mini Challenge #6 – detect the position of a robot using ROS Service

Written by Ruben Alves

22/02/2020

 

 

 

 

 

 

 

 

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

 

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

Once you open the ROsject, in the ~/catkin_ws folder, you will find all the ROS packages associated with this challenge. You can have a look at it using the IDE, for instance.

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

Code Editor (IDE) - ROSDS

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

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

As you can see, this ROSject contains 1 package inside its catkin_ws workspace: rotw6_pkg. This package contains a couple of Python scripts (get_pose_service.py and get_pose_client.py), which contains a Service Server and a Client, respectively.

So, the main purpose of this Service is to be able to get the Pose (position and orientation) of the Husky robot when called. In order to get the Pose of the robot, all you have to do is the following:

First, make sure you source your workspace so that ROS can find your package:
source ~/catkin_ws/devel/setup.bash

 

Now, start your Service Server with the following command:
rosrun rotw6_pkg get_pose_service.py

 

Finally, just launch your Service Client with the following command:

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.

 

Topics:
Masterclass 2023 batch2 blog banner

Check Out These Related Posts

129. ros2ai

129. ros2ai

I would like to dedicate this episode to all the ROS Developers who believe that ChatGPT or...

read more

0 Comments

Submit a Comment

Your email address will not be published.

This site uses Akismet to reduce spam. Learn how your comment data is processed.

Pin It on Pinterest

Share This