def __init__(self,
robot_name: str,
fleet_name: str,
initial_x: float = 0.0,
initial_y: float = 0.0,
initial_yaw: float = 0.0,
initial_level: str = 'L1'):
super().__init__(f'{fleet_name}_{robot_name}_sim_node')
self.robot_properties = RobotProperties(robot_name, fleet_name)
self.robot_properties.load_from_node_parameters(self)
self.nav_graph = Utils.load_nav_graph(
self.robot_properties.nav_graph_file)
self.robot_state = Utils.set_robot_state(
RobotState(), self.robot_properties.robot_name, initial_x,
initial_y, initial_yaw, initial_level)
Utils.logger.info(f'Robot Properties: {self.robot_properties}')
# ROS2
self.state_pub = self.create_publisher(
RobotState, self.robot_properties.update_topic, 10)
self.request_sub = self.create_subscription(
PathRequest, self.robot_properties.path_request_topic,
self.path_request_cb, 1)
self.next_loc_sub = self.create_subscription(
String, self.robot_properties.next_location_topic,
self.next_loc_cb, 1)
self.update_timer = self.create_timer(
self.robot_properties.update_period, self.update_timer_cb)
self.travel_timer = self.create_timer(
self.robot_properties.update_period, self.travel_timer_cb)
self.travelling = False
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 make_robot_state(name: str = "test_robot") -> RobotState:
return RobotState(
name=name,
model="test_model",
task_id="",
seq=0,
mode=RobotMode(mode=RobotMode.MODE_IDLE, mode_request_id=0),
battery_percent=0.0,
location=Location(
t=Time(sec=0, nanosec=0),
x=0.0,
y=0.0,
yaw=0.0,
level_name="test_level",
index=0,
),
path=[],
)
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 __init__(self,
name,
node,
rmf_graph,
robot_state_update_frequency=1,
dry_run=False):
adpt.RobotCommandHandle.__init__(self)
self.name = name # Name of robot object in config yaml
self.node = node
self.dry_run = dry_run # For testing only. Disables REST calls.
self.paused = False
self.paused_path = []
# Robot State =========================================================
self.robot_state = RobotState()
self.last_robot_state_update = -1
self.robot_state_update_frequency = robot_state_update_frequency
# NOTE(CH3): This is a naively monotonically increasing task counter.
#
# There is no interface to get the task request message ID!
# Instead I am just following the behaviour from rmf_core's
# full_control adapter.
self.current_task_id = 0
self.transforms = {'rmf_to_mir': None, 'mir_to_rmf': None}
# RMF Variables =======================================================
self.rmf_updater = None
self.rmf_graph = rmf_graph
self.rmf_lane_dict = {} # Maps entry, exit to lane index
self.rmf_map_name = ""
# This is made out of RMF Plan Waypoints
self.rmf_remaining_path_waypoints = []
# NOTE(CH3): This is required for fleet state publishing
# The path is in reverse order! (i.e. [last, ... first])
# This is made out of RMF Location messages
self.rmf_robot_state_path_locations = []
# Populate lane dict
for i in range(self.rmf_graph.num_lanes):
graph_lane = self.rmf_graph.get_lane(i)
id_tuple = (graph_lane.entry.waypoint_index,
graph_lane.exit.waypoint_index)
self.rmf_lane_dict[id_tuple] = graph_lane
# RMF Location Trackers ===============================================
self.rmf_current_lane_index = None # None when moving
self.rmf_current_waypoint_index = None
self.rmf_target_waypoint_index = None # None when not moving
# RMF Execution Flags =================================================
self.rmf_docking_executed = False
self.rmf_docking_requested = False
self.rmf_path_requested = False
# MiR Variables =======================================================
self.mir_name = "" # Name of robot on MiR REST server
self.mir_missions = {} # MiR Mission Name-GUID Dict
self.mir_positions = {} # MiR Place Name-GUID Dict
self.mir_api = None # MiR REST API
self.mir_state = MiRState.PAUSE
# Thread Management ===================================================
# Path queue execution thread
self._path_following_thread = None
self._path_quit_event = threading.Event()
self._path_quit_cv = threading.Condition()
# Dock queue execution thread
self._docking_thread = None
self._docking_quit_event = threading.Event()
self._docking_quit_cv = threading.Condition()
# Start State Update Timer ============================================
self.state_update_timer = self.node.create_timer(
self.robot_state_update_frequency,
self.update_internal_robot_state)
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)