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)