From 4b0559259e14f31ea937afe4923c7f151767e135 Mon Sep 17 00:00:00 2001 From: youliang Date: Thu, 7 Apr 2022 11:10:08 +0800 Subject: [PATCH 01/16] revamp v2 Signed-off-by: youliang --- .gitignore | 1 + fleet_adapter_ecobot/EcobotClientAPI.py | 25 +- fleet_adapter_ecobot/EcobotCommandHandle.py | 259 ++++++++++++------- fleet_adapter_ecobot/TestClientAPI.py | 136 ++++++++++ fleet_adapter_ecobot/fleet_adapter_ecobot.py | 244 +++++++++-------- fleet_adapter_ecobot/utils.py | 139 ++++++++++ 6 files changed, 599 insertions(+), 205 deletions(-) create mode 100644 .gitignore create mode 100644 fleet_adapter_ecobot/TestClientAPI.py create mode 100644 fleet_adapter_ecobot/utils.py diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..bee8a64 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +__pycache__ diff --git a/fleet_adapter_ecobot/EcobotClientAPI.py b/fleet_adapter_ecobot/EcobotClientAPI.py index b74712b..39ef4b9 100644 --- a/fleet_adapter_ecobot/EcobotClientAPI.py +++ b/fleet_adapter_ecobot/EcobotClientAPI.py @@ -140,6 +140,10 @@ def navigate_to_waypoint(self, waypoint_name, map_name): def start_clean(self, name:str, map_name:str): ''' Returns True if the robot has started the cleaning process, else False''' + return self.start_task(self, name, map_name) + + + def start_task(self, name:str, map_name:str): # we first get the relevant task queue and then start a new task data = {} response = self.task_queues(map_name) @@ -213,6 +217,23 @@ def stop(self): print(f"Other error: {err}") return False + def current_map(self): + url = self.prefix + f"/gs-robot/real_time_data/robot_status" + try: + response = requests.get(url, timeout=self.timeout) + response.raise_for_status() + if self.debug: + # print(f"Response: \n {response.json()}") + print(json.dumps(response.json(), indent=2)) + + return response.json()["data"]["robotStatus"]["map"]["name"] + except HTTPError as http_err: + print(f"HTTP error: {http_err}") + except Exception as err: + print(f"Other error: {err}") + return None + + def data(self): url = self.prefix + f"/gs-robot/data/device_status" try: @@ -273,7 +294,7 @@ def is_task_queue_finished(self): def navigation_completed(self): return self.is_task_queue_finished() - def docking_completed(self): + def task_completed(self): ''' For ecobots the same function is used to check completion of navigation & cleaning''' return self.is_task_queue_finished() @@ -297,3 +318,5 @@ def set_cleaning_mode(self, cleaning_config:str): except Exception as err: print(f"Other error: {err}") return False + + ## TODO: check is charge diff --git a/fleet_adapter_ecobot/EcobotCommandHandle.py b/fleet_adapter_ecobot/EcobotCommandHandle.py index 33d617d..4a00d28 100644 --- a/fleet_adapter_ecobot/EcobotCommandHandle.py +++ b/fleet_adapter_ecobot/EcobotCommandHandle.py @@ -29,8 +29,7 @@ from datetime import timedelta -from .EcobotClientAPI import EcobotAPI - +from typing import Optional, Tuple # States for EcobotCommandHandle's state machine used when guiding robot along # a new path @@ -60,7 +59,7 @@ def __init__(self, vehicle_traits, transforms, map_name, - start, + rmf_map_name, position, charger_waypoint, update_frequency, @@ -74,6 +73,8 @@ def __init__(self, self.vehicle_traits = vehicle_traits self.transforms = transforms self.map_name = map_name + self.rmf_map_name = rmf_map_name + # Get the index of the charger waypoint waypoint = self.graph.find_waypoint(charger_waypoint) # assert waypoint, f"Charger waypoint {charger_waypoint} does not exist in the navigation graph" @@ -89,7 +90,6 @@ def __init__(self, self.position = position # (x,y,theta) in RMF coordinates (meters, radians) self.initialized = False self.state = EcobotState.IDLE - self.dock_name = "" self.adapter = adapter self.requested_waypoints = [] # RMF Plan waypoints @@ -115,17 +115,10 @@ def __init__(self, self._dock_thread = None self._quit_dock_event = threading.Event() - print(f"{self.name} is starting at: [{self.position[0]:.2f}, {self.position[1]:.2f}, {self.position[2]:.2f}") + self.action_execution = None + self.participant = None - # Update tracking variables - if start.lane is not None: # If the robot is on a lane - print(f"Start lane index:{start.lane}") - self.last_known_lane_index = start.lane - self.on_lane = start.lane - # self.last_known_waypoint_index = start.waypoint - else: # Otherwise, the robot is on a waypoint - self.last_known_waypoint_index = start.waypoint - self.on_waypoint = start.waypoint + print(f"{self.name} is starting at: [{self.position[0]:.2f}, {self.position[1]:.2f}, {self.position[2]:.2f}") self.state_update_timer = self.node.create_timer( 1.0 / self.update_frequency, @@ -134,12 +127,11 @@ def __init__(self, # The Ecobot robot has various cleaning modes. Here we turn off the # cleaning systems. These will be activated only during cleaning self.api.set_cleaning_mode(self.config['inactive_cleaning_config']) - self.initialized = True def clear(self): with self._lock: - print("Clearing internal states") + print(f"Clearing internal states with current waypoint {self.on_waypoint}") self.requested_waypoints = [] self.remaining_waypoints = [] self.path_finished_callback = None @@ -148,6 +140,7 @@ def clear(self): # self.target_waypoint = None self.state = EcobotState.IDLE + # override function def stop(self): # Stop motion of the robot. Tracking variables should remain unchanged. while True: @@ -156,6 +149,7 @@ def stop(self): break time.sleep(1.0) + # override function def follow_new_path( self, waypoints, @@ -231,12 +225,8 @@ def _follow_path(): self.path_index = self.remaining_waypoints[0].index # Move robot to next waypoint target_pose = self.target_waypoint.position - [x,y] = self.transforms["rmf_to_ecobot"].transform(target_pose[:2]) - theta = math.degrees(target_pose[2] + self.transforms['orientation_offset']) - if theta > 180.0: - theta = 360.0 - theta - if theta < -180.0: - theta = 360.0 + theta + [x,y, yaw] = self.transforms.to_robot_map(target_pose[:3]) + theta = math.degrees(yaw) print(f"Requesting robot to navigate to " f"[{self.path_index}][{x:.0f},{y:.0f},{theta:.0f}] " @@ -293,7 +283,8 @@ def _follow_path(): # waypoint or the target one if self.target_waypoint.graph_index is not None and self.dist(self.position, target_pose) < 0.5: self.on_waypoint = self.target_waypoint.graph_index - elif self.last_known_waypoint_index is not None and self.dist(self.position, self.graph.get_waypoint(self.last_known_waypoint_index).location) < 0.5: + elif self.last_known_waypoint_index is not None and \ + self.dist(self.position, self.graph.get_waypoint(self.last_known_waypoint_index).location) < 0.5: self.on_waypoint = self.last_known_waypoint_index else: self.on_lane = None # update_off_grid() @@ -316,6 +307,7 @@ def _follow_path(): target=_follow_path) self._follow_path_thread.start() + # override function def dock( self, dock_name, @@ -325,79 +317,40 @@ def dock( if self._dock_thread is not None: self._dock_thread.join() - self.dock_name = dock_name assert docking_finished_callback is not None self.docking_finished_callback = docking_finished_callback # Get the waypoint that the robot is trying to dock into. The dock_name and waypoint_name must match - dock_waypoint = self.graph.find_waypoint(self.dock_name) - assert(dock_waypoint) - self.dock_waypoint_index = dock_waypoint.index + self.node.get_logger().info(f"[DOCK] Start docking to charger with dock param: {dock_name}") + # NOTE: Now only assume that dock is only used by during charging. + self.dock_waypoint_index = self.charger_waypoint_index + self.on_waypoint = self.dock_waypoint_index def _dock(): + # TODO, clean up implementation of dock # Check if the dock waypoint is a charger or cleaning zone and call the # appropriate API. Charger docks should have dock_name as "charger_" - is_cleaning = False # todo [YV]: Map dock names to API callbacks instead of checking substrings - if (dock_name[:7] == "charger"): - while True: - self.node.get_logger().info(f"Requesting robot {self.name} to charge at {self.dock_name}") - # The Ecobot75 requires a TF path to dock. This path is - # named the same as the dock name (charger_ecobot75_x) - if self.name[:8] == "ecobot75": - self.api.set_cleaning_mode(self.config['inactive_cleaning_config']) - if self.api.start_clean(self.dock_name, self.map_name): - break - # For Ecobot40, we will use the navigate_to_waypoint API to - # dock into the charger - else: - if self.api.navigate_to_waypoint(self.dock_name, self.map_name): - break - time.sleep(1.0) - else: - is_cleaning = True - while True: - self.node.get_logger().info(f"Requesting robot {self.name} to clean {self.dock_name}") - self.api.set_cleaning_mode(self.config['active_cleaning_config']) - if self.api.start_clean(self.dock_name, self.map_name): + while True: + self.node.get_logger().info(f"Requesting robot {self.name} to charge at {dock_name}") + # The Ecobot75 requires a TF path to dock to charging location. This path is + # named the same as the dock name (charger_ecobot75_x) + if self.name[:8] == "ecobot75": + self.api.set_cleaning_mode(self.config['inactive_cleaning_config']) + if self.api.start_task(dock_name, self.map_name): break - time.sleep(1.0) - with self._lock: - self.on_waypoint = None - self.on_lane = None - time.sleep(1.0) - dock_positions = copy.copy(self.config["docks"].get(self.dock_name)) - while (not self.api.docking_completed()): + # For Ecobot40 and 50, we will use the navigate_to_waypoint API to + # dock into the charger + else: + if self.api.navigate_to_waypoint(dock_name, self.map_name): + break + time.sleep(1.0) + while (not self.api.task_completed()): # Check if we need to abort if self._quit_dock_event.is_set(): self.node.get_logger().info("Aborting docking") return time.sleep(0.5) - if (is_cleaning): - if dock_positions is None: - self.node.get_logger().error(f"[docking] dock positions undefined for dock_name:{self.dock_name}") - continue - if not dock_positions: - continue - participant = self.update_handle.get_unstable_participant() - if participant is not None: - # positions = [] - # positions.append(self.position) - # # The Ecobot API does not give us the list of waypoints remaining - # # in the cleaning path and hence we need to estimate this - # if (self.dist(dock_positions[0], self.position) < 1.0): - # dock_positions.pop(0) - # positions.extend(dock_positions) - positions = dock_positions - - trajectory = schedule.make_trajectory( - self.vehicle_traits, - self.adapter.now(), - positions) - route = schedule.Route(self.map_name,trajectory) - participant.set_itinerary([route]) - else: - self.node.get_logger().error("[docking] Unable to get participant") # Here we assume that the robot has successfully reached waypoint with name same as dock_name with self._lock: self.on_waypoint = self.dock_waypoint_index @@ -414,14 +367,8 @@ def get_position(self): RMF coordinate frame''' position = self.api.position() if position is not None: - x,y = self.transforms['ecobot_to_rmf'].transform([position[0],position[1]]) - theta = math.radians(position[2]) - self.transforms['orientation_offset'] - # Wrap theta between [-pi, pi]. Very important else arrival estimate - # will assume robot has to do full rotations and delay the schedule - if theta > np.pi: - theta = theta - (2 * np.pi) - if theta < -np.pi: - theta = (2 * np.pi) + theta + x,y,theta = self.transforms.to_rmf_map([position[0],position[1], math.radians(position[2])]) + print(f"Convert pos from {position} grid coor to {x},{y}, {theta} rmf coor") return [x,y,theta] else: self.node.get_logger().error("Unable to retrieve position from robot. Returning last known position...") @@ -441,8 +388,37 @@ def update(self): if self.update_handle is not None: self.update_state() + # call this when starting cleaning execution + def _action_executor(self, + category: str, + description: dict, + execution: + adpt.robot_update_handle.ActionExecution): + with self._lock: + # only accept clean category + assert(category == "clean") + attempts = 0 + while attempts < 2: + self.node.get_logger().info(f"Requesting robot {self.name} to clean {description}") + self.api.set_cleaning_mode(self.config['active_cleaning_config']) + if self.api.start_clean(description["clean_task_name"], self.map_name): + self.start_clean_action_time = self.adapter.now() + self.on_waypoint = None + self.on_lane = None + self.action_execution = execution + # robot moves slower during cleaning + self.vehicle_traits.linear.nominal_velocity *= 0.2 + break + attempts+=1 + time.sleep(1.0) + self.node.get_logger().warn(f"Failed to initiate cleaning action for robot {self.name}") + # TODO: issue error ticket + execution.error("Failed to initiate cleaning action for robot {self.name}") + def update_state(self): self.update_handle.update_battery_soc(self.battery_soc) + + # only run this during init if not self.charger_is_set: if ("max_delay" in self.config.keys()): max_delay = self.config["max_delay"] @@ -452,14 +428,64 @@ def update_state(self): self.update_handle.set_charger_waypoint(self.charger_waypoint_index) else: self.node.get_logger().info("Invalid waypoint supplied for charger. Using default nearest charger in the map") + self.update_handle.set_action_executor(self._action_executor) self.charger_is_set = True - # Update position + self.participant = self.update_handle.get_unstable_participant() + + # Update states and positions with self._lock: - if (self.on_waypoint is not None): # if robot is on a waypoint - # print(f"[update] Calling update_off_grid_position() on waypoint with pose[{self.position[0]:.2f}, {self.position[1]:.2f}, {self.position[2]:.2f}] and waypoint[{self.on_waypoint}]") - # self.update_handle.update_off_grid_position( - # self.position, self.on_waypoint) - print(f"[update] Calling update_current_waypoint() on waypoint with pose[{self.position[0]:.2f}, {self.position[1]:.2f}, {self.position[2]:.2f}] and waypoint[{self.on_waypoint}]") + if (self.action_execution): + print("Executing action, cleaning state") + # check if action is completed/killed/canceled + action_ok = self.action_execution.okay() + if self.api.task_completed() or not action_ok: + if action_ok: + self.node.get_logger().info("Cleaning is completed") + self.action_execution.finished() + else: + self.node.get_logger().warn("cleaning task is killed/canceled") + self.api.set_cleaning_mode(self.config['inactive_cleaning_config']) + self.action_execution = None + self.start_clean_action_time = None + self.vehicle_traits.linear.nominal_velocity *= 5 # change back vel + else: + assert(self.participant) + assert(self.start_clean_action_time) + total_action_time = timedelta(hours=1.0) #TODO: populate actual total time`` + remaining_time = total_action_time - (self.adapter.now() - self.start_clean_action_time) + print(f"Still cleaning =) Estimated remaining time: [{remaining_time}]") + self.action_execution.update_remaining_time(remaining_time) + + # create a short segment of the trajectory according to robot current heading + _x, _y, _theta = self.position + fake_pos = [_x + math.cos(_theta)*2.5, _y + math.sin(_theta)*2.5, _theta] + positions = [self.position, fake_pos] + + # update robot current location with lost position, TODO: fix this as it is + # using the front() of the starts list, it the first start might not be the nearest + # self.update_handle.update_lost_position( + # self.rmf_map_name, self.position, + # max_merge_waypoint_distance = 1.0, max_merge_lane_distance=15.0) + + ## Get Closest point on graph and update location + closest_wp = self.get_closest_waypoint_idx(self.position) + if closest_wp: + self.update_handle.update_off_grid_position( + self.position, closest_wp) + else: + self.node.get_logger().error(f"Cant find closest waypoint!") + self.update_handle.update_off_grid_position( + self.position, self.target_waypoint.graph_index) + + trajectory = schedule.make_trajectory( + self.vehicle_traits, + self.adapter.now(), + positions) + route = schedule.Route(self.rmf_map_name,trajectory) + self.participant.set_itinerary([route]) + elif (self.on_waypoint is not None): # if robot is on a waypoint + print(f"[update] Calling update_current_waypoint() on waypoint with " \ + f"pose[{self.position_str()}] and waypoint[{self.on_waypoint}]") self.update_handle.update_current_waypoint( self.on_waypoint, self.position[2]) elif (self.on_lane is not None): # if robot is on a lane @@ -474,21 +500,55 @@ def update_state(self): lane_indices = [self.on_lane] if reverse_lane is not None: # Unidirectional graph lane_indices.append(reverse_lane.index) - print(f"[update] Calling update_current_lanes() with pose[{self.position[0]:.2f}, {self.position[1]:.2f}, {self.position[2]:.2f}] and lanes:{lane_indices}") + print(f"[update] Calling update_current_lanes() with pose[{self.position_str()}] "\ + f"and lanes:{lane_indices}") self.update_handle.update_current_lanes( self.position, lane_indices) elif (self.dock_waypoint_index is not None): - print(f"[update] Calling update_off_grid_position() dock with pose[{self.position[0]:.2f}, {self.position[1]:.2f}, {self.position[2]:.2f}] and waypoint[{self.dock_waypoint_index}]") + print(f"[update] Calling update_off_grid_position() dock with pose" \ + f"[{self.position_str()}] and waypoint[{self.dock_waypoint_index}]") self.update_handle.update_off_grid_position( self.position, self.dock_waypoint_index) elif (self.target_waypoint is not None and self.target_waypoint.graph_index is not None): # if robot is merging into a waypoint - print(f"[update] Calling update_off_grid_position() with pose[{self.position[0]:.2f}, {self.position[1]:.2f}, {self.position[2]:.2f}] and waypoint[{self.target_waypoint.graph_index}]") + print(f"[update] Calling update_off_grid_position() with pose " \ + f"[{self.position_str()}] and waypoint[{self.target_waypoint.graph_index}]") self.update_handle.update_off_grid_position( self.position, self.target_waypoint.graph_index) else: # if robot is lost print("[update] Calling update_lost_position()") self.update_handle.update_lost_position( - self.map_name, self.position) + self.rmf_map_name, self.position) + + def position_str(self): + return f"{self.position[0]:.2f}, {self.position[1]:.2f}, {self.position[2]:.2f}" + + def get_closest_waypoint_idx( + self, position: Tuple[float, float, float], + max_merge_lane_distance = 40.0 + ) -> Optional[int]: + """ + Find closest graph waypoint to the provided position, return waypoint idx + """ + starts = plan.compute_plan_starts( + self.graph, + self.rmf_map_name, + position, + self.adapter.now(), + max_merge_waypoint_distance = 1.0, + max_merge_lane_distance = max_merge_lane_distance) + if not starts: + return None + cx, cy, _ = position + closest_idx = -1 + closest_dist_sq = max_merge_lane_distance**2 + for s in starts: + idx = s.waypoint + x, y = self.graph.get_waypoint(idx).location + dist_sq = (x-cx)**2 + (y-cy)**2 + if (dist_sq < closest_dist_sq): + closest_idx = idx + closest_dist_sq = dist_sq + return closest_idx def get_current_lane(self): def projection(current_position, @@ -519,6 +579,7 @@ def projection(current_position, return lane_index return None + # TODO: remove the usage of sqrt for efficiency def dist(self, A, B): ''' Euclidian distance between A(x,y) and B(x,y)''' assert(len(A) > 1) diff --git a/fleet_adapter_ecobot/TestClientAPI.py b/fleet_adapter_ecobot/TestClientAPI.py new file mode 100644 index 0000000..e0017bc --- /dev/null +++ b/fleet_adapter_ecobot/TestClientAPI.py @@ -0,0 +1,136 @@ +# Copyright 2022 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import json +import math +from urllib.error import HTTPError + +class ClientAPI: + def __init__(self): + self.connected = True + self.fake_clean_path = [ + [989.96853, 1136.82267, 0], + [1056.7065, 1087.30105, 180], + [1056.7065, 1087.30105, 150], + [1056.7065, 1087.30105, 130], + [1027.0187, 1271.659514, -60], + [1027.0187, 1271.659514, -30], + [1027.0187, 1271.659514, -100], + [1027.0187, 1271.659514, -60], + [1027.0187, 671.659514, -30], + [1027.0187, 671.659514, -60], + [1027.0187, 671.659514, -100], + [1027.0187, 671.659514, -60], + [1027.0187, 671.659514, -30], + [500.0187, 905.659514, 0], + [500.0187, 905.659514, 20], + [500.0187, 905.659514, 40], + [500.0187, 905.659514, 60], + [500.0187, 905.659514, 80], + [500.0187, 905.659514, 100], + [936.238485, 981.95061, 0], + [909.963465, 1139.7444, 180] + ] + self.is_fake_cleaning = False + self.clean_wp_idx = 0 + print("[TEST ECOBOT CLIENT API] successfully setup fake client api class") + self.connected = True + # self.fake_location = [977, 1372, 0 ] + self.fake_location = [977.5834309628409, 1192.0576445043025, 0] + + def position(self): + ''' Returns [x, y, theta] expressed in the robot's coordinate frame or None''' + if self.is_fake_cleaning: + print(f"[TEST CLIENT API] provide fake cleaning position") + return self.fake_clean_path[self.clean_wp_idx] + print(f"[TEST CLIENT API] provide position [{self.fake_location}]") + return self.fake_location + + def navigate(self, pose, map_name:str): + ''' Here pose is a list containing (x,y,theta) where x and y are in grid + coordinate frame and theta is in degrees. This functions should + return True if the robot received the command, else False''' + assert(len(pose) > 2) + self.fake_location = pose + print(f"[TEST CLIENT API] moved to fake location {pose}") + return True + + def navigate_to_waypoint(self, waypoint_name, map_name): + ''' Ask the robot to navigate to a preconfigured waypoint on a map. + Returns True if the robot received the command''' + print(f"[TEST CLIENT API] moved to fake waypoint {waypoint_name}") + self.fake_location = [977, 1372, 0 ] # assume original charging waypoint + self.attempts = 0 + return True + + def start_task(self, name:str, map_name:str): + ''' Returns True if the robot has started the generic task, else False''' + print(f"[TEST CLIENT API] Start fake task : {name}") + self.fake_location = [977, 1372, 0 ] # assume original charging waypoint + return True + + def start_clean(self, name:str, map_name:str): + ''' Returns True if the robot has started the cleaning process, else False''' + print(f"[TEST CLIENT API] Set fake start CLEANING : {name}") + self.is_fake_cleaning = True + return True + + def pause(self): + print(f"[TEST CLIENT API] Pause Robot") + self.is_fake_cleaning = True + return True + + def resume(self): + print(f"[TEST CLIENT API] Resume Robot") + self.is_fake_cleaning = True + return True + + def stop(self): + ''' Returns true if robot was successfully stopped; else False''' + print(f"[TEST CLIENT API] STOP TASK##") + return True + + def navigation_completed(self): + print(f"[TEST CLIENT API] FAKE NAV completed") + return True + + def task_completed(self): + ''' For ecobots the same function is used to check completion of navigation & cleaning''' + if not self.is_fake_cleaning: # note, if immediately, the robot will head back to staging point + if self.attempts < 4: + print(f"[TEST CLIENT API] Fake task in process") + self.attempts += 1 + return False + else: + print(f"[TEST CLIENT API] No fake task in process") + return True + self.clean_wp_idx += 1 + if self.clean_wp_idx < len(self.fake_clean_path): + print(f"[TEST CLIENT API] FAKE DOCK/CLEAN in process") + return False + else: + self.clean_wp_idx = 0 + self.is_fake_cleaning = False + print(f"[TEST CLIENT API] FAKE DOCK/CLean completed") + return True + + def battery_soc(self): + print(f"[TEST CLIENT API] get fake battery 100%") + return 1.0 + + def set_cleaning_mode(self, cleaning_config:str): + print(f"[TEST CLIENT API] Set fake CLEANING MODE: {cleaning_config}") + return True + + ## TODO: check is charge diff --git a/fleet_adapter_ecobot/fleet_adapter_ecobot.py b/fleet_adapter_ecobot/fleet_adapter_ecobot.py index b2df92f..6377e2f 100644 --- a/fleet_adapter_ecobot/fleet_adapter_ecobot.py +++ b/fleet_adapter_ecobot/fleet_adapter_ecobot.py @@ -36,11 +36,12 @@ from .EcobotCommandHandle import EcobotCommandHandle from .EcobotClientAPI import EcobotAPI +from .utils import RmfMapTransform #------------------------------------------------------------------------------ # Helper functions #------------------------------------------------------------------------------ -def initialize_fleet(config_yaml, nav_graph_path, node, use_sim_time, server_uri): +def initialize_fleet(config_yaml, nav_graph_path, node, server_uri, args): # Profile and traits fleet_config = config_yaml['rmf_fleet'] profile = traits.Profile(geometry.make_final_convex_circle( @@ -80,7 +81,7 @@ def initialize_fleet(config_yaml, nav_graph_path, node, use_sim_time, server_uri # Adapter adapter = adpt.Adapter.make('ecobot_fleet_adapter') - if use_sim_time: + if args.use_sim_time: adapter.node.use_sim_time() assert adapter, ("Unable to initialize ecobot adapter. Please ensure " @@ -93,11 +94,14 @@ def initialize_fleet(config_yaml, nav_graph_path, node, use_sim_time, server_uri if not fleet_config['publish_fleet_state']: fleet_handle.fleet_state_publish_period(None) + + task_capabilities_config = fleet_config['task_capabilities'] + # Account for battery drain drain_battery = fleet_config['account_for_battery_drain'] recharge_threshold = fleet_config['recharge_threshold'] recharge_soc = fleet_config['recharge_soc'] - finishing_request = fleet_config['task_capabilities']['finishing_request'] + finishing_request = task_capabilities_config['finishing_request'] node.get_logger().info(f"Finishing request: [{finishing_request}]") # Set task planner params ok = fleet_handle.set_task_planner_params( @@ -112,15 +116,15 @@ def initialize_fleet(config_yaml, nav_graph_path, node, use_sim_time, server_uri assert ok, ("Unable to set task planner params") task_capabilities = [] - if fleet_config['task_capabilities']['loop']: + if task_capabilities_config['loop']: node.get_logger().info( f"Fleet [{fleet_name}] is configured to perform Loop tasks") task_capabilities.append(TaskType.TYPE_LOOP) - if fleet_config['task_capabilities']['delivery']: + if task_capabilities_config['delivery']: node.get_logger().info( f"Fleet [{fleet_name}] is configured to perform Delivery tasks") task_capabilities.append(TaskType.TYPE_DELIVERY) - if fleet_config['task_capabilities']['clean']: + if task_capabilities_config['clean']: node.get_logger().info( f"Fleet [{fleet_name}] is configured to perform Clean tasks") task_capabilities.append(TaskType.TYPE_CLEAN) @@ -135,33 +139,43 @@ def _task_request_check(task_capabilities, msg: TaskProfile): fleet_handle.accept_task_requests( partial(_task_request_check, task_capabilities)) - # Transforms - rmf_coordinates = config_yaml['reference_coordinates']['rmf'] - ecobot_coordinates = config_yaml['reference_coordinates']['ecobot'] - transforms = { - 'rmf_to_ecobot': nudged.estimate(rmf_coordinates, ecobot_coordinates), - 'ecobot_to_rmf': nudged.estimate(ecobot_coordinates, rmf_coordinates)} - transforms['orientation_offset'] = transforms['rmf_to_ecobot'].get_rotation() - mse = nudged.estimate_error(transforms['rmf_to_ecobot'], - rmf_coordinates, - ecobot_coordinates) - print(f"Coordinate transformation error: {mse}") - print("RMF to Ecobot transform:") - print(f" rotation:{transforms['rmf_to_ecobot'].get_rotation()}") - print(f" scale:{transforms['rmf_to_ecobot'].get_scale()}") - print(f" trans:{transforms['rmf_to_ecobot'].get_translation()}") - print("Ecobot to RMF transform:") - print(f" rotation:{transforms['ecobot_to_rmf'].get_rotation()}") - print(f" scale:{transforms['ecobot_to_rmf'].get_scale()}") - print(f" trans:{transforms['ecobot_to_rmf'].get_translation()}") - def _consider(description: dict): + node.get_logger().warn( + f"Accepting action: {description} ") confirm = adpt.fleet_update_handle.Confirmation() confirm.accept() return confirm - # Configure this fleet to perform cleaning action - fleet_handle.add_performable_action("clean", _consider) + # Configure this fleet to perform action category + if 'action_categories' in task_capabilities_config: + for cat in task_capabilities_config['action_categories']: + node.get_logger().info( + f"Fleet [{fleet_name}] is configured" + f" to perform action of category [{cat}]") + fleet_handle.add_performable_action(cat, _consider) + + # use defined transfrom param if avail, else use ref coors + if "rmf_transform" in config_yaml: + tx, ty, r, s = config_yaml["rmf_transform"] + rmf_transform = RmfMapTransform(tx, ty, r, s) + mse = 0.0 # null + else: + rmf_transform = RmfMapTransform() + rmf_coordinates = config_yaml['reference_coordinates']['rmf'] + ecobot_coordinates = config_yaml['reference_coordinates']['ecobot'] + mse = rmf_transform.estimate(ecobot_coordinates, rmf_coordinates) + tx, ty, r, s = rmf_transform.to_robot_map_transform() + print(f"Coordinate transformation error: {mse}") + print("RMF to Ecobot transform:") + print(f" rotation:{r}") + print(f" scale:{s}") + print(f" trans:{tx}, {ty}") + + tx, ty, r, s = rmf_transform.to_rmf_map_transform() + print("Ecobot to RMF transform:") + print(f" rotation:{r}") + print(f" scale:{s}") + print(f" trans:{tx}, {ty}") def updater_inserter(cmd_handle, update_handle): """Insert a RobotUpdateHandle.""" @@ -177,85 +191,103 @@ def _add_fleet_robots(): for robot_name in list(missing_robots.keys()): node.get_logger().debug(f"Connecting to robot: {robot_name}") robot_config = missing_robots[robot_name]['ecobot_config'] - api = EcobotAPI(robot_config['base_url'], robot_config['cleaning_task_prefix']) + + if args.test_client_api: + from .TestClientAPI import ClientAPI + api = ClientAPI() + else: + api = EcobotAPI(robot_config['base_url'], robot_config['cleaning_task_prefix']) + if not api.connected: continue - position = api.position() - if position is not None and len(position) > 2: - node.get_logger().info(f"Initializing robot: {robot_name}") - rmf_config = missing_robots[robot_name]['rmf_config'] - initial_waypoint = rmf_config['start']['waypoint'] - initial_orientation = rmf_config['start']['orientation'] - - starts = [] - time_now = adapter.now() - - if (initial_waypoint is not None) and\ - (initial_orientation is not None): - node.get_logger().info( - f"Using provided initial waypoint " - "[{initial_waypoint}] " - f"and orientation [{initial_orientation:.2f}] to " - f"initialize starts for robot [{robot_name}]") - # Get the waypoint index for initial_waypoint - initial_waypoint_index = nav_graph.find_waypoint( - initial_waypoint).index - starts = [plan.Start(time_now, - initial_waypoint_index, - initial_orientation)] - else: - node.get_logger().info( - f"Running compute_plan_starts for robot: " - "{robot_name}") - starts = plan.compute_plan_starts( - nav_graph, - rmf_config['start']['map_name'], - position, - time_now) - - if starts is None or len(starts) == 0: - node.get_logger().error( - f"Unable to determine StartSet for {robot_name}") - continue - - robot = EcobotCommandHandle( - name=robot_name, - config=robot_config, - node=node, - graph=nav_graph, - vehicle_traits=vehicle_traits, - transforms=transforms, - map_name=rmf_config['start']['map_name'], - start=starts[0], - position=position, - charger_waypoint=rmf_config['charger']['waypoint'], - update_frequency=rmf_config.get( - 'robot_state_update_frequency', 1), - adapter=adapter, - api=api) - - if robot.initialized: - robots[robot_name] = robot - # Add robot to fleet - fleet_handle.add_robot(robot, - robot_name, - profile, - [starts[0]], - partial(updater_inserter, - robot)) - node.get_logger().info( - f"Successfully added new robot: {robot_name}") - - else: - node.get_logger().error( - f"Failed to initialize robot: {robot_name}") - - del missing_robots[robot_name] + # get robot coordinates and transform to rmf_coor + ecobot_pos = api.position() + if ecobot_pos is None: + node.get_logger().warn(f"Failed to get [{robot_name}] position") + continue + + node.get_logger().info(f"Initializing robot: {robot_name}") + rmf_config = missing_robots[robot_name]['rmf_config'] + initial_waypoint = rmf_config['start']['waypoint'] + initial_orientation = rmf_config['start']['orientation'] + + starts = [] + time_now = adapter.now() + + x,y,_ = rmf_transform.to_rmf_map([ecobot_pos[0],ecobot_pos[1], 0]) + position = [x, y, 0] + + # for backwards compatibility, check if rmf_map name is diff + # to robot's map name + if "rmf_map_name" not in rmf_config['start']: + rmf_map_name = rmf_config['start']['map_name'] + else: + rmf_map_name = rmf_config['start']['rmf_map_name'] + + if (initial_waypoint is not None) and\ + (initial_orientation is not None): + node.get_logger().info( + f"Using provided initial waypoint " + "[{initial_waypoint}] " + f"and orientation [{initial_orientation:.2f}] to " + f"initialize starts for robot [{robot_name}]") + # Get the waypoint index for initial_waypoint + initial_waypoint_index = nav_graph.find_waypoint( + initial_waypoint).index + starts = [plan.Start(time_now, + initial_waypoint_index, + initial_orientation)] else: - pass - node.get_logger().debug( - f"{robot_name} not found, trying again...") + node.get_logger().info( + f"Running compute_plan_starts for robot: " + f"{robot_name}, with pos: {position}") + starts = plan.compute_plan_starts( + nav_graph, + rmf_map_name, + position, + time_now) + + if starts is None or len(starts) == 0: + node.get_logger().error( + f"Unable to determine StartSet for {robot_name} " + f"with map {rmf_map_name}") + continue + + robot = EcobotCommandHandle( + name=robot_name, + config=robot_config, + node=node, + graph=nav_graph, + vehicle_traits=vehicle_traits, + transforms=rmf_transform, + map_name=rmf_config['start']['map_name'], + rmf_map_name=rmf_map_name, + position=position, + charger_waypoint=rmf_config['charger']['waypoint'], + update_frequency=rmf_config.get( + 'robot_state_update_frequency', 1), + adapter=adapter, + api=api) + + if robot.initialized: + robots[robot_name] = robot + # Add robot to fleet + fleet_handle.add_robot(robot, + robot_name, + profile, + starts, + partial(updater_inserter, + robot)) + node.get_logger().info( + f"Successfully added new robot: {robot_name}") + + else: + node.get_logger().error( + f"Failed to initialize robot: {robot_name}") + + del missing_robots[robot_name] + return add_robots = threading.Thread(target=_add_fleet_robots, args=()) @@ -283,6 +315,8 @@ def main(argv=sys.argv): help="URI of the api server to transmit state and task information.") parser.add_argument("--use_sim_time", action="store_true", help='Use sim time for testing offline, default: false') + parser.add_argument("--test_client_api", action="store_true", + help='Use Test Client api to test instead of ecobot api') args = parser.parse_args(args_without_ros[1:]) print(f"Starting ecobot fleet adapter...") @@ -309,8 +343,8 @@ def main(argv=sys.argv): config_yaml, nav_graph_path, node, - args.use_sim_time, - server_uri) + server_uri, + args) # Create executor for the command handle node rclpy_executor = rclpy.executors.SingleThreadedExecutor() diff --git a/fleet_adapter_ecobot/utils.py b/fleet_adapter_ecobot/utils.py new file mode 100644 index 0000000..16b8719 --- /dev/null +++ b/fleet_adapter_ecobot/utils.py @@ -0,0 +1,139 @@ +#!/usr/bin/env python3 + +# Copyright 2022 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from typing import List, Tuple, Optional + +import nudged +import numpy as np +import math +import sys + +class RmfMapTransform: + def __init__(self, tx=0.0, ty=0.0, theta=0.0, scale=1.0): + """ + set transformation from rmf map (parent frame) to robot map (child frame) + default no transformation + Parameters: + [tx, ty] translation; theta: radians; scale: robot map scale + """ + + s = math.cos(theta)*scale + r = math.sin(theta)*scale + self.to_rmf_tf = nudged.Transform(s, r, tx, ty) + tx, ty = self.to_rmf_tf.get_translation() + r = self.to_rmf_tf.get_rotation() + s = self.to_rmf_tf.get_scale() + self.__apply_inverse_mat() + + def estimate( + self, + robot_map_points: List[Tuple[float, float]], + rmf_map_points: List[Tuple[float, float]] + ) -> Optional[float]: + ''' + Compute transformation by providing 2 set of coordinates, rmf and robot map + + Parameters + robot_map_points: list of [x, y] 2D lists + rmf_map_points: list of [x, y] 2D lists + Return + mean square error, return None if invalid + ''' + + if len(rmf_map_points) != len(robot_map_points): + print("Err! Input sets have different number of waypoints") + return None + + self.to_rmf_tf = nudged.estimate(robot_map_points, rmf_map_points) + print(self.to_rmf_tf.tx, self.to_rmf_tf.ty) + + self.__apply_inverse_mat() + return nudged.estimate_error(self.to_rmf_tf, rmf_map_points, robot_map_points) + + def to_rmf_map( + self, + robot_coor: Tuple[float, float, float] + ) -> Tuple[float, float, float]: + ''' + Get coordinate from robot map to rmf map + + Parameters + robot_coor: coordinate [x, y, theta] in robot map + Return + coordinate [x, y, theta] in rmf map + ''' + + # Note: this is similar to mat mul of tf mat [3x3] and coor [3x1] + # mat = self.to_rmf_tf.get_matrix() + # x = np.matmul(np.array(mat), np.array([robot_coor[0],robot_coor[1], 1])) + x,y = self.to_rmf_tf.transform([robot_coor[0],robot_coor[1]]) + theta = robot_coor[2] + self.to_rmf_tf.get_rotation() + theta = (theta + math.pi) % (2*math.pi) - math.pi #convert to within range -pi, pi + return (x, y, theta) + + + def to_robot_map( + self, + rmf_coor: Tuple[float, float, float] + ) -> Tuple[float, float, float]: + ''' + Get coordinate from rmf map to robot map + + Parameters + rmf_coor: coordinate [x, y, theta] in rmf map + Return + coordinate [x, y, theta] in robot map + ''' + x,y = self.to_robot_tf.transform([rmf_coor[0],rmf_coor[1]]) + theta = rmf_coor[2] + self.to_robot_tf.get_rotation() + theta = (theta + math.pi) % (2*math.pi) - math.pi #convert to within range -pi, pi + return (x, y, theta) + + + def to_rmf_map_transform(self) -> Tuple[float, float, float, float]: + ''' + Get coordinate from robot map to rmf map + + Return + transformation [tx, ty, theta, scale] + ''' + tx, ty = self.to_rmf_tf.get_translation() + r = self.to_rmf_tf.get_rotation() + s = self.to_rmf_tf.get_scale() + return (tx, ty, r, s) + + + def to_robot_map_transform(self) -> Tuple[float, float, float, float]: + ''' + Get coordinate from rmf map to robot map + + Return + transformation [tx, ty, theta, scale] + ''' + tx, ty = self.to_robot_tf.get_translation() + r = self.to_robot_tf.get_rotation() + s = self.to_robot_tf.get_scale() + return (tx, ty, r, s) + + + def __apply_inverse_mat(self): + # calculate to robot tf + mat = self.to_rmf_tf.get_matrix() + np_mat = np.array(mat) + inv_mat = np.linalg.inv(np_mat) + + self.to_robot_tf = nudged.Transform( + inv_mat[0][0], inv_mat[1][0], inv_mat[0][2], inv_mat[1][2]) From 774a48a31bb6abfed0df5bbb0b1bdbe0889d7b95 Mon Sep 17 00:00:00 2001 From: youliang Date: Thu, 7 Apr 2022 15:50:33 +0800 Subject: [PATCH 02/16] fix clean action start Signed-off-by: youliang --- fleet_adapter_ecobot/EcobotClientAPI.py | 10 ++++++++-- fleet_adapter_ecobot/EcobotCommandHandle.py | 5 +++-- 2 files changed, 11 insertions(+), 4 deletions(-) diff --git a/fleet_adapter_ecobot/EcobotClientAPI.py b/fleet_adapter_ecobot/EcobotClientAPI.py index 39ef4b9..839fa11 100644 --- a/fleet_adapter_ecobot/EcobotClientAPI.py +++ b/fleet_adapter_ecobot/EcobotClientAPI.py @@ -140,7 +140,7 @@ def navigate_to_waypoint(self, waypoint_name, map_name): def start_clean(self, name:str, map_name:str): ''' Returns True if the robot has started the cleaning process, else False''' - return self.start_task(self, name, map_name) + return self.start_task(name, map_name) def start_task(self, name:str, map_name:str): @@ -319,4 +319,10 @@ def set_cleaning_mode(self, cleaning_config:str): print(f"Other error: {err}") return False - ## TODO: check is charge + def is_charging(self): + """Check if robot is charging, will return false if not charging, or not avail""" + response = self.data() + if response is not None: + return response["data"]["charge"] + else: + return False diff --git a/fleet_adapter_ecobot/EcobotCommandHandle.py b/fleet_adapter_ecobot/EcobotCommandHandle.py index 4a00d28..246afdf 100644 --- a/fleet_adapter_ecobot/EcobotCommandHandle.py +++ b/fleet_adapter_ecobot/EcobotCommandHandle.py @@ -255,7 +255,7 @@ def _follow_path(): # self.target_waypoint = None else: if self.path_index is not None: - self.node.get_logger().info(f"Waiting for {(waypoint_wait_time - time_now).seconds}s") + # self.node.get_logger().info(f"Waiting for {(waypoint_wait_time - time_now).seconds}s") self.next_arrival_estimator(self.path_index, timedelta(seconds=0.0)) @@ -402,13 +402,14 @@ def _action_executor(self, self.node.get_logger().info(f"Requesting robot {self.name} to clean {description}") self.api.set_cleaning_mode(self.config['active_cleaning_config']) if self.api.start_clean(description["clean_task_name"], self.map_name): + self.node.get_logger().warn(f"Robot {self.name} start cleaning action") self.start_clean_action_time = self.adapter.now() self.on_waypoint = None self.on_lane = None self.action_execution = execution # robot moves slower during cleaning self.vehicle_traits.linear.nominal_velocity *= 0.2 - break + return attempts+=1 time.sleep(1.0) self.node.get_logger().warn(f"Failed to initiate cleaning action for robot {self.name}") From 70a238254572a1a831af577dbeac0bc198abc098 Mon Sep 17 00:00:00 2001 From: youliang Date: Fri, 8 Apr 2022 15:02:37 +0800 Subject: [PATCH 03/16] robot status and more robust start point Signed-off-by: youliang --- fleet_adapter_ecobot/EcobotClientAPI.py | 13 ++++- fleet_adapter_ecobot/EcobotCommandHandle.py | 56 +++++++++++++++++--- fleet_adapter_ecobot/TestClientAPI.py | 15 +++++- fleet_adapter_ecobot/fleet_adapter_ecobot.py | 11 +++- 4 files changed, 82 insertions(+), 13 deletions(-) diff --git a/fleet_adapter_ecobot/EcobotClientAPI.py b/fleet_adapter_ecobot/EcobotClientAPI.py index 839fa11..a7df397 100644 --- a/fleet_adapter_ecobot/EcobotClientAPI.py +++ b/fleet_adapter_ecobot/EcobotClientAPI.py @@ -320,9 +320,18 @@ def set_cleaning_mode(self, cleaning_config:str): return False def is_charging(self): - """Check if robot is charging, will return false if not charging, or not avail""" + """Check if robot is charging, will return false if not charging, None if not avail""" response = self.data() if response is not None: return response["data"]["charge"] else: - return False + return None + + + def is_localize(self): + """Check if robot is localize, will return false if not charging, None if not avail""" + response = self.data() + if response is not None: + return response["data"]["locationStatus"] + else: + return None diff --git a/fleet_adapter_ecobot/EcobotCommandHandle.py b/fleet_adapter_ecobot/EcobotCommandHandle.py index 246afdf..f03500c 100644 --- a/fleet_adapter_ecobot/EcobotCommandHandle.py +++ b/fleet_adapter_ecobot/EcobotCommandHandle.py @@ -64,7 +64,18 @@ def __init__(self, charger_waypoint, update_frequency, adapter, - api): + api, + max_merge_lane_distance): + """ + :param config + robot config defined in yaml file + :param map_name + map name in ecobot system + :param rmf_map_name + map name in rmf + :param max_merge_lane_distance + means how far will the robot diverge from the defined graph + """ adpt.RobotCommandHandle.__init__(self) self.name = name self.config = config @@ -74,6 +85,7 @@ def __init__(self, self.transforms = transforms self.map_name = map_name self.rmf_map_name = rmf_map_name + self.max_merge_lane_distance = max_merge_lane_distance # Get the index of the charger waypoint waypoint = self.graph.find_waypoint(charger_waypoint) @@ -117,8 +129,10 @@ def __init__(self, self.action_execution = None self.participant = None + self.in_error = None + self.update_robot_status_count = 0 # currently will update robot status every 4 callback counts - print(f"{self.name} is starting at: [{self.position[0]:.2f}, {self.position[1]:.2f}, {self.position[2]:.2f}") + print(f"{self.name} is starting at: [{self.position_str()}]") self.state_update_timer = self.node.create_timer( 1.0 / self.update_frequency, @@ -212,7 +226,9 @@ def follow_new_path( def _follow_path(): target_pose = [] - while (self.remaining_waypoints or self.state == EcobotState.MOVING or self.state == EcobotState.WAITING): + while (self.remaining_waypoints or + self.state == EcobotState.MOVING or + self.state == EcobotState.WAITING): # Check if we need to abort if self._quit_path_event.is_set(): self.node.get_logger().info("Aborting previously followed path") @@ -241,7 +257,9 @@ def _follow_path(): # if self.on_waypoint is not None: # robot starts at a graph waypoint # self.last_known_waypoint_index = self.on_waypoint else: - self.node.get_logger().info(f"Robot {self.name} failed to navigate to [{x:.0f}, {y:.0f}, {theta:.0f}] grid coordinates. Retrying...") + self.node.get_logger().info( + f"Robot {self.name} failed to navigate to [{x:.0f}, {y:.0f}, {theta:.0f}]" \ + "grid coordinates. Retrying...") time.sleep(1.0) elif self.state == EcobotState.WAITING: @@ -414,6 +432,8 @@ def _action_executor(self, time.sleep(1.0) self.node.get_logger().warn(f"Failed to initiate cleaning action for robot {self.name}") # TODO: issue error ticket + # create_issue(() + self.in_error = True # TODO: toggle error back execution.error("Failed to initiate cleaning action for robot {self.name}") def update_state(self): @@ -433,6 +453,22 @@ def update_state(self): self.charger_is_set = True self.participant = self.update_handle.get_unstable_participant() + # Update robot state's status, this will show on the dashboard, update this every 4 callbacks + # TODO: in a seperate callback timer? + if (self.update_robot_status_count >= 4): + if self.api.is_charging(): + self.update_handle.override_status("charging") + elif self.in_error: + self.update_handle.override_status("error") + elif not self.api.is_localize(): + self.node.get_logger().warn(f"Robot {self.name} is not localized") + self.update_handle.override_status("error") + else: + self.update_handle.override_status(None) + self.update_robot_status_count = 0 + else: + self.update_robot_status_count += 1 + # Update states and positions with self._lock: if (self.action_execution): @@ -466,10 +502,11 @@ def update_state(self): # using the front() of the starts list, it the first start might not be the nearest # self.update_handle.update_lost_position( # self.rmf_map_name, self.position, - # max_merge_waypoint_distance = 1.0, max_merge_lane_distance=15.0) + # max_merge_waypoint_distance = 1.0, max_merge_lane_distance) ## Get Closest point on graph and update location - closest_wp = self.get_closest_waypoint_idx(self.position) + closest_wp = self.get_closest_waypoint_idx( + self.position, self.max_merge_lane_distance) if closest_wp: self.update_handle.update_off_grid_position( self.position, closest_wp) @@ -516,16 +553,21 @@ def update_state(self): self.update_handle.update_off_grid_position( self.position, self.target_waypoint.graph_index) else: # if robot is lost + # TODO: use get_closest_waypoint_idx() print("[update] Calling update_lost_position()") self.update_handle.update_lost_position( self.rmf_map_name, self.position) + ######################################################################## + ## Utils + ######################################################################## + def position_str(self): return f"{self.position[0]:.2f}, {self.position[1]:.2f}, {self.position[2]:.2f}" def get_closest_waypoint_idx( self, position: Tuple[float, float, float], - max_merge_lane_distance = 40.0 + max_merge_lane_distance = 10.0 ) -> Optional[int]: """ Find closest graph waypoint to the provided position, return waypoint idx diff --git a/fleet_adapter_ecobot/TestClientAPI.py b/fleet_adapter_ecobot/TestClientAPI.py index e0017bc..7624b5c 100644 --- a/fleet_adapter_ecobot/TestClientAPI.py +++ b/fleet_adapter_ecobot/TestClientAPI.py @@ -46,7 +46,7 @@ def __init__(self): self.clean_wp_idx = 0 print("[TEST ECOBOT CLIENT API] successfully setup fake client api class") self.connected = True - # self.fake_location = [977, 1372, 0 ] + self.dock_position = [977, 1372] self.fake_location = [977.5834309628409, 1192.0576445043025, 0] def position(self): @@ -78,6 +78,7 @@ def start_task(self, name:str, map_name:str): ''' Returns True if the robot has started the generic task, else False''' print(f"[TEST CLIENT API] Start fake task : {name}") self.fake_location = [977, 1372, 0 ] # assume original charging waypoint + self.is_at_dock = True return True def start_clean(self, name:str, map_name:str): @@ -133,4 +134,14 @@ def set_cleaning_mode(self, cleaning_config:str): print(f"[TEST CLIENT API] Set fake CLEANING MODE: {cleaning_config}") return True - ## TODO: check is charge + def is_charging(self): + """Check if robot is charging, will return false if not charging, None if not avail""" + dx, dy, = self.dock_position + x, y, _= self.fake_location + if (abs(x-dx) < 5.0 and abs(y-dy) < 5.0): + print(f"[TEST CLIENT API] Fake robot at dock, is charging") + return True + return False + + def is_localize(self): + return True diff --git a/fleet_adapter_ecobot/fleet_adapter_ecobot.py b/fleet_adapter_ecobot/fleet_adapter_ecobot.py index 6377e2f..c03aa7e 100644 --- a/fleet_adapter_ecobot/fleet_adapter_ecobot.py +++ b/fleet_adapter_ecobot/fleet_adapter_ecobot.py @@ -225,6 +225,10 @@ def _add_fleet_robots(): else: rmf_map_name = rmf_config['start']['rmf_map_name'] + # TODO: this is to find the robot location when offgrid + # add this to config + max_merge_lane_distance = 15.0 # meters + if (initial_waypoint is not None) and\ (initial_orientation is not None): node.get_logger().info( @@ -246,7 +250,9 @@ def _add_fleet_robots(): nav_graph, rmf_map_name, position, - time_now) + time_now, + max_merge_waypoint_distance = 1.0, + max_merge_lane_distance = max_merge_lane_distance) if starts is None or len(starts) == 0: node.get_logger().error( @@ -268,7 +274,8 @@ def _add_fleet_robots(): update_frequency=rmf_config.get( 'robot_state_update_frequency', 1), adapter=adapter, - api=api) + api=api, + max_merge_lane_distance=max_merge_lane_distance) if robot.initialized: robots[robot_name] = robot From e57aa2cb5eda08ea64bc8afef90e7910017cc8cd Mon Sep 17 00:00:00 2001 From: youliang Date: Sat, 9 Apr 2022 18:26:32 +0800 Subject: [PATCH 04/16] verbose bot status, docking off_grid update and cleanups Signed-off-by: youliang --- fleet_adapter_ecobot/EcobotCommandHandle.py | 127 ++++++++++--------- fleet_adapter_ecobot/TestClientAPI.py | 67 ++++++---- fleet_adapter_ecobot/fleet_adapter_ecobot.py | 74 ++++------- 3 files changed, 128 insertions(+), 140 deletions(-) diff --git a/fleet_adapter_ecobot/EcobotCommandHandle.py b/fleet_adapter_ecobot/EcobotCommandHandle.py index f03500c..6d3846f 100644 --- a/fleet_adapter_ecobot/EcobotCommandHandle.py +++ b/fleet_adapter_ecobot/EcobotCommandHandle.py @@ -88,19 +88,16 @@ def __init__(self, self.max_merge_lane_distance = max_merge_lane_distance # Get the index of the charger waypoint - waypoint = self.graph.find_waypoint(charger_waypoint) + self.charger_waypoint = self.graph.find_waypoint(charger_waypoint) # assert waypoint, f"Charger waypoint {charger_waypoint} does not exist in the navigation graph" - if waypoint is None: + if self.charger_waypoint is None: node.get_logger().error(f"Charger waypoint {charger_waypoint} does not exist in the navigation graph") - return - self.charger_waypoint_index = waypoint.index - self.charger_is_set = False + self.update_frequency = update_frequency self.update_handle = None # RobotUpdateHandle self.battery_soc = 1.0 self.api = api self.position = position # (x,y,theta) in RMF coordinates (meters, radians) - self.initialized = False self.state = EcobotState.IDLE self.adapter = adapter @@ -118,7 +115,6 @@ def __init__(self, self.on_waypoint = None # if robot is waiting at a waypoint. This is a Graph::Waypoint index self.on_lane = None # if robot is travelling on a lane. This is a Graph::Lane index self.target_waypoint = None # this is a Plan::Waypoint - self.dock_waypoint_index = None # The graph index of the waypoint the robot is currently docking into # Threading variables self._lock = threading.Lock() @@ -128,20 +124,40 @@ def __init__(self, self._quit_dock_event = threading.Event() self.action_execution = None - self.participant = None - self.in_error = None - self.update_robot_status_count = 0 # currently will update robot status every 4 callback counts + self.in_error = False + self.is_online = False print(f"{self.name} is starting at: [{self.position_str()}]") - self.state_update_timer = self.node.create_timer( - 1.0 / self.update_frequency, - self.update) - # The Ecobot robot has various cleaning modes. Here we turn off the # cleaning systems. These will be activated only during cleaning self.api.set_cleaning_mode(self.config['inactive_cleaning_config']) - self.initialized = True + + # Init RobotUpdateHandle class member + def init_handler(self, handle): + self.update_handle = handle + if ("max_delay" in self.config.keys()): + max_delay = self.config["max_delay"] + print(f"Setting max delay to {max_delay}s") + self.update_handle.set_maximum_delay(max_delay) + if (self.charger_waypoint.index < self.graph.num_waypoints): + self.update_handle.set_charger_waypoint(self.charger_waypoint.index) + else: + self.node.get_logger().info("Invalid waypoint supplied for charger. Using default nearest charger in the map") + self.update_handle.set_action_executor(self._action_executor) + self.participant = self.update_handle.get_unstable_participant() + + self.location_update_timer = self.node.create_timer( + 1.0 / self.update_frequency, + self.update_location) + + # Note: only update robot status 4.5 times the update period, prevent overload + self.status_update_timer = self.node.create_timer( + 4.5 / self.update_frequency, + self.update_robot_status) + + self.node.get_logger().info(f"Start State Update with freq: {self.update_frequency}") + def clear(self): with self._lock: @@ -154,6 +170,7 @@ def clear(self): # self.target_waypoint = None self.state = EcobotState.IDLE + # override function def stop(self): # Stop motion of the robot. Tracking variables should remain unchanged. @@ -163,6 +180,7 @@ def stop(self): break time.sleep(1.0) + # override function def follow_new_path( self, @@ -325,6 +343,7 @@ def _follow_path(): target=_follow_path) self._follow_path_thread.start() + # override function def dock( self, @@ -341,9 +360,9 @@ def dock( # Get the waypoint that the robot is trying to dock into. The dock_name and waypoint_name must match self.node.get_logger().info(f"[DOCK] Start docking to charger with dock param: {dock_name}") - # NOTE: Now only assume that dock is only used by during charging. - self.dock_waypoint_index = self.charger_waypoint_index - self.on_waypoint = self.dock_waypoint_index + # NOTE: Docking called when robot is heading back to charger + self.target_waypoint.graph_index = self.charger_waypoint.index + self.on_waypoint = None def _dock(): # TODO, clean up implementation of dock # Check if the dock waypoint is a charger or cleaning zone and call the @@ -368,11 +387,10 @@ def _dock(): if self._quit_dock_event.is_set(): self.node.get_logger().info("Aborting docking") return - time.sleep(0.5) + time.sleep(1.0) # Here we assume that the robot has successfully reached waypoint with name same as dock_name with self._lock: - self.on_waypoint = self.dock_waypoint_index - self.dock_waypoint_index = None + self.on_waypoint = self.charger_waypoint.index self.docking_finished_callback() self.node.get_logger().info("Docking completed") self.api.set_cleaning_mode(self.config['inactive_cleaning_config']) @@ -380,6 +398,7 @@ def _dock(): self._dock_thread = threading.Thread(target=_dock) self._dock_thread.start() + def get_position(self): ''' This helper function returns the live position of the robot in the RMF coordinate frame''' @@ -387,24 +406,24 @@ def get_position(self): if position is not None: x,y,theta = self.transforms.to_rmf_map([position[0],position[1], math.radians(position[2])]) print(f"Convert pos from {position} grid coor to {x},{y}, {theta} rmf coor") + self.is_online = True return [x,y,theta] else: self.node.get_logger().error("Unable to retrieve position from robot. Returning last known position...") + self.is_online = False return self.position + def get_battery_soc(self): battery_soc = self.api.battery_soc() if battery_soc is not None: + self.is_online = True return battery_soc else: + self.is_online = False self.node.get_logger().error("Unable to retrieve battery data from robot") return self.battery_soc - def update(self): - self.position = self.get_position() - self.battery_soc = self.get_battery_soc() - if self.update_handle is not None: - self.update_state() # call this when starting cleaning execution def _action_executor(self, @@ -436,38 +455,27 @@ def _action_executor(self, self.in_error = True # TODO: toggle error back execution.error("Failed to initiate cleaning action for robot {self.name}") - def update_state(self): - self.update_handle.update_battery_soc(self.battery_soc) - # only run this during init - if not self.charger_is_set: - if ("max_delay" in self.config.keys()): - max_delay = self.config["max_delay"] - print(f"Setting max delay to {max_delay}s") - self.update_handle.set_maximum_delay(max_delay) - if (self.charger_waypoint_index < self.graph.num_waypoints): - self.update_handle.set_charger_waypoint(self.charger_waypoint_index) - else: - self.node.get_logger().info("Invalid waypoint supplied for charger. Using default nearest charger in the map") - self.update_handle.set_action_executor(self._action_executor) - self.charger_is_set = True - self.participant = self.update_handle.get_unstable_participant() - - # Update robot state's status, this will show on the dashboard, update this every 4 callbacks - # TODO: in a seperate callback timer? - if (self.update_robot_status_count >= 4): - if self.api.is_charging(): - self.update_handle.override_status("charging") - elif self.in_error: - self.update_handle.override_status("error") - elif not self.api.is_localize(): - self.node.get_logger().warn(f"Robot {self.name} is not localized") - self.update_handle.override_status("error") - else: - self.update_handle.override_status(None) - self.update_robot_status_count = 0 + # Update robot state's status + def update_robot_status(self): + if self.api.is_charging(): + self.update_handle.override_status("charging") + elif not self.is_online: + self.update_handle.override_status("offline") + elif self.in_error: + self.update_handle.override_status("error") + elif not self.api.is_localize(): + self.node.get_logger().warn(f"Robot {self.name} is not localized") + self.update_handle.override_status("uninitialized") else: - self.update_robot_status_count += 1 + self.update_handle.override_status(None) + + + # Update location and check cleaning action + def update_location(self): + self.position = self.get_position() + self.battery_soc = self.get_battery_soc() + self.update_handle.update_battery_soc(self.battery_soc) # Update states and positions with self._lock: @@ -504,7 +512,7 @@ def update_state(self): # self.rmf_map_name, self.position, # max_merge_waypoint_distance = 1.0, max_merge_lane_distance) - ## Get Closest point on graph and update location + ## Get Closest point on graph and update current robot location closest_wp = self.get_closest_waypoint_idx( self.position, self.max_merge_lane_distance) if closest_wp: @@ -542,11 +550,6 @@ def update_state(self): f"and lanes:{lane_indices}") self.update_handle.update_current_lanes( self.position, lane_indices) - elif (self.dock_waypoint_index is not None): - print(f"[update] Calling update_off_grid_position() dock with pose" \ - f"[{self.position_str()}] and waypoint[{self.dock_waypoint_index}]") - self.update_handle.update_off_grid_position( - self.position, self.dock_waypoint_index) elif (self.target_waypoint is not None and self.target_waypoint.graph_index is not None): # if robot is merging into a waypoint print(f"[update] Calling update_off_grid_position() with pose " \ f"[{self.position_str()}] and waypoint[{self.target_waypoint.graph_index}]") diff --git a/fleet_adapter_ecobot/TestClientAPI.py b/fleet_adapter_ecobot/TestClientAPI.py index 7624b5c..36d32bc 100644 --- a/fleet_adapter_ecobot/TestClientAPI.py +++ b/fleet_adapter_ecobot/TestClientAPI.py @@ -28,11 +28,6 @@ def __init__(self): [1027.0187, 1271.659514, -30], [1027.0187, 1271.659514, -100], [1027.0187, 1271.659514, -60], - [1027.0187, 671.659514, -30], - [1027.0187, 671.659514, -60], - [1027.0187, 671.659514, -100], - [1027.0187, 671.659514, -60], - [1027.0187, 671.659514, -30], [500.0187, 905.659514, 0], [500.0187, 905.659514, 20], [500.0187, 905.659514, 40], @@ -40,20 +35,35 @@ def __init__(self): [500.0187, 905.659514, 80], [500.0187, 905.659514, 100], [936.238485, 981.95061, 0], - [909.963465, 1139.7444, 180] + [709.963465, 1139.7444, 180] + ] + self.fake_dock_path = [ + [968.51, 1325.25, 0], + [965.21, 1338.18, 0], + [963.21, 1348.18, 0], + [962.59, 1360.78, 0], + [960.59, 1370.78, 0], + [960.59, 1363.78, 0], + [964.36, 1356.75, 0] ] self.is_fake_cleaning = False - self.clean_wp_idx = 0 + self.is_fake_docking = False + self.task_wp_idx = 0 + self.task_wp_idx = 0 print("[TEST ECOBOT CLIENT API] successfully setup fake client api class") self.connected = True - self.dock_position = [977, 1372] - self.fake_location = [977.5834309628409, 1192.0576445043025, 0] + self.dock_position = [977, 1372, 0] + # self.fake_location = [977.5834, 1192.0576, 0] + self.fake_location = [1072.43, 899.82, 0] #Offgrid start def position(self): ''' Returns [x, y, theta] expressed in the robot's coordinate frame or None''' if self.is_fake_cleaning: print(f"[TEST CLIENT API] provide fake cleaning position") - return self.fake_clean_path[self.clean_wp_idx] + return self.fake_clean_path[self.task_wp_idx] + elif self.is_fake_docking: + print(f"[TEST CLIENT API] provide fake docking position") + return self.fake_dock_path[self.task_wp_idx] print(f"[TEST CLIENT API] provide position [{self.fake_location}]") return self.fake_location @@ -70,15 +80,13 @@ def navigate_to_waypoint(self, waypoint_name, map_name): ''' Ask the robot to navigate to a preconfigured waypoint on a map. Returns True if the robot received the command''' print(f"[TEST CLIENT API] moved to fake waypoint {waypoint_name}") - self.fake_location = [977, 1372, 0 ] # assume original charging waypoint - self.attempts = 0 + self.is_fake_docking = True return True def start_task(self, name:str, map_name:str): ''' Returns True if the robot has started the generic task, else False''' print(f"[TEST CLIENT API] Start fake task : {name}") - self.fake_location = [977, 1372, 0 ] # assume original charging waypoint - self.is_at_dock = True + self.is_fake_docking = True return True def start_clean(self, name:str, map_name:str): @@ -108,23 +116,26 @@ def navigation_completed(self): def task_completed(self): ''' For ecobots the same function is used to check completion of navigation & cleaning''' - if not self.is_fake_cleaning: # note, if immediately, the robot will head back to staging point - if self.attempts < 4: - print(f"[TEST CLIENT API] Fake task in process") - self.attempts += 1 + self.task_wp_idx += 1 + if self.is_fake_docking: + if self.task_wp_idx < len(self.fake_dock_path): + print(f"[TEST CLIENT API] Fake nav/dock task in process") return False else: - print(f"[TEST CLIENT API] No fake task in process") + self.task_wp_idx = 0 + self.is_fake_docking = False + self.fake_location = self.dock_position + print(f"[TEST CLIENT API] Fake nav/dock task COMPLETED") return True - self.clean_wp_idx += 1 - if self.clean_wp_idx < len(self.fake_clean_path): - print(f"[TEST CLIENT API] FAKE DOCK/CLEAN in process") - return False else: - self.clean_wp_idx = 0 - self.is_fake_cleaning = False - print(f"[TEST CLIENT API] FAKE DOCK/CLean completed") - return True + if self.task_wp_idx < len(self.fake_clean_path): + print(f"[TEST CLIENT API] FAKE CLEANING in process") + return False + else: + self.task_wp_idx = 0 + self.is_fake_cleaning = False + print(f"[TEST CLIENT API] FAKE CLEANING completed") + return True def battery_soc(self): print(f"[TEST CLIENT API] get fake battery 100%") @@ -136,7 +147,7 @@ def set_cleaning_mode(self, cleaning_config:str): def is_charging(self): """Check if robot is charging, will return false if not charging, None if not avail""" - dx, dy, = self.dock_position + dx, dy, _ = self.dock_position x, y, _= self.fake_location if (abs(x-dx) < 5.0 and abs(y-dy) < 5.0): print(f"[TEST CLIENT API] Fake robot at dock, is charging") diff --git a/fleet_adapter_ecobot/fleet_adapter_ecobot.py b/fleet_adapter_ecobot/fleet_adapter_ecobot.py index c03aa7e..41c0192 100644 --- a/fleet_adapter_ecobot/fleet_adapter_ecobot.py +++ b/fleet_adapter_ecobot/fleet_adapter_ecobot.py @@ -179,7 +179,7 @@ def _consider(description: dict): def updater_inserter(cmd_handle, update_handle): """Insert a RobotUpdateHandle.""" - cmd_handle.update_handle = update_handle + cmd_handle.init_handler(update_handle) # Initialize robots for this fleet missing_robots = config_yaml['robots'] @@ -209,8 +209,6 @@ def _add_fleet_robots(): node.get_logger().info(f"Initializing robot: {robot_name}") rmf_config = missing_robots[robot_name]['rmf_config'] - initial_waypoint = rmf_config['start']['waypoint'] - initial_orientation = rmf_config['start']['orientation'] starts = [] time_now = adapter.now() @@ -225,34 +223,17 @@ def _add_fleet_robots(): else: rmf_map_name = rmf_config['start']['rmf_map_name'] - # TODO: this is to find the robot location when offgrid - # add this to config - max_merge_lane_distance = 15.0 # meters - - if (initial_waypoint is not None) and\ - (initial_orientation is not None): - node.get_logger().info( - f"Using provided initial waypoint " - "[{initial_waypoint}] " - f"and orientation [{initial_orientation:.2f}] to " - f"initialize starts for robot [{robot_name}]") - # Get the waypoint index for initial_waypoint - initial_waypoint_index = nav_graph.find_waypoint( - initial_waypoint).index - starts = [plan.Start(time_now, - initial_waypoint_index, - initial_orientation)] - else: - node.get_logger().info( - f"Running compute_plan_starts for robot: " - f"{robot_name}, with pos: {position}") - starts = plan.compute_plan_starts( - nav_graph, - rmf_map_name, - position, - time_now, - max_merge_waypoint_distance = 1.0, - max_merge_lane_distance = max_merge_lane_distance) + # Identify the current location of the robot in rmf's graph + node.get_logger().info( + f"Running compute_plan_starts for robot: " + f"{robot_name}, with pos: {position}") + starts = plan.compute_plan_starts( + nav_graph, + rmf_map_name, + position, + time_now, + max_merge_waypoint_distance = 1.0, + max_merge_lane_distance = rmf_config["max_merge_lane_distance"]) if starts is None or len(starts) == 0: node.get_logger().error( @@ -275,26 +256,19 @@ def _add_fleet_robots(): 'robot_state_update_frequency', 1), adapter=adapter, api=api, - max_merge_lane_distance=max_merge_lane_distance) - - if robot.initialized: - robots[robot_name] = robot - # Add robot to fleet - fleet_handle.add_robot(robot, - robot_name, - profile, - starts, - partial(updater_inserter, - robot)) - node.get_logger().info( - f"Successfully added new robot: {robot_name}") - - else: - node.get_logger().error( - f"Failed to initialize robot: {robot_name}") - + max_merge_lane_distance=rmf_config["max_merge_lane_distance"]) + + robots[robot_name] = robot + # Add robot to fleet + fleet_handle.add_robot(robot, + robot_name, + profile, + starts, + partial(updater_inserter, + robot)) + node.get_logger().info( + f"Successfully added new robot: {robot_name}") del missing_robots[robot_name] - return add_robots = threading.Thread(target=_add_fleet_robots, args=()) From 0c0ce3accdee1b2337e393c523389b4468355d2c Mon Sep 17 00:00:00 2001 From: youliang Date: Tue, 12 Apr 2022 18:19:12 +0800 Subject: [PATCH 05/16] supporting manual control action Signed-off-by: youliang --- fleet_adapter_ecobot/EcobotCommandHandle.py | 73 ++++++++++++-------- fleet_adapter_ecobot/fleet_adapter_ecobot.py | 11 ++- 2 files changed, 53 insertions(+), 31 deletions(-) diff --git a/fleet_adapter_ecobot/EcobotCommandHandle.py b/fleet_adapter_ecobot/EcobotCommandHandle.py index 6d3846f..b78fb9f 100644 --- a/fleet_adapter_ecobot/EcobotCommandHandle.py +++ b/fleet_adapter_ecobot/EcobotCommandHandle.py @@ -126,6 +126,7 @@ def __init__(self, self.action_execution = None self.in_error = False self.is_online = False + self.action_category = None print(f"{self.name} is starting at: [{self.position_str()}]") @@ -363,6 +364,7 @@ def dock( # NOTE: Docking called when robot is heading back to charger self.target_waypoint.graph_index = self.charger_waypoint.index self.on_waypoint = None + self.on_lane = None def _dock(): # TODO, clean up implementation of dock # Check if the dock waypoint is a charger or cleaning zone and call the @@ -432,28 +434,39 @@ def _action_executor(self, execution: adpt.robot_update_handle.ActionExecution): with self._lock: - # only accept clean category - assert(category == "clean") - attempts = 0 - while attempts < 2: - self.node.get_logger().info(f"Requesting robot {self.name} to clean {description}") + # only accept clean and manual_control + assert(category in ["clean", "manual_control"]) + + self.action_category = category + if (category == "clean"): + attempts = 0 self.api.set_cleaning_mode(self.config['active_cleaning_config']) - if self.api.start_clean(description["clean_task_name"], self.map_name): - self.node.get_logger().warn(f"Robot {self.name} start cleaning action") - self.start_clean_action_time = self.adapter.now() - self.on_waypoint = None - self.on_lane = None - self.action_execution = execution - # robot moves slower during cleaning - self.vehicle_traits.linear.nominal_velocity *= 0.2 - return - attempts+=1 - time.sleep(1.0) - self.node.get_logger().warn(f"Failed to initiate cleaning action for robot {self.name}") - # TODO: issue error ticket - # create_issue(() - self.in_error = True # TODO: toggle error back - execution.error("Failed to initiate cleaning action for robot {self.name}") + while True: + self.node.get_logger().info(f"Requesting robot {self.name} to clean {description}") + if self.api.start_clean(description["clean_task_name"], self.map_name): + self.check_task_completion = self.api.task_completed # will check api + break + if (attempts > 3): + self.node.get_logger().warn( + f"Failed to initiate cleaning action for robot [{self.name}]") + # TODO: issue error ticket + self.in_error = True # TODO: toggle error back + execution.error("Failed to initiate cleaning action for robot {self.name}") + execution.finished() + return + attempts+=1 + time.sleep(1.0) + elif (category == "manual_control"): + self.check_task_completion = lambda:False # user can only cancel the manual_control + + # start Perform Action + self.node.get_logger().warn(f"Robot [{self.name}] starts [{category}] action") + self.start_action_time = self.adapter.now() + self.on_waypoint = None + self.on_lane = None + self.action_execution = execution + # robot moves slower during perform action + self.vehicle_traits.linear.nominal_velocity *= 0.2 # Update robot state's status @@ -480,25 +493,25 @@ def update_location(self): # Update states and positions with self._lock: if (self.action_execution): - print("Executing action, cleaning state") + print(f"Executing perform action [{self.action_category}]") # check if action is completed/killed/canceled action_ok = self.action_execution.okay() - if self.api.task_completed() or not action_ok: + if self.check_task_completion() or not action_ok: if action_ok: - self.node.get_logger().info("Cleaning is completed") + self.node.get_logger().info(f"action [{self.action_category}] is completed") self.action_execution.finished() else: - self.node.get_logger().warn("cleaning task is killed/canceled") + self.node.get_logger().warn(f"action [{self.action_category}] is killed/canceled") self.api.set_cleaning_mode(self.config['inactive_cleaning_config']) self.action_execution = None - self.start_clean_action_time = None + self.start_action_time = None self.vehicle_traits.linear.nominal_velocity *= 5 # change back vel else: assert(self.participant) - assert(self.start_clean_action_time) - total_action_time = timedelta(hours=1.0) #TODO: populate actual total time`` - remaining_time = total_action_time - (self.adapter.now() - self.start_clean_action_time) - print(f"Still cleaning =) Estimated remaining time: [{remaining_time}]") + assert(self.start_action_time) + total_action_time = timedelta(hours=1.0) #TODO: populate actual total time + remaining_time = total_action_time - (self.adapter.now() - self.start_action_time) + print(f"Still performing action, Estimated remaining time: [{remaining_time}]") self.action_execution.update_remaining_time(remaining_time) # create a short segment of the trajectory according to robot current heading diff --git a/fleet_adapter_ecobot/fleet_adapter_ecobot.py b/fleet_adapter_ecobot/fleet_adapter_ecobot.py index 41c0192..63687ff 100644 --- a/fleet_adapter_ecobot/fleet_adapter_ecobot.py +++ b/fleet_adapter_ecobot/fleet_adapter_ecobot.py @@ -140,9 +140,18 @@ def _task_request_check(task_capabilities, msg: TaskProfile): partial(_task_request_check, task_capabilities)) def _consider(description: dict): + confirm = adpt.fleet_update_handle.Confirmation() + + # Currently there's no way for user to submit a robot_task_request + # .json file via the rmf-web dashboard. Thus the short term solution + # is to add the fleet_name info into action description. NOTE + # only matching fleet_name action will get accepted + if (description["category"] == "manual_control" and + description["description"]["fleet_name"] != fleet_name): + return confirm + node.get_logger().warn( f"Accepting action: {description} ") - confirm = adpt.fleet_update_handle.Confirmation() confirm.accept() return confirm From a53859e95d3eb9f5ea2d68a2d29408b95b363dd4 Mon Sep 17 00:00:00 2001 From: youliang Date: Wed, 20 Apr 2022 18:09:19 +0800 Subject: [PATCH 06/16] retrieve robot map from gaussian api Signed-off-by: youliang --- fleet_adapter_ecobot/TestClientAPI.py | 24 ++++---- fleet_adapter_ecobot/fleet_adapter_ecobot.py | 60 +++++++++----------- 2 files changed, 38 insertions(+), 46 deletions(-) diff --git a/fleet_adapter_ecobot/TestClientAPI.py b/fleet_adapter_ecobot/TestClientAPI.py index 36d32bc..07fe125 100644 --- a/fleet_adapter_ecobot/TestClientAPI.py +++ b/fleet_adapter_ecobot/TestClientAPI.py @@ -24,18 +24,9 @@ def __init__(self): [1056.7065, 1087.30105, 180], [1056.7065, 1087.30105, 150], [1056.7065, 1087.30105, 130], - [1027.0187, 1271.659514, -60], - [1027.0187, 1271.659514, -30], - [1027.0187, 1271.659514, -100], - [1027.0187, 1271.659514, -60], - [500.0187, 905.659514, 0], - [500.0187, 905.659514, 20], - [500.0187, 905.659514, 40], - [500.0187, 905.659514, 60], - [500.0187, 905.659514, 80], - [500.0187, 905.659514, 100], - [936.238485, 981.95061, 0], - [709.963465, 1139.7444, 180] + [801.63, 1492.17, 0], + [801.60, 1485.17, 20], + [801.55, 1475.17, 40], ] self.fake_dock_path = [ [968.51, 1325.25, 0], @@ -53,8 +44,13 @@ def __init__(self): print("[TEST ECOBOT CLIENT API] successfully setup fake client api class") self.connected = True self.dock_position = [977, 1372, 0] - # self.fake_location = [977.5834, 1192.0576, 0] - self.fake_location = [1072.43, 899.82, 0] #Offgrid start + self.fake_location = [977.5834, 1192.0576, 0] + self.fake_robot_map_name = "sim_test_robot_map" + # self.fake_location = [1072.43, 899.82, 0] #Offgrid start + + def current_map(self): + print(f"[TEST CLIENT API] return testing map: {self.fake_robot_map_name}") + return self.fake_robot_map_name def position(self): ''' Returns [x, y, theta] expressed in the robot's coordinate frame or None''' diff --git a/fleet_adapter_ecobot/fleet_adapter_ecobot.py b/fleet_adapter_ecobot/fleet_adapter_ecobot.py index 63687ff..b54b0b7 100644 --- a/fleet_adapter_ecobot/fleet_adapter_ecobot.py +++ b/fleet_adapter_ecobot/fleet_adapter_ecobot.py @@ -15,7 +15,6 @@ import sys import argparse import yaml -import nudged import threading import time @@ -163,29 +162,6 @@ def _consider(description: dict): f" to perform action of category [{cat}]") fleet_handle.add_performable_action(cat, _consider) - # use defined transfrom param if avail, else use ref coors - if "rmf_transform" in config_yaml: - tx, ty, r, s = config_yaml["rmf_transform"] - rmf_transform = RmfMapTransform(tx, ty, r, s) - mse = 0.0 # null - else: - rmf_transform = RmfMapTransform() - rmf_coordinates = config_yaml['reference_coordinates']['rmf'] - ecobot_coordinates = config_yaml['reference_coordinates']['ecobot'] - mse = rmf_transform.estimate(ecobot_coordinates, rmf_coordinates) - tx, ty, r, s = rmf_transform.to_robot_map_transform() - print(f"Coordinate transformation error: {mse}") - print("RMF to Ecobot transform:") - print(f" rotation:{r}") - print(f" scale:{s}") - print(f" trans:{tx}, {ty}") - - tx, ty, r, s = rmf_transform.to_rmf_map_transform() - print("Ecobot to RMF transform:") - print(f" rotation:{r}") - print(f" scale:{s}") - print(f" trans:{tx}, {ty}") - def updater_inserter(cmd_handle, update_handle): """Insert a RobotUpdateHandle.""" cmd_handle.init_handler(update_handle) @@ -210,6 +186,33 @@ def _add_fleet_robots(): if not api.connected: continue + robot_map_name = api.current_map() + if robot_map_name is None: + node.get_logger().warn(f"Failed to get robot map name: [{robot_map_name}]") + continue + + # use defined transfrom param if avail, else use ref coors + # note that the robot's map_name should be identical to the one in config + if "rmf_transform" in config_yaml: + if robot_map_name not in config_yaml["rmf_transform"]: + assert False, f"Robot map name: {robot_map_name} isnt defined in config" + tx, ty, r, s = config_yaml["rmf_transform"][robot_map_name]["transform"] + rmf_transform = RmfMapTransform(tx, ty, r, s) + rmf_map_name = config_yaml["rmf_transform"][robot_map_name]['rmf_map_name'] + else: + rmf_transform = RmfMapTransform() + rmf_coordinates = config_yaml['reference_coordinates']['rmf'] + ecobot_coordinates = config_yaml['reference_coordinates']['ecobot'] + rmf_map_name = config_yaml['reference_coordinates']['rmf_map_name'] + mse = rmf_transform.estimate(ecobot_coordinates, rmf_coordinates) + print(f"Coordinate transformation error: {mse}") + + print(f"Coordinate Transform from [{robot_map_name}] to [{rmf_map_name}]") + tx, ty, r, s = rmf_transform.to_robot_map_transform() + print(f"RMF to Ecobot transform :: trans [{tx}, {ty}]; rot {r}; scale {s}") + tx, ty, r, s = rmf_transform.to_rmf_map_transform() + print(f"Ecobot to RMF transform :: trans [{tx}, {ty}]; rot {r}; scale {s}") + # get robot coordinates and transform to rmf_coor ecobot_pos = api.position() if ecobot_pos is None: @@ -225,13 +228,6 @@ def _add_fleet_robots(): x,y,_ = rmf_transform.to_rmf_map([ecobot_pos[0],ecobot_pos[1], 0]) position = [x, y, 0] - # for backwards compatibility, check if rmf_map name is diff - # to robot's map name - if "rmf_map_name" not in rmf_config['start']: - rmf_map_name = rmf_config['start']['map_name'] - else: - rmf_map_name = rmf_config['start']['rmf_map_name'] - # Identify the current location of the robot in rmf's graph node.get_logger().info( f"Running compute_plan_starts for robot: " @@ -257,7 +253,7 @@ def _add_fleet_robots(): graph=nav_graph, vehicle_traits=vehicle_traits, transforms=rmf_transform, - map_name=rmf_config['start']['map_name'], + map_name=robot_map_name, rmf_map_name=rmf_map_name, position=position, charger_waypoint=rmf_config['charger']['waypoint'], From 581b55ca40882559b7bd2e1ae525686109b32c44 Mon Sep 17 00:00:00 2001 From: youliang Date: Wed, 4 May 2022 22:59:41 +0800 Subject: [PATCH 07/16] multi- robot and rmf maps support Signed-off-by: youliang --- README.md | 13 ++ config.yaml | 123 ++++++++++--------- fleet_adapter_ecobot/EcobotClientAPI.py | 7 +- fleet_adapter_ecobot/EcobotCommandHandle.py | 72 ++++++----- fleet_adapter_ecobot/fleet_adapter_ecobot.py | 65 +++++----- 5 files changed, 160 insertions(+), 120 deletions(-) diff --git a/README.md b/README.md index 312bc26..92b3576 100644 --- a/README.md +++ b/README.md @@ -23,6 +23,11 @@ ros2 run fleet_adapter_ecobot fleet_adapter_ecobot -c CONFIG_FILE -n NAV_GRAPH ``` +Dispatch a Task +``` +ros2 run rmf_demos_tasks dispatch_action -s lounge -a clean -ad '{ "clean_task_name": "clean_lounge" }' +``` + ## Test the fleet adapter in simulation The adapter can be tested in simulation with the help of the [ecobot_sim_server](fleet_adapter_ecobot/ecobot_sim_server.py). This script emulates the API server of the robot and can be used to command robots in simulation to follow commands from Open-RMF. @@ -39,3 +44,11 @@ ros2 run fleet_adapter_ecobot fleet_adapter_ecobot -c CONFIG_FILE -n NAV_GRAPH - ``` Ensure that the `base_url` in the config matches the `LOCALHOST:PORT` specified to the server. + +## Get the Transformation from `traffic-editor` + +To get the transformation of the robot map to rmf map, user can add a "floorplan" of a robot map. Then annotate and the corresponding "constraint-pairs", lastly ctrl-T to let traffic-editor calculate the respective transformation. + +![](../media/media/traffic-editor-transform.png) + +Specify this transformation to the `rmf_transform` in the `config.yaml` file. Note that the value of Y-axis is applied with a -ve. diff --git a/config.yaml b/config.yaml index 9e4d5ca..d945c5d 100644 --- a/config.yaml +++ b/config.yaml @@ -31,6 +31,7 @@ rmf_fleet: delivery: False clean: True finishing_request: "park" + action_categories: ["clean", "manual_control"] # Ecobot CONFIG ================================================================= @@ -45,73 +46,75 @@ robots: cleaning_task_prefix: "" # the prefix of the cleaning task created active_cleaning_config: "light_cleaning" # the cleaning config used during cleaning inactive_cleaning_config: "no_cleaning" # the cleaning config used during navigation - docks: # in rmf coordinates - vacuum_zone_1: [ [106.3, -20.63, -1.57], - [106.3, -21.23, 0.0], - [111.2, -21.15, 1.57], - [111.1, -17.12, -3.14], - [106.84, -17.07, -1.57], - [106.4, -18.98, -1.57]] - vacuum_zone_2: [ [87.14, -57.59, 0.0], - [89.66, -57.6, 1.57], - [89.56, -62.35, 3.14], - [83.93, -62.35, 3.14], - [83.93, -59.58, 0.0], - [87.03, -59.69, 0.0]] - vacuum_zone_3: [ [26.81, -62.43, -1.57], - [26.95, -65.09, -3.14], - [23.57, -65.09, 1.57], - [23.54, -62.5, 0.0], - [26.81, -62.43, 0.0]] - vacuum_zone_4: [ [22.31, -54.06, 3.14], - [15.3, -53.9, 1.57], - [15.31, -49.5, 0.0], - [22.53, -49.56, -1.57], - [22.5, -52.59, -1.57]] - vacuum_zone_5: [ [83.0, -33.06, 0.0], - [81.56, -36.48, -1.57], - [83.78, -37.62, 0.0], - [84.91, -34.19, 1.57]] + # docks: # in rmf coordinates + # vacuum_zone_1: [ [106.3, -20.63, -1.57], + # [106.3, -21.23, 0.0], + # [111.2, -21.15, 1.57], + # [111.1, -17.12, -3.14], + # [106.84, -17.07, -1.57], + # [106.4, -18.98, -1.57]] + # vacuum_zone_2: [ [87.14, -57.59, 0.0], + # [89.66, -57.6, 1.57], + # [89.56, -62.35, 3.14], + # [83.93, -62.35, 3.14], + # [83.93, -59.58, 0.0], + # [87.03, -59.69, 0.0]] + # vacuum_zone_3: [ [26.81, -62.43, -1.57], + # [26.95, -65.09, -3.14], + # [23.57, -65.09, 1.57], + # [23.54, -62.5, 0.0], + # [26.81, -62.43, 0.0]] + # vacuum_zone_4: [ [22.31, -54.06, 3.14], + # [15.3, -53.9, 1.57], + # [15.31, -49.5, 0.0], + # [22.53, -49.56, -1.57], + # [22.5, -52.59, -1.57]] + # vacuum_zone_5: [ [83.0, -33.06, 0.0], + # [81.56, -36.48, -1.57], + # [83.78, -37.62, 0.0], + # [84.91, -34.19, 1.57]] rmf_config: robot_state_update_frequency: 0.5 - start: - map_name: "L1" + max_merge_lane_distance: 15.0 # means how far will the robot diverge from the defined graph + # start: + # map_name: "L1" # waypoint: "charger_ecobot1" # Optional # orientation: 0.0 # Optional, radians - waypoint: null - orientation: null + # waypoint: null + # orientation: null charger: waypoint: "charger_ecobot40_1" # TRANSFORM CONFIG ============================================================= # For computing transforms between Ecobot and RMF coordinate systems -reference_coordinates: - rmf_to_ecobot: - rotation: 3.1721 - scale: 20.2633 - trans_x: 4562.9901 - trans_y: 123.5718 - ecobot_to_rmf: - rotation: -3.1721 - scale: 0.04935 - trans_x: 225.2659 - trans_y: 0.7730 - rmf: [[33.11, -18.99], - [111.3, -19.06], - [86.16, -19.02], - [79.23, -46.94], - [82.22, -58.3], - [52.65, -58.37], - [8.234, -58.35], - [9.23, -30.5], - [24.68, -19.08] ] - ecobot: [ [3876, 741], - [2293, 703], - [2816, 710], - [2930, 1250], - [2861, 1501], - [3460, 1524], - [4360, 1555], - [4347, 991], - [4058, 747] ] \ No newline at end of file +# TRANSFORM CONFIG ============================================================= +# For computing transforms between Ecobot and RMF coordinate systems +# Format robot_map: {rmf_map_name: rmf_map, transform: [tx,ty,r,s] } +# Format robot_map: {rmf_map_name: rmf_map, reference_coordinates: +# {rmf: [[x, y]....], rmf: [[x, y]....]}} +rmf_transform: + L1: + rmf_map_name: "L1" + reference_coordinates: + rmf: [[33.11, -18.99], + [111.3, -19.06], + [86.16, -19.02], + [79.23, -46.94], + [82.22, -58.3], + [52.65, -58.37], + [8.234, -58.35], + [9.23, -30.5], + [24.68, -19.08] ] + robot: [ [3876, 741], + [2293, 703], + [2816, 710], + [2930, 1250], + [2861, 1501], + [3460, 1524], + [4360, 1555], + [4347, 991], + [4058, 747] ] + office_map: + rmf_map_name: "L1" + transform: [0.556, 0.02, -1.58, 0.05] diff --git a/fleet_adapter_ecobot/EcobotClientAPI.py b/fleet_adapter_ecobot/EcobotClientAPI.py index a7df397..e0ef900 100644 --- a/fleet_adapter_ecobot/EcobotClientAPI.py +++ b/fleet_adapter_ecobot/EcobotClientAPI.py @@ -323,11 +323,14 @@ def is_charging(self): """Check if robot is charging, will return false if not charging, None if not avail""" response = self.data() if response is not None: - return response["data"]["charge"] + # return response["data"]["charge"] # Faced an edge case: robot didnt dock well + if response["data"]["chargerCurrent"] > 0.0: + return True + else: + return False else: return None - def is_localize(self): """Check if robot is localize, will return false if not charging, None if not avail""" response = self.data() diff --git a/fleet_adapter_ecobot/EcobotCommandHandle.py b/fleet_adapter_ecobot/EcobotCommandHandle.py index b78fb9f..da6b19e 100644 --- a/fleet_adapter_ecobot/EcobotCommandHandle.py +++ b/fleet_adapter_ecobot/EcobotCommandHandle.py @@ -29,7 +29,7 @@ from datetime import timedelta -from typing import Optional, Tuple +from typing import List, Optional, Tuple # States for EcobotCommandHandle's state machine used when guiding robot along # a new path @@ -58,9 +58,6 @@ def __init__(self, graph, vehicle_traits, transforms, - map_name, - rmf_map_name, - position, charger_waypoint, update_frequency, adapter, @@ -69,10 +66,6 @@ def __init__(self, """ :param config robot config defined in yaml file - :param map_name - map name in ecobot system - :param rmf_map_name - map name in rmf :param max_merge_lane_distance means how far will the robot diverge from the defined graph """ @@ -83,8 +76,6 @@ def __init__(self, self.graph = graph self.vehicle_traits = vehicle_traits self.transforms = transforms - self.map_name = map_name - self.rmf_map_name = rmf_map_name self.max_merge_lane_distance = max_merge_lane_distance # Get the index of the charger waypoint @@ -97,7 +88,6 @@ def __init__(self, self.update_handle = None # RobotUpdateHandle self.battery_soc = 1.0 self.api = api - self.position = position # (x,y,theta) in RMF coordinates (meters, radians) self.state = EcobotState.IDLE self.adapter = adapter @@ -128,6 +118,14 @@ def __init__(self, self.is_online = False self.action_category = None + # Get the latest position (x,y,theta) in RMF coordinates (meters, radians) + robot_pos = self.get_robot_position() + while robot_pos is None: + print("Not able to retrieve latest position, trying again...") + time.sleep(2.0) + robot_pos = self.get_robot_position() + + self.position = robot_pos print(f"{self.name} is starting at: [{self.position_str()}]") # The Ecobot robot has various cleaning modes. Here we turn off the @@ -260,7 +258,7 @@ def _follow_path(): self.path_index = self.remaining_waypoints[0].index # Move robot to next waypoint target_pose = self.target_waypoint.position - [x,y, yaw] = self.transforms.to_robot_map(target_pose[:3]) + [x,y, yaw] = self.transforms[self.robot_map_name]["tf"].to_robot_map(target_pose[:3]) theta = math.degrees(yaw) print(f"Requesting robot to navigate to " @@ -268,7 +266,7 @@ def _follow_path(): f"grid coordinates and [{target_pose[0]:.2f}. {target_pose[1]:.2f}, {target_pose[2]:.2f}] " f"RMF coordinates...") - response = self.api.navigate([x, y, theta], self.map_name) + response = self.api.navigate([x, y, theta], self.robot_map_name) if response: self.remaining_waypoints = self.remaining_waypoints[1:] self.state = EcobotState.MOVING @@ -318,7 +316,8 @@ def _follow_path(): else: # The robot may either be on the previous # waypoint or the target one - if self.target_waypoint.graph_index is not None and self.dist(self.position, target_pose) < 0.5: + if (self.target_waypoint.graph_index is not None and + self.dist(self.position, target_pose) < 0.5): self.on_waypoint = self.target_waypoint.graph_index elif self.last_known_waypoint_index is not None and \ self.dist(self.position, self.graph.get_waypoint(self.last_known_waypoint_index).location) < 0.5: @@ -376,12 +375,12 @@ def _dock(): # named the same as the dock name (charger_ecobot75_x) if self.name[:8] == "ecobot75": self.api.set_cleaning_mode(self.config['inactive_cleaning_config']) - if self.api.start_task(dock_name, self.map_name): + if self.api.start_task(dock_name, self.robot_map_name): break # For Ecobot40 and 50, we will use the navigate_to_waypoint API to # dock into the charger else: - if self.api.navigate_to_waypoint(dock_name, self.map_name): + if self.api.navigate_to_waypoint(dock_name, self.robot_map_name): break time.sleep(1.0) while (not self.api.task_completed()): @@ -401,20 +400,34 @@ def _dock(): self._dock_thread.start() - def get_position(self): + def get_robot_position(self) -> List[int]: ''' This helper function returns the live position of the robot in the - RMF coordinate frame''' + RMF coordinate frame + ''' position = self.api.position() - if position is not None: - x,y,theta = self.transforms.to_rmf_map([position[0],position[1], math.radians(position[2])]) - print(f"Convert pos from {position} grid coor to {x},{y}, {theta} rmf coor") - self.is_online = True - return [x,y,theta] - else: - self.node.get_logger().error("Unable to retrieve position from robot. Returning last known position...") + map_name = self.api.current_map() + + if position is None or map_name is None: + self.node.get_logger().error( + "Unable to retrieve position from robot. Returning last known position...") self.is_online = False return self.position + if map_name not in self.transforms: + self.node.get_logger().error( + f"Robot map name [{map_name}] is not known. return last known position.") + self.is_online = True + return self.position + + tf = self.transforms[map_name] + x,y,theta = tf['tf'].to_rmf_map([position[0],position[1], math.radians(position[2])]) + print(f"Convert pos from {position} grid coor to {x},{y}, {theta} rmf coor") + self.is_online = True + # will update the member function directly + self.robot_map_name = map_name + self.rmf_map_name = tf['rmf_map_name'] + return [x,y,theta] + def get_battery_soc(self): battery_soc = self.api.battery_soc() @@ -443,7 +456,7 @@ def _action_executor(self, self.api.set_cleaning_mode(self.config['active_cleaning_config']) while True: self.node.get_logger().info(f"Requesting robot {self.name} to clean {description}") - if self.api.start_clean(description["clean_task_name"], self.map_name): + if self.api.start_clean(description["clean_task_name"], self.robot_map_name): self.check_task_completion = self.api.task_completed # will check api break if (attempts > 3): @@ -486,7 +499,7 @@ def update_robot_status(self): # Update location and check cleaning action def update_location(self): - self.position = self.get_position() + self.position = self.get_robot_position() self.battery_soc = self.get_battery_soc() self.update_handle.update_battery_soc(self.battery_soc) @@ -540,7 +553,7 @@ def update_location(self): self.vehicle_traits, self.adapter.now(), positions) - route = schedule.Route(self.rmf_map_name,trajectory) + route = schedule.Route(self.rmf_map_name, trajectory) self.participant.set_itinerary([route]) elif (self.on_waypoint is not None): # if robot is on a waypoint print(f"[update] Calling update_current_waypoint() on waypoint with " \ @@ -563,7 +576,8 @@ def update_location(self): f"and lanes:{lane_indices}") self.update_handle.update_current_lanes( self.position, lane_indices) - elif (self.target_waypoint is not None and self.target_waypoint.graph_index is not None): # if robot is merging into a waypoint + # if robot is merging into a waypoint + elif (self.target_waypoint is not None and self.target_waypoint.graph_index is not None): print(f"[update] Calling update_off_grid_position() with pose " \ f"[{self.position_str()}] and waypoint[{self.target_waypoint.graph_index}]") self.update_handle.update_off_grid_position( diff --git a/fleet_adapter_ecobot/fleet_adapter_ecobot.py b/fleet_adapter_ecobot/fleet_adapter_ecobot.py index b54b0b7..a451ee8 100644 --- a/fleet_adapter_ecobot/fleet_adapter_ecobot.py +++ b/fleet_adapter_ecobot/fleet_adapter_ecobot.py @@ -191,28 +191,6 @@ def _add_fleet_robots(): node.get_logger().warn(f"Failed to get robot map name: [{robot_map_name}]") continue - # use defined transfrom param if avail, else use ref coors - # note that the robot's map_name should be identical to the one in config - if "rmf_transform" in config_yaml: - if robot_map_name not in config_yaml["rmf_transform"]: - assert False, f"Robot map name: {robot_map_name} isnt defined in config" - tx, ty, r, s = config_yaml["rmf_transform"][robot_map_name]["transform"] - rmf_transform = RmfMapTransform(tx, ty, r, s) - rmf_map_name = config_yaml["rmf_transform"][robot_map_name]['rmf_map_name'] - else: - rmf_transform = RmfMapTransform() - rmf_coordinates = config_yaml['reference_coordinates']['rmf'] - ecobot_coordinates = config_yaml['reference_coordinates']['ecobot'] - rmf_map_name = config_yaml['reference_coordinates']['rmf_map_name'] - mse = rmf_transform.estimate(ecobot_coordinates, rmf_coordinates) - print(f"Coordinate transformation error: {mse}") - - print(f"Coordinate Transform from [{robot_map_name}] to [{rmf_map_name}]") - tx, ty, r, s = rmf_transform.to_robot_map_transform() - print(f"RMF to Ecobot transform :: trans [{tx}, {ty}]; rot {r}; scale {s}") - tx, ty, r, s = rmf_transform.to_rmf_map_transform() - print(f"Ecobot to RMF transform :: trans [{tx}, {ty}]; rot {r}; scale {s}") - # get robot coordinates and transform to rmf_coor ecobot_pos = api.position() if ecobot_pos is None: @@ -220,12 +198,44 @@ def _add_fleet_robots(): continue node.get_logger().info(f"Initializing robot: {robot_name}") + + # use defined transfrom param if avail, else use ref coors + # note that the robot's map_name should be identical to the one in config + robot_map_transforms = {} + robot_map_tf = config_yaml["rmf_transform"] + for map_name, tf in robot_map_tf.items(): + print(f"Loading Map transform for robot map: {map_name} ") + rmf_transform = RmfMapTransform() + if "reference_coordinates" in tf: + rmf_coords = tf['reference_coordinates']['rmf'] + ecobot_coords = tf['reference_coordinates']['robot'] + mse = rmf_transform.estimate(ecobot_coords, rmf_coords) + print(f"Coordinate transformation error: {mse}") + elif "transform" in tf: + tx, ty, r, s = tf["transform"] + rmf_transform = RmfMapTransform(tx, ty, r, s) + else: + assert False, f"no transform provided for map {map_name}" + robot_map_transforms[map_name] = { + "rmf_map_name": tf["rmf_map_name"], + "tf": rmf_transform + } + + print(f"Coordinate Transform from [{map_name}] to [{tf['rmf_map_name']}]") + tx, ty, r, s = rmf_transform.to_robot_map_transform() + print(f"RMF to Ecobot transform :: trans [{tx}, {ty}]; rot {r}; scale {s}") + tx, ty, r, s = rmf_transform.to_rmf_map_transform() + print(f"Ecobot to RMF transform :: trans [{tx}, {ty}]; rot {r}; scale {s}") + rmf_config = missing_robots[robot_name]['rmf_config'] + assert robot_map_name in robot_map_transforms, "robot map isnt recognized" + current_map = robot_map_transforms[robot_map_name]["rmf_map_name"] starts = [] time_now = adapter.now() - x,y,_ = rmf_transform.to_rmf_map([ecobot_pos[0],ecobot_pos[1], 0]) + x,y,_ = robot_map_transforms[robot_map_name]["tf"].to_rmf_map( + [ecobot_pos[0],ecobot_pos[1], 0]) position = [x, y, 0] # Identify the current location of the robot in rmf's graph @@ -234,7 +244,7 @@ def _add_fleet_robots(): f"{robot_name}, with pos: {position}") starts = plan.compute_plan_starts( nav_graph, - rmf_map_name, + current_map, position, time_now, max_merge_waypoint_distance = 1.0, @@ -243,7 +253,7 @@ def _add_fleet_robots(): if starts is None or len(starts) == 0: node.get_logger().error( f"Unable to determine StartSet for {robot_name} " - f"with map {rmf_map_name}") + f"with map {current_map}") continue robot = EcobotCommandHandle( @@ -252,10 +262,7 @@ def _add_fleet_robots(): node=node, graph=nav_graph, vehicle_traits=vehicle_traits, - transforms=rmf_transform, - map_name=robot_map_name, - rmf_map_name=rmf_map_name, - position=position, + transforms=robot_map_transforms, charger_waypoint=rmf_config['charger']['waypoint'], update_frequency=rmf_config.get( 'robot_state_update_frequency', 1), From 1261d7580deed1f91781e18db6fb9fec2a180322 Mon Sep 17 00:00:00 2001 From: youliang Date: Thu, 12 May 2022 21:39:18 +0800 Subject: [PATCH 08/16] use new generic update_position(starts) and printout err with faulthandler Signed-off-by: youliang --- README.md | 5 ++ fleet_adapter_ecobot/EcobotCommandHandle.py | 55 +++++--------------- fleet_adapter_ecobot/fleet_adapter_ecobot.py | 2 + 3 files changed, 20 insertions(+), 42 deletions(-) diff --git a/README.md b/README.md index 92b3576..436c880 100644 --- a/README.md +++ b/README.md @@ -43,6 +43,11 @@ Then run the fleet adapter ros2 run fleet_adapter_ecobot fleet_adapter_ecobot -c CONFIG_FILE -n NAV_GRAPH --USE_SIM_TIME ``` +To run the fleet adapter with `rmf-web`, specify the server_uri: +``` +ros2 run fleet_adapter_ecobot fleet_adapter_ecobot -c CONFIG_FILE -n NAV_GRAPH --USE_SIM_TIME -s ws://localhost:8000/_internal +``` + Ensure that the `base_url` in the config matches the `LOCALHOST:PORT` specified to the server. ## Get the Transformation from `traffic-editor` diff --git a/fleet_adapter_ecobot/EcobotCommandHandle.py b/fleet_adapter_ecobot/EcobotCommandHandle.py index da6b19e..adee284 100644 --- a/fleet_adapter_ecobot/EcobotCommandHandle.py +++ b/fleet_adapter_ecobot/EcobotCommandHandle.py @@ -532,20 +532,19 @@ def update_location(self): fake_pos = [_x + math.cos(_theta)*2.5, _y + math.sin(_theta)*2.5, _theta] positions = [self.position, fake_pos] - # update robot current location with lost position, TODO: fix this as it is - # using the front() of the starts list, it the first start might not be the nearest - # self.update_handle.update_lost_position( - # self.rmf_map_name, self.position, - # max_merge_waypoint_distance = 1.0, max_merge_lane_distance) - - ## Get Closest point on graph and update current robot location - closest_wp = self.get_closest_waypoint_idx( - self.position, self.max_merge_lane_distance) - if closest_wp: - self.update_handle.update_off_grid_position( - self.position, closest_wp) + # Get potential startset, and update_position() + starts = plan.compute_plan_starts( + self.graph, + self.rmf_map_name, + self.position, + self.adapter.now(), + max_merge_waypoint_distance = 1.0, + max_merge_lane_distance = self.max_merge_lane_distance) + + if starts: + self.update_handle.update_position(starts) else: - self.node.get_logger().error(f"Cant find closest waypoint!") + self.node.get_logger().error(f"Cant get startset during perform action") self.update_handle.update_off_grid_position( self.position, self.target_waypoint.graph_index) @@ -583,7 +582,7 @@ def update_location(self): self.update_handle.update_off_grid_position( self.position, self.target_waypoint.graph_index) else: # if robot is lost - # TODO: use get_closest_waypoint_idx() + # TODO: use generic update_position() instead print("[update] Calling update_lost_position()") self.update_handle.update_lost_position( self.rmf_map_name, self.position) @@ -595,34 +594,6 @@ def update_location(self): def position_str(self): return f"{self.position[0]:.2f}, {self.position[1]:.2f}, {self.position[2]:.2f}" - def get_closest_waypoint_idx( - self, position: Tuple[float, float, float], - max_merge_lane_distance = 10.0 - ) -> Optional[int]: - """ - Find closest graph waypoint to the provided position, return waypoint idx - """ - starts = plan.compute_plan_starts( - self.graph, - self.rmf_map_name, - position, - self.adapter.now(), - max_merge_waypoint_distance = 1.0, - max_merge_lane_distance = max_merge_lane_distance) - if not starts: - return None - cx, cy, _ = position - closest_idx = -1 - closest_dist_sq = max_merge_lane_distance**2 - for s in starts: - idx = s.waypoint - x, y = self.graph.get_waypoint(idx).location - dist_sq = (x-cx)**2 + (y-cy)**2 - if (dist_sq < closest_dist_sq): - closest_idx = idx - closest_dist_sq = dist_sq - return closest_idx - def get_current_lane(self): def projection(current_position, target_position, diff --git a/fleet_adapter_ecobot/fleet_adapter_ecobot.py b/fleet_adapter_ecobot/fleet_adapter_ecobot.py index a451ee8..6dd6b89 100644 --- a/fleet_adapter_ecobot/fleet_adapter_ecobot.py +++ b/fleet_adapter_ecobot/fleet_adapter_ecobot.py @@ -17,6 +17,7 @@ import yaml import threading import time +import faulthandler import rclpy import rclpy.node @@ -295,6 +296,7 @@ def main(argv=sys.argv): # Init rclpy and adapter rclpy.init(args=argv) adpt.init_rclcpp() + faulthandler.enable() args_without_ros = rclpy.utilities.remove_ros_args(argv) parser = argparse.ArgumentParser( From de25210fb530b94a4976ff316ab34528412e8ec4 Mon Sep 17 00:00:00 2001 From: youliang Date: Fri, 20 May 2022 10:35:44 +0800 Subject: [PATCH 09/16] make perform action as a stubborn negotiator Signed-off-by: youliang --- fleet_adapter_ecobot/EcobotCommandHandle.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/fleet_adapter_ecobot/EcobotCommandHandle.py b/fleet_adapter_ecobot/EcobotCommandHandle.py index adee284..0771892 100644 --- a/fleet_adapter_ecobot/EcobotCommandHandle.py +++ b/fleet_adapter_ecobot/EcobotCommandHandle.py @@ -114,6 +114,7 @@ def __init__(self, self._quit_dock_event = threading.Event() self.action_execution = None + self.stubbornness = None self.in_error = False self.is_online = False self.action_category = None @@ -478,6 +479,7 @@ def _action_executor(self, self.on_waypoint = None self.on_lane = None self.action_execution = execution + self.stubbornness = self.update_handle.unstable_be_stubborn() # robot moves slower during perform action self.vehicle_traits.linear.nominal_velocity *= 0.2 @@ -516,6 +518,8 @@ def update_location(self): else: self.node.get_logger().warn(f"action [{self.action_category}] is killed/canceled") self.api.set_cleaning_mode(self.config['inactive_cleaning_config']) + self.stubbornness.release() + self.stubbornness = None self.action_execution = None self.start_action_time = None self.vehicle_traits.linear.nominal_velocity *= 5 # change back vel @@ -538,7 +542,7 @@ def update_location(self): self.rmf_map_name, self.position, self.adapter.now(), - max_merge_waypoint_distance = 1.0, + max_merge_waypoint_distance = 0.5, max_merge_lane_distance = self.max_merge_lane_distance) if starts: From 70dab6e657ca281bb0a299b7fe03785d4d4e1d54 Mon Sep 17 00:00:00 2001 From: youliang Date: Tue, 24 May 2022 15:32:08 +0800 Subject: [PATCH 10/16] fix localize api rout Signed-off-by: youliang --- fleet_adapter_ecobot/EcobotClientAPI.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fleet_adapter_ecobot/EcobotClientAPI.py b/fleet_adapter_ecobot/EcobotClientAPI.py index e0ef900..d0388ee 100644 --- a/fleet_adapter_ecobot/EcobotClientAPI.py +++ b/fleet_adapter_ecobot/EcobotClientAPI.py @@ -55,7 +55,7 @@ def localize(self, init_point:str, map_name:str, rotate=False): if rotate: # The robot will rotate to improve localization url = self.prefix + f"/gs-robot/cmd/initialize?map_name={map_name}&init_point_name={init_point}" else: # The specified init point must be accurate - url = self.prefix + f"/gs-robot/cmd/initialize_directly?map_name={map_name}&init_point_name=?{init_point}" + url = self.prefix + f"/gs-robot/cmd/initialize_directly?map_name={map_name}&init_point_name={init_point}" try: response = requests.get(url, timeout=self.timeout) response.raise_for_status() From e108fb4699648a8e75011276e39e35ae73cd47a7 Mon Sep 17 00:00:00 2001 From: youliang Date: Tue, 21 Jun 2022 14:34:17 +0800 Subject: [PATCH 11/16] code cleanups and update docs and demo example Signed-off-by: youliang --- README.md | 56 +++- config.yaml => configs/robot_config.yaml | 78 +---- configs/test_api_config.yaml | 22 ++ fleet_adapter_ecobot/EcobotClientAPI.py | 6 +- fleet_adapter_ecobot/EcobotCommandHandle.py | 276 ++++++++-------- fleet_adapter_ecobot/TestClientAPI.py | 120 ++++--- .../clicked_point_transform.py | 75 +++++ fleet_adapter_ecobot/ecobot_sim_server.py | 300 ------------------ fleet_adapter_ecobot/fleet_adapter_ecobot.py | 66 ++-- setup.py | 4 +- 10 files changed, 383 insertions(+), 620 deletions(-) rename config.yaml => configs/robot_config.yaml (51%) create mode 100644 configs/test_api_config.yaml create mode 100644 fleet_adapter_ecobot/clicked_point_transform.py delete mode 100644 fleet_adapter_ecobot/ecobot_sim_server.py diff --git a/README.md b/README.md index 436c880..824721d 100644 --- a/README.md +++ b/README.md @@ -20,7 +20,6 @@ Ensure the robot can be pinged. ``` ros2 run fleet_adapter_ecobot fleet_adapter_ecobot -c CONFIG_FILE -n NAV_GRAPH - ``` Dispatch a Task @@ -28,32 +27,61 @@ Dispatch a Task ros2 run rmf_demos_tasks dispatch_action -s lounge -a clean -ad '{ "clean_task_name": "clean_lounge" }' ``` -## Test the fleet adapter in simulation -The adapter can be tested in simulation with the help of the [ecobot_sim_server](fleet_adapter_ecobot/ecobot_sim_server.py). -This script emulates the API server of the robot and can be used to command robots in simulation to follow commands from Open-RMF. - -First run the server +To run the fleet adapter with [rmf-web](https://github.com/open-rmf/rmf-web/), specify the server_uri (`-s`): +```bash +ros2 run fleet_adapter_ecobot fleet_adapter_ecobot -c CONFIG_FILE -n NAV_GRAPH -s ws://localhost:8000/_internal ``` -ros2 run fleet_adapter_ecobot ecobot_sim_server -c CONFIG_FILE -n NAV_GRAPH -p PORT -``` +## Test the fleet adapter in "Mock Mode" +The adapter can be tested in mock mode with the help of the [TestClientAPI](fleet_adapter_ecobot/TestClientAPI.py). This class emulates the member functions of `EcobotClientAPI.py` which calls the rest apis of the robot. This "mock mode" is enabled by providing `-tf` argument. -Then run the fleet adapter +Run this example in office world: +```bash +ros2 launch rmf_demos office.launch.xml run_fleet_adapters:=0 ``` -ros2 run fleet_adapter_ecobot fleet_adapter_ecobot -c CONFIG_FILE -n NAV_GRAPH --USE_SIM_TIME + +Then run the ecobot fleet adapter +```bash +# Note the addition of "--test_client_api" and "-tf" +ros2 run fleet_adapter_ecobot fleet_adapter_ecobot \ + -c src/fleet_adapter_ecobot/configs/robot_config.yaml \ + -n install/rmf_demos_maps/share/rmf_demos_maps/maps/office/nav_graphs/0.yaml \ + -s "ws://localhost:8000/_internal" \ + -tf src/fleet_adapter_ecobot/configs/test_api_config.yaml ``` -To run the fleet adapter with `rmf-web`, specify the server_uri: +Different to the simulation running on gazebo, this `TestClientAPI` mocks the behavior of the fleet adapter when receives command from RMF. Thus, the script is running on actual system wall time. + +### Patrol Task +Now try to command robot to move to `Pantry` +```bash +ros2 run rmf_demos_tasks dispatch_patrol -p pantry ``` -ros2 run fleet_adapter_ecobot fleet_adapter_ecobot -c CONFIG_FILE -n NAV_GRAPH --USE_SIM_TIME -s ws://localhost:8000/_internal + +### Custom Clean Task +Send the robot to clean an area. This custom clean task is created by composing `go_to_place` and custom `perform_action`. +```bash +ros2 run rmf_demos_tasks dispatch_action -s patrol_D2 -a clean -ad '{ "clean_task_name": "clean_hallway" }' ``` -Ensure that the `base_url` in the config matches the `LOCALHOST:PORT` specified to the server. +### Docking +Add a `dock_name` on a charger waypoint in traffic editor. This will then call the `dock()` function when the robot is approaching the charger. ## Get the Transformation from `traffic-editor` -To get the transformation of the robot map to rmf map, user can add a "floorplan" of a robot map. Then annotate and the corresponding "constraint-pairs", lastly ctrl-T to let traffic-editor calculate the respective transformation. +To get the transformation of the robot map to rmf map, user can add a "floorplan" of a robot map. Then annotate and the corresponding "constraint-pairs", lastly `ctrl-T` to let traffic-editor calculate the respective transformation. ![](../media/media/traffic-editor-transform.png) Specify this transformation to the `rmf_transform` in the `config.yaml` file. Note that the value of Y-axis is applied with a -ve. + + +Then if you wish to configure your custom waypoints in the `configs/test_api_config.yaml`, you can use rviz to obtain those points in ecobot coordinates. Run this on a separate terminal. +```bash +# first run the office map +ros2 launch rmf_demos office.launch.xml run_fleet_adapters:=0 +# then run this on a seperate terminal +ros2 run fleet_adapter_ecobot clicked_point_transform -tf 1.33 0.057 -1.598 0.057 +``` + +Subsequently, select "publish point" on rviz, then click on the respective point on the map. Immediately, the point in rmf and robot coordinate will get published on `clicked_point_transform` terminal. These coordinates are helpful during debugging. diff --git a/config.yaml b/configs/robot_config.yaml similarity index 51% rename from config.yaml rename to configs/robot_config.yaml index d945c5d..6f06ba8 100644 --- a/config.yaml +++ b/configs/robot_config.yaml @@ -29,8 +29,8 @@ rmf_fleet: task_capabilities: # Specify the types of RMF Tasks that robots in this fleet are capable of performing loop: True delivery: False - clean: True - finishing_request: "park" + clean: False # deprecated, now replace as custom action + finishing_request: "nothing" action_categories: ["clean", "manual_control"] # Ecobot CONFIG ================================================================= @@ -39,51 +39,16 @@ robots: ecobot40_1: ecobot_config: base_url: "http://10.7.5.88:8080" - user: "some_user" #unused - password: "some_password" #unused max_delay: 30.0 # allowed seconds of delay of the current itinerary before it gets interrupted and replanned filter_waypoints: True cleaning_task_prefix: "" # the prefix of the cleaning task created active_cleaning_config: "light_cleaning" # the cleaning config used during cleaning inactive_cleaning_config: "no_cleaning" # the cleaning config used during navigation - # docks: # in rmf coordinates - # vacuum_zone_1: [ [106.3, -20.63, -1.57], - # [106.3, -21.23, 0.0], - # [111.2, -21.15, 1.57], - # [111.1, -17.12, -3.14], - # [106.84, -17.07, -1.57], - # [106.4, -18.98, -1.57]] - # vacuum_zone_2: [ [87.14, -57.59, 0.0], - # [89.66, -57.6, 1.57], - # [89.56, -62.35, 3.14], - # [83.93, -62.35, 3.14], - # [83.93, -59.58, 0.0], - # [87.03, -59.69, 0.0]] - # vacuum_zone_3: [ [26.81, -62.43, -1.57], - # [26.95, -65.09, -3.14], - # [23.57, -65.09, 1.57], - # [23.54, -62.5, 0.0], - # [26.81, -62.43, 0.0]] - # vacuum_zone_4: [ [22.31, -54.06, 3.14], - # [15.3, -53.9, 1.57], - # [15.31, -49.5, 0.0], - # [22.53, -49.56, -1.57], - # [22.5, -52.59, -1.57]] - # vacuum_zone_5: [ [83.0, -33.06, 0.0], - # [81.56, -36.48, -1.57], - # [83.78, -37.62, 0.0], - # [84.91, -34.19, 1.57]] rmf_config: robot_state_update_frequency: 0.5 max_merge_lane_distance: 15.0 # means how far will the robot diverge from the defined graph - # start: - # map_name: "L1" - # waypoint: "charger_ecobot1" # Optional - # orientation: 0.0 # Optional, radians - # waypoint: null - # orientation: null charger: - waypoint: "charger_ecobot40_1" + waypoint: "tinyRobot1_charger" # TRANSFORM CONFIG ============================================================= # For computing transforms between Ecobot and RMF coordinate systems @@ -94,27 +59,18 @@ robots: # Format robot_map: {rmf_map_name: rmf_map, reference_coordinates: # {rmf: [[x, y]....], rmf: [[x, y]....]}} rmf_transform: - L1: + mock_test_robot_map: rmf_map_name: "L1" - reference_coordinates: - rmf: [[33.11, -18.99], - [111.3, -19.06], - [86.16, -19.02], - [79.23, -46.94], - [82.22, -58.3], - [52.65, -58.37], - [8.234, -58.35], - [9.23, -30.5], - [24.68, -19.08] ] - robot: [ [3876, 741], - [2293, 703], - [2816, 710], - [2930, 1250], - [2861, 1501], - [3460, 1524], - [4360, 1555], - [4347, 991], - [4058, 747] ] - office_map: - rmf_map_name: "L1" - transform: [0.556, 0.02, -1.58, 0.05] + transform: [1.33, 0.057, -1.598, 0.057] # This is obtained from traffic-editor + ## User can also provide a pair of "reference_coordinates" + # mock_test_robot_map2: + # rmf_map_name: "L1" + # reference_coordinates: + # rmf: [[33.11, -18.99], + # [111.3, -19.06], + # [86.16, -19.02], + # [24.68, -19.08] ] + # robot: [ [3876, 741], + # [2293, 703], + # [2816, 710], + # [4058, 747] ] diff --git a/configs/test_api_config.yaml b/configs/test_api_config.yaml new file mode 100644 index 0000000..f7c43ab --- /dev/null +++ b/configs/test_api_config.yaml @@ -0,0 +1,22 @@ +# note that yaw (x, y, yaw) from gaussian api is in degree, not radian +# mock_dock_path is empty since dock_name/dock() is not used in this scenario + +mock_clean_path: [ + [57.06, 141.63, 0], + [70.43, 132.84, 0], + [84.60, 139.63, 45], + [108.08, 163.16, 90], + [112.99, 195.95, 90], + [114.13, 221.31, 90], + [114.09, 256.41, 90], + [111.45, 308.84, 180], + [61.71, 307.48, 180], + [54.43, 260.89, -90], + [59.49, 220.74, -90], + [64.15, 172.95, -90], + [49.28, 158.20, -90], +] +mock_dock_path: [] +dock_position: [85.28, 338.95, 0] # charging point +mock_location: [50.89, 325.29, 0] # this indicate where the robot will start +mock_robot_map_name: "mock_test_robot_map" diff --git a/fleet_adapter_ecobot/EcobotClientAPI.py b/fleet_adapter_ecobot/EcobotClientAPI.py index d0388ee..3bb5792 100644 --- a/fleet_adapter_ecobot/EcobotClientAPI.py +++ b/fleet_adapter_ecobot/EcobotClientAPI.py @@ -26,7 +26,6 @@ def __init__(self, prefix:str, cleaning_task_prefix="", timeout=5.0, debug=False self.cleaning_task_prefix = cleaning_task_prefix self.debug = debug self.timeout = timeout - self.connected = False # Test connectivity data = self.data() if data is not None: @@ -34,6 +33,10 @@ def __init__(self, prefix:str, cleaning_task_prefix="", timeout=5.0, debug=False self.connected = True else: print("[EcobotAPI] unable to query API server") + self.connected = False + + def online(self): + return self.connected def load_map(self, map_name: str): url = self.prefix + f"/gs-robot/cmd/load_map?map_name={map_name}" @@ -109,6 +112,7 @@ def navigate(self, pose, map_name:str): return response.json()['successed'] except HTTPError as http_err: print(f"HTTP error: {http_err}") + self.connected = False except Exception as err: print(f"Other error: {err}") return False diff --git a/fleet_adapter_ecobot/EcobotCommandHandle.py b/fleet_adapter_ecobot/EcobotCommandHandle.py index 0771892..191dd9d 100644 --- a/fleet_adapter_ecobot/EcobotCommandHandle.py +++ b/fleet_adapter_ecobot/EcobotCommandHandle.py @@ -17,8 +17,6 @@ import rmf_adapter.plan as plan import rmf_adapter.schedule as schedule -from rmf_fleet_msgs.msg import DockSummary - import numpy as np import threading @@ -37,19 +35,19 @@ class EcobotState(enum.IntEnum): IDLE = 0 WAITING = 1 MOVING = 2 - CLEANING =3 # Custom wrapper for Plan::Waypoint. We use this to modify position of waypoints # to prevent backtracking class PlanWaypoint: def __init__(self, index, wp:plan.Waypoint): - self.index = index # this is the index of the Plan::Waypoint in the waypoints in follow_new_path + # the index of the Plan::Waypoint in the waypoints in follow_new_path + self.index = index self.position = wp.position self.time = wp.time self.graph_index = wp.graph_index self.approach_lanes = wp.approach_lanes - +############################################################################## class EcobotCommandHandle(adpt.RobotCommandHandle): def __init__(self, name, @@ -80,10 +78,6 @@ def __init__(self, # Get the index of the charger waypoint self.charger_waypoint = self.graph.find_waypoint(charger_waypoint) - # assert waypoint, f"Charger waypoint {charger_waypoint} does not exist in the navigation graph" - if self.charger_waypoint is None: - node.get_logger().error(f"Charger waypoint {charger_waypoint} does not exist in the navigation graph") - self.update_frequency = update_frequency self.update_handle = None # RobotUpdateHandle self.battery_soc = 1.0 @@ -91,7 +85,6 @@ def __init__(self, self.state = EcobotState.IDLE self.adapter = adapter - self.requested_waypoints = [] # RMF Plan waypoints self.remaining_waypoints = [] self.path_finished_callback = None self.next_arrival_estimator = None @@ -133,6 +126,7 @@ def __init__(self, # cleaning systems. These will be activated only during cleaning self.api.set_cleaning_mode(self.config['inactive_cleaning_config']) + ############################################################################## # Init RobotUpdateHandle class member def init_handler(self, handle): self.update_handle = handle @@ -140,10 +134,12 @@ def init_handler(self, handle): max_delay = self.config["max_delay"] print(f"Setting max delay to {max_delay}s") self.update_handle.set_maximum_delay(max_delay) - if (self.charger_waypoint.index < self.graph.num_waypoints): + if self.charger_waypoint is not None: self.update_handle.set_charger_waypoint(self.charger_waypoint.index) else: - self.node.get_logger().info("Invalid waypoint supplied for charger. Using default nearest charger in the map") + self.node.get_logger().error( + f"Charger waypoint {self.charger_waypoint} does not exist in the nav graph." + " Using default nearest charger in the map") self.update_handle.set_action_executor(self._action_executor) self.participant = self.update_handle.get_unstable_participant() @@ -156,13 +152,13 @@ def init_handler(self, handle): 4.5 / self.update_frequency, self.update_robot_status) - self.node.get_logger().info(f"Start State Update with freq: {self.update_frequency}") - + self.node.get_logger().info( + f"Start State Update with freq: {self.update_frequency}") + ############################################################################## def clear(self): with self._lock: print(f"Clearing internal states with current waypoint {self.on_waypoint}") - self.requested_waypoints = [] self.remaining_waypoints = [] self.path_finished_callback = None self.next_arrival_estimator = None @@ -170,7 +166,7 @@ def clear(self): # self.target_waypoint = None self.state = EcobotState.IDLE - + ############################################################################## # override function def stop(self): # Stop motion of the robot. Tracking variables should remain unchanged. @@ -180,7 +176,7 @@ def stop(self): break time.sleep(1.0) - + ############################################################################## # override function def follow_new_path( self, @@ -264,8 +260,8 @@ def _follow_path(): print(f"Requesting robot to navigate to " f"[{self.path_index}][{x:.0f},{y:.0f},{theta:.0f}] " - f"grid coordinates and [{target_pose[0]:.2f}. {target_pose[1]:.2f}, {target_pose[2]:.2f}] " - f"RMF coordinates...") + f"grid coordinates and [{target_pose[0]:.2f}. {target_pose[1]:.2f}, " + f"{target_pose[2]:.2f}] RMF coordinates...") response = self.api.navigate([x, y, theta], self.robot_map_name) if response: @@ -289,17 +285,16 @@ def _follow_path(): if (waypoint_wait_time < time_now): self.state = EcobotState.IDLE # self.target_waypoint = None - else: - if self.path_index is not None: - # self.node.get_logger().info(f"Waiting for {(waypoint_wait_time - time_now).seconds}s") - self.next_arrival_estimator(self.path_index, timedelta(seconds=0.0)) - + elif self.path_index is not None: + # self.node.get_logger().info(f"Waiting for {(waypoint_wait_time - time_now).seconds}s") + self.next_arrival_estimator(self.path_index, timedelta(seconds=0.0)) elif self.state == EcobotState.MOVING: time.sleep(0.5) self.node.get_logger().info("Moving...") # Check if we have reached the target with self._lock: + lane = self.get_current_lane() if (self.api.navigation_completed()): print(f"Robot [{self.name}] has reached its target waypoint") self.state = EcobotState.WAITING @@ -308,35 +303,30 @@ def _follow_path(): self.last_known_waypoint_index = self.on_waypoint else: self.on_waypoint = None # we are still on a lane + elif lane is not None: + self.on_waypoint = None + self.on_lane = lane + # The robot may either be on the previous + # waypoint or the target one + elif (self.target_waypoint.graph_index is not None and + self.dist(self.position, target_pose) < 0.5): + self.on_waypoint = self.target_waypoint.graph_index + elif self.last_known_waypoint_index is not None and \ + self.dist( + self.position, + self.graph.get_waypoint(self.last_known_waypoint_index).location) < 0.5: + self.on_waypoint = self.last_known_waypoint_index else: - # Update the lane the robot is on - lane = self.get_current_lane() - if lane is not None: - self.on_waypoint = None - self.on_lane = lane - else: - # The robot may either be on the previous - # waypoint or the target one - if (self.target_waypoint.graph_index is not None and - self.dist(self.position, target_pose) < 0.5): - self.on_waypoint = self.target_waypoint.graph_index - elif self.last_known_waypoint_index is not None and \ - self.dist(self.position, self.graph.get_waypoint(self.last_known_waypoint_index).location) < 0.5: - self.on_waypoint = self.last_known_waypoint_index - else: - self.on_lane = None # update_off_grid() - self.on_waypoint = None - # Find next arrival estimate - if self.path_index is not None: - dist_to_target = self.dist(self.position, target_pose) - ori_delta = abs(abs(self.position[2]) - abs(target_pose[2])) - if ori_delta > np.pi: - ori_delta = ori_delta - (2 * np.pi) - if ori_delta < -np.pi: - ori_delta = (2 * np.pi) + ori_delta - duration = dist_to_target/self.vehicle_traits.linear.nominal_velocity +\ - ori_delta/self.vehicle_traits.rotational.nominal_velocity - self.next_arrival_estimator(self.path_index, timedelta(seconds=duration)) + self.on_lane = None # update_off_grid() + self.on_waypoint = None + # Find next arrival estimate + if self.path_index is not None: + dist_to_target = self.dist(self.position, target_pose) + ori_delta = abs(abs(self.position[2]) - abs(target_pose[2])) + ori_delta = (ori_delta + math.pi) % (2*math.pi) - math.pi #convert to within range -pi, pi + duration = dist_to_target/self.vehicle_traits.linear.nominal_velocity +\ + ori_delta/self.vehicle_traits.rotational.nominal_velocity + self.next_arrival_estimator(self.path_index, timedelta(seconds=duration)) self.path_finished_callback() self.node.get_logger().info(f"Robot {self.name} has successfully navigated along requested path.") # self.target_waypoint = None @@ -344,7 +334,7 @@ def _follow_path(): target=_follow_path) self._follow_path_thread.start() - + ############################################################################## # override function def dock( self, @@ -400,7 +390,7 @@ def _dock(): self._dock_thread = threading.Thread(target=_dock) self._dock_thread.start() - + ############################################################################## def get_robot_position(self) -> List[int]: ''' This helper function returns the live position of the robot in the RMF coordinate frame @@ -429,7 +419,7 @@ def get_robot_position(self) -> List[int]: self.rmf_map_name = tf['rmf_map_name'] return [x,y,theta] - + ############################################################################## def get_battery_soc(self): battery_soc = self.api.battery_soc() if battery_soc is not None: @@ -440,7 +430,7 @@ def get_battery_soc(self): self.node.get_logger().error("Unable to retrieve battery data from robot") return self.battery_soc - + ############################################################################## # call this when starting cleaning execution def _action_executor(self, category: str, @@ -455,10 +445,12 @@ def _action_executor(self, if (category == "clean"): attempts = 0 self.api.set_cleaning_mode(self.config['active_cleaning_config']) + # Will try to make max 3 attempts to start the clean task while True: - self.node.get_logger().info(f"Requesting robot {self.name} to clean {description}") + self.node.get_logger().info( + f"Requesting robot {self.name} to clean {description}") if self.api.start_clean(description["clean_task_name"], self.robot_map_name): - self.check_task_completion = self.api.task_completed # will check api + self.check_task_completion = self.api.task_completed # check api func break if (attempts > 3): self.node.get_logger().warn( @@ -483,7 +475,7 @@ def _action_executor(self, # robot moves slower during perform action self.vehicle_traits.linear.nominal_velocity *= 0.2 - + ############################################################################## # Update robot state's status def update_robot_status(self): if self.api.is_charging(): @@ -498,7 +490,68 @@ def update_robot_status(self): else: self.update_handle.override_status(None) - + ############################################################################## + # This function will be called periodically to check if the action if completed + def check_perform_action(self): + print(f"Executing perform action [{self.action_category}]") + # check if action is completed/killed/canceled + action_ok = self.action_execution.okay() + if self.check_task_completion() or not action_ok: + if action_ok: + self.node.get_logger().info( + f"action [{self.action_category}] is completed") + self.action_execution.finished() + else: + self.node.get_logger().warn( + f"action [{self.action_category}] is killed/canceled") + self.api.set_cleaning_mode(self.config['inactive_cleaning_config']) + self.stubbornness.release() + self.stubbornness = None + self.action_execution = None + self.start_action_time = None + self.vehicle_traits.linear.nominal_velocity *= 5 # change back vel + return + + # still executing perform action + assert(self.participant) + assert(self.start_action_time) + total_action_time = timedelta(hours=1.0) #TODO: populate actual total time + remaining_time = total_action_time - (self.adapter.now() - self.start_action_time) + print(f"Still performing action, Estimated remaining time: [{remaining_time}]") + self.action_execution.update_remaining_time(remaining_time) + + # create a short segment of the trajectory according to robot current heading + _x, _y, _theta = self.position + mock_pos = [_x + math.cos(_theta)*2.5, _y + math.sin(_theta)*2.5, _theta] + positions = [self.position, mock_pos] + + starts = self.get_start_sets() + if starts is not None: + self.update_handle.update_position(starts) + else: + self.node.get_logger().error(f"Cant get startset during perform action") + self.update_handle.update_off_grid_position( + self.position, self.target_waypoint.graph_index) + + trajectory = schedule.make_trajectory( + self.vehicle_traits, + self.adapter.now(), + positions) + route = schedule.Route(self.rmf_map_name, trajectory) + self.participant.set_itinerary([route]) + + ############################################################################## + # Get start sets, for update_position(startsets) + def get_start_sets(self): + return plan.compute_plan_starts( + self.graph, + self.rmf_map_name, + self.position, + self.adapter.now(), + max_merge_waypoint_distance = 0.5, + max_merge_lane_distance = self.max_merge_lane_distance) + + ############################################################################## # Update location and check cleaning action def update_location(self): self.position = self.get_robot_position() @@ -508,56 +561,7 @@ def update_location(self): # Update states and positions with self._lock: if (self.action_execution): - print(f"Executing perform action [{self.action_category}]") - # check if action is completed/killed/canceled - action_ok = self.action_execution.okay() - if self.check_task_completion() or not action_ok: - if action_ok: - self.node.get_logger().info(f"action [{self.action_category}] is completed") - self.action_execution.finished() - else: - self.node.get_logger().warn(f"action [{self.action_category}] is killed/canceled") - self.api.set_cleaning_mode(self.config['inactive_cleaning_config']) - self.stubbornness.release() - self.stubbornness = None - self.action_execution = None - self.start_action_time = None - self.vehicle_traits.linear.nominal_velocity *= 5 # change back vel - else: - assert(self.participant) - assert(self.start_action_time) - total_action_time = timedelta(hours=1.0) #TODO: populate actual total time - remaining_time = total_action_time - (self.adapter.now() - self.start_action_time) - print(f"Still performing action, Estimated remaining time: [{remaining_time}]") - self.action_execution.update_remaining_time(remaining_time) - - # create a short segment of the trajectory according to robot current heading - _x, _y, _theta = self.position - fake_pos = [_x + math.cos(_theta)*2.5, _y + math.sin(_theta)*2.5, _theta] - positions = [self.position, fake_pos] - - # Get potential startset, and update_position() - starts = plan.compute_plan_starts( - self.graph, - self.rmf_map_name, - self.position, - self.adapter.now(), - max_merge_waypoint_distance = 0.5, - max_merge_lane_distance = self.max_merge_lane_distance) - - if starts: - self.update_handle.update_position(starts) - else: - self.node.get_logger().error(f"Cant get startset during perform action") - self.update_handle.update_off_grid_position( - self.position, self.target_waypoint.graph_index) - - trajectory = schedule.make_trajectory( - self.vehicle_traits, - self.adapter.now(), - positions) - route = schedule.Route(self.rmf_map_name, trajectory) - self.participant.set_itinerary([route]) + self.check_perform_action() elif (self.on_waypoint is not None): # if robot is on a waypoint print(f"[update] Calling update_current_waypoint() on waypoint with " \ f"pose[{self.position_str()}] and waypoint[{self.on_waypoint}]") @@ -585,11 +589,15 @@ def update_location(self): f"[{self.position_str()}] and waypoint[{self.target_waypoint.graph_index}]") self.update_handle.update_off_grid_position( self.position, self.target_waypoint.graph_index) - else: # if robot is lost - # TODO: use generic update_position() instead - print("[update] Calling update_lost_position()") - self.update_handle.update_lost_position( - self.rmf_map_name, self.position) + else: # if unsure which waypoint the robot is near to + starts = self.get_start_sets() + if starts is not None: + print("[update] Calling generic update_position()") + self.update_handle.update_position(starts) + else: + print("[update] Calling update_lost_position()") + self.update_handle.update_lost_position( + self.rmf_map_name, self.position) ######################################################################## ## Utils @@ -627,7 +635,6 @@ def projection(current_position, return lane_index return None - # TODO: remove the usage of sqrt for efficiency def dist(self, A, B): ''' Euclidian distance between A(x,y) and B(x,y)''' assert(len(A) > 1) @@ -666,11 +673,10 @@ def filter_waypoints(self, wps:list, threshold = 1.0): # Find the first waypoint index = 0 while (not changed and index < len(waypoints)): - if (self.dist(last_pose,waypoints[index].position) < threshold): - first = waypoints[index] - last_pose = waypoints[index].position - else: + if (self.dist(last_pose,waypoints[index].position) > threshold): break + first = waypoints[index] + last_pose = waypoints[index].position index = index + 1 while (index < len(waypoints)): @@ -680,26 +686,26 @@ def filter_waypoints(self, wps:list, threshold = 1.0): changed = False while (not changed): next_index = index + 1 - if (next_index < len(waypoints)): - if (self.dist(waypoints[next_index].position, waypoints[index].position) < threshold): - if (next_index == len(waypoints) - 1): - # append last waypoint - changed = True - wp = waypoints[next_index] - wp.approach_lanes = waypoints[parent_index].approach_lanes - second.append(wp) - else: - # append if next waypoint changes - changed = True - wp = waypoints[index] - wp.approach_lanes = waypoints[parent_index].approach_lanes - second.append(wp) - else: + if (next_index >= len(waypoints)): # we add the current index to second changed = True wp = waypoints[index] wp.approach_lanes = waypoints[parent_index].approach_lanes second.append(wp) + elif (self.dist( + waypoints[next_index].position, + waypoints[index].position) >= threshold): + # append if next waypoint changes + changed = True + wp = waypoints[index] + wp.approach_lanes = waypoints[parent_index].approach_lanes + second.append(wp) + elif (next_index == len(waypoints) - 1): + # append last waypoint + changed = True + wp = waypoints[next_index] + wp.approach_lanes = waypoints[parent_index].approach_lanes + second.append(wp) last_pose = waypoints[index].position index = next_index else: diff --git a/fleet_adapter_ecobot/TestClientAPI.py b/fleet_adapter_ecobot/TestClientAPI.py index 07fe125..be3c068 100644 --- a/fleet_adapter_ecobot/TestClientAPI.py +++ b/fleet_adapter_ecobot/TestClientAPI.py @@ -12,93 +12,81 @@ # See the License for the specific language governing permissions and # limitations under the License. -import json -import math -from urllib.error import HTTPError +import sys +import yaml class ClientAPI: - def __init__(self): + def __init__(self, config_file): self.connected = True - self.fake_clean_path = [ - [989.96853, 1136.82267, 0], - [1056.7065, 1087.30105, 180], - [1056.7065, 1087.30105, 150], - [1056.7065, 1087.30105, 130], - [801.63, 1492.17, 0], - [801.60, 1485.17, 20], - [801.55, 1475.17, 40], - ] - self.fake_dock_path = [ - [968.51, 1325.25, 0], - [965.21, 1338.18, 0], - [963.21, 1348.18, 0], - [962.59, 1360.78, 0], - [960.59, 1370.78, 0], - [960.59, 1363.78, 0], - [964.36, 1356.75, 0] - ] - self.is_fake_cleaning = False - self.is_fake_docking = False + with open(config_file, "r") as f: + config_yaml = yaml.safe_load(f) + + # These configs are provided in the yaml file + self.mock_clean_path = config_yaml["mock_clean_path"] + self.mock_dock_path = config_yaml["mock_dock_path"] + self.dock_position = config_yaml["dock_position"] + self.mock_location = config_yaml["mock_location"] + self.mock_robot_map_name = config_yaml["mock_robot_map_name"] + + self.is_mock_cleaning = False + self.is_mock_docking = False self.task_wp_idx = 0 - self.task_wp_idx = 0 - print("[TEST ECOBOT CLIENT API] successfully setup fake client api class") - self.connected = True - self.dock_position = [977, 1372, 0] - self.fake_location = [977.5834, 1192.0576, 0] - self.fake_robot_map_name = "sim_test_robot_map" - # self.fake_location = [1072.43, 899.82, 0] #Offgrid start + print("[TEST ECOBOT CLIENT API] successfully setup mock client api class") + + def online(self): + return True def current_map(self): - print(f"[TEST CLIENT API] return testing map: {self.fake_robot_map_name}") - return self.fake_robot_map_name + print(f"[TEST CLIENT API] return testing map: {self.mock_robot_map_name}") + return self.mock_robot_map_name def position(self): ''' Returns [x, y, theta] expressed in the robot's coordinate frame or None''' - if self.is_fake_cleaning: - print(f"[TEST CLIENT API] provide fake cleaning position") - return self.fake_clean_path[self.task_wp_idx] - elif self.is_fake_docking: - print(f"[TEST CLIENT API] provide fake docking position") - return self.fake_dock_path[self.task_wp_idx] - print(f"[TEST CLIENT API] provide position [{self.fake_location}]") - return self.fake_location + if self.is_mock_cleaning: + print(f"[TEST CLIENT API] provide mock cleaning position") + return self.mock_clean_path[self.task_wp_idx] + elif self.is_mock_docking: + print(f"[TEST CLIENT API] provide mock docking position") + return self.mock_dock_path[self.task_wp_idx] + print(f"[TEST CLIENT API] provide position [{self.mock_location}]") + return self.mock_location def navigate(self, pose, map_name:str): ''' Here pose is a list containing (x,y,theta) where x and y are in grid coordinate frame and theta is in degrees. This functions should return True if the robot received the command, else False''' assert(len(pose) > 2) - self.fake_location = pose - print(f"[TEST CLIENT API] moved to fake location {pose}") + self.mock_location = pose + print(f"[TEST CLIENT API] moved to mock location {pose}") return True def navigate_to_waypoint(self, waypoint_name, map_name): ''' Ask the robot to navigate to a preconfigured waypoint on a map. Returns True if the robot received the command''' - print(f"[TEST CLIENT API] moved to fake waypoint {waypoint_name}") - self.is_fake_docking = True + print(f"[TEST CLIENT API] moved to mock waypoint {waypoint_name}") + self.is_mock_docking = True return True def start_task(self, name:str, map_name:str): ''' Returns True if the robot has started the generic task, else False''' - print(f"[TEST CLIENT API] Start fake task : {name}") - self.is_fake_docking = True + print(f"[TEST CLIENT API] Start mock task : {name}") + self.is_mock_docking = True return True def start_clean(self, name:str, map_name:str): ''' Returns True if the robot has started the cleaning process, else False''' - print(f"[TEST CLIENT API] Set fake start CLEANING : {name}") - self.is_fake_cleaning = True + print(f"[TEST CLIENT API] Set mock start CLEANING : {name}") + self.is_mock_cleaning = True return True def pause(self): print(f"[TEST CLIENT API] Pause Robot") - self.is_fake_cleaning = True + self.is_mock_cleaning = True return True def resume(self): print(f"[TEST CLIENT API] Resume Robot") - self.is_fake_cleaning = True + self.is_mock_cleaning = True return True def stop(self): @@ -107,46 +95,46 @@ def stop(self): return True def navigation_completed(self): - print(f"[TEST CLIENT API] FAKE NAV completed") + print(f"[TEST CLIENT API] MOCK NAV completed") return True def task_completed(self): ''' For ecobots the same function is used to check completion of navigation & cleaning''' self.task_wp_idx += 1 - if self.is_fake_docking: - if self.task_wp_idx < len(self.fake_dock_path): - print(f"[TEST CLIENT API] Fake nav/dock task in process") + if self.is_mock_docking: + if self.task_wp_idx < len(self.mock_dock_path): + print(f"[TEST CLIENT API] Mock nav/dock task in process") return False else: self.task_wp_idx = 0 - self.is_fake_docking = False - self.fake_location = self.dock_position - print(f"[TEST CLIENT API] Fake nav/dock task COMPLETED") + self.is_mock_docking = False + self.mock_location = self.dock_position + print(f"[TEST CLIENT API] Mock nav/dock task COMPLETED") return True else: - if self.task_wp_idx < len(self.fake_clean_path): - print(f"[TEST CLIENT API] FAKE CLEANING in process") + if self.task_wp_idx < len(self.mock_clean_path): + print(f"[TEST CLIENT API] MOCK CLEANING in process") return False else: self.task_wp_idx = 0 - self.is_fake_cleaning = False - print(f"[TEST CLIENT API] FAKE CLEANING completed") + self.is_mock_cleaning = False + print(f"[TEST CLIENT API] MOCK CLEANING completed") return True def battery_soc(self): - print(f"[TEST CLIENT API] get fake battery 100%") + print(f"[TEST CLIENT API] get mock battery 100%") return 1.0 def set_cleaning_mode(self, cleaning_config:str): - print(f"[TEST CLIENT API] Set fake CLEANING MODE: {cleaning_config}") + print(f"[TEST CLIENT API] Set mock CLEANING MODE: {cleaning_config}") return True def is_charging(self): """Check if robot is charging, will return false if not charging, None if not avail""" dx, dy, _ = self.dock_position - x, y, _= self.fake_location + x, y, _= self.mock_location if (abs(x-dx) < 5.0 and abs(y-dy) < 5.0): - print(f"[TEST CLIENT API] Fake robot at dock, is charging") + print(f"[TEST CLIENT API] Mock robot at dock, is charging") return True return False diff --git a/fleet_adapter_ecobot/clicked_point_transform.py b/fleet_adapter_ecobot/clicked_point_transform.py new file mode 100644 index 0000000..5dbf757 --- /dev/null +++ b/fleet_adapter_ecobot/clicked_point_transform.py @@ -0,0 +1,75 @@ +#!/usr/bin/env python3 + +# Copyright 2022 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +""" +This utility script is used to get the robot coordinated (in robot map) +via rviz2. User can use the "publish point" feature, and click on the +map to get both `rmf_coordinate` and `robot_coordinate` on rviz2 map +""" + +import sys +import argparse +import rclpy +from rclpy.node import Node +from .utils import RmfMapTransform +from geometry_msgs.msg import PointStamped + +class ClickPointTransform(Node): + def __init__(self, transform): + super().__init__("click_point_transform") + self.tf = transform + self.subscription = self.create_subscription( + PointStamped, + '/clicked_point', + self.clickpoint_callback, + 10) + self.get_logger().info(f"Running click_point_transform") + + + def clickpoint_callback(self, msg): + x = msg.point.x + y = msg.point.y + gaussian_tf = RmfMapTransform( + tx=self.tf[0], ty=self.tf[1], + theta=self.tf[2], scale=self.tf[3]) + rx, ry, _ = gaussian_tf.to_robot_map([x, y, 0]) + print(f"\n\t rmf coord (x, y) : [{x:.2f}, {y:.2f}]" + f"\n\t robot coord (x, y) : [{rx:.2f}, {ry:.2f}]") + + +########################################################################## +########################################################################## + +def main(argv=sys.argv): + args_without_ros = rclpy.utilities.remove_ros_args(argv) + parser = argparse.ArgumentParser( + prog="click_point_transform", + description="script to printout coordinates in"\ + " reference to robot_map, when click point on rviz2") + parser.add_argument( + "-tf", "--transform", required=True, nargs='+', type=float, + help='transform in the form of [x, y, yaw, scale]') + args = parser.parse_args(args_without_ros[1:]) + + assert len(args.transform) == 4, "-tf should be [x, y, yaw, scale]" + print(f"Starting click point transform...") + rclpy.init() + node = ClickPointTransform(args.transform) + rclpy.spin(node) + rclpy.shutdown() + +if __name__ == "__main__": + main() diff --git a/fleet_adapter_ecobot/ecobot_sim_server.py b/fleet_adapter_ecobot/ecobot_sim_server.py deleted file mode 100644 index a730f58..0000000 --- a/fleet_adapter_ecobot/ecobot_sim_server.py +++ /dev/null @@ -1,300 +0,0 @@ -# Copyright 2022 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -#! /usr/bin/env python3 -import sys -import math -import yaml -import argparse - -import rclpy -from rclpy.node import Node -from rclpy.qos import qos_profile_system_default -from rclpy.parameter import Parameter - -from rmf_fleet_msgs.msg import RobotState, FleetState, Location, PathRequest - -import rmf_adapter.vehicletraits as traits -import rmf_adapter.geometry as geometry -import rmf_adapter.graph as graph -import rmf_adapter.plan as plan - -import numpy as np - -import flask -from flask import request, jsonify -import threading - -# Global parameters -named_waypoints = { "charger_ecobot40_1":[[43.78, -19.36, 1.57], [43.81, -16.93, 1.57]]} - -class EcobotFleetManager(Node): - def __init__(self, config_path, nav_path, port): - super().__init__('ecobot_fleet_manager') - self.port = port - # We set use_sim_time to true always - param = Parameter("use_sim_time", Parameter.Type.BOOL, True) - self.set_parameters([param]) - self.config = None - with open(config_path, "r") as f: - self.config = yaml.safe_load(f) - self.fleet_name = self.config["rmf_fleet"]["name"] - robot_names = [] - self.task_queue_data = [] - self.docks = {} - for robot_name, robot_config in self.config["robots"].items(): - robot_names.append(robot_name) - self.map_name = robot_config["rmf_config"]["start"]["map_name"] - for path_name, waypoints in robot_config["ecobot_config"]["docks"].items(): - self.task_queue_data.append({"name":path_name, "map_name":self.map_name, "tasks":[{"name":"PlayPathTask","start_param":{"map_name":self.map_name,"path_name":path_name}}]}) - self.docks[path_name] = waypoints - assert(len(robot_names) > 0) - # TODO currently this fleet manager only supports one robot. This is how - # the ecobot manager behaves. In the future we should extend this to - # fleet managers that actually manage fleets - self.robot_name = robot_names[0] - - # The planner is not being used currently - ######################################################################## - profile = traits.Profile(geometry.make_final_convex_circle( - self.config['rmf_fleet']['profile']['footprint']), - geometry.make_final_convex_circle(self.config['rmf_fleet']['profile']['vicinity'])) - self.vehicle_traits = traits.VehicleTraits( - linear=traits.Limits(*self.config['rmf_fleet']['limits']['linear']), - angular=traits.Limits(*self.config['rmf_fleet']['limits']['angular']), - profile=profile) - # Ecobot robots are unable to reverse - self.vehicle_traits.differential.reversible = False - nav_graph = graph.parse_graph(nav_path, self.vehicle_traits) - config = plan.Configuration(nav_graph, self.vehicle_traits) - self.planner = plan.Planner(config) - ########################################################################## - - self.get_logger().info(f"hello i am {self.fleet_name} fleet manager for robot {self.robot_name}") - - self.create_subscription( - RobotState, - 'robot_state', - self.robot_state_cb, - 10) - - self.path_pub = self.create_publisher( - PathRequest, - 'robot_path_requests', - qos_profile=qos_profile_system_default) - - self.state = None - self.task_id = -1 - - self.app = flask.Flask('ecobot_fleet_manager') - self.app.config["DEBUG"] = False - - @self.app.route('/gs-robot/real_time_data/position', methods=['GET']) - def position(): - position = [self.state.location.x, self.state.location.y] - angle = math.degrees(self.state.location.yaw) - data = {"angle":angle,"gridPosition":{"x":position[0],"y":position[1]},"mapInfo":{"gridHeight":992,"gridWidth":992,"originX":-24.8,"originY":-24.8,"resolution":0.05000000074505806},"worldPosition":{"orientation":{"w":-0.05743089347363588,"x":0,"y":0,"z":0.9983494841361015},"position":{"x":-6.189813393986145,"y":0.3017086724551712,"z":0}}} - return jsonify(data) - - @self.app.route('/gs-robot/data/device_status', methods=['GET']) - def device_status(): - battery_soc = self.state.battery_percent - data = {"data":{"battery":battery_soc}} - return jsonify(data) - - @self.app.route('/gs-robot/cmd/is_task_queue_finished', methods=['GET']) - def is_task_queue_finished(): - finished = False - if ((self.state.mode.mode == 0 or self.state.mode.mode ==1) and len(self.state.path) == 0): - finished = True - data ={"data":finished, "errorCode":"","msg":"successed","successed":True} - return jsonify(data) - - @self.app.route('/gs-robot/cmd/stop_task_queue', methods=['GET']) - def stop_task_queue(): - path_request = PathRequest() - path_request.fleet_name = self.fleet_name - path_request.robot_name = self.robot_name - path_request.path = [] - self.task_id = self.task_id + 1 - path_request.task_id = str(self.task_id) - self.path_pub.publish(path_request) - data ={"data":"", "errorCode":"","msg":"successed","successed":True} - return jsonify(data) - - @self.app.route('/gs-robot/cmd/set_cleaning_mode', methods=['GET']) - def set_cleaning_mode(): - cleaning_mode = request.args.get('cleaning_mode') - data = {"data": [],"errorCode": "","msg": "successed","successed": True} - return jsonify(data) - - @self.app.route('/gs-robot/data/task_queues', methods=['GET']) - def task_queues(): - map_name = request.args.get('map_name') - data = {"data": [],"errorCode": "","msg": "successed","successed": False} - if (map_name != self.map_name): - return jsonify(data) - data["data"] = self.task_queue_data - return jsonify(data) - - @self.app.route('/gs-robot/cmd/start_task_queue', methods=['POST']) - def start_task_queue(): - data ={"data":"", "errorCode":"","msg":"successed","successed":False} - request_data = request.get_json() - print(f"Request data: {request_data}") - if (request_data["map_name"] != self.map_name or\ - len(request_data['tasks']) < 1): - return jsonify(data) - # Process navigation tasks - task = request_data['tasks'][0] - # The ecobot has two broad kind of tasks: 1) Navigation: navigate in space 2) Clean - if (task['name'] == "NavigationTask"): - # The ecobot has two kinds of navigation tasks: 1) To a gridPosition 2) Named waypoint - if task['start_param'].get('destination') is not None: - target_x = task['start_param']['destination']['gridPosition']['x'] - target_y = task['start_param']['destination']['gridPosition']['y'] - target_yaw = math.radians(task['start_param']['destination']['angle']) - - # Add some noise to the actual location the robot will navigate to - target_x = target_x + np.random.uniform(-0.5, 0.5) - target_y = target_y + np.random.uniform(-0.5, 0.5) - - t = self.get_clock().now().to_msg() - - path_request = PathRequest() - cur_x = self.state.location.x - cur_y = self.state.location.y - cur_yaw = self.state.location.yaw - cur_loc = self.state.location - path_request.path.append(cur_loc) - - disp = self.disp([target_x, target_y], [cur_x, cur_y]) - duration = int(disp/self.vehicle_traits.linear.nominal_velocity) - t.sec = t.sec + duration - target_loc = Location() - target_loc.t = t - target_loc.x = target_x - target_loc.y = target_y - target_loc.yaw = target_yaw - target_loc.level_name = self.map_name - - path_request.fleet_name = self.fleet_name - path_request.robot_name = self.robot_name - path_request.path.append(target_loc) - self.task_id = self.task_id + 1 - path_request.task_id = str(self.task_id) - self.path_pub.publish(path_request) - data["successed"] = True - return jsonify(data) - elif task['start_param'].get('position_name') is not None \ - and task['start_param'].get('map_name') == self.map_name: - name = task['start_param']['position_name'] - docking_path = named_waypoints[name] - - t = self.get_clock().now().to_msg() - - path_request = PathRequest() - cur_x = self.state.location.x - cur_y = self.state.location.y - cur_yaw = self.state.location.yaw - cur_loc = self.state.location - path_request.path.append(cur_loc) - previous_wp = [cur_x, cur_y] - for wp in docking_path: - l = Location() - disp = self.disp(wp, previous_wp) - duration = int(disp/self.vehicle_traits.linear.nominal_velocity) - t.sec = t.sec + duration - l.t = t - l.x = wp[0] - l.y = wp[1] - l.yaw = wp[2] - l.level_name = self.map_name - path_request.path.append(l) - previous_wp = wp - - path_request.fleet_name = self.fleet_name - path_request.robot_name = self.robot_name - self.task_id = self.task_id + 1 - path_request.task_id = str(self.task_id) - self.path_pub.publish(path_request) - data["successed"] = True - return jsonify(data) - else: - return jsonify(data) - - elif (task["name"] == "PlayPathTask"): - t = self.get_clock().now().to_msg() - path_request = PathRequest() - cur_x = self.state.location.x - cur_y = self.state.location.y - cur_yaw = self.state.location.yaw - cur_loc = self.state.location - path_request.path.append(cur_loc) - previous_wp = [cur_x, cur_y] - for wp in self.docks[task["start_param"]["path_name"]]: - l = Location() - disp = self.disp(wp, previous_wp) - duration = int(disp/self.vehicle_traits.linear.nominal_velocity) - t.sec = t.sec + duration - l.t = t - l.x = wp[0] - l.y = wp[1] - l.yaw = wp[2] - l.level_name = self.map_name - path_request.path.append(l) - previous_wp = wp - - path_request.fleet_name = self.fleet_name - path_request.robot_name = self.robot_name - self.task_id = self.task_id + 1 - path_request.task_id = str(self.task_id) - self.path_pub.publish(path_request) - data["successed"] = True - return jsonify(data) - - return jsonify(data) - - self.app_thread = threading.Thread(target=self.start_app) - self.app_thread.start() - - def start_app(self): - self.app.run(port=self.port,threaded=True) - - def robot_state_cb(self, msg): - if (msg.name == self.robot_name): - self.state = msg - - def disp(self, A, B): - return math.sqrt((A[0]-B[0])**2 + (A[1]-B[1])**2) - - -def main(argv=sys.argv): - rclpy.init(args=argv) - args_without_ros = rclpy.utilities.remove_ros_args(argv) - - parser = argparse.ArgumentParser() - parser.add_argument('-c', '--config', required=True, help='Path to config file') - parser.add_argument('-n', '--nav_graph', required=True, help='Path to nav graph file') - parser.add_argument('-p', '--port', help='Port for API Server', default=8888) - args = parser.parse_args(args_without_ros[1:]) - - n = EcobotFleetManager(args.config, args.nav_graph, args.port) - try: - rclpy.spin(n) - except KeyboardInterrupt: - pass - -if __name__ == '__main__': - main(sys.argv) diff --git a/fleet_adapter_ecobot/fleet_adapter_ecobot.py b/fleet_adapter_ecobot/fleet_adapter_ecobot.py index 6dd6b89..c085bf7 100644 --- a/fleet_adapter_ecobot/fleet_adapter_ecobot.py +++ b/fleet_adapter_ecobot/fleet_adapter_ecobot.py @@ -30,10 +30,6 @@ import rmf_adapter.graph as graph import rmf_adapter.plan as plan -from rmf_task_msgs.msg import TaskProfile, TaskType - -from functools import partial - from .EcobotCommandHandle import EcobotCommandHandle from .EcobotClientAPI import EcobotAPI from .utils import RmfMapTransform @@ -115,30 +111,19 @@ def initialize_fleet(config_yaml, nav_graph_path, node, server_uri, args): finishing_request) assert ok, ("Unable to set task planner params") - task_capabilities = [] + # Accept Standard RMF Task which are defined in config.yaml + always_accept = adpt.fleet_update_handle.Confirmation() + always_accept.accept() if task_capabilities_config['loop']: node.get_logger().info( f"Fleet [{fleet_name}] is configured to perform Loop tasks") - task_capabilities.append(TaskType.TYPE_LOOP) + fleet_handle.consider_patrol_requests(lambda desc: always_accept) if task_capabilities_config['delivery']: node.get_logger().info( f"Fleet [{fleet_name}] is configured to perform Delivery tasks") - task_capabilities.append(TaskType.TYPE_DELIVERY) - if task_capabilities_config['clean']: - node.get_logger().info( - f"Fleet [{fleet_name}] is configured to perform Clean tasks") - task_capabilities.append(TaskType.TYPE_CLEAN) - - # Callable for validating requests that this fleet can accommodate - def _task_request_check(task_capabilities, msg: TaskProfile): - if msg.description.task_type in task_capabilities: - return True - else: - return False - - fleet_handle.accept_task_requests( - partial(_task_request_check, task_capabilities)) + fleet_handle.consider_delivery_requests(lambda desc: always_accept) + # Whether to accept Custom RMF Action Task def _consider(description: dict): confirm = adpt.fleet_update_handle.Confirmation() @@ -163,10 +148,6 @@ def _consider(description: dict): f" to perform action of category [{cat}]") fleet_handle.add_performable_action(cat, _consider) - def updater_inserter(cmd_handle, update_handle): - """Insert a RobotUpdateHandle.""" - cmd_handle.init_handler(update_handle) - # Initialize robots for this fleet missing_robots = config_yaml['robots'] @@ -178,13 +159,16 @@ def _add_fleet_robots(): node.get_logger().debug(f"Connecting to robot: {robot_name}") robot_config = missing_robots[robot_name]['ecobot_config'] - if args.test_client_api: + # Switch between using Robot's API or Testing API + if args.test_api_config_file != "": from .TestClientAPI import ClientAPI - api = ClientAPI() + node.get_logger().warn( + f"Testing fleet adapter with test api: {args.test_api_config_file}") + api = ClientAPI(args.test_api_config_file) else: api = EcobotAPI(robot_config['base_url'], robot_config['cleaning_task_prefix']) - if not api.connected: + if not api.online(): continue robot_map_name = api.current_map() @@ -272,13 +256,15 @@ def _add_fleet_robots(): max_merge_lane_distance=rmf_config["max_merge_lane_distance"]) robots[robot_name] = robot + # Add robot to fleet - fleet_handle.add_robot(robot, - robot_name, - profile, - starts, - partial(updater_inserter, - robot)) + fleet_handle.add_robot( + robot, + robot_name, + profile, + starts, + lambda update_handle: robot.init_handler(update_handle)) + node.get_logger().info( f"Successfully added new robot: {robot_name}") del missing_robots[robot_name] @@ -303,15 +289,15 @@ def main(argv=sys.argv): prog="fleet_adapter_ecobot", description="Configure and spin up fleet adapter for Gaussian Ecobot robots ") parser.add_argument("-c", "--config_file", type=str, required=True, - help="Path to the config.yaml file for this fleet adapter") + help="Path to the config.yaml file for this fleet adapter") parser.add_argument("-n", "--nav_graph", type=str, required=True, - help="Path to the nav_graph for this fleet adapter") + help="Path to the nav_graph for this fleet adapter") parser.add_argument("-s", "--server_uri", type=str, required=False, default="", - help="URI of the api server to transmit state and task information.") + help="URI of the api server to transmit state and task information.") parser.add_argument("--use_sim_time", action="store_true", - help='Use sim time for testing offline, default: false') - parser.add_argument("--test_client_api", action="store_true", - help='Use Test Client api to test instead of ecobot api') + help='Use sim time for testing offline, default: false') + parser.add_argument("-tf", "--test_api_config_file", type=str, required=False, default="", + help='supply a test_client_api config file to test ecobot client api as sim') args = parser.parse_args(args_without_ros[1:]) print(f"Starting ecobot fleet adapter...") diff --git a/setup.py b/setup.py index 7b38c20..91b2e25 100644 --- a/setup.py +++ b/setup.py @@ -10,8 +10,6 @@ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), - ('share/' + package_name,['config.yaml']), - ], install_requires=['setuptools'], zip_safe=True, @@ -23,7 +21,7 @@ entry_points={ 'console_scripts': [ 'fleet_adapter_ecobot=fleet_adapter_ecobot.fleet_adapter_ecobot:main', - 'ecobot_sim_server=fleet_adapter_ecobot.ecobot_sim_server:main', + 'clicked_point_transform=fleet_adapter_ecobot.clicked_point_transform:main', ], }, ) From 4ecdcb6007ac720e29d72aaf49407e9e5bb90c7c Mon Sep 17 00:00:00 2001 From: youliang Date: Tue, 21 Jun 2022 14:47:50 +0800 Subject: [PATCH 12/16] nit readme Signed-off-by: youliang --- README.md | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/README.md b/README.md index 824721d..ff5dfe9 100644 --- a/README.md +++ b/README.md @@ -22,11 +22,6 @@ Ensure the robot can be pinged. ros2 run fleet_adapter_ecobot fleet_adapter_ecobot -c CONFIG_FILE -n NAV_GRAPH ``` -Dispatch a Task -``` -ros2 run rmf_demos_tasks dispatch_action -s lounge -a clean -ad '{ "clean_task_name": "clean_lounge" }' -``` - To run the fleet adapter with [rmf-web](https://github.com/open-rmf/rmf-web/), specify the server_uri (`-s`): ```bash ros2 run fleet_adapter_ecobot fleet_adapter_ecobot -c CONFIG_FILE -n NAV_GRAPH -s ws://localhost:8000/_internal @@ -53,21 +48,24 @@ ros2 run fleet_adapter_ecobot fleet_adapter_ecobot \ Different to the simulation running on gazebo, this `TestClientAPI` mocks the behavior of the fleet adapter when receives command from RMF. Thus, the script is running on actual system wall time. ### Patrol Task + Now try to command robot to move to `Pantry` ```bash ros2 run rmf_demos_tasks dispatch_patrol -p pantry ``` ### Custom Clean Task + Send the robot to clean an area. This custom clean task is created by composing `go_to_place` and custom `perform_action`. ```bash ros2 run rmf_demos_tasks dispatch_action -s patrol_D2 -a clean -ad '{ "clean_task_name": "clean_hallway" }' ``` -### Docking -Add a `dock_name` on a charger waypoint in traffic editor. This will then call the `dock()` function when the robot is approaching the charger. +### Docking to Charger + +Add a `dock_name` on a charger waypoint in traffic editor. This will then call the `dock()` function when the robot is approaching the charger. Note that this is not demonstrated in this demo. -## Get the Transformation from `traffic-editor` +## Get the Ecobot to RMF map Transformation with `traffic-editor` To get the transformation of the robot map to rmf map, user can add a "floorplan" of a robot map. Then annotate and the corresponding "constraint-pairs", lastly `ctrl-T` to let traffic-editor calculate the respective transformation. @@ -84,4 +82,6 @@ ros2 launch rmf_demos office.launch.xml run_fleet_adapters:=0 ros2 run fleet_adapter_ecobot clicked_point_transform -tf 1.33 0.057 -1.598 0.057 ``` -Subsequently, select "publish point" on rviz, then click on the respective point on the map. Immediately, the point in rmf and robot coordinate will get published on `clicked_point_transform` terminal. These coordinates are helpful during debugging. +![](../media/media/rviz2_publish_point.png) + +Subsequently, select "publish point" on rviz, then click on the respective point on the map. Immediately, the point in rmf and robot coordinate will get printed on `clicked_point_transform` terminal. These coordinates are helpful during debugging. From 98e180d2722f2e826b1fee8b0f1be6f2ede98d80 Mon Sep 17 00:00:00 2001 From: youliang Date: Thu, 23 Jun 2022 14:02:31 +0800 Subject: [PATCH 13/16] cleanups docs impl and fix incorrect map scale value Signed-off-by: youliang --- README.md | 13 +++++++-- configs/robot_config.yaml | 2 +- configs/test_api_config.yaml | 31 +++++++++++---------- fleet_adapter_ecobot/EcobotClientAPI.py | 6 +--- fleet_adapter_ecobot/EcobotCommandHandle.py | 2 +- fleet_adapter_ecobot/TestClientAPI.py | 9 ++---- 6 files changed, 32 insertions(+), 31 deletions(-) diff --git a/README.md b/README.md index ff5dfe9..25c39e0 100644 --- a/README.md +++ b/README.md @@ -9,6 +9,7 @@ For more information on the APIs, please reference [this document](http://downlo ## Requirements * [Open-RMF](https://github.com/open-rmf/rmf) +* nudged: `pip3 install nudged` ## Setup * Clone this repository into a workspace @@ -30,6 +31,8 @@ ros2 run fleet_adapter_ecobot fleet_adapter_ecobot -c CONFIG_FILE -n NAV_GRAPH - ## Test the fleet adapter in "Mock Mode" The adapter can be tested in mock mode with the help of the [TestClientAPI](fleet_adapter_ecobot/TestClientAPI.py). This class emulates the member functions of `EcobotClientAPI.py` which calls the rest apis of the robot. This "mock mode" is enabled by providing `-tf` argument. +![](../media/media/office-world-rviz.png) + Run this example in office world: ```bash ros2 launch rmf_demos office.launch.xml run_fleet_adapters:=0 @@ -37,7 +40,7 @@ ros2 launch rmf_demos office.launch.xml run_fleet_adapters:=0 Then run the ecobot fleet adapter ```bash -# Note the addition of "--test_client_api" and "-tf" +# Note the addition of "--test_api_config_file" and "-tf" ros2 run fleet_adapter_ecobot fleet_adapter_ecobot \ -c src/fleet_adapter_ecobot/configs/robot_config.yaml \ -n install/rmf_demos_maps/share/rmf_demos_maps/maps/office/nav_graphs/0.yaml \ @@ -61,6 +64,12 @@ Send the robot to clean an area. This custom clean task is created by composing ros2 run rmf_demos_tasks dispatch_action -s patrol_D2 -a clean -ad '{ "clean_task_name": "clean_hallway" }' ``` +### Show overlayed ecobot map +Show overlayed lidar map on rviz2 office map +```bash +ros2 launch rmf_demos map_server.launch.py map_name:=ecobot_office tx:=1.33 ty:=0.057 yaw:=-1.598 +``` + ### Docking to Charger Add a `dock_name` on a charger waypoint in traffic editor. This will then call the `dock()` function when the robot is approaching the charger. Note that this is not demonstrated in this demo. @@ -79,7 +88,7 @@ Then if you wish to configure your custom waypoints in the `configs/test_api_con # first run the office map ros2 launch rmf_demos office.launch.xml run_fleet_adapters:=0 # then run this on a seperate terminal -ros2 run fleet_adapter_ecobot clicked_point_transform -tf 1.33 0.057 -1.598 0.057 +ros2 run fleet_adapter_ecobot clicked_point_transform -tf 1.33 0.057 -1.598 0.049 ``` ![](../media/media/rviz2_publish_point.png) diff --git a/configs/robot_config.yaml b/configs/robot_config.yaml index 6f06ba8..bbe905e 100644 --- a/configs/robot_config.yaml +++ b/configs/robot_config.yaml @@ -61,7 +61,7 @@ robots: rmf_transform: mock_test_robot_map: rmf_map_name: "L1" - transform: [1.33, 0.057, -1.598, 0.057] # This is obtained from traffic-editor + transform: [1.33, 0.057, -1.598, 0.049] # This is obtained from traffic-editor ## User can also provide a pair of "reference_coordinates" # mock_test_robot_map2: # rmf_map_name: "L1" diff --git a/configs/test_api_config.yaml b/configs/test_api_config.yaml index f7c43ab..a983472 100644 --- a/configs/test_api_config.yaml +++ b/configs/test_api_config.yaml @@ -2,21 +2,22 @@ # mock_dock_path is empty since dock_name/dock() is not used in this scenario mock_clean_path: [ - [57.06, 141.63, 0], - [70.43, 132.84, 0], - [84.60, 139.63, 45], - [108.08, 163.16, 90], - [112.99, 195.95, 90], - [114.13, 221.31, 90], - [114.09, 256.41, 90], - [111.45, 308.84, 180], - [61.71, 307.48, 180], - [54.43, 260.89, -90], - [59.49, 220.74, -90], - [64.15, 172.95, -90], - [49.28, 158.20, -90], + [60.58, 186.84, 0], + [81.59, 169.53, 0], + [108.94, 162.66, 45], + [137.94, 180.66, 90], + [137.01, 226.99, 90], + [132.72, 275.21, 90], + [131.54, 318.55, 120], + [127.56, 355.20, 180], + [99.97, 358.42, -140], + [66.74, 325.73, -90], + [71.04, 289.43, -90], + [73.83, 235.87, -90], + [77.11, 200.20, -90], + [60.58, 186.84, -90] ] mock_dock_path: [] -dock_position: [85.28, 338.95, 0] # charging point -mock_location: [50.89, 325.29, 0] # this indicate where the robot will start +dock_position: [99.70, 392.85, 0] # charging point +mock_location: [60.58, 381.85, 0] # this indicate where the robot will start mock_robot_map_name: "mock_test_robot_map" diff --git a/fleet_adapter_ecobot/EcobotClientAPI.py b/fleet_adapter_ecobot/EcobotClientAPI.py index 3bb5792..4de0d39 100644 --- a/fleet_adapter_ecobot/EcobotClientAPI.py +++ b/fleet_adapter_ecobot/EcobotClientAPI.py @@ -142,12 +142,8 @@ def navigate_to_waypoint(self, waypoint_name, map_name): return False - def start_clean(self, name:str, map_name:str): - ''' Returns True if the robot has started the cleaning process, else False''' - return self.start_task(name, map_name) - - def start_task(self, name:str, map_name:str): + ''' Returns True if the robot has started a task/cleaning process, else False''' # we first get the relevant task queue and then start a new task data = {} response = self.task_queues(map_name) diff --git a/fleet_adapter_ecobot/EcobotCommandHandle.py b/fleet_adapter_ecobot/EcobotCommandHandle.py index 191dd9d..d2b2dd2 100644 --- a/fleet_adapter_ecobot/EcobotCommandHandle.py +++ b/fleet_adapter_ecobot/EcobotCommandHandle.py @@ -449,7 +449,7 @@ def _action_executor(self, while True: self.node.get_logger().info( f"Requesting robot {self.name} to clean {description}") - if self.api.start_clean(description["clean_task_name"], self.robot_map_name): + if self.api.start_task(description["clean_task_name"], self.robot_map_name): self.check_task_completion = self.api.task_completed # check api func break if (attempts > 3): diff --git a/fleet_adapter_ecobot/TestClientAPI.py b/fleet_adapter_ecobot/TestClientAPI.py index be3c068..895b862 100644 --- a/fleet_adapter_ecobot/TestClientAPI.py +++ b/fleet_adapter_ecobot/TestClientAPI.py @@ -70,13 +70,8 @@ def navigate_to_waypoint(self, waypoint_name, map_name): def start_task(self, name:str, map_name:str): ''' Returns True if the robot has started the generic task, else False''' print(f"[TEST CLIENT API] Start mock task : {name}") - self.is_mock_docking = True - return True - - def start_clean(self, name:str, map_name:str): - ''' Returns True if the robot has started the cleaning process, else False''' - print(f"[TEST CLIENT API] Set mock start CLEANING : {name}") - self.is_mock_cleaning = True + if not self.is_mock_docking: + self.is_mock_cleaning = True return True def pause(self): From 2689baf452c1de65c27a2e5dbd98de725236b6d0 Mon Sep 17 00:00:00 2001 From: youliang Date: Tue, 16 Aug 2022 13:07:31 +0800 Subject: [PATCH 14/16] add unstable gaussian api, comments and fix robot error toggle Signed-off-by: youliang --- fleet_adapter_ecobot/EcobotClientAPI.py | 52 ++++++++++++++++++++ fleet_adapter_ecobot/EcobotCommandHandle.py | 20 +++++--- fleet_adapter_ecobot/fleet_adapter_ecobot.py | 1 + 3 files changed, 66 insertions(+), 7 deletions(-) diff --git a/fleet_adapter_ecobot/EcobotClientAPI.py b/fleet_adapter_ecobot/EcobotClientAPI.py index 4de0d39..6905f73 100644 --- a/fleet_adapter_ecobot/EcobotClientAPI.py +++ b/fleet_adapter_ecobot/EcobotClientAPI.py @@ -117,6 +117,25 @@ def navigate(self, pose, map_name:str): print(f"Other error: {err}") return False + # NOTE: Unstable gaussian api 2.0. Get task status + def __navigate(self, pose): + assert(len(pose) > 2) + url = self.prefix + f"/gs-robot/cmd/quick/navigate?type=2" + data = {} + data["destination"] = {"gridPosition": {"x": pose[0], "y": pose[1]}, "angle": pose[2]} + try: + response = requests.post(url, timeout=self.timeout, json=data) + response.raise_for_status() + if self.debug: + print(f"Response: {response.json()}") + return response.json()['successed'] + except HTTPError as http_err: + print(f"HTTP error: {http_err}") + self.connected = False + except Exception as err: + print(f"Other error: {err}") + return False + def navigate_to_waypoint(self, waypoint_name, map_name): ''' Ask the robot to navigate to a preconfigured waypoint on a map. @@ -141,6 +160,23 @@ def navigate_to_waypoint(self, waypoint_name, map_name): print(f"Other error: {err}") return False + # NOTE: Unstable gaussian api 2.0 + def __navigate_to_waypoint(self, waypoint_name, map_name): + ''' Ask the robot to navigate to a preconfigured waypoint on a map. + Returns True if the robot received the command''' + url = self.prefix + f"/gs-robot/cmd/start_cross_task?map_name={map_name}&position_name={waypoint_name}" + try: + response = requests.get(url, timeout=self.timeout) + response.raise_for_status() + if self.debug: + print(f"Response: {response.json()}") + return response.json()['successed'] + except HTTPError as http_err: + print(f"HTTP error: {http_err}") + except Exception as err: + print(f"Other error: {err}") + return False + def start_task(self, name:str, map_name:str): ''' Returns True if the robot has started a task/cleaning process, else False''' @@ -233,6 +269,22 @@ def current_map(self): print(f"Other error: {err}") return None + #NOTE: Unstable gaussian api 2.0. Get task status + def __state(self): + url = self.prefix + f"/gs-robot/real_time_data/robot_status" + try: + response = requests.get(url, timeout=self.timeout) + response.raise_for_status() + if self.debug: + # print(f"Response: \n {response.json()}") + data = response.json()["data"]["statusData"] + print(json.dumps(data, indent=2)) + return data + except HTTPError as http_err: + print(f"HTTP error: {http_err}") + except Exception as err: + print(f"Other error: {err}") + return None def data(self): url = self.prefix + f"/gs-robot/data/device_status" diff --git a/fleet_adapter_ecobot/EcobotCommandHandle.py b/fleet_adapter_ecobot/EcobotCommandHandle.py index d2b2dd2..1908fae 100644 --- a/fleet_adapter_ecobot/EcobotCommandHandle.py +++ b/fleet_adapter_ecobot/EcobotCommandHandle.py @@ -408,8 +408,10 @@ def get_robot_position(self) -> List[int]: self.node.get_logger().error( f"Robot map name [{map_name}] is not known. return last known position.") self.is_online = True + self.in_error = True return self.position + self.in_error = False tf = self.transforms[map_name] x,y,theta = tf['tf'].to_rmf_map([position[0],position[1], math.radians(position[2])]) print(f"Convert pos from {position} grid coor to {x},{y}, {theta} rmf coor") @@ -453,10 +455,9 @@ def _action_executor(self, self.check_task_completion = self.api.task_completed # check api func break if (attempts > 3): - self.node.get_logger().warn( + self.node.get_logger().error( f"Failed to initiate cleaning action for robot [{self.name}]") - # TODO: issue error ticket - self.in_error = True # TODO: toggle error back + # TODO: kill_task() or fail the task (api is not avail in adapter) execution.error("Failed to initiate cleaning action for robot {self.name}") execution.finished() return @@ -481,6 +482,7 @@ def update_robot_status(self): if self.api.is_charging(): self.update_handle.override_status("charging") elif not self.is_online: + self.node.get_logger().warn(f"Robot {self.name} is offline") self.update_handle.override_status("offline") elif self.in_error: self.update_handle.override_status("error") @@ -558,13 +560,17 @@ def update_location(self): self.battery_soc = self.get_battery_soc() self.update_handle.update_battery_soc(self.battery_soc) + ## TODO: there's a tendency that the robot stop updating the system when it is offline, why!!! + ## TODO: clean up complex lock and multi threaded system + ## TODO: make it single threaded, and only use api.update_position() + ## TODO: check if waypoint is close enough, and mark navigation ask as completed. # Update states and positions with self._lock: if (self.action_execution): self.check_perform_action() elif (self.on_waypoint is not None): # if robot is on a waypoint print(f"[update] Calling update_current_waypoint() on waypoint with " \ - f"pose[{self.position_str()}] and waypoint[{self.on_waypoint}]") + f"pose[{self.position_str()}] and waypoint[{self.on_waypoint}]") self.update_handle.update_current_waypoint( self.on_waypoint, self.position[2]) elif (self.on_lane is not None): # if robot is on a lane @@ -580,13 +586,13 @@ def update_location(self): if reverse_lane is not None: # Unidirectional graph lane_indices.append(reverse_lane.index) print(f"[update] Calling update_current_lanes() with pose[{self.position_str()}] "\ - f"and lanes:{lane_indices}") + f"and lanes:{lane_indices}") self.update_handle.update_current_lanes( self.position, lane_indices) # if robot is merging into a waypoint elif (self.target_waypoint is not None and self.target_waypoint.graph_index is not None): print(f"[update] Calling update_off_grid_position() with pose " \ - f"[{self.position_str()}] and waypoint[{self.target_waypoint.graph_index}]") + f"[{self.position_str()}] and waypoint[{self.target_waypoint.graph_index}]") self.update_handle.update_off_grid_position( self.position, self.target_waypoint.graph_index) else: # if unsure which waypoint the robot is near to @@ -597,7 +603,7 @@ def update_location(self): else: print("[update] Calling update_lost_position()") self.update_handle.update_lost_position( - self.rmf_map_name, self.position) + self.rmf_map_name, self.position) ######################################################################## ## Utils diff --git a/fleet_adapter_ecobot/fleet_adapter_ecobot.py b/fleet_adapter_ecobot/fleet_adapter_ecobot.py index c085bf7..20c08bc 100644 --- a/fleet_adapter_ecobot/fleet_adapter_ecobot.py +++ b/fleet_adapter_ecobot/fleet_adapter_ecobot.py @@ -169,6 +169,7 @@ def _add_fleet_robots(): api = EcobotAPI(robot_config['base_url'], robot_config['cleaning_task_prefix']) if not api.online(): + node.get_logger().error(f"Robot [{robot_map_name}] is offline") continue robot_map_name = api.current_map() From f3ce1e65f3133b4174b78e8a454e52fc4d90dcdc Mon Sep 17 00:00:00 2001 From: johntgz Date: Thu, 15 Sep 2022 12:38:20 +0800 Subject: [PATCH 15/16] Added back ecobot_sim_server.py --- fleet_adapter_ecobot/ecobot_sim_server.py | 300 ++++++++++++++++++++++ 1 file changed, 300 insertions(+) create mode 100644 fleet_adapter_ecobot/ecobot_sim_server.py diff --git a/fleet_adapter_ecobot/ecobot_sim_server.py b/fleet_adapter_ecobot/ecobot_sim_server.py new file mode 100644 index 0000000..a730f58 --- /dev/null +++ b/fleet_adapter_ecobot/ecobot_sim_server.py @@ -0,0 +1,300 @@ +# Copyright 2022 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +#! /usr/bin/env python3 +import sys +import math +import yaml +import argparse + +import rclpy +from rclpy.node import Node +from rclpy.qos import qos_profile_system_default +from rclpy.parameter import Parameter + +from rmf_fleet_msgs.msg import RobotState, FleetState, Location, PathRequest + +import rmf_adapter.vehicletraits as traits +import rmf_adapter.geometry as geometry +import rmf_adapter.graph as graph +import rmf_adapter.plan as plan + +import numpy as np + +import flask +from flask import request, jsonify +import threading + +# Global parameters +named_waypoints = { "charger_ecobot40_1":[[43.78, -19.36, 1.57], [43.81, -16.93, 1.57]]} + +class EcobotFleetManager(Node): + def __init__(self, config_path, nav_path, port): + super().__init__('ecobot_fleet_manager') + self.port = port + # We set use_sim_time to true always + param = Parameter("use_sim_time", Parameter.Type.BOOL, True) + self.set_parameters([param]) + self.config = None + with open(config_path, "r") as f: + self.config = yaml.safe_load(f) + self.fleet_name = self.config["rmf_fleet"]["name"] + robot_names = [] + self.task_queue_data = [] + self.docks = {} + for robot_name, robot_config in self.config["robots"].items(): + robot_names.append(robot_name) + self.map_name = robot_config["rmf_config"]["start"]["map_name"] + for path_name, waypoints in robot_config["ecobot_config"]["docks"].items(): + self.task_queue_data.append({"name":path_name, "map_name":self.map_name, "tasks":[{"name":"PlayPathTask","start_param":{"map_name":self.map_name,"path_name":path_name}}]}) + self.docks[path_name] = waypoints + assert(len(robot_names) > 0) + # TODO currently this fleet manager only supports one robot. This is how + # the ecobot manager behaves. In the future we should extend this to + # fleet managers that actually manage fleets + self.robot_name = robot_names[0] + + # The planner is not being used currently + ######################################################################## + profile = traits.Profile(geometry.make_final_convex_circle( + self.config['rmf_fleet']['profile']['footprint']), + geometry.make_final_convex_circle(self.config['rmf_fleet']['profile']['vicinity'])) + self.vehicle_traits = traits.VehicleTraits( + linear=traits.Limits(*self.config['rmf_fleet']['limits']['linear']), + angular=traits.Limits(*self.config['rmf_fleet']['limits']['angular']), + profile=profile) + # Ecobot robots are unable to reverse + self.vehicle_traits.differential.reversible = False + nav_graph = graph.parse_graph(nav_path, self.vehicle_traits) + config = plan.Configuration(nav_graph, self.vehicle_traits) + self.planner = plan.Planner(config) + ########################################################################## + + self.get_logger().info(f"hello i am {self.fleet_name} fleet manager for robot {self.robot_name}") + + self.create_subscription( + RobotState, + 'robot_state', + self.robot_state_cb, + 10) + + self.path_pub = self.create_publisher( + PathRequest, + 'robot_path_requests', + qos_profile=qos_profile_system_default) + + self.state = None + self.task_id = -1 + + self.app = flask.Flask('ecobot_fleet_manager') + self.app.config["DEBUG"] = False + + @self.app.route('/gs-robot/real_time_data/position', methods=['GET']) + def position(): + position = [self.state.location.x, self.state.location.y] + angle = math.degrees(self.state.location.yaw) + data = {"angle":angle,"gridPosition":{"x":position[0],"y":position[1]},"mapInfo":{"gridHeight":992,"gridWidth":992,"originX":-24.8,"originY":-24.8,"resolution":0.05000000074505806},"worldPosition":{"orientation":{"w":-0.05743089347363588,"x":0,"y":0,"z":0.9983494841361015},"position":{"x":-6.189813393986145,"y":0.3017086724551712,"z":0}}} + return jsonify(data) + + @self.app.route('/gs-robot/data/device_status', methods=['GET']) + def device_status(): + battery_soc = self.state.battery_percent + data = {"data":{"battery":battery_soc}} + return jsonify(data) + + @self.app.route('/gs-robot/cmd/is_task_queue_finished', methods=['GET']) + def is_task_queue_finished(): + finished = False + if ((self.state.mode.mode == 0 or self.state.mode.mode ==1) and len(self.state.path) == 0): + finished = True + data ={"data":finished, "errorCode":"","msg":"successed","successed":True} + return jsonify(data) + + @self.app.route('/gs-robot/cmd/stop_task_queue', methods=['GET']) + def stop_task_queue(): + path_request = PathRequest() + path_request.fleet_name = self.fleet_name + path_request.robot_name = self.robot_name + path_request.path = [] + self.task_id = self.task_id + 1 + path_request.task_id = str(self.task_id) + self.path_pub.publish(path_request) + data ={"data":"", "errorCode":"","msg":"successed","successed":True} + return jsonify(data) + + @self.app.route('/gs-robot/cmd/set_cleaning_mode', methods=['GET']) + def set_cleaning_mode(): + cleaning_mode = request.args.get('cleaning_mode') + data = {"data": [],"errorCode": "","msg": "successed","successed": True} + return jsonify(data) + + @self.app.route('/gs-robot/data/task_queues', methods=['GET']) + def task_queues(): + map_name = request.args.get('map_name') + data = {"data": [],"errorCode": "","msg": "successed","successed": False} + if (map_name != self.map_name): + return jsonify(data) + data["data"] = self.task_queue_data + return jsonify(data) + + @self.app.route('/gs-robot/cmd/start_task_queue', methods=['POST']) + def start_task_queue(): + data ={"data":"", "errorCode":"","msg":"successed","successed":False} + request_data = request.get_json() + print(f"Request data: {request_data}") + if (request_data["map_name"] != self.map_name or\ + len(request_data['tasks']) < 1): + return jsonify(data) + # Process navigation tasks + task = request_data['tasks'][0] + # The ecobot has two broad kind of tasks: 1) Navigation: navigate in space 2) Clean + if (task['name'] == "NavigationTask"): + # The ecobot has two kinds of navigation tasks: 1) To a gridPosition 2) Named waypoint + if task['start_param'].get('destination') is not None: + target_x = task['start_param']['destination']['gridPosition']['x'] + target_y = task['start_param']['destination']['gridPosition']['y'] + target_yaw = math.radians(task['start_param']['destination']['angle']) + + # Add some noise to the actual location the robot will navigate to + target_x = target_x + np.random.uniform(-0.5, 0.5) + target_y = target_y + np.random.uniform(-0.5, 0.5) + + t = self.get_clock().now().to_msg() + + path_request = PathRequest() + cur_x = self.state.location.x + cur_y = self.state.location.y + cur_yaw = self.state.location.yaw + cur_loc = self.state.location + path_request.path.append(cur_loc) + + disp = self.disp([target_x, target_y], [cur_x, cur_y]) + duration = int(disp/self.vehicle_traits.linear.nominal_velocity) + t.sec = t.sec + duration + target_loc = Location() + target_loc.t = t + target_loc.x = target_x + target_loc.y = target_y + target_loc.yaw = target_yaw + target_loc.level_name = self.map_name + + path_request.fleet_name = self.fleet_name + path_request.robot_name = self.robot_name + path_request.path.append(target_loc) + self.task_id = self.task_id + 1 + path_request.task_id = str(self.task_id) + self.path_pub.publish(path_request) + data["successed"] = True + return jsonify(data) + elif task['start_param'].get('position_name') is not None \ + and task['start_param'].get('map_name') == self.map_name: + name = task['start_param']['position_name'] + docking_path = named_waypoints[name] + + t = self.get_clock().now().to_msg() + + path_request = PathRequest() + cur_x = self.state.location.x + cur_y = self.state.location.y + cur_yaw = self.state.location.yaw + cur_loc = self.state.location + path_request.path.append(cur_loc) + previous_wp = [cur_x, cur_y] + for wp in docking_path: + l = Location() + disp = self.disp(wp, previous_wp) + duration = int(disp/self.vehicle_traits.linear.nominal_velocity) + t.sec = t.sec + duration + l.t = t + l.x = wp[0] + l.y = wp[1] + l.yaw = wp[2] + l.level_name = self.map_name + path_request.path.append(l) + previous_wp = wp + + path_request.fleet_name = self.fleet_name + path_request.robot_name = self.robot_name + self.task_id = self.task_id + 1 + path_request.task_id = str(self.task_id) + self.path_pub.publish(path_request) + data["successed"] = True + return jsonify(data) + else: + return jsonify(data) + + elif (task["name"] == "PlayPathTask"): + t = self.get_clock().now().to_msg() + path_request = PathRequest() + cur_x = self.state.location.x + cur_y = self.state.location.y + cur_yaw = self.state.location.yaw + cur_loc = self.state.location + path_request.path.append(cur_loc) + previous_wp = [cur_x, cur_y] + for wp in self.docks[task["start_param"]["path_name"]]: + l = Location() + disp = self.disp(wp, previous_wp) + duration = int(disp/self.vehicle_traits.linear.nominal_velocity) + t.sec = t.sec + duration + l.t = t + l.x = wp[0] + l.y = wp[1] + l.yaw = wp[2] + l.level_name = self.map_name + path_request.path.append(l) + previous_wp = wp + + path_request.fleet_name = self.fleet_name + path_request.robot_name = self.robot_name + self.task_id = self.task_id + 1 + path_request.task_id = str(self.task_id) + self.path_pub.publish(path_request) + data["successed"] = True + return jsonify(data) + + return jsonify(data) + + self.app_thread = threading.Thread(target=self.start_app) + self.app_thread.start() + + def start_app(self): + self.app.run(port=self.port,threaded=True) + + def robot_state_cb(self, msg): + if (msg.name == self.robot_name): + self.state = msg + + def disp(self, A, B): + return math.sqrt((A[0]-B[0])**2 + (A[1]-B[1])**2) + + +def main(argv=sys.argv): + rclpy.init(args=argv) + args_without_ros = rclpy.utilities.remove_ros_args(argv) + + parser = argparse.ArgumentParser() + parser.add_argument('-c', '--config', required=True, help='Path to config file') + parser.add_argument('-n', '--nav_graph', required=True, help='Path to nav graph file') + parser.add_argument('-p', '--port', help='Port for API Server', default=8888) + args = parser.parse_args(args_without_ros[1:]) + + n = EcobotFleetManager(args.config, args.nav_graph, args.port) + try: + rclpy.spin(n) + except KeyboardInterrupt: + pass + +if __name__ == '__main__': + main(sys.argv) From aae8d30b75c368e46784b4387584b36a503d1ac6 Mon Sep 17 00:00:00 2001 From: johntgz Date: Thu, 15 Sep 2022 12:39:43 +0800 Subject: [PATCH 16/16] added sim_server to entry_points --- setup.py | 1 + 1 file changed, 1 insertion(+) diff --git a/setup.py b/setup.py index 91b2e25..3f3fb04 100644 --- a/setup.py +++ b/setup.py @@ -22,6 +22,7 @@ 'console_scripts': [ 'fleet_adapter_ecobot=fleet_adapter_ecobot.fleet_adapter_ecobot:main', 'clicked_point_transform=fleet_adapter_ecobot.clicked_point_transform:main', + 'ecobot_sim_server=fleet_adapter_ecobot.ecobot_sim_server:main', ], }, )