Solution for Exercise 3.5, 3.6 and 3.7 ROS Node Level Test

Course Support ROS Unit Testing
1

As the solutions are not available on the topic, i wanted to share mine for other students.
Feel free to comment or correct

  • Create a test suite for all my ROS service tests
  • Add test cases like:
    * Testing if stop_robot() actually stops the robot
    * What happens when invalid input is passed to the /rotate_robot service
  • Modify the core service code so all tests pass

Files Included

  • rotate_robot_complete.py – the full service + class
  • rotate_robot_test_final.py – the test suite
  • rotate_robot_test.launch – test runner for rostest

What’s Tested

:white_check_mark: Service: /rotate_robot

  • Inputs: angular speed (°/s), angle (°), and clockwise (y/n)
  • Invalid direction input like 'f' or 'x' returns rotation_successfull = False

:white_check_mark: stop_robot()

  • Unit test ensures calling stop_robot() results in linear.x = 0.0 and angular.z = 0.0

:white_check_mark: Publish check

  • Confirms velocity commands are published successfully to /cmd_vel
<launch>
    <node pkg="robot_control" type="rotate_robot_complete.py" name='rotate_robot_srv_node'/>
    <test test-name="rotate_robot_test_ros_srv" pkg="robot_control" type="rotate_robot_test_final.py"/>
</launch>

#! /usr/bin/env python

from robot_control.rotate_robot_complete import RobotControl
from robot_control.srv import RotateRobot, RotateRobotRequest
import rospy
import rosunit
import unittest
import rostest
from geometry_msgs.msg import Twist
import time

PKG = 'robot_control'
NAME = 'rotate_robot_test_ros_srv'


class TestRobotControl(unittest.TestCase):

    # In Python's unittest, the setUp() method is run once per test method.  
    # Use @classmethod with setUpClass() if you want to only instantiate RobotControl() once for the entire test class  

    @classmethod
    def setUpClass(cls):
        cls.rc = RobotControl()
        cls.success = False
        rospy.loginfo("SetUpClass Done")

    # def setUp(self):
    #     self.rc = RobotControl(start_service = False)
    #     rospy.loginfo("SetUp Done")
    #     self.success = False

    def callback(self, msg):
        print(rospy.get_caller_id(), "Angular Speed: %s" % msg.angular.z)
        self.success = msg.angular.z and msg.angular.z == 1

    def test_publish_cmd_vel(self):
        rospy.loginfo("\nRunning Test Publish cmd ")

        test_sub = rospy.Subscriber("/cmd_vel", Twist, self.callback)
        self.rc.cmd.angular.z = 1
        self.rc.publish_once_in_cmd_vel()
        timeout_t = time.time() + 10.0 # 10 seconds
        while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
            time.sleep(0.1)
        self.assert_(self.success)

    def test_rotate_robot_service(self):
        rospy.loginfo("\nRunning Test Rotate Service ")

        rospy.wait_for_service('rotate_robot')
        s = rospy.ServiceProxy('rotate_robot', RotateRobot)
        # test = [(60,90,'y'), (43,'d','y')]
        test = [
                (60, 90, 'y', True),   # valid
                (43, 43, 'f', False),  # invalid
                (30, 30, 'n', True),   # valid
                (10, 10, 'x', False),  # invalid
            ]
        for x,y,z,expected_success in test:
            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.assertEquals(resp.rotation_successfull, resp2.rotation_successfull)
            # self.assertTrue(resp.rotation_successfull, "integration failure, service responce was not True")
            self.assertEqual(resp.rotation_successfull, expected_success, "Failed for input {}".format(z))

    def test_stop_robot(self):
        # Set some dummy motion values before calling stop
        rospy.loginfo("\nRunning Test STOP")
        self.rc.cmd.linear.x = 1.5
        self.rc.cmd.angular.z = 2.5


        self.rc.stop_robot()
        rospy.loginfo("Robot Stopped, Vel angular.z = %f", self.rc.cmd.angular.z)

        self.assertEqual(self.rc.cmd.linear.x, 0.0, "Expected linear.x to be 0.0 after stop_robot")
        self.assertEqual(self.rc.cmd.angular.z, 0.0, "Expected angular.z to be 0.0 after stop_robot")




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



