def set_robot_state(robot_state: RobotState, robot_name: str, x: float, y: float, yaw: float, level: str): robot_state.name = robot_name robot_mode = RobotMode() robot_mode.mode = robot_mode.MODE_IDLE robot_state.mode = robot_mode robot_state.battery_percent = 100.0 robot_location = Location() robot_location.x = x robot_location.y = y robot_location.yaw = yaw robot_location.level_name = level robot_state.location = robot_location return robot_state
def update_internal_robot_state(self, api_response=None): """Update internal robot state message. Does not publish!""" # NOTE(CH3): You might need to use robot.mir_name depending # on whether you want to use the config yaml name or MiR server # name whereever the FleetState message is intended to be used robot_state = RobotState() # Temporary msg to avoid race conditions robot_state.name = self.name # NOTE(CH3): Presuming model here means robot model, not sim model robot_state.model = "MiR100" if self.dry_run: self.robot_state = robot_state return try: if api_response is None: api_response = self.mir_api.status_get() now_sec, now_ns = math.modf( self.node.get_clock().now().seconds_nanoseconds()) # Populate Location message rmf_location = Location() rmf_location.x, rmf_location.y, rmf_location.yaw = ( self.get_position(rmf=True, api_response=api_response)) rmf_location.level_name = self.rmf_map_name # Populate RobotState message robot_state.task_id = str(self.current_task_id) robot_state.battery_percent = api_response.battery_percentage robot_state.location = rmf_location robot_state.path = self.rmf_robot_state_path_locations robot_state.location.t.sec = now_sec robot_state.location.t.nanosec = now_ns if api_response.mission_text.startswith('Charging'): # Charging robot_state.mode.mode = RobotMode.MODE_CHARGING self.mir_state = MiRState.READY elif api_response.state_id == MiRState.PAUSE: # Paused/Pre-empted self.pause() robot_state.mode.mode = RobotMode.MODE_PAUSED self.mir_state = MiRState.PAUSE elif (api_response.state_id == MiRState.EXECUTING # Moving and not api_response.mission_text.startswith('Charging')): self.resume() robot_state.mode.mode = RobotMode.MODE_MOVING self.mir_state = MiRState.EXECUTING elif api_response.state_id == MiRState.READY: # Idle/Moved self.resume() robot_state.mode.mode = RobotMode.MODE_IDLE self.mir_state = MiRState.READY if self.rmf_docking_requested: if self.rmf_docking_executed: # Docked if api_response.state_id == MiRState.READY: robot_state.mode.mode = RobotMode.MODE_IDLE else: # Docking robot_state.mode.mode = RobotMode.MODE_DOCKING # Update internal RobotState self.robot_state = robot_state self.last_robot_state_update = ( self.node.get_clock().now().nanoseconds / 1e9) except ApiException as e: self.node.get_logger().warn('Exception when calling ' 'DefaultApi->status_get: %s\n' % e)
def pub_fleet(self): fleet_state = FleetState() fleet_state.name = self.FLEET_NAME now = time.time() now_sec = int(now) now_ns = int((now - now_sec) * 1e9) try: for robot in self.robots.values(): api_response = robot.api.status_get() robot_state = RobotState() robot_state.name = robot.name robot_state.task_id = robot.current_task_id robot_state.battery_percent = api_response.battery_percentage location = Location() location.x = api_response.position.x location.y = api_response.position.y location.yaw = api_response.position.orientation # TODO Transform yaw from MiR frame to RMF frame mir_pos = [location.x, location.y] rmf_pos = self.mir2rmf_transform.transform(mir_pos) rmf_location = Location() rmf_location.x = rmf_pos[0] rmf_location.y = rmf_pos[1] rmf_location.yaw = math.radians( location.yaw) + self.mir2rmf_transform.get_rotation() robot_state.location = rmf_location robot_state.path = robot.remaining_path robot_state.location.t.sec = now_sec robot_state.location.t.nanosec = now_ns if api_response.mission_text.startswith('Charging'): robot_state.mode.mode = RobotMode.MODE_CHARGING robot.mode = MirState.READY elif api_response.state_id == MirState.PAUSE: robot_state.mode.mode = RobotMode.MODE_PAUSED robot.mode = MirState.PAUSE elif api_response.state_id == MirState.EXECUTING and \ not api_response.mission_text.startswith('Charging'): robot_state.mode.mode = RobotMode.MODE_MOVING robot.mode = MirState.EXECUTING elif api_response.state_id == MirState.READY: robot_state.mode.mode = RobotMode.MODE_IDLE robot.mode = MirState.READY # print(f'[{api_response.state_id}] [{api_response.state_text}] [{api_response.mission_text}]') fleet_state.robots.append(robot_state) if robot.docking_requested: if not robot.docking_executed: robot.docking_executed = ( 'docking' in api_response.mission_text.lower()) if robot.docking_executed and api_response.state_id == MirState.READY: robot_state.mode.mode = RobotMode.MODE_IDLE else: robot_state.mode.mode = RobotMode.MODE_DOCKING self.status_pub.publish(fleet_state) except ApiException as e: self.get_logger().warn('Exception when calling \ DefaultApi->status_get: %s\n' % e)