Odometry reading not considered during unit testing

In the program below from ROS Unit Testing, chapter 4 ROS-Node Level Test, the goal was to check if the robot has reached a particular position based on user input.

Question 1:
I noticed that we are only using the concept of Angular_distance=Angular velocity * Time taken to get the robot to the desired position. Since the goal of this exercise is to check if the robot is performing as intended, shouldn’t we also check for data from gazebo on robot odometry to give us a more real time data?

I got correct results when I ran the code as it is, but when i modified the code to also include Odometry data and found that its always falling behind or ahead by some radiands. Below is the result for 15 degree and 25 degree:

Since this is not the intended result, why are we still getting Correct result during testing?

The main code :

#!/usr/bin/env python

import rospy
from geometry_msgs.msg import Twist
from robot_control.srv import RotateRobot, RotateRobotResponse
from nav_msgs.msg import Odometry
import time
from tf.transformations import euler_from_quaternion, quaternion_from_euler
from math import degrees, radians

class RobotControl():

    def __init__(self):
        rospy.init_node('robot_control_node', anonymous=True)
        self.vel_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
        self.service_server = rospy.Service(
            'rotate_robot', RotateRobot, self.srv_callback)
        self.cmd = Twist()
        self.ctrl_c = False
        self.rate = rospy.Rate(10)

    def callback(self,data):
        # print("Quaternion val=",self.quaternion_val)
        # self.euler_val=euler_from_quaternion(self.quaternion_val)
        # print("Euler val=",self.euler_val)

    def srv_callback(self, request):
        # Initilize velocities
        self.cmd.linear.x = 0
        self.cmd.linear.y = 0
        self.cmd.linear.z = 0
        self.cmd.angular.x = 0
        self.cmd.angular.y = 0

        self.convert_degree_to_rad(request.speed_d, request.angle_d)
        # self.angular_speed_r and self.angle_r are returned or updated
        if request.clockwise_yn == "y":
            self.clockwise = True
        if request.clockwise_yn == "n":
            self.clockwise = False

        if self.clockwise:
            print "Clockwise"
            self.cmd.angular.z = -abs(self.angular_speed_r)
            print "Not clockwise"
            self.cmd.angular.z = abs(self.angular_speed_r)

        #finding the angle moved:
        print("initial angle=",initial_angle)

        # t0 is the current time
        t0 = rospy.Time.now().secs

        current_angle = 0
        # Publish the velocity
        # self.vel_publisher.publish(self.cmd)

        # loop to publish the velocity estimate, current_distance = velocity * (t1 - t0)
        while (current_angle < self.angle_r):

            # Publish the velocity
            # t1 is the current time
            t1 = rospy.Time.now().secs
            # Calculate current_distance
            current_angle = self.angular_speed_r * (t1 - t0)
            # ros::spinOnce();

        # set velocity to zero to stop the robot
        #find the angle moved from Gazebo data
        print("final angle=",final_angle)
        print("Angle Rotated=",angle_rotated)
        response = RotateRobotResponse()
        response.rotation_successfull = True
        return response

    def publish_once_in_cmd_vel(self):
        This is because publishing in topics sometimes fails the first time you publish.
        In continuos publishing systems there is no big deal but in systems that publish only
        once it IS very important.
        while not self.ctrl_c:
            connections = self.vel_publisher.get_num_connections()
            if connections > 0:
                #rospy.loginfo("Cmd Published")

    def shutdownhook(self):
        # works better than the rospy.is_shutdown()
        self.ctrl_c = True

    def stop_robot(self):
        #rospy.loginfo("shutdown time! Stop the robot")
        self.cmd.linear.x = 0.0
        self.cmd.angular.z = 0.0

    def convert_degree_to_rad(self, speed_deg, angle_deg):

        self.angular_speed_r = speed_deg * 3.14 / 180
        self.angle_r = angle_deg * 3.14 / 180
        return [self.angular_speed_r, self.angle_r]

if __name__ == '__main__':
    #rospy.init_node('robot_control_node', anonymous=True)
    robotcontrol_object = RobotControl()

Question 2:
Below is the code for testing the service :

#! /usr/bin/env python

from robot_control.rotate_robot_srv import RobotControl
from robot_control.srv import RotateRobot, RotateRobotRequest
import rospy
import rosunit
import unittest
import rostest
PKG = 'robot_control'
NAME = 'rotate_robot_test_ros_srv'

class TestRobotControl(unittest.TestCase):

    def test_rotate_robot_service(self):

        s = rospy.ServiceProxy('rotate_robot', RotateRobot)
        tests = [(60, 90, 'y')]
        for x, y, z in tests:
            print("Requesting %s+%s+%s" % (x, y, z))
            # test both simple and formal call syntax
            resp = s(x, y, z)
            resp2 = s.call(RotateRobotRequest(x, y, z))
            self.assertTrue(resp.rotation_successfull, "integration failure, service response was not True")

if __name__ == '__main__':
    rostest.rosrun(PKG, NAME, TestRobotControl)

Why are we calling both simple and formal service calls, does one of them give more error free results?

Hi @Joseph1001 ,

First Question

For the first question, the reason why the test succeeds is that it is only checking whether the response.rotation_successfull is True, and as you can see, the RobotControl class assigns that value to True after the robot has moved for a while. Even if the robot had not moved at all, if RobotControl sets the value to True, the test would succeed.

For the second question:

Why are we calling both simple and formal service calls, does one of them give more error free results?

They both provide the same results. They are just different ways of doing the same thing.

We compare the output of both just to confirm that they do exactly the same thing.

If we check the ServiceProxy.__call__ method, for example, you can confirm __call__ in the end calls the call() method:

    def __call__(self, *args, **kwds):
        Callable-style version of the service API. This accepts either a request message instance,
        or you can call directly with arguments to create a new request instance. e.g.::
          add_two_ints(AddTwoIntsRequest(1, 2))
          add_two_ints(1, 2)
          add_two_ints(a=1, b=2)          
        @param args: arguments to remote service
        @param kwds: message keyword arguments
        @raise ROSSerializationException: If unable to serialize
        message. This is usually a type error with one of the fields.
        return self.call(*args, **kwds)

If you just create a file named testing.py with the following content:

class CustomService:
    def __call__(self):

    def call(self):
        print(' I was called by: %s' % self.call.__name__)

if __name__ == '__main__':

    myService = CustomService()

    print('First test:')

    print('\nSecond test:')

and call it with “python service.py”, you will see the exact same output.

First test:
 I was called by: call

Second test:
 I was called by: call
1 Like

Thank you @ralves for helping out

1 Like