'''

Description

This Python script contains unit and integration tests for the RobotControl class using unittest and rostest. 
It verifies that the robot correctly publishes to /cmd_vel and that the /rotate_robot service works as expected.

## Test Cases

1. **test_publish_cmd_vel**
    Publishes an angular velocity (angular.z = 1) to /cmd_vel.
    Checks if the message is received successfully.

2. **test_rotate_robot_service**
    Calls the /rotate_robot service with test values (speed, angle, direction).
    Verifies that the service responds with rotation_successfull == True.
    Compares both simple and formal service call formats.

## Usage

**Step 1:** Run the service node in a separate terminal:

bash
rosrun robot_control rotate_robot_complete.py
'''

#!/usr/bin/env python

import rospy
from geometry_msgs.msg import Twist
from robot_control.srv import RotateRobot, RotateRobotResponse
import time
from sensor_msgs.msg import LaserScan
import math


class RobotControl():

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

    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
        response = RotateRobotResponse()

        self.convert_degree_to_rad(request.speed_d, request.angle_d)

        if request.clockwise_yn == "y":
            self.clockwise = True
        elif request.clockwise_yn == "n":
            self.clockwise = False
        else:
            response.rotation_successfull = False
            return response
                
        if self.clockwise:
            print ("Clockwise")
            self.cmd.angular.z = -abs(self.angular_speed_r)
        else:
            print ("Not clockwise")
            self.cmd.angular.z = abs(self.angular_speed_r)

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

        current_angle = 0

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

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

        # set velocity to zero to stop the robot
        self.stop_robot()
        
        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:
                self.vel_publisher.publish(self.cmd)
                #rospy.loginfo("Cmd Published")
                break
            else:
                self.rate.sleep()

    def shutdownhook(self):
        # works better than the rospy.is_shutdown()
        self.stop_robot()
        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
        self.publish_once_in_cmd_vel()

    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]

    def rotate(self):

        # 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

        speed_d, angle_d = self.get_inputs_rotate()
        self.convert_degree_to_rad(speed_d, angle_d)

        print (self.clockwise)

        if self.clockwise:
            self.cmd.angular.z = -abs(self.angular_speed_r)
        else:
            self.cmd.angular.z = abs(self.angular_speed_r)

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

        current_angle = 0

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

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

        # set velocity to zero to stop the robot
        self.stop_robot()

    def get_inputs_rotate(self):
        self.angular_speed_d = int( raw_input('Enter desired angular speed (degrees): '))
        self.angle_d = int(raw_input('Enter desired angle (degrees): '))
        clockwise_yn = raw_input('Do you want to rotate clockwise? (y/n): ')
        if clockwise_yn == "y":
            self.clockwise = True
        if clockwise_yn == "n":
            self.clockwise = False

        return [self.angular_speed_d, self.angle_d]

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


'''
## Description

This ROS node implements the RobotControl class, which allows a mobile robot to rotate by publishing angular velocity commands. 
It also provides a service called /rotate_robot that enables the robot to rotate by a specified angle, speed, and direction (clockwise or counterclockwise).

## Features

- Publishes velocity commands to the /cmd_vel topic using Twist messages.
- Provides a service /rotate_robot (type RotateRobot.srv) that:
    - Takes input parameters: angular speed (degrees/sec), angle (degrees), and direction (y or n for clockwise).
    - Converts degrees to radians and sends angular commands until the target rotation is reached.
- Includes helper methods for:
    - Publishing once to /cmd_vel safely.
    - Stopping the robot on shutdown.
    - Manual command-line rotation input.

## Usage

Run the node with:

bash
rosrun robot_control rotate_robot_complete.py
'''