def test__on_odom_receive(self, mocked_rospy):
mocked_rospy.get_param.return_value = utils.get_params()
odom = Odometry()
receiver = CommandReceiver()
receiver._on_odom_receive(odom)
assert receiver._CommandReceiver__current_odometry == odom
def test__on_command_receive_wo_odometry(self, mocked_rospy):
mocked_rospy.get_param.return_value = utils.get_params()
msg = String(data='circle')
receiver = CommandReceiver()
receiver._move = MagicMock()
receiver._on_command_receive(msg)
receiver._move.assert_not_called()
def test__move(self, mocked_rospy):
mocked_rospy.get_param.return_value = utils.get_params()
receiver = CommandReceiver()
counter = type('Counter', (object, ), {"count": 0})()
def callback():
counter.count += 1
assert receiver._CommandReceiver__is_moving is True
assert receiver._CommandReceiver__is_moving is False
assert counter.count == 0
t = receiver._move(callback)
t.join()
assert receiver._CommandReceiver__is_moving is False
assert counter.count == 1
def test__do_square(self, mocked_rospy):
mocked_rospy.get_param.return_value = utils.get_params()
receiver = CommandReceiver()
mocked_forward = MagicMock()
receiver._forward = mocked_forward
mocked_rotate = MagicMock()
receiver._rotate = mocked_rotate
odom = Odometry
receiver._CommandReceiver__current_odometry = odom
receiver._do_square()
assert mocked_forward.call_args_list == [
call(odom, 0.4),
call(odom, 0.4),
call(odom, 0.4),
call(odom, 0.4)
]
assert mocked_rotate.call_args_list == [
call(odom, math.pi / 2),
call(odom, math.pi),
call(odom, math.pi * 3 / 2),
call(odom, math.pi * 2)
]
def test__do_forward(self, mocked_rospy, reverse):
mocked_rospy.get_param.return_value = utils.get_params()
mocked_rospy.is_shutdown.return_value = False
receiver = CommandReceiver()
receiver._CommandReceiver__is_moving = True
mocked_odometry = MagicMock()
counter = {'x': 0, 'y': 0}
def mocked_x():
if counter['x'] == 0:
result = 0.0
elif counter['x'] == 1:
result = 0.0001
elif counter['x'] == 2:
result = 1.0
elif counter['x'] == 3:
result = 0.0001
else:
result = 0.0
counter['x'] += 1
return result
def mocked_y():
if counter['y'] == 0:
result = 0.0
elif counter['y'] == 1:
result = 0.0001
elif counter['y'] == 2:
result = 1.0
elif counter['y'] == 3:
result = 0.0001
else:
result = 0.0
counter['y'] += 1
return result
type(mocked_odometry.pose.pose.position).x = PropertyMock(
side_effect=mocked_x)
type(mocked_odometry.pose.pose.position).y = PropertyMock(
side_effect=mocked_y)
receiver._CommandReceiver__current_odometry = mocked_odometry
receiver._forward(Odometry(), 1.0, reverse=reverse)
twist_list = receiver._CommandReceiver__turtlebot3_cmd_pub.publish.call_args_list
if reverse:
assert twist_list == [
call(Twist(linear=Vector3(x=-0.2))),
call(Twist(linear=Vector3(x=-0.2))),
call(Twist())
]
else:
assert twist_list == [
call(Twist(linear=Vector3(x=0.2))),
call(Twist(linear=Vector3(x=0.2))),
call(Twist())
]
def test_init(self, mocked_rospy):
mocked_rospy.get_param.return_value = utils.get_params()
receiver = CommandReceiver()
mocked_rospy.Subscriber('/turtlebot3_bridge/cmd',
String,
receiver._on_command_receive,
queue_size=10)
mocked_rospy.Publisher('/cmd_vel', Twist, queue_size=10)
mocked_rospy.Subscriber('/odom',
Odometry,
receiver._on_odom_receive,
queue_size=10)
def test__do_right(self, mocked_rospy):
mocked_rospy.get_param.return_value = utils.get_params()
receiver = CommandReceiver()
mocked_forward = MagicMock()
receiver._forward = mocked_forward
mocked_rotate = MagicMock()
receiver._rotate = mocked_rotate
odom = Odometry
receiver._CommandReceiver__current_odometry = odom
receiver._do_right()
mocked_forward.assert_not_called()
mocked_rotate.assert_called_once_with(odom, -1 * 0.01, reverse=True)
def test__on_command_receive_w_odometry(self, mocked_rospy, cmd, method):
mocked_rospy.get_param.return_value = utils.get_params()
msg = String(data=cmd)
receiver = CommandReceiver()
receiver._move = MagicMock()
odom = Odometry()
receiver._CommandReceiver__current_odometry = odom
receiver._CommandReceiver__is_moving = True
receiver._on_command_receive(msg)
if cmd in ('circle', 'square', 'triangle', 'up', 'down', 'left',
'right'):
receiver._move.assert_called_once_with(getattr(receiver, method))
assert receiver._CommandReceiver__is_moving is True
elif cmd == 'stop':
receiver._move.assert_not_called()
assert receiver._CommandReceiver__is_moving is False
else:
receiver._move.assert_not_called()
assert receiver._CommandReceiver__is_moving is True
def test__do_rotate(self, mocked_rospy, mocked_euler_from_quaternion,
reverse):
mocked_rospy.get_param.return_value = utils.get_params()
mocked_rospy.is_shutdown.return_value = False
receiver = CommandReceiver()
receiver._CommandReceiver__is_moving = True
mocked_odometry = MagicMock()
counter = {'theta': 0}
def mocked_theta(q):
if counter['theta'] == 0:
result = (0.0, 0.0, math.pi)
elif counter['theta'] == 1:
result = (0.0, 0.0, 0.0)
elif counter['theta'] == 2:
result = (0.0, 0.0, 0.001)
elif counter['theta'] == 3:
result = (0.0, 0.0, math.pi)
else:
result = (0.0, 0.0, 0.0)
counter['theta'] += 1
return result
mocked_euler_from_quaternion.side_effect = mocked_theta
receiver._CommandReceiver__current_odometry = mocked_odometry
receiver._rotate(Odometry(), math.pi, reverse=reverse)
twist_list = receiver._CommandReceiver__turtlebot3_cmd_pub.publish.call_args_list
if reverse:
assert twist_list == [
call(Twist(angular=Vector3(z=-0.2))),
call(Twist(angular=Vector3(z=-0.2))),
call(Twist())
]
else:
assert twist_list == [
call(Twist(angular=Vector3(z=0.2))),
call(Twist(angular=Vector3(z=0.2))),
call(Twist())
]
def main():
try:
rospy.init_node(NODE_NAME)
CommandReceiver().start()
except rospy.ROSInterruptException:
pass
def test_start(self, mocked_rospy):
mocked_rospy.get_param.return_value = utils.get_params()
CommandReceiver().start()
mocked_rospy.spin.assert_called_once_with()
def test__do_circle(self, mocked_rospy, mocked_euler_from_quaternion):
mocked_rospy.get_param.return_value = utils.get_params()
mocked_rospy.is_shutdown.return_value = False
receiver = CommandReceiver()
receiver._CommandReceiver__is_moving = True
mocked_odometry = MagicMock()
counter = {'x': 0, 'y': 0, 'theta': 0}
def mocked_x():
if counter['x'] == 0:
result = 0.0
elif counter['x'] == 1:
result = 0.0001
elif counter['x'] == 2:
result = 1.0
elif counter['x'] == 3:
result = 0.0001
else:
result = 0.0
counter['x'] += 1
return result
def mocked_y():
if counter['y'] == 0:
result = 0.0
elif counter['y'] == 1:
result = 0.0001
elif counter['y'] == 2:
result = 1.0
elif counter['y'] == 3:
result = 0.0001
else:
result = 0.0
counter['y'] += 1
return result
def mocked_theta(q):
if counter['theta'] == 0:
result = (0.0, 0.0, 0.0)
elif counter['theta'] == 1:
result = (0.0, 0.0, 0.0)
elif counter['theta'] == 2:
result = (0.0, 0.0, math.pi)
elif counter['theta'] == 3:
result = (0.0, 0.0, math.pi)
else:
result = (0.0, 0.0, 0.0)
counter['theta'] += 1
return result
type(mocked_odometry.pose.pose.position).x = PropertyMock(
side_effect=mocked_x)
type(mocked_odometry.pose.pose.position).y = PropertyMock(
side_effect=mocked_y)
mocked_euler_from_quaternion.side_effect = mocked_theta
receiver._CommandReceiver__current_odometry = mocked_odometry
receiver._do_circle()
twist_list = receiver._CommandReceiver__turtlebot3_cmd_pub.publish.call_args_list
assert twist_list == [
call(Twist(linear=Vector3(x=0.1), angular=Vector3(z=0.4))),
call(Twist(linear=Vector3(x=0.1), angular=Vector3(z=0.4))),
call(Twist(linear=Vector3(x=0.1), angular=Vector3(z=0.4))),
call(Twist())
]
mocked_rospy.Rate.assert_called_once_with(10)
