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__move_when_moving(self, mocked_rospy):
mocked_rospy.get_param.return_value = utils.get_params()
receiver = CommandReceiver()
receiver._CommandReceiver__is_moving = True
counter = type('Counter', (object, ), {"count": 0})()
def callback():
assert False
counter.count += 1
assert receiver._CommandReceiver__is_moving is True
assert counter.count == 0
t = receiver._move(callback)
t.join()
assert receiver._CommandReceiver__is_moving is True
assert counter.count == 0
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 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_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)