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