def setUp(self):
self.motors = self._TEST_MOTORS
self.safety = Safety()
# Message was passed
self.proxy_pass = False
# Wrong value
self.safe_val = 100
# In case there are issues
self.ignore_msgs = False
# Create publishers and subscribers for testing
self.pub = {}
self.sub = {}
self.motor_states_pub = rospy.Publisher("safe/motor_states", MotorStateList, queue_size=10)
for m in self.motors:
if motor_type(m) == 'pololu':
topic = '%s/command' % m['topic']
self.pub[m['topic']] = rospy.Publisher(topic, MotorCommand, queue_size=30)
safe_topic = 'safe/%s/command' % m['topic']
self.sub[m['topic']] = rospy.Subscriber(safe_topic, MotorCommand, partial(self.check_safe_msgs, motor=m))
else:
topic = '%s_controller/command' % m['topic']
self.pub[m['topic']] = rospy.Publisher(topic, Float64, queue_size=30)
safe_topic = 'safe/%s_controller/command' % m['topic']
self.sub[m['topic']] = rospy.Subscriber(safe_topic, Float64, partial(self.check_safe_msgs, motor=m))
time.sleep(self._TIMEOUT)
pass
def setUp(self):
self.motors = self._TEST_MOTORS
self.motor_states_pub = rospy.Publisher("safe/motor_states", MotorStateList, queue_size=10)
time.sleep(self._TIMEOUT)
self.safety = Safety()
# Message was passed
self.proxy_pass = False
# Wrong value
self.safe_val = 100
# In case there are issues
self.ignore_msgs = False
# Create publishers and subscribers for testing
self.pub = {}
self.sub = {}
for m in self.motors:
if motor_type(m) == 'pololu':
topic = '%s/command' % m['topic']
self.pub[m['topic']] = rospy.Publisher(topic, MotorCommand, queue_size=30)
safe_topic = 'safe/%s/command' % m['topic']
self.sub[m['topic']] = rospy.Subscriber(safe_topic, MotorCommand, partial(self.check_safe_msgs, motor=m))
else:
topic = '%s_controller/command' % m['topic']
self.pub[m['topic']] = rospy.Publisher(topic, Float64, queue_size=30)
safe_topic = 'safe/%s_controller/command' % m['topic']
self.sub[m['topic']] = rospy.Subscriber(safe_topic, Float64, partial(self.check_safe_msgs, motor=m))
time.sleep(self._TIMEOUT)
pass
#!/usr/bin/env python3
# Copyright (c) 2013-2018 Hanson Robotics, Ltd, all rights reserved
import rospy
from motors_safety import Safety
import time
ROS_RATE = 20
if __name__ == '__main__':
# Wait for motors to be loaded in param server
time.sleep(3)
rospy.init_node('motors_safety')
MS = Safety()
MS.ros_rate = ROS_RATE
r = rospy.Rate(ROS_RATE)
while not rospy.is_shutdown():
MS.timing()
r.sleep()
class MotorSafetyTest(unittest.TestCase):
# Test data for the motor rules
_TEST_MOTORS = [
{
'name': 'motor1',
'topic': 'motor1',
'motor_id': 3,
'default': 0.1,
'min': -0.5,
'max': 1.5,
'hardware': 'pololu'
},
{
'name': 'motor2',
'topic': 'motor2',
'default': 0,
'min': -0.6,
'max': 1.2,
'hardware': 'dynamixel'
},
]
_TEST_RULES = {
'motor1': [
{
'type': 'timing',
'direction': 'min',
'extreme': 0.8,
't1': 1,
't2': 1,
't3': 5,
't4': 1,
},
{
'type': 'sine',
'enabled': False,
'amplitude': 0.1,
'phase_offset': 0.0,
'phase_mult': 10,
'value_offset': 0.0,
}
],
'motor2': [
{
'type': 'prevent',
'direction': 'max',
'extreme': 0.5,
'depends': 'motor1',
'dep_dir': 'max',
'dep_extreme': 0.6
},
{
'type': 'load',
'direction': 'min',
'extreme': 0.4,
'motor_id': 2,
'rest': 0.1,
't1': 1,
't2': 4,
}
]
}
_TIMEOUT = 0.1
def __init__(self, *args, **kwargs):
super(MotorSafetyTest, self).__init__(*args, **kwargs)
rospy.init_node('motors_safety_test')
# Data for actual motors
rospy.set_param('motors', self._TEST_MOTORS)
rospy.set_param('safety_rules', self._TEST_RULES)
def setUp(self):
self.motors = self._TEST_MOTORS
self.motor_states_pub = rospy.Publisher("safe/motor_states", MotorStateList, queue_size=10)
time.sleep(self._TIMEOUT)
self.safety = Safety()
# Message was passed
self.proxy_pass = False
# Wrong value
self.safe_val = 100
# In case there are issues
self.ignore_msgs = False
# Create publishers and subscribers for testing
self.pub = {}
self.sub = {}
for m in self.motors:
if motor_type(m) == 'pololu':
topic = '%s/command' % m['topic']
self.pub[m['topic']] = rospy.Publisher(topic, MotorCommand, queue_size=30)
safe_topic = 'safe/%s/command' % m['topic']
self.sub[m['topic']] = rospy.Subscriber(safe_topic, MotorCommand, partial(self.check_safe_msgs, motor=m))
else:
topic = '%s_controller/command' % m['topic']
self.pub[m['topic']] = rospy.Publisher(topic, Float64, queue_size=30)
safe_topic = 'safe/%s_controller/command' % m['topic']
self.sub[m['topic']] = rospy.Subscriber(safe_topic, Float64, partial(self.check_safe_msgs, motor=m))
time.sleep(self._TIMEOUT)
pass
# Checks the received messages for given values
def check_safe_msgs(self, msg, motor):
if self.ignore_msgs:
return
if motor_type(motor) == 'pololu':
self.safe_val = msg.position
else:
self.safe_val = msg.data
self.proxy_pass = True
# Creates message with value
def create_msg(self, motor, v):
if motor_type(motor) == 'pololu':
msg = MotorCommand()
msg.position = v
msg.joint_name = motor['name']
else:
msg = Float64()
msg.data = v
return msg
def assertMessageVal(self, expected, motor, equal=True):
self.assertTrue(self.proxy_pass, 'Message was not forwarded for %s' % motor['name'])
if equal:
self.assertAlmostEqual(expected, self.safe_val, msg="Wrong safe value. Expected %1.3f got %1.3f for %s" %
(expected,self.safe_val,motor['name']))
else:
self.assertNotAlmostEqual(expected, self.safe_val,msg="Expected not equal values, got equal values %1.3f for %s" %
(expected, motor['name']))
# Messages should pass without changes to the safe topic for neutral values.
def send_default_messages(self):
for m in self.motors:
self.proxy_pass = False
msg = self.create_msg(m, m['default'])
self.pub[m['topic']].publish(msg)
time.sleep(self._TIMEOUT)
self.assertMessageVal(m['default'], m)
#Simple test for default messages
def test_default_messages(self):
self.send_default_messages()
def test_set_motor_relative_position(self):
# Sends initial default messages
self.send_default_messages()
for m in self.motors:
self.proxy_pass = False
self.safety.set_motor_relative_pos(m['name'], 0.9, 'max')
time.sleep(self._TIMEOUT)
self.assertMessageVal(m['default'] + (m['max']-m['default'])*0.9, m)
# Tests the rule for timing
@patch('time.time', mock_time)
def test_rule_timing(self):
# Init the default messages
self.send_default_messages()
start = time.time()
# Send extreme position
m = self.motors[0]
self.proxy_pass = False
msg = self.create_msg(m, self.safety.get_abs_pos('motor1','min',0.9,))
self.pub['motor1'].publish(msg)
time.sleep(self._TIMEOUT)
self.assertMessageVal(m['default'] + (m['min']-m['default'])*0.9, m)
self.safety.timing()
# Extreme value should be not limited in 1.2s
mock_time.return_value = start + 1.2
msg = self.create_msg(m, self.safety.get_abs_pos('motor1','min',0.9))
self.proxy_pass = False
self.pub['motor1'].publish(msg)
self.safety.timing()
time.sleep(self._TIMEOUT)
self.assertMessageVal(m['default'] + (m['min']-m['default'])*0.9, m)
# Extreme position should be on its way down in 2nd second
mock_time.return_value = start + 1.80
self.safety.timing()
self.proxy_pass = False
msg = self.create_msg(m, self.safety.get_abs_pos('motor1','min',0.9))
self.pub['motor1'].publish(msg)
time.sleep(self._TIMEOUT)
self.assertMessageVal(m['default'] + (m['min']-m['default'])*(0.8+0.2*0.2), m)
# Extreme position should be limited for t3 period
mock_time.return_value = start + 2.1
self.safety.timing()
self.proxy_pass = False
msg = self.create_msg(m, self.safety.get_abs_pos('motor1','min',0.9))
self.pub['motor1'].publish(msg)
time.sleep(self._TIMEOUT)
self.assertMessageVal(m['default'] + (m['min']-m['default'])*0.8, m)
mock_time.return_value = start + 7
self.safety.timing()
self.proxy_pass = False
msg = self.create_msg(m, self.safety.get_abs_pos('motor1','min',0.9))
self.pub['motor1'].publish(msg)
time.sleep(self._TIMEOUT)
self.assertMessageVal(m['default'] + (m['min']-m['default'])*0.8, m)
# Extreme position should be back in t4
mock_time.return_value = start + 7.5
self.safety.timing()
self.proxy_pass = False
msg = self.create_msg(m, self.safety.get_abs_pos('motor1','min',0.9))
self.pub['motor1'].publish(msg)
time.sleep(self._TIMEOUT)
self.assertMessageVal(m['default'] + (m['min']-m['default'])*0.9, m)
# Test prevention rule
def test_prevent(self):
# Send the default messages
self.send_default_messages()
# Sends message close to extreme that would limit motor2
msg = self.create_msg(self.motors[0], self.safety.get_abs_pos('motor1','max',0.55))
self.pub['motor1'].publish(msg)
time.sleep(self._TIMEOUT)
# Sends the extreme message to motor2
self.proxy_pass = False
m = self.motors[1]
msg = self.create_msg(m, self.safety.get_abs_pos('motor2','max',0.95))
self.pub['motor2'].publish(msg)
time.sleep(self._TIMEOUT)
# Expects the value to not be limited
self.assertMessageVal(m['default'] + (m['max']-m['default'])*0.95, m)
# Sending the first motor value that would limit motor 2
msg = self.create_msg(self.motors[0], self.safety.get_abs_pos('motor1','max',0.65))
self.pub['motor1'].publish(msg)
time.sleep(self._TIMEOUT)
# Sends the extreme message to motor2
self.proxy_pass = False
m = self.motors[1]
msg = self.create_msg(m, self.safety.get_abs_pos('motor2','max',0.95))
self.pub['motor2'].publish(msg)
time.sleep(self._TIMEOUT)
# Expects the value to not be limited to its limit before extreme position
self.assertMessageVal(m['default'] + (m['max']-m['default'])*0.5, m)
def send_motor_state(self, id, load):
state = MotorState()
state.id = id
state.load = load
states = MotorStateList()
states.motor_states = [state]
self.motor_states_pub.publish(states)
# Test the laod related changes. Applies only for dynamixels
@patch('time.time', mock_time)
def test_load(self):
# Send the default messages
# Send load not reaching threshold
m = self.motors[1]
self.send_motor_state(2, -0.35)
time.sleep(self._TIMEOUT)
self.safety.timing()
self.assertFalse(self.safety.rules['motor2'][1]['started'],msg= "Unexpected rule start")
# Send extreme message
msg = self.create_msg(m, self.safety.get_abs_pos('motor2','min',0.95))
self.pub['motor2'].publish(msg)
time.sleep(self._TIMEOUT)
#Start
start = time.time()
self.send_motor_state(2, -0.45)
time.sleep(self._TIMEOUT)
self.assertEqual(self.safety.motor_loads[2], -0.45, msg="Current laod {} expected {}"
.format(self.safety.motor_loads[2], -0.45))
self.safety.timing()
self.assertEqual(self.safety.rules['motor2'][1]['started'], start, msg="Wrong starting time was {} wxpected {}"
.format(self.safety.rules['motor2'][1]['started'], start))
# Check value before starting to decline
mock_time.return_value = start + 0.99
self.safety.timing()
self.proxy_pass = False
msg = self.create_msg(m, self.safety.get_abs_pos('motor2','min',0.95))
self.pub['motor2'].publish(msg)
time.sleep(self._TIMEOUT)
self.assertMessageVal(m['default'] + (m['min']-m['default'])*0.95, m)
# Limit should start declining
mock_time.return_value = start + 1.1
self.safety.timing()
msg = self.create_msg(m, self.safety.get_abs_pos('motor2','min',0.95))
self.pub['motor2'].publish(msg)
time.sleep(self._TIMEOUT)
#Expects the value to be changed by filter
self.assertMessageVal(m['default'] + (m['min']-m['default'])*0.95, m, equal=False)
# Each timing call should decrease the motor limit towards natural
self.safety.timing()
# previous value
expected = self.safe_val
self.proxy_pass = False
msg = self.create_msg(m, self.safety.get_abs_pos('motor2','min',0.95))
self.pub['motor2'].publish(msg)
time.sleep(self._TIMEOUT)
self.assertMessageVal(expected, m, equal=False)
# Send the motor to recovered state
self.send_motor_state(2,0.18)
time.sleep(self._TIMEOUT)
self.safety.timing()
expected = self.safe_val
self.assertNotEqual(self.safety.rules['motor2'][1]['limit'], 1,
msg="Limit should be lower than 1. current limit is {}"
.format(self.safety.rules['motor2'][1]['limit']))
self.assertNotEqual(expected, self.safety.get_abs_pos('motor2','min',0.95),
msg="Expected {} should be different".format(expected))
self.proxy_pass = False
msg = self.create_msg(m, self.safety.get_abs_pos('motor2','min',0.95))
self.pub['motor2'].publish(msg)
time.sleep(self._TIMEOUT)
# value should keep consistent
self.assertMessageVal(expected, m)
# Keeps motor until rule expires
mock_time.return_value = start + 4.9
self.safety.timing()
self.proxy_pass = False
msg = self.create_msg(m, self.safety.get_abs_pos('motor2','min',0.95))
self.pub['motor2'].publish(msg)
time.sleep(self._TIMEOUT)
# Check if same limit applies
self.assertMessageVal(expected,m)
# After time is passed limit is gradually increased with timing call
mock_time.return_value = start + 5.1
self.safety.timing()
msg = self.create_msg(m, self.safety.get_abs_pos('motor2','min',0.95))
self.pub['motor2'].publish(msg)
time.sleep(self._TIMEOUT)
self.assertMessageVal(expected, m, equal=False)
# Limit is back to extreme position after multiple timing calls
# Should be similar number of calls as for decreasing the limit
self.safety.timing()
self.safety.timing()
self.safety.timing()
# Check if extreme messages are not modified after returniong back to normal value
self.proxy_pass = False
msg = self.create_msg(m, self.safety.get_abs_pos('motor2','min',0.99))
self.pub['motor2'].publish(msg)
time.sleep(self._TIMEOUT)
self.assertMessageVal(m['default'] + (m['min']-m['default'])*0.99,m)
# Testing sine generation
@patch('time.time', mock_time)
def test_sines(self):
m = self.motors[0]
start = time.time()
time.sleep(self._TIMEOUT)
self.safety.enable_sines()
self.assertTrue(self.safety.rules['motor1'][1]['enabled'], "Rule is not enabled")
self.assertFalse(self.safety.rules['motor1'][1]['started'], "Rule should not be started until "
"first message recieved")
self.send_default_messages()
time.sleep(self._TIMEOUT)
self.assertTrue(self.safety.rules['motor1'][1]['started'], "Rule should be started already")
self.proxy_pass = False
msg = self.create_msg(m, m['default'])
self.pub['motor1'].publish(msg)
time.sleep(self._TIMEOUT)
self.assertEqual(self.safe_val, m['default'], "safe val is {} default 0.1".format(self.safe_val))
self.safety.timing()
self.assertEqual(self.safe_val, m['default'], "Value should remain same if time hasnt changed")
mock_time.return_value += start+0.1
self.safety.timing()
time.sleep(self._TIMEOUT)
self.assertNotEqual(self.safe_val, m['default'], "Value changed after some time")
self.safety.enable_sines(disable=True)
class MotorSafetyTest(unittest.TestCase):
# Test data for the motor rules
_TEST_MOTORS = [
{
'name': 'motor1',
'topic': 'motor1',
'motor_id': 3,
'default': 0.1,
'min': -0.5,
'max': 1.5,
'hardware': 'pololu'
},
{
'name': 'motor2',
'topic': 'motor2',
'default': 0,
'min': -0.6,
'max': 1.2,
'hardware': 'dynamixel'
},
]
_TEST_RULES = {
'motor1': [{
'type': 'timing',
'direction': 'min',
'extreme': 0.8,
't1': 1,
't2': 1,
't3': 5,
't4': 1,
}, {
'type': 'sine',
'enabled': False,
'amplitude': 0.1,
'phase_offset': 0.0,
'phase_mult': 10,
'value_offset': 0.0,
}],
'motor2': [{
'type': 'prevent',
'direction': 'max',
'extreme': 0.5,
'depends': 'motor1',
'dep_dir': 'max',
'dep_extreme': 0.6
}, {
'type': 'load',
'direction': 'min',
'extreme': 0.4,
'motor_id': 2,
'rest': 0.1,
't1': 1,
't2': 4,
}]
}
_TIMEOUT = 0.1
def __init__(self, *args, **kwargs):
super(MotorSafetyTest, self).__init__(*args, **kwargs)
rospy.init_node('motors_safety_test')
# Data for actual motors
rospy.set_param('motors', self._TEST_MOTORS)
rospy.set_param('safety_rules', self._TEST_RULES)
def setUp(self):
self.motors = self._TEST_MOTORS
self.motor_states_pub = rospy.Publisher("safe/motor_states",
MotorStateList,
queue_size=10)
time.sleep(self._TIMEOUT)
self.safety = Safety()
# Message was passed
self.proxy_pass = False
# Wrong value
self.safe_val = 100
# In case there are issues
self.ignore_msgs = False
# Create publishers and subscribers for testing
self.pub = {}
self.sub = {}
for m in self.motors:
if motor_type(m) == 'pololu':
topic = '%s/command' % m['topic']
self.pub[m['topic']] = rospy.Publisher(topic,
MotorCommand,
queue_size=30)
safe_topic = 'safe/%s/command' % m['topic']
self.sub[m['topic']] = rospy.Subscriber(
safe_topic, MotorCommand,
partial(self.check_safe_msgs, motor=m))
else:
topic = '%s_controller/command' % m['topic']
self.pub[m['topic']] = rospy.Publisher(topic,
Float64,
queue_size=30)
safe_topic = 'safe/%s_controller/command' % m['topic']
self.sub[m['topic']] = rospy.Subscriber(
safe_topic, Float64, partial(self.check_safe_msgs,
motor=m))
time.sleep(self._TIMEOUT)
pass
# Checks the received messages for given values
def check_safe_msgs(self, msg, motor):
if self.ignore_msgs:
return
if motor_type(motor) == 'pololu':
self.safe_val = msg.position
else:
self.safe_val = msg.data
self.proxy_pass = True
# Creates message with value
def create_msg(self, motor, v):
if motor_type(motor) == 'pololu':
msg = MotorCommand()
msg.position = v
msg.joint_name = motor['name']
else:
msg = Float64()
msg.data = v
return msg
def assertMessageVal(self, expected, motor, equal=True):
self.assertTrue(self.proxy_pass,
'Message was not forwarded for %s' % motor['name'])
if equal:
self.assertAlmostEqual(
expected,
self.safe_val,
msg="Wrong safe value. Expected %1.3f got %1.3f for %s" %
(expected, self.safe_val, motor['name']))
else:
self.assertNotAlmostEqual(
expected,
self.safe_val,
msg="Expected not equal values, got equal values %1.3f for %s"
% (expected, motor['name']))
# Messages should pass without changes to the safe topic for neutral values.
def send_default_messages(self):
for m in self.motors:
self.proxy_pass = False
msg = self.create_msg(m, m['default'])
self.pub[m['topic']].publish(msg)
time.sleep(self._TIMEOUT)
self.assertMessageVal(m['default'], m)
#Simple test for default messages
def test_default_messages(self):
self.send_default_messages()
def test_set_motor_relative_position(self):
# Sends initial default messages
self.send_default_messages()
for m in self.motors:
self.proxy_pass = False
self.safety.set_motor_relative_pos(m['name'], 0.9, 'max')
time.sleep(self._TIMEOUT)
self.assertMessageVal(
m['default'] + (m['max'] - m['default']) * 0.9, m)
# Tests the rule for timing
@patch('time.time', mock_time)
def test_rule_timing(self):
# Init the default messages
self.send_default_messages()
start = time.time()
# Send extreme position
m = self.motors[0]
self.proxy_pass = False
msg = self.create_msg(m, self.safety.get_abs_pos(
'motor1',
'min',
0.9,
))
self.pub['motor1'].publish(msg)
time.sleep(self._TIMEOUT)
self.assertMessageVal(m['default'] + (m['min'] - m['default']) * 0.9,
m)
self.safety.timing()
# Extreme value should be not limited in 1.2s
mock_time.return_value = start + 1.2
msg = self.create_msg(m, self.safety.get_abs_pos('motor1', 'min', 0.9))
self.proxy_pass = False
self.pub['motor1'].publish(msg)
self.safety.timing()
time.sleep(self._TIMEOUT)
self.assertMessageVal(m['default'] + (m['min'] - m['default']) * 0.9,
m)
# Extreme position should be on its way down in 2nd second
mock_time.return_value = start + 1.80
self.safety.timing()
self.proxy_pass = False
msg = self.create_msg(m, self.safety.get_abs_pos('motor1', 'min', 0.9))
self.pub['motor1'].publish(msg)
time.sleep(self._TIMEOUT)
self.assertMessageVal(
m['default'] + (m['min'] - m['default']) * (0.8 + 0.2 * 0.2), m)
# Extreme position should be limited for t3 period
mock_time.return_value = start + 2.1
self.safety.timing()
self.proxy_pass = False
msg = self.create_msg(m, self.safety.get_abs_pos('motor1', 'min', 0.9))
self.pub['motor1'].publish(msg)
time.sleep(self._TIMEOUT)
self.assertMessageVal(m['default'] + (m['min'] - m['default']) * 0.8,
m)
mock_time.return_value = start + 7
self.safety.timing()
self.proxy_pass = False
msg = self.create_msg(m, self.safety.get_abs_pos('motor1', 'min', 0.9))
self.pub['motor1'].publish(msg)
time.sleep(self._TIMEOUT)
self.assertMessageVal(m['default'] + (m['min'] - m['default']) * 0.8,
m)
# Extreme position should be back in t4
mock_time.return_value = start + 7.5
self.safety.timing()
self.proxy_pass = False
msg = self.create_msg(m, self.safety.get_abs_pos('motor1', 'min', 0.9))
self.pub['motor1'].publish(msg)
time.sleep(self._TIMEOUT)
self.assertMessageVal(m['default'] + (m['min'] - m['default']) * 0.9,
m)
# Test prevention rule
def test_prevent(self):
# Send the default messages
self.send_default_messages()
# Sends message close to extreme that would limit motor2
msg = self.create_msg(self.motors[0],
self.safety.get_abs_pos('motor1', 'max', 0.55))
self.pub['motor1'].publish(msg)
time.sleep(self._TIMEOUT)
# Sends the extreme message to motor2
self.proxy_pass = False
m = self.motors[1]
msg = self.create_msg(m,
self.safety.get_abs_pos('motor2', 'max', 0.95))
self.pub['motor2'].publish(msg)
time.sleep(self._TIMEOUT)
# Expects the value to not be limited
self.assertMessageVal(m['default'] + (m['max'] - m['default']) * 0.95,
m)
# Sending the first motor value that would limit motor 2
msg = self.create_msg(self.motors[0],
self.safety.get_abs_pos('motor1', 'max', 0.65))
self.pub['motor1'].publish(msg)
time.sleep(self._TIMEOUT)
# Sends the extreme message to motor2
self.proxy_pass = False
m = self.motors[1]
msg = self.create_msg(m,
self.safety.get_abs_pos('motor2', 'max', 0.95))
self.pub['motor2'].publish(msg)
time.sleep(self._TIMEOUT)
# Expects the value to not be limited to its limit before extreme position
self.assertMessageVal(m['default'] + (m['max'] - m['default']) * 0.5,
m)
def send_motor_state(self, id, load):
state = MotorState()
state.id = id
state.load = load
states = MotorStateList()
states.motor_states = [state]
self.motor_states_pub.publish(states)
# Test the laod related changes. Applies only for dynamixels
@patch('time.time', mock_time)
def test_load(self):
# Send the default messages
# Send load not reaching threshold
m = self.motors[1]
self.send_motor_state(2, -0.35)
time.sleep(self._TIMEOUT)
self.safety.timing()
self.assertFalse(self.safety.rules['motor2'][1]['started'],
msg="Unexpected rule start")
# Send extreme message
msg = self.create_msg(m,
self.safety.get_abs_pos('motor2', 'min', 0.95))
self.pub['motor2'].publish(msg)
time.sleep(self._TIMEOUT)
#Start
start = time.time()
self.send_motor_state(2, -0.45)
time.sleep(self._TIMEOUT)
self.assertEqual(self.safety.motor_loads[2],
-0.45,
msg="Current laod {} expected {}".format(
self.safety.motor_loads[2], -0.45))
self.safety.timing()
self.assertEqual(self.safety.rules['motor2'][1]['started'],
start,
msg="Wrong starting time was {} wxpected {}".format(
self.safety.rules['motor2'][1]['started'], start))
# Check value before starting to decline
mock_time.return_value = start + 0.99
self.safety.timing()
self.proxy_pass = False
msg = self.create_msg(m,
self.safety.get_abs_pos('motor2', 'min', 0.95))
self.pub['motor2'].publish(msg)
time.sleep(self._TIMEOUT)
self.assertMessageVal(m['default'] + (m['min'] - m['default']) * 0.95,
m)
# Limit should start declining
mock_time.return_value = start + 1.1
self.safety.timing()
msg = self.create_msg(m,
self.safety.get_abs_pos('motor2', 'min', 0.95))
self.pub['motor2'].publish(msg)
time.sleep(self._TIMEOUT)
#Expects the value to be changed by filter
self.assertMessageVal(m['default'] + (m['min'] - m['default']) * 0.95,
m,
equal=False)
# Each timing call should decrease the motor limit towards natural
self.safety.timing()
# previous value
expected = self.safe_val
self.proxy_pass = False
msg = self.create_msg(m,
self.safety.get_abs_pos('motor2', 'min', 0.95))
self.pub['motor2'].publish(msg)
time.sleep(self._TIMEOUT)
self.assertMessageVal(expected, m, equal=False)
# Send the motor to recovered state
self.send_motor_state(2, 0.18)
time.sleep(self._TIMEOUT)
self.safety.timing()
expected = self.safe_val
self.assertNotEqual(
self.safety.rules['motor2'][1]['limit'],
1,
msg="Limit should be lower than 1. current limit is {}".format(
self.safety.rules['motor2'][1]['limit']))
self.assertNotEqual(
expected,
self.safety.get_abs_pos('motor2', 'min', 0.95),
msg="Expected {} should be different".format(expected))
self.proxy_pass = False
msg = self.create_msg(m,
self.safety.get_abs_pos('motor2', 'min', 0.95))
self.pub['motor2'].publish(msg)
time.sleep(self._TIMEOUT)
# value should keep consistent
self.assertMessageVal(expected, m)
# Keeps motor until rule expires
mock_time.return_value = start + 4.9
self.safety.timing()
self.proxy_pass = False
msg = self.create_msg(m,
self.safety.get_abs_pos('motor2', 'min', 0.95))
self.pub['motor2'].publish(msg)
time.sleep(self._TIMEOUT)
# Check if same limit applies
self.assertMessageVal(expected, m)
# After time is passed limit is gradually increased with timing call
mock_time.return_value = start + 5.1
self.safety.timing()
msg = self.create_msg(m,
self.safety.get_abs_pos('motor2', 'min', 0.95))
self.pub['motor2'].publish(msg)
time.sleep(self._TIMEOUT)
self.assertMessageVal(expected, m, equal=False)
# Limit is back to extreme position after multiple timing calls
# Should be similar number of calls as for decreasing the limit
self.safety.timing()
self.safety.timing()
self.safety.timing()
# Check if extreme messages are not modified after returniong back to normal value
self.proxy_pass = False
msg = self.create_msg(m,
self.safety.get_abs_pos('motor2', 'min', 0.99))
self.pub['motor2'].publish(msg)
time.sleep(self._TIMEOUT)
self.assertMessageVal(m['default'] + (m['min'] - m['default']) * 0.99,
m)
# Testing sine generation
@patch('time.time', mock_time)
def test_sines(self):
m = self.motors[0]
start = time.time()
time.sleep(self._TIMEOUT)
self.safety.enable_sines()
self.assertTrue(self.safety.rules['motor1'][1]['enabled'],
"Rule is not enabled")
self.assertFalse(
self.safety.rules['motor1'][1]['started'],
"Rule should not be started until "
"first message recieved")
self.send_default_messages()
time.sleep(self._TIMEOUT)
self.assertTrue(self.safety.rules['motor1'][1]['started'],
"Rule should be started already")
self.proxy_pass = False
msg = self.create_msg(m, m['default'])
self.pub['motor1'].publish(msg)
time.sleep(self._TIMEOUT)
self.assertEqual(self.safe_val, m['default'],
"safe val is {} default 0.1".format(self.safe_val))
self.safety.timing()
self.assertEqual(self.safe_val, m['default'],
"Value should remain same if time hasnt changed")
mock_time.return_value += start + 0.1
self.safety.timing()
time.sleep(self._TIMEOUT)
self.assertNotEqual(self.safe_val, m['default'],
"Value changed after some time")
self.safety.enable_sines(disable=True)
#!/usr/bin/env python3
import rospy
from motors_safety import Safety
import time
ROS_RATE = 20
if __name__ == '__main__':
# Wait for motors to be loaded in param server
time.sleep(3)
rospy.init_node('motors_safety')
MS = Safety()
MS.ros_rate = ROS_RATE
r = rospy.Rate(ROS_RATE)
while not rospy.is_shutdown():
MS.timing()
r.sleep()
