def _on_quit(self):
"""Cleanup on quit from the command line interface."""
# Sit the robot down + power off after the navigation command is complete.
if self._powered_on and not self._started_powered_on:
self._robot_command_client.robot_command(
RobotCommandBuilder.safe_power_off_command(),
end_time_secs=time.time())
def power_off(self):
"""Power off the robot."""
safe_power_off_cmd = RobotCommandBuilder.safe_power_off_command()
self._robot_command_client.robot_command(lease=None,
command=safe_power_off_cmd)
time.sleep(2.5)
print("Powered Off " + str(not self._robot.is_powered_on()))
def _force_safe_power_off(self):
"""Safely powers off the robot.
"""
if self.robot.is_powered_on():
lease = self.lease_client.take()
with LeaseKeepAlive(self.lease_client):
cmd = RobotCommandBuilder.safe_power_off_command()
self.command_client.robot_command(cmd)
self.lease_client.return_lease(lease)
def _safe_power_off(self):
self._start_robot_command('safe_power_off',
RobotCommandBuilder.safe_power_off_command())
def manual_control_loop(self):
# Create robot object.
if self.config.norobot:
robot_command_client = None
robot_state_client = None
else:
sdk = bosdyn.client.create_standard_sdk('RobotCommandMaster')
sdk.load_app_token(self.config.app_token)
robot = sdk.create_robot(self.config.hostname)
robot.authenticate(self.config.username, self.config.password)
# Check that an estop is connected with the robot so that the robot commands can be executed.
verify_estop(robot)
# Create the lease client.
lease_client = robot.ensure_client(
LeaseClient.default_service_name)
lease = lease_client.acquire()
robot.time_sync.wait_for_sync()
lk = bosdyn.client.lease.LeaseKeepAlive(lease_client)
# Setup clients for the robot state and robot command services.
robot_state_client = robot.ensure_client(
RobotStateClient.default_service_name)
robot_command_client = robot.ensure_client(
RobotCommandClient.default_service_name)
# Power on the robot and stand it up.
if not self.config.nopower:
robot.power_on()
blocking_stand(robot_command_client)
# Initialize camera
camera_pipeline = camera.init_camera()
# Print pose
self.get_pose(robot_state_client)
is_server_on = False
# Control loop
while True:
cmd = input('Command (wasd/qe/c/r):')
if cmd == 'c':
break
elif cmd == 'r':
print("Remote control..")
try:
self.reset_last_frame()
if is_server_on:
asyncio.get_event_loop().run_forever()
else:
is_server_on = True
self.remote_control_loop(
robot_command_client,
robot_state_client,
camera_pipeline=camera_pipeline,
dummy=(robot_command_client is None))
except KeyboardInterrupt:
print("ending remote control")
pass
else:
self.reset_last_frame()
pos = self.execute_command(robot_command_client,
robot_state_client,
cmd,
wait_to_stabilize=False)
# move_relative(robot_command_client, robot_state_client, timeout=5.0)
robot_cmd = RobotCommandBuilder.safe_power_off_command()
robot_command_client.robot_command(lease=None,
command=robot_cmd,
end_time_secs=time.time() + 2.)
time.sleep(2.)
return True
def safe_power_off(self):
"""Stop the robot's motion and sit if possible. Once sitting, disable motor power."""
response = self._robot_command(
RobotCommandBuilder.safe_power_off_command())
return response[0], response[1]
def test_safe_power_off_command():
command = RobotCommandBuilder.safe_power_off_command()
_test_has_full_body(command)
assert command.full_body_command.HasField("safe_power_off_request")
def _perform_and_monitor_power_off(self, cmd_client, session_id, response):
"""Attempts to power off the robot and check on power off progress.
Builds the response in-place, depending on how the power off command is going.
"""
# Grab the Future for the original command and the feedback.
command_future, feedback_future = self.sessions_by_id[session_id]
# If the original command has not been sent...
if command_future is None:
command_future = cmd_client.robot_command_async(
RobotCommandBuilder.safe_power_off_command())
# Store the future, so we can reference it on later ticks.
self.sessions_by_id[session_id] = (command_future, None)
# If the original command is done...
if command_future.done():
# See what the result was. On a successful RPC, the result is the ID of the command.
# If the command failed, an exception will be raised.
try:
command_id = command_future.result()
except bosdyn.client.LeaseUseError as exc:
self.logger.error('LeaseUseError: "%s"', str(exc))
# We often treat LeaseUseErrors as a recoverable thing, so you may want to do the
# same. A newer lease may come in with the next TickRequest.
# It could be that the lease received for this tick is already newer than the lease
# that was used in the command_future being checked right now. In that case, the
# mission service will typically just try again. You have the option of doing your
# own "try again" logic in this servicer, but it's not typically necessary.
response.status = remote_pb2.TickResponse.STATUS_RUNNING
response.lease_use_results.add().CopyFrom(
exc.response.lease_use_result)
# Reset the information for this session, so the next tick will make another call.
self.sessions_by_id[session_id] = (None, None)
return
except bosdyn.client.Error as exc:
self.logger.error('Command failed: "%s"', str(exc))
response.error_message = str(exc)
response.status = remote_pb2.TickResponse.STATUS_FAILURE
# We've failed!
# We do not reset the session information here, expecting that Stop will be called.
return
# Issue the request for feedback if no request is outstanding.
if feedback_future is None:
feedback_future = cmd_client.robot_command_feedback_async(
command_id)
self.sessions_by_id[session_id] = (command_future,
feedback_future)
# If the feedback request is done...
if feedback_future.done():
try:
command_response = feedback_future.result()
except bosdyn.client.Error as exc:
self.logger.exception('Feedback failed!')
response.error_message = str(exc)
response.status = remote_pb2.TickResponse.STATUS_FAILURE
# We've failed!
# We do not reset the session information here, expecting that Stop will be
# called.
return
# Shortname for the object that holds the top-level command status codes.
cmd_status_codes = robot_command_pb2.RobotCommandFeedbackResponse
# Shortname for the object that holds the SafePowerOff-specific codes.
safe_off_status_codes = basic_command_pb2.SafePowerOffCommand.Feedback
# Shortname for the recently-read SafePowerOff status code.
status = command_response.feedback.full_body_feedback.safe_power_off_feedback.status
# Translate the feedback status to tick status.
# We check STATUS_PROCESSING and STATUS_ROBOT_FROZEN because the robot will
# transition to "frozen" while it powers off.
if (command_response.status
== cmd_status_codes.STATUS_PROCESSING
or command_response.status
== cmd_status_codes.STATUS_ROBOT_FROZEN):
if status == safe_off_status_codes.STATUS_POWERED_OFF:
response.status = remote_pb2.TickResponse.STATUS_SUCCESS
elif status == safe_off_status_codes.STATUS_IN_PROGRESS:
response.status = remote_pb2.TickResponse.STATUS_RUNNING
# We need to issue another feedback request, so reset that future to None
# for the next tick.
self.sessions_by_id[session_id] = (command_future,
None)
else:
response.status = remote_pb2.TickResponse.STATUS_FAILURE
response.error_message = 'Unexpected feedback status "{}"!'.format(
safe_off_status_codes.Status.Name(status))
else:
response.status = remote_pb2.TickResponse.STATUS_FAILURE
response.error_message = cmd_status_codes.Status.Name(
command_response.status)
return
response.status = remote_pb2.TickResponse.STATUS_RUNNING
