def _selfright(self):
"""Executes selfright command, which causes the robot to automatically turn if
it is on its back.
"""
cmd = RobotCommandBuilder.selfright_command()
self._issue_robot_command(cmd)
def self_right_cmd_srv(self, stand):
""" Callback that sends self-right cmd"""
cmd = RobotCommandBuilder.selfright_command()
ret = self.command_client.robot_command(cmd)
rospy.loginfo("Robot self right cmd sent. {}".format(ret))
return []
def _selfright(self):
"""Executes selfright command, which causes the robot to automatically turn if
it is on its back.
"""
if not self.motors_powered:
return
cmd = RobotCommandBuilder.selfright_command()
self.command_client.robot_command_async(cmd)
def test_build_synchro_command():
# two synchro subcommands of the same type:
arm_command_1 = RobotCommandBuilder.arm_ready_command()
arm_command_2 = RobotCommandBuilder.arm_stow_command()
command = RobotCommandBuilder.build_synchro_command(arm_command_1, arm_command_2)
_test_has_synchronized(command)
_test_has_arm(command.synchronized_command)
assert not command.synchronized_command.HasField("gripper_command")
assert not command.synchronized_command.HasField("mobility_command")
command_position = command.synchronized_command.arm_command.named_arm_position_command.position
assert command_position == arm_command_pb2.NamedArmPositionsCommand.POSITIONS_STOW
# two synchro subcommands of a different type:
arm_command = RobotCommandBuilder.arm_ready_command()
mobility_command = RobotCommandBuilder.synchro_stand_command()
command = RobotCommandBuilder.build_synchro_command(arm_command, mobility_command)
_test_has_synchronized(command)
_test_has_mobility(command.synchronized_command)
_test_has_arm(command.synchronized_command)
assert not command.synchronized_command.HasField("gripper_command")
assert command.synchronized_command.mobility_command.HasField("stand_request")
command_position = command.synchronized_command.arm_command.named_arm_position_command.position
assert command_position == arm_command_pb2.NamedArmPositionsCommand.POSITIONS_READY
# one synchro command and one deprecated mobility command:
deprecated_mobility_command = RobotCommandBuilder.sit_command()
command = RobotCommandBuilder.build_synchro_command(mobility_command,
deprecated_mobility_command)
_test_has_synchronized(command)
_test_has_mobility(command.synchronized_command)
assert command.synchronized_command.mobility_command.HasField("sit_request")
# fullbody command is rejected
full_body_command = RobotCommandBuilder.selfright_command()
with pytest.raises(Exception):
command = RobotCommandBuilder.build_synchro_command(arm_command, full_body_command)
def _self_right(self):
self._start_robot_command('self_right',
RobotCommandBuilder.selfright_command())
def self_right(self):
"""Have the robot self-right itself."""
response = self._robot_command(RobotCommandBuilder.selfright_command())
return response[0], response[1]
def test_selfright_command():
command = RobotCommandBuilder.selfright_command()
_test_has_full_body(command)
assert command.full_body_command.HasField("selfright_request")
