diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..bee8a64 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +__pycache__ diff --git a/README.md b/README.md index 312bc26..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 @@ -20,22 +21,76 @@ Ensure the robot can be pinged. ``` ros2 run fleet_adapter_ecobot fleet_adapter_ecobot -c CONFIG_FILE -n NAV_GRAPH +``` +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 ``` -## 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. +## 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) -First run the server +Run this example in office world: +```bash +ros2 launch rmf_demos office.launch.xml run_fleet_adapters:=0 ``` -ros2 run fleet_adapter_ecobot ecobot_sim_server -c CONFIG_FILE -n NAV_GRAPH -p PORT +Then run the ecobot fleet adapter +```bash +# 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 \ + -s "ws://localhost:8000/_internal" \ + -tf src/fleet_adapter_ecobot/configs/test_api_config.yaml +``` + +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" }' ``` -Then run the fleet adapter +### 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 ``` -ros2 run fleet_adapter_ecobot fleet_adapter_ecobot -c CONFIG_FILE -n NAV_GRAPH --USE_SIM_TIME + +### 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 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. + +![](../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.049 ``` -Ensure that the `base_url` in the config matches the `LOCALHOST:PORT` specified to the server. +![](../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. diff --git a/config.yaml b/config.yaml deleted file mode 100644 index 9e4d5ca..0000000 --- a/config.yaml +++ /dev/null @@ -1,117 +0,0 @@ -# FLEET CONFIG ================================================================= -# RMF Fleet parameters - -rmf_fleet: - name: "ecobot40" - limits: - linear: [1.2, 1.5] # velocity, acceleration - angular: [0.6, 0.6] - profile: # Robot profile is modelled as a circle - footprint: 0.5 - vicinity: 0.6 - reversible: False - battery_system: - voltage: 24.0 - capacity: 40.0 - charging_current: 26.4 - mechanical_system: - mass: 80.0 - moment_of_inertia: 20.0 - friction_coefficient: 0.20 - ambient_system: - power: 20.0 - cleaning_system: - power: 760.0 - recharge_threshold: 0.05 - recharge_soc: 1.0 - publish_fleet_state: True - account_for_battery_drain: True - 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" - -# Ecobot CONFIG ================================================================= - -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 - start: - map_name: "L1" - # waypoint: "charger_ecobot1" # Optional - # orientation: 0.0 # Optional, radians - 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 diff --git a/configs/robot_config.yaml b/configs/robot_config.yaml new file mode 100644 index 0000000..bbe905e --- /dev/null +++ b/configs/robot_config.yaml @@ -0,0 +1,76 @@ +# FLEET CONFIG ================================================================= +# RMF Fleet parameters + +rmf_fleet: + name: "ecobot40" + limits: + linear: [1.2, 1.5] # velocity, acceleration + angular: [0.6, 0.6] + profile: # Robot profile is modelled as a circle + footprint: 0.5 + vicinity: 0.6 + reversible: False + battery_system: + voltage: 24.0 + capacity: 40.0 + charging_current: 26.4 + mechanical_system: + mass: 80.0 + moment_of_inertia: 20.0 + friction_coefficient: 0.20 + ambient_system: + power: 20.0 + cleaning_system: + power: 760.0 + recharge_threshold: 0.05 + recharge_soc: 1.0 + publish_fleet_state: True + account_for_battery_drain: True + task_capabilities: # Specify the types of RMF Tasks that robots in this fleet are capable of performing + loop: True + delivery: False + clean: False # deprecated, now replace as custom action + finishing_request: "nothing" + action_categories: ["clean", "manual_control"] + +# Ecobot CONFIG ================================================================= + +robots: + ecobot40_1: + ecobot_config: + base_url: "http://10.7.5.88:8080" + 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 + 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 + charger: + waypoint: "tinyRobot1_charger" + +# TRANSFORM CONFIG ============================================================= +# For computing transforms between Ecobot and RMF coordinate systems + +# 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: + mock_test_robot_map: + rmf_map_name: "L1" + 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" + # 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..a983472 --- /dev/null +++ b/configs/test_api_config.yaml @@ -0,0 +1,23 @@ +# 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: [ + [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: [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 b74712b..6905f73 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}" @@ -55,7 +58,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() @@ -109,6 +112,26 @@ 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 + + # 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 @@ -137,9 +160,26 @@ 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_clean(self, name:str, map_name:str): - ''' Returns True if the robot has started the cleaning process, else False''' + 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) @@ -213,6 +253,39 @@ 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 + + #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" try: @@ -273,7 +346,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 +370,23 @@ def set_cleaning_mode(self, cleaning_config:str): except Exception as err: print(f"Other error: {err}") return False + + 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"] # 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() + 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 33d617d..1908fae 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 @@ -29,8 +27,7 @@ from datetime import timedelta -from .EcobotClientAPI import EcobotAPI - +from typing import List, Optional, Tuple # States for EcobotCommandHandle's state machine used when guiding robot along # a new path @@ -38,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, @@ -59,13 +56,17 @@ def __init__(self, graph, vehicle_traits, transforms, - map_name, - start, - position, charger_waypoint, update_frequency, adapter, - api): + api, + max_merge_lane_distance): + """ + :param config + robot config defined in yaml file + :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 @@ -73,26 +74,17 @@ def __init__(self, self.graph = graph self.vehicle_traits = vehicle_traits self.transforms = transforms - self.map_name = 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) - # assert waypoint, f"Charger waypoint {charger_waypoint} does not exist in the navigation graph" - if 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.charger_waypoint = self.graph.find_waypoint(charger_waypoint) 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.dock_name = "" self.adapter = adapter - self.requested_waypoints = [] # RMF Plan waypoints self.remaining_waypoints = [] self.path_finished_callback = None self.next_arrival_estimator = None @@ -106,7 +98,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() @@ -115,32 +106,59 @@ 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.stubbornness = None + self.in_error = False + self.is_online = False + self.action_category = 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 + # 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.state_update_timer = self.node.create_timer( - 1.0 / self.update_frequency, - self.update) + 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 # 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 is not None: + self.update_handle.set_charger_waypoint(self.charger_waypoint.index) + else: + 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() + + 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: - print("Clearing internal states") - self.requested_waypoints = [] + print(f"Clearing internal states with current waypoint {self.on_waypoint}") self.remaining_waypoints = [] self.path_finished_callback = None self.next_arrival_estimator = None @@ -148,6 +166,8 @@ 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 +176,8 @@ def stop(self): break time.sleep(1.0) + ############################################################################## + # override function def follow_new_path( self, waypoints, @@ -218,7 +240,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") @@ -231,19 +255,15 @@ 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[self.robot_map_name]["tf"].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}] " - 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.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 @@ -251,7 +271,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: @@ -263,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 @@ -282,33 +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 @@ -316,6 +334,8 @@ def _follow_path(): target=_follow_path) self._follow_path_thread.start() + ############################################################################## + # override function def dock( self, dock_name, @@ -325,83 +345,44 @@ 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: 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 # 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.robot_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.robot_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") + 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']) @@ -409,57 +390,187 @@ def _dock(): self._dock_thread = threading.Thread(target=_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 = 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 - 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 + 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") + 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() 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, + category: str, + description: dict, + execution: + adpt.robot_update_handle.ActionExecution): + with self._lock: + # 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']) + # 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}") + 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): + self.node.get_logger().error( + f"Failed to initiate cleaning action for robot [{self.name}]") + # 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 + 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 + self.stubbornness = self.update_handle.unstable_be_stubborn() + # 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(): + 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") + 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_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 - def update_state(self): + # 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() + self.battery_soc = self.get_battery_soc() self.update_handle.update_battery_soc(self.battery_soc) - 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.charger_is_set = True - # Update position + + ## 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.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): + 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}]") 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 +585,32 @@ 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}]") - 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}]") + # 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( 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) + 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 + ######################################################################## + + def position_str(self): + return f"{self.position[0]:.2f}, {self.position[1]:.2f}, {self.position[2]:.2f}" def get_current_lane(self): def projection(current_position, @@ -557,11 +679,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)): @@ -571,26 +692,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 new file mode 100644 index 0000000..895b862 --- /dev/null +++ b/fleet_adapter_ecobot/TestClientAPI.py @@ -0,0 +1,137 @@ +# 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 sys +import yaml + +class ClientAPI: + def __init__(self, config_file): + self.connected = True + 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 + 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.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_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.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 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 mock task : {name}") + if not self.is_mock_docking: + self.is_mock_cleaning = True + return True + + def pause(self): + print(f"[TEST CLIENT API] Pause Robot") + self.is_mock_cleaning = True + return True + + def resume(self): + print(f"[TEST CLIENT API] Resume Robot") + self.is_mock_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] 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_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_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.mock_clean_path): + print(f"[TEST CLIENT API] MOCK CLEANING in process") + return False + else: + self.task_wp_idx = 0 + 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 mock battery 100%") + return 1.0 + + def set_cleaning_mode(self, cleaning_config:str): + 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.mock_location + if (abs(x-dx) < 5.0 and abs(y-dy) < 5.0): + print(f"[TEST CLIENT API] Mock robot at dock, is charging") + return True + return False + + def is_localize(self): + return True 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/fleet_adapter_ecobot.py b/fleet_adapter_ecobot/fleet_adapter_ecobot.py index de9301f..20c08bc 100644 --- a/fleet_adapter_ecobot/fleet_adapter_ecobot.py +++ b/fleet_adapter_ecobot/fleet_adapter_ecobot.py @@ -15,10 +15,9 @@ import sys import argparse import yaml -import nudged import threading import time -import math +import faulthandler import rclpy import rclpy.node @@ -31,17 +30,14 @@ 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 #------------------------------------------------------------------------------ # 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( @@ -81,7 +77,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 " @@ -94,11 +90,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,61 +111,42 @@ def initialize_fleet(config_yaml, nav_graph_path, node, use_sim_time, server_uri finishing_request) assert ok, ("Unable to set task planner params") - task_capabilities = [] - if fleet_config['task_capabilities']['loop']: + # 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) - if fleet_config['task_capabilities']['delivery']: + 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 fleet_config['task_capabilities']['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)) - - # 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()}") + 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() + + # 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.accept() return confirm - # Configure this fleet to perform cleaning action - fleet_handle.add_performable_action("clean", _consider) - - def updater_inserter(cmd_handle, update_handle): - """Insert a RobotUpdateHandle.""" - cmd_handle.update_handle = update_handle + # 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) # Initialize robots for this fleet missing_robots = config_yaml['robots'] @@ -178,90 +158,117 @@ 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 not api.connected: + + # Switch between using Robot's API or Testing API + if args.test_api_config_file != "": + from .TestClientAPI import 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.online(): + node.get_logger().error(f"Robot [{robot_map_name}] is offline") 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 + + # get robot coordinates and transform to rmf_coor ecobot_pos = api.position() - if ecobot_pos is not None and len(ecobot_pos) > 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() - - x,y = transforms['ecobot_to_rmf'].transform([ecobot_pos[0],ecobot_pos[1]]) - theta = math.radians(ecobot_pos[2]) - transforms['orientation_offset'] - theta = (theta + math.pi) % (2*math.pi) - math.pi #ensure within [-pi, pi] - position = [x, y, theta] - - 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, - partial(updater_inserter, - robot)) - node.get_logger().info( - f"Successfully added new robot: {robot_name}") + 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}") + + # 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: - node.get_logger().error( - f"Failed to initialize robot: {robot_name}") - - del missing_robots[robot_name] + 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,_ = 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 + node.get_logger().info( + f"Running compute_plan_starts for robot: " + f"{robot_name}, with pos: {position}") + starts = plan.compute_plan_starts( + nav_graph, + current_map, + 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( + f"Unable to determine StartSet for {robot_name} " + f"with map {current_map}") + continue - else: - pass - node.get_logger().debug( - f"{robot_name} not found, trying again...") + robot = EcobotCommandHandle( + name=robot_name, + config=robot_config, + node=node, + graph=nav_graph, + vehicle_traits=vehicle_traits, + transforms=robot_map_transforms, + charger_waypoint=rmf_config['charger']['waypoint'], + update_frequency=rmf_config.get( + 'robot_state_update_frequency', 1), + adapter=adapter, + api=api, + 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, + lambda update_handle: robot.init_handler(update_handle)) + + 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=()) @@ -276,19 +283,22 @@ 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( 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') + 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...") @@ -315,8 +325,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]) diff --git a/setup.py b/setup.py index 7b38c20..3f3fb04 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,6 +21,7 @@ entry_points={ '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', ], },