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)
rospy.on_shutdown(self.shutdownhook)
self.odom=Odometry()
self.sub=rospy.Subscriber("/odom",Odometry,self.callback)
def callback(self,data):
self.quaternion_val=data.pose.pose.orientation
# print("Quaternion val=",self.quaternion_val)
self.euler_val=euler_from_quaternion([self.quaternion_val.x,self.quaternion_val.y,self.quaternion_val.z,self.quaternion_val.w])
# 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)
else:
print "Not clockwise"
self.cmd.angular.z = abs(self.angular_speed_r)
#finding the angle moved:
initial_angle=degrees(self.euler_val[2])
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
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()
#find the angle moved from Gazebo data
final_angle=degrees(self.euler_val[2])
print("final angle=",final_angle)
angle_rotated=final_angle-initial_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:
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]
if __name__ == '__main__':
#rospy.init_node('robot_control_node', anonymous=True)
robotcontrol_object = RobotControl()
rospy.spin()
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):
rospy.wait_for_service('rotate_robot')
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.assertEquals(resp.rotation_successfull,resp2.rotation_successfull)
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?