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)