diff --git a/free_fleet_adapter/free_fleet_adapter/fleet_adapter.py b/free_fleet_adapter/free_fleet_adapter/fleet_adapter.py index 662ad650..ed178d73 100644 --- a/free_fleet_adapter/free_fleet_adapter/fleet_adapter.py +++ b/free_fleet_adapter/free_fleet_adapter/fleet_adapter.py @@ -218,7 +218,7 @@ def update_robot(robot: Nav2RobotAdapter): return None state = rmf_easy.RobotState( - robot.map, + robot.get_map_name(), robot_pose, robot.get_battery_soc() ) diff --git a/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py b/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py index f6cce076..ada63922 100644 --- a/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py +++ b/free_fleet_adapter/free_fleet_adapter/nav2_robot_adapter.py @@ -108,20 +108,16 @@ def __init__( fleet_handle, tf_buffer ): - RobotAdapter.__init__(self) + RobotAdapter.__init__(self, name, node, fleet_handle) - self.name = name self.execution = None - self.update_handle = None self.configuration = configuration self.robot_config_yaml = robot_config_yaml - self.node = node self.zenoh_session = zenoh_session - self.fleet_handle = fleet_handle self.tf_buffer = tf_buffer self.nav_goal_id = None - self.map = self.robot_config_yaml['initial_map'] + self.map_name = self.robot_config_yaml['initial_map'] # TODO(ac): Only use full battery if sim is indicated self.battery_soc = 1.0 @@ -149,6 +145,9 @@ def _battery_state_callback(sample: zenoh.Sample): def get_battery_soc(self) -> float: return self.battery_soc + def get_map_name(self) -> str: + return self.map_name + def get_pose(self) -> Annotated[list[float], 3] | None: transform = self.tf_handler.get_transform() if transform is None: @@ -258,12 +257,12 @@ def _handle_navigate_to_pose( z: float, yaw: float ): - if map_name != self.map: + if map_name != self.map_name: # TODO(ac): test this map related replanning behavior self.replan_counts += 1 self.node.get_logger().error( f'Destination is on map [{map_name}], while robot ' - f'[{self.name}] is on map [{self.map}], replan count ' + f'[{self.name}] is on map [{self.map_name}], replan count ' f'[{self.replan_counts}]' ) diff --git a/free_fleet_adapter/free_fleet_adapter/robot_adapter.py b/free_fleet_adapter/free_fleet_adapter/robot_adapter.py index 943e7e31..91a34261 100644 --- a/free_fleet_adapter/free_fleet_adapter/robot_adapter.py +++ b/free_fleet_adapter/free_fleet_adapter/robot_adapter.py @@ -24,6 +24,17 @@ class RobotAdapter(ABC): """Abstract Robot Adapter to be used by the free fleet adapter.""" + def __init__( + self, + name: str, + node, + fleet_handle + ): + self.name = name + self.node = node + self.fleet_handle = fleet_handle + self.update_handle = None + """ This method returns the battery state of charge as a float, with value between 0 and 1.0. @@ -32,6 +43,14 @@ class RobotAdapter(ABC): def get_battery_soc(self) -> float: ... + """ + This method returns the name of the current map that the robot is + localized on. + """ + @abstractmethod + def get_map_name(self) -> str: + ... + """ This method returns the last known 2D position in meters and orientation (yaw) of the robot in radians as a list of 3 floats, in the form of