def test_normal_left_right(self):
self.roboclaw = RoboclawStub('/dev/ttyACM0', 115200)
self.rbc_ctl = RoboclawControl(self.roboclaw)
# Turn left
self.rbc_ctl.driveM1M2qpps(1000, -1000, ACCEL, 2)
self.roboclaw._simulate(sim_secs=0.5)
self.check_stats(1000, -1000, 500, -500)
# Check that stopped
self.roboclaw._simulate(sim_secs=1.5)
self.roboclaw._simulate() # need to run simulate once more
self.check_stats(0, 0, 2000, -2000)
# Turn right
self.rbc_ctl.driveM1M2qpps(-2000, 2000, ACCEL, 2)
self.roboclaw._simulate(sim_secs=0.5)
self.check_stats(-2000, 2000, 1000, -1000)
# Check that stopped
self.roboclaw._simulate(sim_secs=1.5) # 0.5 seconds
self.roboclaw._simulate() # need to run simulate once more
self.check_stats(0, 0, -2000, 2000)
def test_normal_stop(self):
self.roboclaw = RoboclawStub('/dev/ttyACM0', 115200)
self.rbc_ctl = RoboclawControl(self.roboclaw)
# Start moving forward
self.rbc_ctl.driveM1M2qpps(1000, 1000, 5)
self.roboclaw._simulate(sim_secs=0.5)
self.check_stats(1000, 1000, 500, 500)
# Issue stop command
self.rbc_ctl.stop()
self.roboclaw._simulate()
# Stop command was received before simulate() so the loop of simulate should not
# add any distance
self.check_stats(0, 0, 500, 500)
def test_normal_diag(self):
self.roboclaw = RoboclawStub('/dev/ttyACM0', 115200)
self.rbc_ctl = RoboclawControl(self.roboclaw)
# Set up the diag values
m1_current = 0.1 # Amps
m2_current = 0.2 # Amps
temp1 = 28.1 # Celsius
temp2 = 38.4 # Celsius
main_volts = 7.3 # Volts
logic_volts = 4.9 # Volts
error_bitmap = 0x0040 | 0x2000 # Low logic battery + Temp2 warning
self.roboclaw._m1_current = int(m1_current * 100)
self.roboclaw._m2_current = int(m2_current * 100)
self.roboclaw._temp1 = int(temp1 * 10)
self.roboclaw._temp2 = int(temp2 * 10)
self.roboclaw._main_bat_voltage = int(main_volts * 10)
self.roboclaw._logic_bat_voltage = int(logic_volts * 10)
self.roboclaw._error_bitmap = error_bitmap
# Check reading diagnostics
diag = self.rbc_ctl.read_diag()
tests = [
("M1 current", diag.m1_current, m1_current),
("M2 current", diag.m2_current, m2_current),
("Temp1", diag.temp1, temp1),
("Temp2", diag.temp2, temp2),
("Main voltage", diag.main_battery_v, main_volts),
("Logic voltage", diag.logic_battery_v, logic_volts),
]
for label, actual_val, expected_val in tests:
self.assertEqual(
actual_val, expected_val,
msg="{} expected: {}, actual: {}".format(label, expected_val, actual_val)
)
tests = [
"Logic batt voltage high error",
"Temperature2 warning",
]
for test in tests:
self.assertIn(
test, diag.error_messages,
msg="error_message {} not found [error_bitmap: {}]"
.format(test, self.roboclaw._error_bitmap)
)
def test_normal_fwd_bwd(self):
self.roboclaw = RoboclawStub('/dev/ttyACM0', 115200)
self.rbc_ctl = RoboclawControl(self.roboclaw)
# Check not moving
self.roboclaw._simulate()
self.check_stats(0, 0, 0, 0)
# Drive forward
self.rbc_ctl.driveM1M2qpps(1000, 1000, ACCEL, 2)
self.roboclaw._simulate(sim_secs=0.5)
self.check_stats(1000, 1000, 500, 500)
self.roboclaw._simulate(sim_secs=0.5)
self.check_stats(1000, 1000, 1000, 1000)
# Stop after reaching distance
self.roboclaw._simulate(sim_secs=1.0)
self.roboclaw._simulate() # need to run simulate once more
self.check_stats(0, 0, 2000, 2000)
# Drive backward
self.rbc_ctl.driveM1M2qpps(-2000, -2000, ACCEL, 2)
self.roboclaw._simulate(sim_secs=0.5)
self.check_stats(-2000, -2000, 1000, 1000)
self.roboclaw._simulate(sim_secs=0.5)
self.check_stats(-2000, -2000, 0, 0)
self.roboclaw._simulate(sim_secs=0.5)
self.check_stats(-2000, -2000, -1000, -1000)
# Stop after reaching distance
self.roboclaw._simulate(sim_secs=0.5)
self.roboclaw._simulate() # need to run simulate once more
self.check_stats(0, 0, -2000, -2000)
def connect(self, dev_name, baud_rate, address, test_mode=False):
"""Connects the node to the Roboclaw controller, or the test stub
Parameters:
:param str dev_name: Serial device name (e.g. /dev/ttyACM0)
:param int baud_rate: Serial baud rate (e.g. 115200)
:param int address: Serial address (default 0x80)
:param bool test_mode: if connecting to the controller test stub
"""
rospy.loginfo("Connecting to roboclaw")
if not test_mode:
roboclaw = Roboclaw(dev_name, baud_rate)
else:
roboclaw = RoboclawStub(dev_name, baud_rate)
self._rbc_ctls.append(RoboclawControl(roboclaw, address))
class TestRoboclawStub(unittest.TestCase):
def __init__(self, *args):
super(TestRoboclawStub, self).__init__(*args)
self.roboclaw = None
self.rbc_ctl = None
def test_normal_fwd_bwd(self):
self.roboclaw = RoboclawStub('/dev/ttyACM0', 115200)
self.rbc_ctl = RoboclawControl(self.roboclaw)
# Check not moving
self.roboclaw._simulate()
self.check_stats(0, 0, 0, 0)
# Drive forward
self.rbc_ctl.driveM1M2qpps(1000, 1000, ACCEL, 2)
self.roboclaw._simulate(sim_secs=0.5)
self.check_stats(1000, 1000, 500, 500)
self.roboclaw._simulate(sim_secs=0.5)
self.check_stats(1000, 1000, 1000, 1000)
# Stop after reaching distance
self.roboclaw._simulate(sim_secs=1.0)
self.roboclaw._simulate() # need to run simulate once more
self.check_stats(0, 0, 2000, 2000)
# Drive backward
self.rbc_ctl.driveM1M2qpps(-2000, -2000, ACCEL, 2)
self.roboclaw._simulate(sim_secs=0.5)
self.check_stats(-2000, -2000, 1000, 1000)
self.roboclaw._simulate(sim_secs=0.5)
self.check_stats(-2000, -2000, 0, 0)
self.roboclaw._simulate(sim_secs=0.5)
self.check_stats(-2000, -2000, -1000, -1000)
# Stop after reaching distance
self.roboclaw._simulate(sim_secs=0.5)
self.roboclaw._simulate() # need to run simulate once more
self.check_stats(0, 0, -2000, -2000)
def test_normal_left_right(self):
self.roboclaw = RoboclawStub('/dev/ttyACM0', 115200)
self.rbc_ctl = RoboclawControl(self.roboclaw)
# Turn left
self.rbc_ctl.driveM1M2qpps(1000, -1000, ACCEL, 2)
self.roboclaw._simulate(sim_secs=0.5)
self.check_stats(1000, -1000, 500, -500)
# Check that stopped
self.roboclaw._simulate(sim_secs=1.5)
self.roboclaw._simulate() # need to run simulate once more
self.check_stats(0, 0, 2000, -2000)
# Turn right
self.rbc_ctl.driveM1M2qpps(-2000, 2000, ACCEL, 2)
self.roboclaw._simulate(sim_secs=0.5)
self.check_stats(-2000, 2000, 1000, -1000)
# Check that stopped
self.roboclaw._simulate(sim_secs=1.5) # 0.5 seconds
self.roboclaw._simulate() # need to run simulate once more
self.check_stats(0, 0, -2000, 2000)
def test_normal_stop(self):
self.roboclaw = RoboclawStub('/dev/ttyACM0', 115200)
self.rbc_ctl = RoboclawControl(self.roboclaw)
# Start moving forward
self.rbc_ctl.driveM1M2qpps(1000, 1000, ACCEL, 5)
self.roboclaw._simulate(sim_secs=0.5)
self.check_stats(1000, 1000, 500, 500)
# Issue stop command
self.rbc_ctl.stop()
self.roboclaw._simulate()
# Stop command was received before simulate() so the loop of simulate should not
# add any distance
self.check_stats(0, 0, 500, 500)
# Issue stop command w/ decel
self.rbc_ctl.stop(decel=ACCEL)
self.roboclaw._simulate()
# Stop command was received before simulate() so the loop of simulate should not
# add any distance
self.check_stats(0, 0, 500, 500)
def test_normal_diag(self):
self.roboclaw = RoboclawStub('/dev/ttyACM0', 115200)
self.rbc_ctl = RoboclawControl(self.roboclaw)
# Set up the diag values
m1_current = 0.1 # Amps
m2_current = 0.2 # Amps
temp1 = 28.1 # Celsius
temp2 = 38.4 # Celsius
main_volts = 7.3 # Volts
logic_volts = 4.9 # Volts
error_bitmap = 0x0040 | 0x2000 # Low logic battery + Temp2 warning
self.roboclaw._m1_current = int(m1_current * 100)
self.roboclaw._m2_current = int(m2_current * 100)
self.roboclaw._temp1 = int(temp1 * 10)
self.roboclaw._temp2 = int(temp2 * 10)
self.roboclaw._main_bat_voltage = int(main_volts * 10)
self.roboclaw._logic_bat_voltage = int(logic_volts * 10)
self.roboclaw._error_bitmap = error_bitmap
# Check reading diagnostics
diag = self.rbc_ctl.read_diag()
tests = [
("M1 current", diag.m1_current, m1_current),
("M2 current", diag.m2_current, m2_current),
("Temp1", diag.temp1, temp1),
("Temp2", diag.temp2, temp2),
("Main voltage", diag.main_battery_v, main_volts),
("Logic voltage", diag.logic_battery_v, logic_volts),
]
for label, actual_val, expected_val in tests:
self.assertEqual(
actual_val, expected_val,
msg="{} expected: {}, actual: {}".format(label, expected_val, actual_val)
)
tests = [
"Logic batt voltage high error",
"Temperature2 warning",
]
for test in tests:
self.assertIn(
test, diag.error_messages,
msg="error_message {} not found [error_bitmap: {}]"
.format(test, self.roboclaw._error_bitmap)
)
def check_stats(self, m1_qpps, m2_qpps, m1_val, m2_val):
"""Check actual stats values against expected values.
Parameters:
:param int m1_qpps: Expected motor 1 QPPS value
:param int m2_qpps: Expected motor 2 QPPS value
:param int m1_val: Expected motor 1 encoder value
:param int m2_val: Expected motor 2 encoder value
"""
success, stats = self.rbc_ctl.read_stats()
self.assertTrue(success)
tests = [
("M1 QPPS", stats.m1_enc_qpps, m1_qpps),
("M2 QPPS", stats.m2_enc_qpps, m2_qpps),
("M1 encoder value", stats.m1_enc_val, m1_val),
("M1 encoder value", stats.m2_enc_val, m2_val),
]
for label, actual_val, expected_val in tests:
self.assertEqual(
round(actual_val), round(expected_val),
msg="{} expected: {}, actual: {}".format(label, expected_val, actual_val)
)