From 8c5bcafa58fbe2e305019e16fb92596c14e93dc2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sun, 16 Aug 2026 13:20:17 +0200 Subject: [PATCH 1/2] Python support MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../__init__.py | 0 .../mission.py | 256 +++++++++++++++ .../launch/easynav_collaboration_launch.yaml | 74 +++++ .../package.xml | 45 +++ ...nav_collaboration_simple_api_deployment_py | 0 .../setup.cfg | 4 + .../setup.py | 29 ++ .../test/__init__.py | 0 .../test/test_copyright.py | 23 ++ .../test/test_flake8.py | 25 ++ .../test/test_pep257.py | 23 ++ .../test/test_xmllint.py | 23 ++ .../easyfleet_mission_manager_py/__init__.py | 85 +++++ .../easyfleet_mission_manager_py/ansi.py | 26 ++ .../capability_client.py | 147 +++++++++ .../capability_discovery.py | 83 +++++ .../capability_info.py | 129 ++++++++ .../capability_state.py | 43 +++ .../detail/__init__.py | 14 + .../detail/running_capability.py | 99 ++++++ .../fleet_session.py | 159 +++++++++ .../mission_helpers.py | 149 +++++++++ .../easyfleet_mission_manager_py/output.py | 26 ++ .../robot_handle.py | 192 +++++++++++ .../simple_controller.py | 72 ++++ .../status_markers.py | 111 +++++++ .../easyfleet_mission_manager_py/throttle.py | 45 +++ easyfleet_mission_manager_py/package.xml | 35 ++ .../resource/easyfleet_mission_manager_py | 0 easyfleet_mission_manager_py/setup.cfg | 4 + easyfleet_mission_manager_py/setup.py | 22 ++ easyfleet_mission_manager_py/test/__init__.py | 0 .../test/test_capability_discovery.py | 186 +++++++++++ .../test/test_capability_state.py | 23 ++ .../test/test_copyright.py | 23 ++ .../test/test_flake8.py | 25 ++ .../test/test_fleet_session_robot_handle.py | 308 ++++++++++++++++++ .../test/test_fleet_session_shutdown.py | 38 +++ .../test/test_nav_fake_capability.py | 132 ++++++++ .../test/test_pep257.py | 23 ++ .../test/test_throttle.py | 38 +++ .../test/test_xmllint.py | 23 ++ 42 files changed, 2762 insertions(+) create mode 100644 easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/easyfleet_easynav_collaboration_simple_api_deployment_py/__init__.py create mode 100644 easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/easyfleet_easynav_collaboration_simple_api_deployment_py/mission.py create mode 100644 easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/launch/easynav_collaboration_launch.yaml create mode 100644 easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/package.xml create mode 100644 easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/resource/easyfleet_easynav_collaboration_simple_api_deployment_py create mode 100644 easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/setup.cfg create mode 100644 easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/setup.py create mode 100644 easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/test/__init__.py create mode 100644 easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/test/test_copyright.py create mode 100644 easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/test/test_flake8.py create mode 100644 easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/test/test_pep257.py create mode 100644 easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/test/test_xmllint.py create mode 100644 easyfleet_mission_manager_py/easyfleet_mission_manager_py/__init__.py create mode 100644 easyfleet_mission_manager_py/easyfleet_mission_manager_py/ansi.py create mode 100644 easyfleet_mission_manager_py/easyfleet_mission_manager_py/capability_client.py create mode 100644 easyfleet_mission_manager_py/easyfleet_mission_manager_py/capability_discovery.py create mode 100644 easyfleet_mission_manager_py/easyfleet_mission_manager_py/capability_info.py create mode 100644 easyfleet_mission_manager_py/easyfleet_mission_manager_py/capability_state.py create mode 100644 easyfleet_mission_manager_py/easyfleet_mission_manager_py/detail/__init__.py create mode 100644 easyfleet_mission_manager_py/easyfleet_mission_manager_py/detail/running_capability.py create mode 100644 easyfleet_mission_manager_py/easyfleet_mission_manager_py/fleet_session.py create mode 100644 easyfleet_mission_manager_py/easyfleet_mission_manager_py/mission_helpers.py create mode 100644 easyfleet_mission_manager_py/easyfleet_mission_manager_py/output.py create mode 100644 easyfleet_mission_manager_py/easyfleet_mission_manager_py/robot_handle.py create mode 100644 easyfleet_mission_manager_py/easyfleet_mission_manager_py/simple_controller.py create mode 100644 easyfleet_mission_manager_py/easyfleet_mission_manager_py/status_markers.py create mode 100644 easyfleet_mission_manager_py/easyfleet_mission_manager_py/throttle.py create mode 100644 easyfleet_mission_manager_py/package.xml create mode 100644 easyfleet_mission_manager_py/resource/easyfleet_mission_manager_py create mode 100644 easyfleet_mission_manager_py/setup.cfg create mode 100644 easyfleet_mission_manager_py/setup.py create mode 100644 easyfleet_mission_manager_py/test/__init__.py create mode 100644 easyfleet_mission_manager_py/test/test_capability_discovery.py create mode 100644 easyfleet_mission_manager_py/test/test_capability_state.py create mode 100644 easyfleet_mission_manager_py/test/test_copyright.py create mode 100644 easyfleet_mission_manager_py/test/test_flake8.py create mode 100644 easyfleet_mission_manager_py/test/test_fleet_session_robot_handle.py create mode 100644 easyfleet_mission_manager_py/test/test_fleet_session_shutdown.py create mode 100644 easyfleet_mission_manager_py/test/test_nav_fake_capability.py create mode 100644 easyfleet_mission_manager_py/test/test_pep257.py create mode 100644 easyfleet_mission_manager_py/test/test_throttle.py create mode 100644 easyfleet_mission_manager_py/test/test_xmllint.py diff --git a/easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/easyfleet_easynav_collaboration_simple_api_deployment_py/__init__.py b/easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/easyfleet_easynav_collaboration_simple_api_deployment_py/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/easyfleet_easynav_collaboration_simple_api_deployment_py/mission.py b/easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/easyfleet_easynav_collaboration_simple_api_deployment_py/mission.py new file mode 100644 index 0000000..15991b4 --- /dev/null +++ b/easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/easyfleet_easynav_collaboration_simple_api_deployment_py/mission.py @@ -0,0 +1,256 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +""" +The Python-API mission script for this scenario. + +Written against easyfleet_mission_manager_py's RobotHandle/ +SimpleController -- see +easyfleet_easynav_collaboration_simple_api_deployment/src/main_easynav_collaboration.cpp +for the C++ sibling this is meant to behave identically to (same +robots, same waypoints, same five phases, same status markers). Status +markers above each robot in RViz are not set here explicitly: +RobotHandle.run_capability() publishes them automatically for the life +of each goal. + +Mission control demo for this deployment scenario: two robots, both +real-EasyNav-navigating on the same shared map -- + - robot_1: navigation (real EasyNav), perception (mock) + - robot_2: navigation (real EasyNav), manipulation (mock) + +A five-phase choreography exercising real navigation end to end (goals +are left to actually SUCCEED, not stopped early -- see LONG_TIMEOUT_SEC +below -- except where phase 4 deliberately cancels one): + 1. robot_1 -> "kitchen" (perceiving throughout) while robot_2 -> + "dock", simultaneously. Waits for both navigations to actually + finish (not a timeout). + 2. robot_1 -> "kitchen_standby" (1m short of "kitchen", still facing + it, perception still running from phase 1) to clear space, while + robot_2 -> "kitchen", simultaneously. + 3. Once robot_2 is at "kitchen" (and robot_1's perception is + stopped, its job here done), robot_2 runs manipulation for up to + 10s. + 4. robot_1 -> "dock" while robot_2 -> "dock" too, simultaneously; + 10s into robot_1's navigation it is explicitly stopped and + redirected to "charging_station" instead. + 5. Once both navigations from phase 4 finish, both robots return to + their own starting pose ("robot_1_home"/"robot_2_home") and any + still-running capability goal is stopped. +""" + +from easyfleet_mission_manager_py import ( + ansi, + make_manipulation_goal, + make_navigation_goal, + make_perception_goal, + Manipulation, + Navigation, + Perception, + print_section, + print_step, + RobotHandle, + SimpleController, +) +from easyfleet_mission_manager_py.output import safe_print +import rclpy + +# This demo is specific to this deployment scenario, which defines +# exactly these two robots (see .../launch/). +ROBOT_1 = 'robot_1' +ROBOT_2 = 'robot_2' + +# Named waypoints configured on both robots' navigation capability, see +# config/navigation_params.yaml -- real, reachable points on the shared +# home2 map. +DOCK = 'dock' +KITCHEN = 'kitchen' +KITCHEN_STANDBY = 'kitchen_standby' +CHARGING_STATION = 'charging_station' +ROBOT_1_HOME = 'robot_1_home' +ROBOT_2_HOME = 'robot_2_home' + +# Real navigation goals are meant to run to actual completion in this +# mission -- this is a generous safety net, not the expected way a +# goal ends, unlike RUN_TIMEOUT_SEC (10s), which is both too short for +# real navigation and -- for every navigation goal below except one -- +# not what's wanted here. +LONG_TIMEOUT_SEC = 180.0 + + +def main(args=None): + rclpy.init(args=args) + + controller = SimpleController() + + robot_1 = RobotHandle(ROBOT_1) + robot_2 = RobotHandle(ROBOT_2) + + controller.add_robot(robot_1) + controller.add_robot(robot_2) + + safe_print( + f'{ansi.BOLD}{ansi.CYAN}Control Center -- two-robot "easynav" mission ' + f'({ROBOT_1}, {ROBOT_2}){ansi.RESET}') + + print_section('Phase 0: Discovering capabilities') + print_step('listening on /capabilities and /capabilities_status ...') + controller.discover_capabilities() + + robot_1.print_capabilities() + robot_2.print_capabilities() + + if not robot_1.has_capability('navigation') or not robot_2.has_capability('navigation'): + safe_print( + f'{ansi.BOLD}{ansi.RED}Both robots must have a navigation capability for this ' + f'demo to work.{ansi.RESET}') + return 1 + + if not robot_1.has_capability('perception'): + safe_print( + f'{ansi.BOLD}{ansi.RED}Robot 1 must have a perception capability for this demo ' + f'to work.{ansi.RESET}') + return 1 + + if not robot_2.has_capability('manipulation'): + safe_print( + f'{ansi.BOLD}{ansi.RED}Robot 2 must have a manipulation capability for this demo ' + f'to work.{ansi.RESET}') + return 1 + + # Phase 1: robot_1 -> "kitchen" (perceiving throughout) while + # robot_2 -> "dock", simultaneously. robot_1's perception is + # started here and kept running (untouched) across phase 2 too -- + # it's only stopped once robot_1 actually reaches + # "kitchen_standby" at the end of phase 2, see there. + + print_section( + f'Phase 1: {ROBOT_1} -> "{KITCHEN}" (perceiving), {ROBOT_2} -> "{DOCK}" ' + '(simultaneously)') + + robot_1.run_capability( + 'navigation', Navigation, make_navigation_goal(KITCHEN), timeout_sec=LONG_TIMEOUT_SEC) + robot_1.run_capability( + 'perception', Perception, make_perception_goal(), timeout_sec=LONG_TIMEOUT_SEC) + robot_2.run_capability( + 'navigation', Navigation, make_navigation_goal(DOCK), timeout_sec=LONG_TIMEOUT_SEC) + + while robot_1.is_capability_running('navigation') or robot_2.is_capability_running( + 'navigation'): + controller.spin_some() + + print_step('Phase 1 done: both robots reached their waypoint.') + + # Phase 2: robot_1 -> "kitchen_standby" (still perceiving) while + # robot_2 -> "kitchen", simultaneously. + print_section( + f'Phase 2: {ROBOT_1} -> "{KITCHEN_STANDBY}" (clearing space, still perceiving), ' + f'{ROBOT_2} -> "{KITCHEN}" (simultaneously)') + + # robot_1's perception keeps running, untouched, from phase 1 -- + # no need to (re-)issue it here; doing so would just preempt the + # still-running goal with an identical one. + robot_1.run_capability( + 'navigation', Navigation, make_navigation_goal(KITCHEN_STANDBY), + timeout_sec=LONG_TIMEOUT_SEC) + robot_2.run_capability( + 'navigation', Navigation, make_navigation_goal(KITCHEN), timeout_sec=LONG_TIMEOUT_SEC) + + while robot_1.is_capability_running('navigation') or robot_2.is_capability_running( + 'navigation'): + controller.spin_some() + + robot_1.stop_capability('perception') + + print_step(f'Phase 2 done: {ROBOT_1} clear of "{KITCHEN}", {ROBOT_2} there.') + + # Phase 3: robot_2, now at "kitchen", runs manipulation for up to + # 10s (the mock's own configured duration finishes well within + # that). + print_section(f'Phase 3: {ROBOT_2} manipulation at "{KITCHEN}"') + + # Default timeout (RUN_TIMEOUT_SEC, 10s) already matches what this + # phase wants. + robot_2.run_capability('manipulation', Manipulation, make_manipulation_goal()) + controller.spin_for(10.0) + + robot_2.stop_capability('manipulation') + + print_step('Phase 3 done.') + + # Phase 4: robot_1 -> "dock" while robot_2 -> "dock" too (robot_2 + # was still at "kitchen" from phase 3, so this is a real move, not + # a no-op), simultaneously; 10s into robot_1's navigation it is + # explicitly stopped (via run_capability's own timeout-then-cancel + # behavior) and redirected to "charging_station" instead. + print_section( + f'Phase 4: {ROBOT_1} -> "{DOCK}" (canceled after 10s) -> "{CHARGING_STATION}", ' + f'{ROBOT_2} -> "{DOCK}" (simultaneously)') + + # Default timeout (RUN_TIMEOUT_SEC, 10s) here is deliberate: this + # is the goal meant to be capped and redirected below, unlike + # every other navigation goal in this mission. + robot_1.run_capability('navigation', Navigation, make_navigation_goal(DOCK)) + robot_2.run_capability( + 'navigation', Navigation, make_navigation_goal(DOCK), timeout_sec=LONG_TIMEOUT_SEC) + + controller.spin_for(10.0) + + # A new goal on the same capability preempts the one still in + # flight (server-side preemption) -- no explicit stop_capability() + # needed first. + robot_1.run_capability( + 'navigation', Navigation, make_navigation_goal(CHARGING_STATION), + timeout_sec=LONG_TIMEOUT_SEC) + + while robot_1.is_capability_running('navigation') or robot_2.is_capability_running( + 'navigation'): + controller.spin_some() + + print_step( + f'Phase 4 done: {ROBOT_1} at "{CHARGING_STATION}", {ROBOT_2} at "{DOCK}".') + + # Phase 5: both robots return to their own starting pose, + # simultaneously. + print_section( + f'Phase 5: {ROBOT_1} -> "{ROBOT_1_HOME}", {ROBOT_2} -> "{ROBOT_2_HOME}" ' + '(simultaneously)') + + robot_1.run_capability( + 'navigation', Navigation, make_navigation_goal(ROBOT_1_HOME), timeout_sec=LONG_TIMEOUT_SEC) + robot_2.run_capability( + 'navigation', Navigation, make_navigation_goal(ROBOT_2_HOME), timeout_sec=LONG_TIMEOUT_SEC) + + while robot_1.is_capability_running('navigation') or robot_2.is_capability_running( + 'navigation'): + controller.spin_some() + + # Safety net: nothing should still be running by this point + # (perception was stopped after phase 2, manipulation after phase + # 3, and every navigation goal above already ran to completion or + # was explicitly redirected), but stop anything left active + # regardless before declaring the mission over. + robot_1.stop_capability('perception') + robot_2.stop_capability('manipulation') + + print_step('Phase 5 done: both robots back at their starting pose.') + print_section('Mission complete') + + controller.shutdown() + + return 0 + + +if __name__ == '__main__': + raise SystemExit(main()) diff --git a/easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/launch/easynav_collaboration_launch.yaml b/easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/launch/easynav_collaboration_launch.yaml new file mode 100644 index 0000000..95845ad --- /dev/null +++ b/easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/launch/easynav_collaboration_launch.yaml @@ -0,0 +1,74 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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 deployment's top-level launch file -- identical to +# easyfleet_easynav_collaboration_simple_api_deployment's own +# easynav_collaboration_launch.yaml except for the very last node: the +# mission controller here is easyfleet_easynav_collaboration_simple_api_deployment_py's +# Python one instead of that package's C++ one. Everything else (both +# robots, the navigation manager, the fleet-wide RViz) is reused +# directly from the sibling C++ package -- only the mission controller +# differs, per this package's own purpose. +# +# Does NOT launch Gazebo -- start +# `ros2 launch easynav_playground_kobuki playgorund_multirobot_kobuki.launch.py` +# separately first, so both robots are already spawned (namespaced +# "robot_1"/"robot_2") and publishing scan_raw/odom/TF before +# system_main tries to localize against them. +# +# easynav_collaboration_mission_node is delayed a few seconds so both +# robots have finished activating (and published their /capabilities +# announcements) before mission control's discovery window opens -- +# launching everything at literally the same instant risks discovering +# nothing. +launch: + - include: + file: "$(find-pkg-share easyfleet_easynav_collaboration_simple_api_deployment)/launch/robot_1_launch.yaml" + - include: + file: "$(find-pkg-share easyfleet_easynav_collaboration_simple_api_deployment)/launch/robot_2_launch.yaml" + + # Fleet-wide navigation manager: publishes the same map/routes both + # robots above would otherwise load locally on /global_map and + # /global_routes. + - node: + pkg: "easyfleet_navigation_manager" + exec: "navigation_manager_node" + output: "screen" + param: + - name: "use_sim_time" + value: true + - from: "$(find-pkg-share easyfleet_easynav_collaboration_simple_api_deployment)/config/navigation_manager.params.yaml" + + # Fleet-wide view: navigation_manager_node's own topics are all + # unnamespaced, so this rviz2 needs no remap. + - node: + pkg: "rviz2" + exec: "rviz2" + output: "screen" + args: "-d $(find-pkg-share easyfleet_easynav_deployment)/rviz/navigation_manager.rviz" + param: + - name: "use_sim_time" + value: true + + - timer: + period: 5.0 + children: + - node: + pkg: "easyfleet_easynav_collaboration_simple_api_deployment_py" + exec: "easynav_collaboration_mission_node" + output: "screen" + param: + - name: "use_sim_time" + value: true diff --git a/easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/package.xml b/easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/package.xml new file mode 100644 index 0000000..ca47d00 --- /dev/null +++ b/easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/package.xml @@ -0,0 +1,45 @@ + + + + easyfleet_easynav_collaboration_simple_api_deployment_py + 0.1.0 + + The Python-mission-controller mirror of + easyfleet_easynav_collaboration_simple_api_deployment: the exact same + two-robot scenario (real EasyNav navigation on both robots, robot_1 + also carrying a fake perception capability, robot_2 a fake + manipulation one, same Gazebo world/map, same launch/config/rviz + shape, reused directly from that sibling package -- robot_node, + perception/manipulation capabilities, navigation, launch files), but + the mission script is written in Python against + easyfleet_mission_manager_py's RobotHandle/SimpleController instead + of the sibling package's C++ one. Only the mission controller + differs; everything else is the same package, included directly. + + + Francisco Martín Rico + + Apache-2.0 + + Francisco Martín Rico + + rclpy + easyfleet_mission_manager_py + + easyfleet_easynav_collaboration_simple_api_deployment + easyfleet_navigation_manager + launch + launch_ros + launch_yaml + rviz2 + + ament_copyright + ament_flake8 + ament_pep257 + ament_xmllint + python3-pytest + + + ament_python + + diff --git a/easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/resource/easyfleet_easynav_collaboration_simple_api_deployment_py b/easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/resource/easyfleet_easynav_collaboration_simple_api_deployment_py new file mode 100644 index 0000000..e69de29 diff --git a/easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/setup.cfg b/easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/setup.cfg new file mode 100644 index 0000000..51af6b2 --- /dev/null +++ b/easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/easyfleet_easynav_collaboration_simple_api_deployment_py +[install] +install_scripts=$base/lib/easyfleet_easynav_collaboration_simple_api_deployment_py diff --git a/easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/setup.py b/easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/setup.py new file mode 100644 index 0000000..d5098ff --- /dev/null +++ b/easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/setup.py @@ -0,0 +1,29 @@ +from setuptools import find_packages, setup + +package_name = 'easyfleet_easynav_collaboration_simple_api_deployment_py' + +setup( + name=package_name, + version='0.1.0', + packages=find_packages(include=[package_name, package_name + '.*'], exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', [f'resource/{package_name}']), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name + '/launch', ['launch/easynav_collaboration_launch.yaml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Francisco Martín Rico', + maintainer_email='fmrico@gmail.com', + description=( + 'Python-mission-controller mirror of ' + 'easyfleet_easynav_collaboration_simple_api_deployment.' + ), + license='Apache-2.0', + entry_points={ + 'console_scripts': [ + 'easynav_collaboration_mission_node = ' + 'easyfleet_easynav_collaboration_simple_api_deployment_py.mission:main', + ], + }, +) diff --git a/easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/test/__init__.py b/easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/test/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/test/test_copyright.py b/easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/test/test_copyright.py new file mode 100644 index 0000000..cc8ff03 --- /dev/null +++ b/easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 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 ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/test/test_flake8.py b/easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/test/test_flake8.py new file mode 100644 index 0000000..2603011 --- /dev/null +++ b/easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 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 ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, 'Found %d code style errors / warnings:\n' % len( + errors + ) + '\n'.join(errors) diff --git a/easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/test/test_pep257.py b/easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 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 ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/test/test_xmllint.py b/easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/test/test_xmllint.py new file mode 100644 index 0000000..3e08c02 --- /dev/null +++ b/easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/test/test_xmllint.py @@ -0,0 +1,23 @@ +# Copyright 2015 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 ament_xmllint.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.xmllint +def test_xmllint() -> None: + rc = main(argv=[]) + assert rc == 0, 'Found code style errors / warnings' diff --git a/easyfleet_mission_manager_py/easyfleet_mission_manager_py/__init__.py b/easyfleet_mission_manager_py/easyfleet_mission_manager_py/__init__.py new file mode 100644 index 0000000..4c486ea --- /dev/null +++ b/easyfleet_mission_manager_py/easyfleet_mission_manager_py/__init__.py @@ -0,0 +1,85 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +""" +Python mirror of easyfleet_mission_manager. + +FleetSession/RobotHandle/SimpleController for writing mission +controllers in Python instead of C++, talking the same wire protocol +(/capabilities, /capabilities_status, the easyfleet_interfaces +actions) -- a Python mission script and a C++ one are interchangeable +against the same fleet. +""" + +from .capability_client import CapabilityClient +from .capability_client import Outcome as CapabilityOutcome +from .capability_client import Response as CapabilityResponse +from .capability_discovery import discover_capabilities +from .capability_info import CapabilityInfo, print_capability_info, print_capability_summary_line +from .capability_state import CapabilityState +from .capability_state import to_string as capability_state_to_string +from .fleet_session import FleetSession +from .mission_helpers import ( + find_robot_capability, + make_manipulation_feedback_printer, + make_manipulation_goal, + make_navigation_feedback_printer, + make_navigation_goal, + make_perception_feedback_printer, + make_perception_goal, + Manipulation, + Navigation, + Perception, + print_section, + print_step, + RUN_TIMEOUT_SEC, + spin_in_background, +) +from .robot_handle import MissionManagerError, RobotHandle +from .simple_controller import SimpleController +from .status_markers import StatusMarkerPublisher +from .throttle import Throttle + +__all__ = [ + 'RUN_TIMEOUT_SEC', + 'CapabilityClient', + 'CapabilityInfo', + 'CapabilityOutcome', + 'CapabilityResponse', + 'CapabilityState', + 'FleetSession', + 'Manipulation', + 'MissionManagerError', + 'Navigation', + 'Perception', + 'RobotHandle', + 'SimpleController', + 'StatusMarkerPublisher', + 'Throttle', + 'capability_state_to_string', + 'discover_capabilities', + 'find_robot_capability', + 'make_manipulation_feedback_printer', + 'make_manipulation_goal', + 'make_navigation_feedback_printer', + 'make_navigation_goal', + 'make_perception_feedback_printer', + 'make_perception_goal', + 'print_capability_info', + 'print_capability_summary_line', + 'print_section', + 'print_step', + 'spin_in_background', +] diff --git a/easyfleet_mission_manager_py/easyfleet_mission_manager_py/ansi.py b/easyfleet_mission_manager_py/easyfleet_mission_manager_py/ansi.py new file mode 100644 index 0000000..28162f6 --- /dev/null +++ b/easyfleet_mission_manager_py/easyfleet_mission_manager_py/ansi.py @@ -0,0 +1,26 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +"""Terminal ANSI color codes, mirroring easyfleet_mission_manager's ansi.hpp.""" + +RESET = '\033[0m' +BOLD = '\033[1m' +DIM = '\033[2m' +RED = '\033[31m' +GREEN = '\033[32m' +YELLOW = '\033[33m' +BLUE = '\033[34m' +MAGENTA = '\033[35m' +CYAN = '\033[36m' diff --git a/easyfleet_mission_manager_py/easyfleet_mission_manager_py/capability_client.py b/easyfleet_mission_manager_py/easyfleet_mission_manager_py/capability_client.py new file mode 100644 index 0000000..d741e3d --- /dev/null +++ b/easyfleet_mission_manager_py/easyfleet_mission_manager_py/capability_client.py @@ -0,0 +1,147 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +""" +CapabilityClient, mirroring easyfleet_core's CapabilityClient/ActionClient. + +The C++ original splits these into two layers (a generic ActionClient +that owns its own internal node/executor/thread, and a thin +CapabilityClient wrapper over it) because rclcpp_action's own client +needs *some* node spinning it, and a caller's own node might not be +spinning yet. In Python, a FleetSession already spins one node's +executor in the background for its whole life (see fleet_session.py), +so every CapabilityClient here is created on -- and serviced by -- that +same node/executor: no per-client internal node/thread is needed, which +is why this is a single class instead of two. +""" + +import enum +import threading + +from action_msgs.msg import GoalStatus +from rclpy.action import ActionClient + + +class Outcome(enum.Enum): + """The action's own terminal states, plus rejection and server-unavailability.""" + + SUCCEEDED = 'SUCCEEDED' + ABORTED = 'ABORTED' + CANCELED = 'CANCELED' + REJECTED = 'REJECTED' + SERVER_UNAVAILABLE = 'SERVER_UNAVAILABLE' + + +_STATUS_TO_OUTCOME = { + GoalStatus.STATUS_SUCCEEDED: Outcome.SUCCEEDED, + GoalStatus.STATUS_CANCELED: Outcome.CANCELED, +} + + +class Response: + """Final outcome of a request, as delivered to a `request()` response callback.""" + + __slots__ = ('outcome', 'result') + + def __init__(self, outcome: Outcome, result=None): + self.outcome = outcome + self.result = result + + +class CapabilityClient: + """ + The simplest possible way to ask a capability to do something. + + Talks to a capability by name, exactly as advertised by + easyfleet_core::Capability (the capability name is the action name). + """ + + def __init__(self, node, action_type, capability_name: str): + self._action_name = capability_name + self._client = ActionClient(node, action_type, capability_name) + self._lock = threading.Lock() + self._active_goal_handle = None + # Unlike rclcpp_action::Client's async_cancel_all_goals() (which + # rclpy has no equivalent of), rclpy can only cancel a specific, + # already-accepted GoalHandle -- so cancel() arriving before the + # goal-response callback has run (a real race: is_capability_running() + # already reads RUNNING synchronously from run(), before the goal + # is even accepted) has nothing to cancel yet. This flag makes + # that cancel() request stick: _on_goal_response() checks it and + # cancels immediately once a handle actually exists. + self._cancel_requested = False + + @property + def action_name(self) -> str: + return self._action_name + + def request(self, goal, on_response=None, on_feedback=None) -> None: + """ + Ask the capability to do something. Returns immediately. + + `on_response` is called exactly once with the final outcome; + `on_feedback` (if given) once per feedback message while the + request is running. + """ + if not self._client.server_is_ready(): + if on_response: + on_response(Response(Outcome.SERVER_UNAVAILABLE)) + return + + with self._lock: + self._cancel_requested = False + + def unwrap_feedback(feedback_msg): + on_feedback(feedback_msg.feedback) + + send_future = self._client.send_goal_async( + goal, feedback_callback=unwrap_feedback if on_feedback else None) + send_future.add_done_callback( + lambda future: self._on_goal_response(future, on_response)) + + def _on_goal_response(self, future, on_response) -> None: + goal_handle = future.result() + if goal_handle is None or not goal_handle.accepted: + if on_response: + on_response(Response(Outcome.REJECTED)) + return + + cancel_now = False + with self._lock: + self._active_goal_handle = goal_handle + cancel_now = self._cancel_requested + if cancel_now: + goal_handle.cancel_goal_async() + + result_future = goal_handle.get_result_async() + result_future.add_done_callback( + lambda future: self._on_result(future, goal_handle, on_response)) + + def _on_result(self, future, goal_handle, on_response) -> None: + with self._lock: + if self._active_goal_handle is goal_handle: + self._active_goal_handle = None + response = future.result() + outcome = _STATUS_TO_OUTCOME.get(response.status, Outcome.ABORTED) + if on_response: + on_response(Response(outcome, response.result)) + + def cancel(self) -> None: + """Ask the capability to stop whatever it is currently doing, if anything.""" + with self._lock: + self._cancel_requested = True + goal_handle = self._active_goal_handle + if goal_handle is not None: + goal_handle.cancel_goal_async() diff --git a/easyfleet_mission_manager_py/easyfleet_mission_manager_py/capability_discovery.py b/easyfleet_mission_manager_py/easyfleet_mission_manager_py/capability_discovery.py new file mode 100644 index 0000000..1c26175 --- /dev/null +++ b/easyfleet_mission_manager_py/easyfleet_mission_manager_py/capability_discovery.py @@ -0,0 +1,83 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +"""discover_capabilities(), mirroring easyfleet_mission_manager's capability_discovery.hpp/.cpp.""" + +import json +import threading +import time + +from easyfleet_interfaces.msg import CapabilityDescription, CapabilityStatus +from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy + +from .capability_info import CapabilityInfo + + +def discover_capabilities(node, window_sec: float = 2.5) -> list[CapabilityInfo]: + """ + Listen on /capabilities and /capabilities_status for `window_sec`. + + Returns one CapabilityInfo per distinct `action_name` seen on either + topic -- this is the right key because two robots can offer the same + `capability` (e.g. "navigation") under different namespaces. + `node` must already be part of an executor that is being spun on + another thread: this function only subscribes and waits, it does not + spin. + """ + lock = threading.Lock() + by_action_name: dict[str, CapabilityInfo] = {} + + def on_description(msg: CapabilityDescription) -> None: + with lock: + info = by_action_name.setdefault(msg.action_name, CapabilityInfo()) + info.robot = msg.robot + info.capability = msg.capability + info.action_name = msg.action_name + info.description_json_raw = msg.description_json + try: + parsed = json.loads(msg.description_json) + info.description_json = parsed if isinstance(parsed, dict) else {} + info.description_json_valid = True + except (ValueError, TypeError): + info.description_json = {} + info.description_json_valid = False + + def on_status(msg: CapabilityStatus) -> None: + with lock: + info = by_action_name.setdefault(msg.action_name, CapabilityInfo()) + info.robot = msg.robot + info.capability = msg.capability + info.action_name = msg.action_name + info.active = True + info.busy = msg.busy + + capabilities_sub = node.create_subscription( + CapabilityDescription, '/capabilities', on_description, + QoSProfile( + depth=10, reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.TRANSIENT_LOCAL)) + status_sub = node.create_subscription( + CapabilityStatus, '/capabilities_status', on_status, + QoSProfile(depth=10, reliability=ReliabilityPolicy.RELIABLE)) + + time.sleep(window_sec) + + with lock: + result = list(by_action_name.values()) + + node.destroy_subscription(capabilities_sub) + node.destroy_subscription(status_sub) + + return result diff --git a/easyfleet_mission_manager_py/easyfleet_mission_manager_py/capability_info.py b/easyfleet_mission_manager_py/easyfleet_mission_manager_py/capability_info.py new file mode 100644 index 0000000..4610014 --- /dev/null +++ b/easyfleet_mission_manager_py/easyfleet_mission_manager_py/capability_info.py @@ -0,0 +1,129 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +"""CapabilityInfo + pretty-printers, mirroring capability_info.hpp/.cpp.""" + +from . import ansi +from .output import safe_print + + +class CapabilityInfo: + """ + Everything known about one running capability instance. + + Gathered from its /capabilities (easyfleet_interfaces/CapabilityDescription) + and /capabilities_status (easyfleet_interfaces/CapabilityStatus) + messages. `action_name` is the unique identity: two robots can both + offer a `capability` named "navigation", but each has its own + `action_name`. + """ + + def __init__( + self, + robot: str = '', + capability: str = '', + action_name: str = '', + description_json_raw: str = '', + description_json: dict | None = None, + description_json_valid: bool = False, + active: bool = False, + busy: bool = False, + ): + self.robot = robot + self.capability = capability + self.action_name = action_name + self.description_json_raw = description_json_raw + self.description_json: dict = description_json if description_json is not None else {} + self.description_json_valid = description_json_valid + self.active = active + self.busy = busy + + +def _identity(info: CapabilityInfo) -> str: + return f'{info.robot}/{info.capability}' if info.robot else info.capability + + +def _status_label(info: CapabilityInfo) -> str: + if not info.active: + return f'{ansi.RED}INACTIVE{ansi.RESET}' + if info.busy: + return f'{ansi.YELLOW}BUSY{ansi.RESET}' + return f'{ansi.GREEN}IDLE{ansi.RESET}' + + +def _display_name(info: CapabilityInfo) -> str: + if info.description_json_valid: + return info.description_json.get('display_name', info.capability) + return info.capability + + +def _bullet_list(lines: list[str], j: dict, key: str) -> None: + values = j.get(key) + if not isinstance(values, list): + return + lines.append(f'\n {ansi.BOLD}{key}:{ansi.RESET}') + for item in values: + if isinstance(item, str): + lines.append(f' {ansi.DIM}-{ansi.RESET} {item}') + + +def print_capability_summary_line(info: CapabilityInfo) -> None: + """Print a single summary line, e.g. for a "known capabilities" list.""" + safe_print( + f' {ansi.BOLD}{_identity(info)}{ansi.RESET} - {_display_name(info)} ' + f'({ansi.DIM}{info.action_name}{ansi.RESET}) [{_status_label(info)}]') + + +def print_capability_info(info: CapabilityInfo) -> None: + """Pretty-print the full capability description (requirements, effects, parameters, ...).""" + title = f'{_identity(info)} -- {_display_name(info)}' + bar = '=' * (len(title) + 4) + + lines = [ + f'{ansi.BOLD}{ansi.CYAN}{bar}\n {title} [{_status_label(info)}{ansi.CYAN}]\n' + f'{bar}{ansi.RESET}', + f'\n {ansi.BOLD}Action:{ansi.RESET} {info.action_name}', + ] + + if not info.description_json_valid: + lines.append( + f'{ansi.RED} (could not parse the JSON description published on ' + f'/capabilities)\n{ansi.RESET}{ansi.DIM}{info.description_json_raw}{ansi.RESET}') + safe_print('\n'.join(lines)) + return + + j = info.description_json + lines.append(f"\n {j.get('description', '(no description)')}") + + action = j.get('action') + if isinstance(action, dict): + lines.append(f"\n {ansi.BOLD}Action type:{ansi.RESET} {action.get('type', '')}") + + _bullet_list(lines, j, 'requirements') + _bullet_list(lines, j, 'effects') + _bullet_list(lines, j, 'notes') + + parameters = j.get('parameters') + if isinstance(parameters, dict): + lines.append(f'\n {ansi.BOLD}Parameters:{ansi.RESET}') + for param_name, param_info in parameters.items(): + line = f' {ansi.YELLOW}{param_name}{ansi.RESET}' + if isinstance(param_info, dict) and 'type' in param_info: + line += f" ({param_info['type']})" + lines.append(line) + if isinstance(param_info, dict) and 'description' in param_info: + lines.append(f" {ansi.DIM}{param_info['description']}{ansi.RESET}") + + safe_print('\n'.join(lines)) diff --git a/easyfleet_mission_manager_py/easyfleet_mission_manager_py/capability_state.py b/easyfleet_mission_manager_py/easyfleet_mission_manager_py/capability_state.py new file mode 100644 index 0000000..674c9a4 --- /dev/null +++ b/easyfleet_mission_manager_py/easyfleet_mission_manager_py/capability_state.py @@ -0,0 +1,43 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +"""CapabilityState, mirroring easyfleet_mission_manager's capability_state.hpp.""" + +import enum + + +class CapabilityState(enum.Enum): + """ + What a RobotHandle-driven capability is doing right now. + + Deliberately richer than a plain running/not-running bool -- see the + C++ CapabilityState's own doc comment for the full rationale. Every + terminal value mirrors easyfleet_core::CapabilityClient::Outcome + (capability_client.Outcome here) one to one, via + detail.running_capability's own outcome-to-state mapping. + """ + + IDLE = 'IDLE' + RUNNING = 'RUNNING' + SUCCEEDED = 'SUCCEEDED' + ABORTED = 'ABORTED' + CANCELED = 'CANCELED' + REJECTED = 'REJECTED' + TIMEOUT = 'TIMEOUT' + UNREACHABLE = 'UNREACHABLE' + + +def to_string(state: CapabilityState) -> str: + return state.value diff --git a/easyfleet_mission_manager_py/easyfleet_mission_manager_py/detail/__init__.py b/easyfleet_mission_manager_py/easyfleet_mission_manager_py/detail/__init__.py new file mode 100644 index 0000000..3f1bc88 --- /dev/null +++ b/easyfleet_mission_manager_py/easyfleet_mission_manager_py/detail/__init__.py @@ -0,0 +1,14 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. diff --git a/easyfleet_mission_manager_py/easyfleet_mission_manager_py/detail/running_capability.py b/easyfleet_mission_manager_py/easyfleet_mission_manager_py/detail/running_capability.py new file mode 100644 index 0000000..35af6f7 --- /dev/null +++ b/easyfleet_mission_manager_py/easyfleet_mission_manager_py/detail/running_capability.py @@ -0,0 +1,99 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +"""RunningCapability, mirroring easyfleet_mission_manager's detail/running_capability.hpp.""" + +import threading +import time + +from .. import ansi +from ..capability_client import CapabilityClient, Outcome +from ..capability_state import CapabilityState +from ..output import safe_print + +_OUTCOME_TO_STATE = { + Outcome.SUCCEEDED: CapabilityState.SUCCEEDED, + Outcome.ABORTED: CapabilityState.ABORTED, + Outcome.CANCELED: CapabilityState.CANCELED, + Outcome.REJECTED: CapabilityState.REJECTED, + Outcome.SERVER_UNAVAILABLE: CapabilityState.UNREACHABLE, +} + + +class RunningCapability: + """ + Tracks one capability_type's in-flight/last-finished goal on a RobotHandle. + + Owns the one CapabilityClient a given (robot, capability_type) pair + is ever run through -- a later run() call reuses it, sending a fresh + goal (server-side preemption). + """ + + def __init__(self, node, action_type, action_name, robot_name, capability_type, status): + self._client = CapabilityClient(node, action_type, action_name) + self._robot_name = robot_name + self._capability_type = capability_type + self._status = status + self._lock = threading.Lock() + self._state = CapabilityState.IDLE + self._deadline = None + self._generation = 0 + + def run(self, goal, timeout_sec: float) -> None: + """Send `goal` and start tracking it, stopping it after `timeout_sec` if still running.""" + with self._lock: + self._generation += 1 + generation = self._generation + self._state = CapabilityState.RUNNING + self._deadline = time.monotonic() + timeout_sec + self._status.set_status(self._robot_name, f'Running {self._capability_type}') + + def on_response(response): + # A new run() call means a new goal supersedes whatever was + # in flight (server-side preemption). The *client*-side + # outcome of that superseded goal can still arrive after + # this point -- only the response whose generation still + # matches the current one is allowed to update state, so a + # late-arriving stale outcome can never stomp a newer goal's + # already-settled state. + with self._lock: + if generation != self._generation: + return + new_state = _OUTCOME_TO_STATE.get(response.outcome, CapabilityState.ABORTED) + self._state = new_state + self._status.set_status( + self._robot_name, f'{self._capability_type} -> {new_state.value}') + safe_print( + f' {ansi.DIM}[{self._robot_name}/{self._capability_type}] {ansi.RESET}' + f'finished with outcome {ansi.MAGENTA}{new_state.value}{ansi.RESET}') + + self._client.request(goal, on_response) + + def stop(self) -> None: + self._client.cancel() + + @property + def state(self) -> CapabilityState: + with self._lock: + return self._state + + def check_timeout(self) -> None: + """Cancel the in-flight goal if `timeout_sec` (from the last run()) has elapsed.""" + with self._lock: + state = self._state + deadline = self._deadline + timed_out = deadline is not None and time.monotonic() >= deadline + if state == CapabilityState.RUNNING and timed_out: + self._client.cancel() diff --git a/easyfleet_mission_manager_py/easyfleet_mission_manager_py/fleet_session.py b/easyfleet_mission_manager_py/easyfleet_mission_manager_py/fleet_session.py new file mode 100644 index 0000000..21d737e --- /dev/null +++ b/easyfleet_mission_manager_py/easyfleet_mission_manager_py/fleet_session.py @@ -0,0 +1,159 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +"""FleetSession, mirroring easyfleet_mission_manager's fleet_session.hpp/.cpp.""" + +import time + +from easyfleet_interfaces.msg import CapabilityStatus +import rclpy +from rclpy.executors import SingleThreadedExecutor +from rclpy.qos import QoSProfile, ReliabilityPolicy + +from .capability_discovery import discover_capabilities as _discover_capabilities +from .mission_helpers import spin_in_background +from .status_markers import StatusMarkerPublisher + + +class FleetSession: + """ + Everything talking to a fleet from a mission-control process actually needs. + + Regardless of what decides *when* to command which robot: the ROS + node and background-spinning executor every RobotHandle rides on, + one shared capability discovery pass, the registry of added robots, + and the automatic status markers RobotHandle.run_capability() + publishes. + + `SimpleController` -- a plain mission script driving robots by hand + -- is the simplest possible thing built on top of a FleetSession, + but it's deliberately not the *only* thing that can be: a + FleetSession has no notion of "how a mission decides what to do + next", only "how to talk to the robots once something has decided". + + Usage: + session = FleetSession() + robot_1 = RobotHandle('robot_1') + session.add_robot(robot_1) + session.discover_capabilities() + + robot_1.run_capability('navigation', Navigation, make_navigation_goal('kitchen')) + while robot_1.is_capability_running('navigation'): + session.spin_some() + + session.shutdown() + """ + + def __init__(self): + self._node = rclpy.create_node('mission_control') + self._executor = SingleThreadedExecutor() + self._executor.add_node(self._node) + self._spin_thread = spin_in_background(self._executor) + self._status = StatusMarkerPublisher(self._node) + self._robots: list = [] + + # Persistent (unlike discover_capabilities()'s own temporary + # subscription) so is_alive() reflects the *current* liveness of + # every robot's capabilities for the whole mission, not just a + # discovery-time snapshot -- dispatched to whichever attached + # RobotHandle matches the message's robot field. + self._capability_status_sub = self._node.create_subscription( + CapabilityStatus, '/capabilities_status', self._on_capability_status, + QoSProfile(depth=10, reliability=ReliabilityPolicy.RELIABLE)) + + def _on_capability_status(self, msg: CapabilityStatus) -> None: + now = time.monotonic() + robot = self.find_robot(msg.robot) + if robot is not None: + robot._note_heartbeat(msg.capability, now) + + def add_robot(self, robot) -> None: + """Add `robot` to this session, attaching it so it can be discovered and commanded.""" + self._robots.append(robot) + robot._attach(self) + + @property + def robots(self) -> list: + """Every robot added so far, in add_robot() order.""" + return list(self._robots) + + def find_robot(self, name: str): + """Look up a robot added so far by RobotHandle.name, or None if not found.""" + for robot in self._robots: + if robot.name == name: + return robot + return None + + def discover_capabilities(self, window_sec: float = 2.5) -> None: + """ + Run exactly one discovery scan and fan the results out to every added RobotHandle. + + Never a redundant per-robot re-scan. + """ + all_capabilities = _discover_capabilities(self._node, window_sec) + for robot in self._robots: + this_robot = [info for info in all_capabilities if info.robot == robot.name] + robot._set_capabilities(this_robot) + + def spin_some(self) -> None: + """ + Give the background spin thread a time slice and check every robot's timeouts. + + The actual spinning happens continuously on the background + thread started in __init__ -- this just gives it a slice of + time to make progress and lets every added robot's in-flight + run_capability() timeouts get checked. + """ + time.sleep(0.01) + for robot in self._robots: + robot._check_timeouts() + + def spin_for(self, duration_sec: float) -> None: + """Block for exactly `duration_sec`, still spinning underneath.""" + deadline = time.monotonic() + duration_sec + while time.monotonic() < deadline: + self.spin_some() + + @property + def node(self): + """Underlying node, for anything not yet covered by RobotHandle.""" + return self._node + + @property + def status_marker_publisher(self) -> StatusMarkerPublisher: + """Status marker publisher RobotHandle.run_capability() publishes through.""" + return self._status + + def shutdown(self) -> None: + """Stop the executor, join its background thread, and shut down the ROS context.""" + self._teardown() + rclpy.shutdown() + + def _teardown(self) -> None: + """ + Stop the background spin thread and destroy the node, without touching rclpy itself. + + Internal: unlike shutdown(), this leaves rclpy usable for the + rest of the process -- Python has no deterministic destructor to + call this automatically the way ~FleetSession() does in C++, so + tests that construct several FleetSessions in one process + (sharing one rclpy.init()/rclpy.shutdown() pair) call this + directly between them instead of shutdown(). + """ + if self._spin_thread.is_alive(): + self._executor.shutdown() + self._spin_thread.join() + self._executor.remove_node(self._node) + self._node.destroy_node() diff --git a/easyfleet_mission_manager_py/easyfleet_mission_manager_py/mission_helpers.py b/easyfleet_mission_manager_py/easyfleet_mission_manager_py/mission_helpers.py new file mode 100644 index 0000000..42d489a --- /dev/null +++ b/easyfleet_mission_manager_py/easyfleet_mission_manager_py/mission_helpers.py @@ -0,0 +1,149 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +""" +Shared by every mission script, mirroring easyfleet_mission_manager's mission_helpers.hpp. + +The easyfleet_interfaces action types the mock capabilities speak, how +to build a demo goal and a feedback printer for each of them, and small +terminal/discovery helpers. +""" + +import json +import threading +import time + +from easyfleet_interfaces.action import Manipulation, Navigation, Perception + +from . import ansi +from .output import safe_print +from .throttle import Throttle + +__all__ = [ + 'Manipulation', 'Navigation', 'Perception', + 'RUN_TIMEOUT_SEC', + 'find_robot_capability', + 'make_manipulation_feedback_printer', 'make_manipulation_goal', + 'make_navigation_feedback_printer', 'make_navigation_goal', + 'make_perception_feedback_printer', 'make_perception_goal', + 'print_section', 'print_step', 'spin_in_background', +] + +# How long a mission script lets a capability run before stopping it, if +# it hasn't finished on its own by then. +RUN_TIMEOUT_SEC = 10.0 + + +def spin_in_background(executor) -> threading.Thread: + """Start `executor.spin()` on a background thread; block until it has actually begun.""" + thread = threading.Thread(target=executor.spin, daemon=True) + thread.start() + while not executor.is_spinning: + time.sleep(0) + return thread + + +def print_section(title: str) -> None: + """Print a bold, boxed header marking a new phase of a mission script.""" + safe_print(f'\n{ansi.BOLD}{ansi.BLUE}== {title} =={ansi.RESET}') + + +def print_step(text: str) -> None: + """Print a dim, indented line explaining what a phase is about to do (or just did).""" + safe_print(f' {ansi.DIM}{text}{ansi.RESET}') + + +def find_robot_capability(capabilities, robot: str, capability: str): + """Find `robot`'s active capability of the given short type (e.g. "navigation").""" + for info in capabilities: + if info.robot == robot and info.capability == capability and info.active: + return info + return None + + +def make_navigation_goal(waypoint_id: str | None = None): + """ + Build a Navigation goal. + + With no `waypoint_id`, a fixed demo pose; with one, targets a named + waypoint via the `parameters_json: {"goal_id": ...}` convention the + real EasyNav-backed navigation capability reads -- ignored by (and + therefore also safe against) the mock navigation capability. + """ + goal = Navigation.Goal() + if waypoint_id is None: + goal.target_pose.header.frame_id = 'map' + goal.target_pose.pose.position.x = 2.0 + goal.target_pose.pose.position.y = 1.0 + goal.target_pose.pose.orientation.w = 1.0 + else: + goal.parameters_json = json.dumps({'goal_id': waypoint_id}) + return goal + + +def make_navigation_feedback_printer(label: str): + """Build a throttled feedback printer for navigation goals.""" + throttle = Throttle(0.5) + + def printer(feedback) -> None: + if not throttle.ready(): + return + safe_print( + f' {ansi.DIM}[{label}] {ansi.RESET}' + f'distance_remaining={feedback.distance_remaining}m, ' + f'elapsed={feedback.navigation_time.sec}s, ' + f'recoveries={feedback.number_of_recoveries}') + return printer + + +def make_manipulation_goal(pose=None): + """Build a Manipulation goal: a fixed demo joint target, or an end-effector `pose`.""" + goal = Manipulation.Goal() + if pose is None: + goal.mode = Manipulation.Goal.MODE_JOINT_TARGET + goal.joint_target.name = ['joint1'] + goal.joint_target.position = [1.0] + else: + goal.mode = Manipulation.Goal.MODE_POSE_TARGET + goal.pose_target = pose + return goal + + +def make_manipulation_feedback_printer(label: str): + throttle = Throttle(0.5) + + def printer(feedback) -> None: + if not throttle.ready(): + return + safe_print(f' {ansi.DIM}[{label}] {ansi.RESET}state={feedback.state}') + return printer + + +def make_perception_goal(object_classes: list[str] | None = None): + """Build a Perception goal detecting the given object classes (default: a demo "gato").""" + goal = Perception.Goal() + goal.object_classes = object_classes if object_classes is not None else ['gato'] + return goal + + +def make_perception_feedback_printer(label: str): + throttle = Throttle(0.5) + + def printer(feedback) -> None: + if not throttle.ready(): + return + count = len(feedback.detections_3d.detections) + safe_print(f' {ansi.DIM}[{label}] {ansi.RESET}{count} cat(s) detected (3D + 2D)') + return printer diff --git a/easyfleet_mission_manager_py/easyfleet_mission_manager_py/output.py b/easyfleet_mission_manager_py/easyfleet_mission_manager_py/output.py new file mode 100644 index 0000000..6726384 --- /dev/null +++ b/easyfleet_mission_manager_py/easyfleet_mission_manager_py/output.py @@ -0,0 +1,26 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +"""Serialized terminal output, mirroring easyfleet_mission_manager's output.hpp.""" + +import threading + +_output_lock = threading.Lock() + + +def safe_print(line: str) -> None: + """Print `line`, serialized across every thread that calls this.""" + with _output_lock: + print(line, flush=True) diff --git a/easyfleet_mission_manager_py/easyfleet_mission_manager_py/robot_handle.py b/easyfleet_mission_manager_py/easyfleet_mission_manager_py/robot_handle.py new file mode 100644 index 0000000..347746c --- /dev/null +++ b/easyfleet_mission_manager_py/easyfleet_mission_manager_py/robot_handle.py @@ -0,0 +1,192 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +"""RobotHandle, mirroring easyfleet_mission_manager's robot_handle.hpp/.cpp.""" + +import threading +import time + +from . import ansi +from .capability_info import print_capability_info, print_capability_summary_line +from .capability_state import CapabilityState +from .detail.running_capability import RunningCapability +from .output import safe_print + +# How long without a heartbeat before is_alive() gives up on a capability +# -- a few times the 1 Hz heartbeat period a capability itself publishes +# at, so one or two dropped messages don't flip it. +_ALIVE_TIMEOUT_SEC = 3.0 + +# Default: how long a mission script lets a capability run before +# stopping it, if it hasn't finished on its own by then. +RUN_TIMEOUT_SEC = 10.0 + + +class MissionManagerError(RuntimeError): + """Caller-usage error on a RobotHandle, mirroring easyfleet::RobotHandle's std::logic_error.""" + + +class _RunningEntry: + __slots__ = ('action_type', 'running') + + def __init__(self, action_type, running: RunningCapability): + self.action_type = action_type + self.running = running + + +class RobotHandle: + """ + A remote robot's capabilities, as seen and commanded from a mission script. + + A thin proxy over whatever this robot announced on /capabilities, + discovered by the FleetSession it's added to. Owns no ROS nodes of + its own. + + Usage: + controller = SimpleController() + robot_1 = RobotHandle('robot_1') + controller.add_robot(robot_1) + controller.discover_capabilities() + + if not robot_1.has_capability('navigation'): + ... + + robot_1.run_capability('navigation', Navigation, make_navigation_goal('kitchen')) + while robot_1.is_capability_running('navigation'): + controller.spin_some() + if robot_1.capability_state('navigation') == CapabilityState.FAILED: + ... + """ + + def __init__(self, name: str): + self._name = name + self._capabilities = [] + self._running: dict[str, _RunningEntry] = {} + self._last_heartbeat: dict[str, float] = {} + self._session = None + self._lock = threading.Lock() + + @property + def name(self) -> str: + return self._name + + def has_capability(self, capability_type: str) -> bool: + """Return whether this robot announced an *active* capability of this type.""" + return any( + info.capability == capability_type and info.active + for info in self._capabilities) + + def run_capability( + self, capability_type: str, action_type, goal=None, timeout_sec=RUN_TIMEOUT_SEC, + ): + """ + Send `goal` to `capability_type` and return immediately. + + `action_type` is the easyfleet_interfaces.action module (e.g. + Navigation) this capability_type talks -- fixed by the *first* + run_capability() call for a given capability_type on this handle; + a later call with a *different* action_type for the same + capability_type is a caller bug and raises MissionManagerError. + + `goal` defaults to a default-constructed `action_type.Goal()`. + """ + if self._session is None: + raise MissionManagerError( + f'RobotHandle.run_capability("{capability_type}", ...): this handle was ' + 'never added to a FleetSession (call FleetSession.add_robot()/' + 'SimpleController.add_robot() first).') + + info = next( + (i for i in self._capabilities if i.capability == capability_type and i.active), + None) + if info is None: + safe_print( + f'{ansi.YELLOW}[{self._name}/{capability_type}] not active, skipping.' + f'{ansi.RESET}') + return + + if goal is None: + goal = action_type.Goal() + + with self._lock: + entry = self._running.get(capability_type) + if entry is None: + running = RunningCapability( + self._session.node, action_type, info.action_name, self._name, + capability_type, self._session.status_marker_publisher) + entry = _RunningEntry(action_type, running) + self._running[capability_type] = entry + elif entry.action_type is not action_type: + raise MissionManagerError( + f'RobotHandle.run_capability("{capability_type}", ...): this capability ' + 'type was already run with a different action type on this handle -- a ' + 'capability_type must mean the same action type for the life of a ' + 'RobotHandle.') + + entry.running.run(goal, timeout_sec) + + def stop_capability(self, capability_type: str) -> None: + """Ask `capability_type` to stop whatever it's doing, if anything.""" + with self._lock: + entry = self._running.get(capability_type) + if entry is not None: + entry.running.stop() + + def is_capability_running(self, capability_type: str) -> bool: + return self.capability_state(capability_type) == CapabilityState.RUNNING + + def capability_state(self, capability_type: str) -> CapabilityState: + with self._lock: + entry = self._running.get(capability_type) + if entry is None: + return CapabilityState.IDLE + return entry.running.state + + def is_alive(self, capability_type: str) -> bool: + """Return whether a heartbeat has been seen recently on /capabilities_status.""" + with self._lock: + last = self._last_heartbeat.get(capability_type) + if last is None: + return False + return (time.monotonic() - last) < _ALIVE_TIMEOUT_SEC + + def print_capabilities(self) -> None: + for info in self._capabilities: + print_capability_summary_line(info) + for info in self._capabilities: + safe_print('') + print_capability_info(info) + + @property + def capabilities(self) -> list: + return list(self._capabilities) + + # --- Internal: called by FleetSession only, not part of the public API --- + + def _attach(self, session) -> None: + self._session = session + + def _set_capabilities(self, capabilities: list) -> None: + self._capabilities = capabilities + + def _note_heartbeat(self, capability_type: str, when: float) -> None: + with self._lock: + self._last_heartbeat[capability_type] = when + + def _check_timeouts(self) -> None: + with self._lock: + entries = list(self._running.values()) + for entry in entries: + entry.running.check_timeout() diff --git a/easyfleet_mission_manager_py/easyfleet_mission_manager_py/simple_controller.py b/easyfleet_mission_manager_py/easyfleet_mission_manager_py/simple_controller.py new file mode 100644 index 0000000..e5ec186 --- /dev/null +++ b/easyfleet_mission_manager_py/easyfleet_mission_manager_py/simple_controller.py @@ -0,0 +1,72 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +"""SimpleController, mirroring easyfleet_mission_manager's simple_controller.hpp.""" + +from .fleet_session import FleetSession + + +class SimpleController: + """ + The plain, manual controller: a mission script decides everything by hand. + + SimpleController just hosts the FleetSession that makes talking to + those robots possible -- there is no behavior here beyond the + session's own. This is deliberate: it's meant to be the reference + example of "a controller with zero decision-making logic of its + own", also useful as a template for writing a different one. + + Usage: + controller = SimpleController() + + robot_1 = RobotHandle('robot_1') + robot_2 = RobotHandle('robot_2') + controller.add_robot(robot_1) + controller.add_robot(robot_2) + + controller.discover_capabilities() + # ... robot_1.run_capability(...), controller.spin_some()/spin_for(...) ... + + controller.shutdown() + """ + + def __init__(self): + self._session = FleetSession() + + def add_robot(self, robot) -> None: + self._session.add_robot(robot) + + @property + def robots(self) -> list: + return self._session.robots + + def find_robot(self, name: str): + return self._session.find_robot(name) + + def discover_capabilities(self, window_sec: float = 2.5) -> None: + self._session.discover_capabilities(window_sec) + + def spin_some(self) -> None: + self._session.spin_some() + + def spin_for(self, duration_sec: float) -> None: + self._session.spin_for(duration_sec) + + @property + def node(self): + return self._session.node + + def shutdown(self) -> None: + self._session.shutdown() diff --git a/easyfleet_mission_manager_py/easyfleet_mission_manager_py/status_markers.py b/easyfleet_mission_manager_py/easyfleet_mission_manager_py/status_markers.py new file mode 100644 index 0000000..3560d64 --- /dev/null +++ b/easyfleet_mission_manager_py/easyfleet_mission_manager_py/status_markers.py @@ -0,0 +1,111 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +"""StatusMarkerPublisher, mirroring easyfleet_mission_manager's status_markers.hpp.""" + +import threading + +from rclpy.duration import Duration +from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy +from visualization_msgs.msg import Marker, MarkerArray + + +class StatusMarkerPublisher: + """ + Publishes a floating TEXT_VIEW_FACING marker above each robot's own base_link. + + Shows a short, human-readable line of what that robot is currently + doing -- so watching the mission in RViz alone is enough to follow + along. One marker per robot, keyed by name: each set_status() call + *replaces* that robot's previous text (same marker id). + """ + + def __init__( + self, node, topic: str = 'mission_status_markers', + height: float = 0.75, text_size: float = 0.25, + ): + self._node = node + self._height = height + self._text_size = text_size + # Transient-local: a subscriber (RViz) that joins after the + # mission has already started still needs to see every robot's + # last known status immediately, not wait for its next + # transition. + self._pub = node.create_publisher( + MarkerArray, topic, + QoSProfile( + depth=10, reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.TRANSIENT_LOCAL)) + self._lock = threading.Lock() + self._texts: dict[str, str] = {} + self._marker_ids: dict[str, int] = {} + self._next_id = 0 + # set_status() is only called at mission phase transitions -- + # re-publishing every robot's current marker on a short timer + # keeps every stamp recent, so RViz keeps resolving + # "/base_link" and moving the text with the robot, + # independent of how often the *text* itself actually changes. + self._refresh_timer = node.create_timer(0.2, self._publish_all) + + def set_status(self, robot: str, text: str) -> None: + """Set `robot`'s current status text and publish it immediately.""" + with self._lock: + self._texts[robot] = text + self._marker_id(robot) + self._publish_all() + + def _marker_id(self, robot: str) -> int: + # Requires self._lock to already be held by the caller. + marker_id = self._marker_ids.get(robot) + if marker_id is None: + marker_id = self._next_id + self._next_id += 1 + self._marker_ids[robot] = marker_id + return marker_id + + def _publish_all(self) -> None: + array = MarkerArray() + stamp = self._node.get_clock().now().to_msg() + with self._lock: + items = list(self._texts.items()) + marker_ids = dict(self._marker_ids) + + for robot, text in items: + marker = Marker() + marker.header.frame_id = f'{robot}/base_link' + marker.header.stamp = stamp + marker.ns = 'mission_status' + marker.id = marker_ids[robot] + marker.type = Marker.TEXT_VIEW_FACING + marker.action = Marker.ADD + # Re-transform against the *latest* available transform on + # every render frame instead of an exact-header.stamp + # lookup, which is racy under sim time. + marker.frame_locked = True + marker.pose.position.z = self._height + marker.pose.orientation.w = 1.0 + marker.scale.z = self._text_size + marker.color.r = 1.0 + marker.color.g = 1.0 + marker.color.b = 1.0 + marker.color.a = 1.0 + marker.text = text + # Slightly longer than the refresh period, so a marker never + # visibly blinks out between two refresh ticks. + marker.lifetime = Duration(seconds=0.5).to_msg() + array.markers.append(marker) + + if array.markers: + self._pub.publish(array) diff --git a/easyfleet_mission_manager_py/easyfleet_mission_manager_py/throttle.py b/easyfleet_mission_manager_py/easyfleet_mission_manager_py/throttle.py new file mode 100644 index 0000000..cbdf746 --- /dev/null +++ b/easyfleet_mission_manager_py/easyfleet_mission_manager_py/throttle.py @@ -0,0 +1,45 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +"""Rate limiter, mirroring easyfleet_mission_manager's throttle.hpp.""" + +import threading +import time + + +class Throttle: + """ + Minimum-interval rate limiter. + + Unlike the C++ original (which uses a shared_ptr so + copies of the same Throttle share one clock, since a feedback + callback captures it by value into a std::function), a Python + Throttle instance captured by a closure is already shared by + reference -- no extra indirection needed for the same effect. + """ + + def __init__(self, interval_sec: float): + self._interval_sec = interval_sec + self._last = 0.0 + self._lock = threading.Lock() + + def ready(self) -> bool: + """Return whether at least `interval_sec` has elapsed since the last True.""" + now = time.monotonic() + with self._lock: + if now - self._last >= self._interval_sec: + self._last = now + return True + return False diff --git a/easyfleet_mission_manager_py/package.xml b/easyfleet_mission_manager_py/package.xml new file mode 100644 index 0000000..efa6960 --- /dev/null +++ b/easyfleet_mission_manager_py/package.xml @@ -0,0 +1,35 @@ + + + + easyfleet_mission_manager_py + 0.1.0 + + Python mirror of easyfleet_mission_manager: FleetSession/RobotHandle/ + SimpleController for writing mission controllers in Python instead of + C++. Same wire protocol (/capabilities, /capabilities_status, the + easyfleet_interfaces actions), so a Python mission script and a C++ one + are interchangeable against the same fleet. + + + Francisco Martín Rico + + Apache-2.0 + + Francisco Martín Rico + + rclpy + easyfleet_interfaces + visualization_msgs + geometry_msgs + action_msgs + + ament_copyright + ament_flake8 + ament_pep257 + ament_xmllint + python3-pytest + + + ament_python + + diff --git a/easyfleet_mission_manager_py/resource/easyfleet_mission_manager_py b/easyfleet_mission_manager_py/resource/easyfleet_mission_manager_py new file mode 100644 index 0000000..e69de29 diff --git a/easyfleet_mission_manager_py/setup.cfg b/easyfleet_mission_manager_py/setup.cfg new file mode 100644 index 0000000..a47326c --- /dev/null +++ b/easyfleet_mission_manager_py/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/easyfleet_mission_manager_py +[install] +install_scripts=$base/lib/easyfleet_mission_manager_py diff --git a/easyfleet_mission_manager_py/setup.py b/easyfleet_mission_manager_py/setup.py new file mode 100644 index 0000000..c44816f --- /dev/null +++ b/easyfleet_mission_manager_py/setup.py @@ -0,0 +1,22 @@ +from setuptools import find_packages, setup + +package_name = 'easyfleet_mission_manager_py' + +setup( + name=package_name, + version='0.1.0', + packages=find_packages(include=[package_name, package_name + '.*'], exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', [f'resource/{package_name}']), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Francisco Martín Rico', + maintainer_email='fmrico@gmail.com', + description=( + 'Python mirror of easyfleet_mission_manager: FleetSession/RobotHandle/' + 'SimpleController for writing mission controllers in Python.' + ), + license='Apache-2.0', +) diff --git a/easyfleet_mission_manager_py/test/__init__.py b/easyfleet_mission_manager_py/test/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/easyfleet_mission_manager_py/test/test_capability_discovery.py b/easyfleet_mission_manager_py/test/test_capability_discovery.py new file mode 100644 index 0000000..944d298 --- /dev/null +++ b/easyfleet_mission_manager_py/test/test_capability_discovery.py @@ -0,0 +1,186 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +"""Integration tests for discover_capabilities(), mirroring test_capability_discovery.cpp.""" + +import itertools +import threading + +from easyfleet_interfaces.msg import CapabilityDescription, CapabilityStatus +from easyfleet_mission_manager_py.capability_discovery import discover_capabilities +import pytest +import rclpy +from rclpy.executors import SingleThreadedExecutor +from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy + +_name_counter = itertools.count() + + +def _unique_name(base: str) -> str: + return f'{base}_{next(_name_counter)}' + + +def _spin_in_background(executor) -> threading.Thread: + thread = threading.Thread(target=executor.spin, daemon=True) + thread.start() + while not executor.is_spinning: + pass + return thread + + +def _make_description(capability: str, action_name: str, description_json: str, robot: str = ''): + msg = CapabilityDescription() + msg.robot = robot + msg.capability = capability + msg.action_name = action_name + msg.description_json = description_json + return msg + + +class _Fixture: + + def __init__(self): + self.consumer_node = rclpy.create_node(_unique_name('test_discovery_consumer')) + self.publisher_node = rclpy.create_node(_unique_name('test_discovery_publisher')) + self.executor = SingleThreadedExecutor() + self.executor.add_node(self.consumer_node) + self.executor.add_node(self.publisher_node) + self.spin_thread = _spin_in_background(self.executor) + + self.capabilities_pub = self.publisher_node.create_publisher( + CapabilityDescription, '/capabilities', + QoSProfile( + depth=10, reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.TRANSIENT_LOCAL)) + self.status_pub = self.publisher_node.create_publisher( + CapabilityStatus, '/capabilities_status', + QoSProfile(depth=10, reliability=ReliabilityPolicy.RELIABLE)) + self._timers = [] + + def start_heartbeat(self, capability: str, action_name: str, robot: str = '', busy=False): + def publish(): + msg = CapabilityStatus() + msg.robot = robot + msg.capability = capability + msg.action_name = action_name + msg.busy = busy + self.status_pub.publish(msg) + + timer = self.publisher_node.create_timer(0.05, publish) + self._timers.append(timer) + return timer + + def teardown(self): + self.executor.shutdown() + if self.spin_thread.is_alive(): + self.spin_thread.join() + self.consumer_node.destroy_node() + self.publisher_node.destroy_node() + + +@pytest.fixture +def fixture(): + rclpy.init() + f = _Fixture() + yield f + f.teardown() + rclpy.shutdown() + + +def test_returns_empty_when_nothing_is_published(fixture): + result = discover_capabilities(fixture.consumer_node, window_sec=0.3) + assert result == [] + + +def test_parses_json_and_marks_active_when_heartbeat_seen(fixture): + fixture.capabilities_pub.publish( + _make_description( + 'fake_cap', '/fake_cap', '{"name":"fake_cap","display_name":"Fake Capability"}')) + fixture.start_heartbeat('fake_cap', '/fake_cap') + + result = discover_capabilities(fixture.consumer_node, window_sec=0.8) + + assert len(result) == 1 + info = result[0] + assert info.capability == 'fake_cap' + assert info.action_name == '/fake_cap' + assert info.description_json_valid is True + assert info.active is True + assert info.busy is False + assert info.description_json['display_name'] == 'Fake Capability' + + +def test_tracks_busy_state_from_the_latest_heartbeat(fixture): + fixture.capabilities_pub.publish(_make_description('fake_cap', '/fake_cap', '{}')) + fixture.start_heartbeat('fake_cap', '/fake_cap', busy=True) + + result = discover_capabilities(fixture.consumer_node, window_sec=0.8) + + assert len(result) == 1 + assert result[0].active is True + assert result[0].busy is True + + +def test_capability_without_heartbeat_is_not_active(fixture): + fixture.capabilities_pub.publish(_make_description('silent_cap', '/silent_cap', '{}')) + + result = discover_capabilities(fixture.consumer_node, window_sec=0.5) + + assert len(result) == 1 + assert result[0].capability == 'silent_cap' + assert result[0].description_json_valid is True + assert result[0].active is False + + +def test_malformed_json_still_registers_the_capability_but_flags_it_invalid(fixture): + fixture.capabilities_pub.publish( + _make_description('broken_cap', '/broken_cap', 'not valid json {{{')) + + result = discover_capabilities(fixture.consumer_node, window_sec=0.3) + + assert len(result) == 1 + assert result[0].capability == 'broken_cap' + assert result[0].description_json_valid is False + + +def test_discovers_multiple_distinct_capabilities(fixture): + fixture.capabilities_pub.publish(_make_description('cap_a', '/cap_a', '{}')) + fixture.capabilities_pub.publish(_make_description('cap_b', '/cap_b', '{}')) + fixture.start_heartbeat('cap_a', '/cap_a') + + result = discover_capabilities(fixture.consumer_node, window_sec=0.8) + + assert len(result) == 2 + by_action_name = {info.action_name: info for info in result} + assert by_action_name['/cap_a'].active is True + assert by_action_name['/cap_b'].active is False + + +def test_same_capability_from_two_robots_are_kept_distinct_by_action_name(fixture): + fixture.capabilities_pub.publish( + _make_description('navigation', '/robot1/navigation', '{}', robot='robot1')) + fixture.capabilities_pub.publish( + _make_description('navigation', '/robot2/navigation', '{}', robot='robot2')) + fixture.start_heartbeat('navigation', '/robot1/navigation', robot='robot1') + fixture.start_heartbeat('navigation', '/robot2/navigation', robot='robot2') + + result = discover_capabilities(fixture.consumer_node, window_sec=0.8) + + assert len(result) == 2 + for info in result: + assert info.capability == 'navigation' + assert info.active is True + assert info.robot in ('robot1', 'robot2') + assert result[0].action_name != result[1].action_name diff --git a/easyfleet_mission_manager_py/test/test_capability_state.py b/easyfleet_mission_manager_py/test/test_capability_state.py new file mode 100644 index 0000000..14bb242 --- /dev/null +++ b/easyfleet_mission_manager_py/test/test_capability_state.py @@ -0,0 +1,23 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +"""Unit tests for CapabilityState.to_string(). No ROS node required.""" + +from easyfleet_mission_manager_py.capability_state import CapabilityState, to_string + + +def test_to_string_matches_member_name_for_every_value(): + for state in CapabilityState: + assert to_string(state) == state.name diff --git a/easyfleet_mission_manager_py/test/test_copyright.py b/easyfleet_mission_manager_py/test/test_copyright.py new file mode 100644 index 0000000..cc8ff03 --- /dev/null +++ b/easyfleet_mission_manager_py/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 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 ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/easyfleet_mission_manager_py/test/test_flake8.py b/easyfleet_mission_manager_py/test/test_flake8.py new file mode 100644 index 0000000..2603011 --- /dev/null +++ b/easyfleet_mission_manager_py/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 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 ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, 'Found %d code style errors / warnings:\n' % len( + errors + ) + '\n'.join(errors) diff --git a/easyfleet_mission_manager_py/test/test_fleet_session_robot_handle.py b/easyfleet_mission_manager_py/test/test_fleet_session_robot_handle.py new file mode 100644 index 0000000..783cc63 --- /dev/null +++ b/easyfleet_mission_manager_py/test/test_fleet_session_robot_handle.py @@ -0,0 +1,308 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +"""Integration tests for FleetSession/RobotHandle/SimpleController, mirroring the C++ suite.""" + +import itertools +import time + +from easyfleet_mission_manager_py.capability_state import CapabilityState +from easyfleet_mission_manager_py.fleet_session import FleetSession +from easyfleet_mission_manager_py.mission_helpers import ( + make_manipulation_goal, + make_navigation_goal, + Manipulation, + Navigation, +) +from easyfleet_mission_manager_py.robot_handle import MissionManagerError, RobotHandle +from easyfleet_mission_manager_py.simple_controller import SimpleController +import pytest +import rclpy + +from .test_nav_fake_capability import FakeNavCapabilityProcess, REJECT_WAYPOINT_ID + +_name_counter = itertools.count() + + +def _unique_name(base: str) -> str: + return f'{base}_{next(_name_counter)}' + + +def _wait_until(condition, timeout_sec: float) -> bool: + deadline = time.monotonic() + timeout_sec + while time.monotonic() < deadline: + if condition(): + return True + time.sleep(0.01) + return condition() + + +@pytest.fixture(scope='module', autouse=True) +def ros_context(): + rclpy.init() + yield + rclpy.shutdown() + + +@pytest.fixture +def session(): + s = FleetSession() + yield s + s._teardown() + + +# --- RobotHandle, standalone (no FleetSession needed) --- + +def test_name_returns_what_was_constructed_with(): + robot = RobotHandle(_unique_name('robot')) + assert robot.name != '' + + +def test_has_no_capabilities_before_any_discovery(): + robot = RobotHandle(_unique_name('robot')) + assert robot.has_capability('navigation') is False + assert robot.capabilities == [] + + +def test_capability_state_is_idle_before_ever_running(): + robot = RobotHandle(_unique_name('robot')) + assert robot.capability_state('navigation') == CapabilityState.IDLE + assert robot.is_capability_running('navigation') is False + + +def test_is_alive_is_false_without_any_heartbeat_seen(): + robot = RobotHandle(_unique_name('robot')) + assert robot.is_alive('navigation') is False + + +def test_run_capability_on_a_handle_never_added_to_a_fleet_session_raises(): + # Never add_robot()-ed to a FleetSession -- must fail loudly, not + # silently no-op, since that's otherwise indistinguishable from a + # properly attached robot that simply hasn't announced this + # capability yet. + robot = RobotHandle(_unique_name('robot')) + with pytest.raises(MissionManagerError): + robot.run_capability('navigation', Navigation, make_navigation_goal('kitchen')) + + +# --- FleetSession, without a real robot process --- + +def test_find_robot_returns_none_before_adding(session): + assert session.find_robot('robot_1') is None + assert session.robots == [] + + +def test_add_robot_makes_it_findable_and_attached(session): + robot = RobotHandle(_unique_name('robot')) + session.add_robot(robot) + + assert len(session.robots) == 1 + found = session.find_robot(robot.name) + assert found is robot + + +# --- FleetSession + RobotHandle + a real (fake) robot process --- + +def test_discover_run_and_succeed(session): + robot_name = _unique_name('robot') + fake_robot = FakeNavCapabilityProcess(robot_name, mock_duration_sec=0.05) + try: + robot = RobotHandle(robot_name) + session.add_robot(robot) + + session.discover_capabilities() + assert robot.has_capability('navigation') is True + + robot.run_capability( + 'navigation', Navigation, make_navigation_goal('kitchen'), timeout_sec=5.0) + assert robot.is_capability_running('navigation') is True + + assert _wait_until( + lambda: not robot.is_capability_running('navigation'), timeout_sec=3.0) + assert robot.capability_state('navigation') == CapabilityState.SUCCEEDED + finally: + fake_robot.shutdown() + + +def test_run_capability_with_a_mismatched_action_type_raises(session): + robot_name = _unique_name('robot') + fake_robot = FakeNavCapabilityProcess(robot_name, mock_duration_sec=30.0) + try: + robot = RobotHandle(robot_name) + session.add_robot(robot) + session.discover_capabilities() + assert robot.has_capability('navigation') is True + + robot.run_capability( + 'navigation', Navigation, make_navigation_goal('kitchen'), timeout_sec=30.0) + assert _wait_until( + lambda: robot.is_capability_running('navigation'), timeout_sec=0.5) + + with pytest.raises(MissionManagerError): + robot.run_capability( + 'navigation', Manipulation, make_manipulation_goal(), timeout_sec=5.0) + + robot.stop_capability('navigation') + finally: + fake_robot.shutdown() + + +def test_stop_capability_cancels_a_long_running_goal(session): + robot_name = _unique_name('robot') + fake_robot = FakeNavCapabilityProcess(robot_name, mock_duration_sec=30.0) + try: + robot = RobotHandle(robot_name) + session.add_robot(robot) + session.discover_capabilities() + assert robot.has_capability('navigation') is True + + robot.run_capability( + 'navigation', Navigation, make_navigation_goal('kitchen'), timeout_sec=60.0) + assert _wait_until( + lambda: robot.is_capability_running('navigation'), timeout_sec=0.5) + + robot.stop_capability('navigation') + + assert _wait_until( + lambda: not robot.is_capability_running('navigation'), timeout_sec=3.0) + assert robot.capability_state('navigation') == CapabilityState.CANCELED + finally: + fake_robot.shutdown() + + +def test_run_capability_timeout_stops_it_automatically(session): + robot_name = _unique_name('robot') + fake_robot = FakeNavCapabilityProcess(robot_name, mock_duration_sec=30.0) + try: + robot = RobotHandle(robot_name) + session.add_robot(robot) + session.discover_capabilities() + assert robot.has_capability('navigation') is True + + # A short client-side timeout, well under the goal's own (much + # longer) mock duration: only spin_some()'s periodic + # check_timeout() should be what stops it. + robot.run_capability( + 'navigation', Navigation, make_navigation_goal('kitchen'), timeout_sec=1.0) + + deadline = time.monotonic() + 3.0 + while robot.is_capability_running('navigation') and time.monotonic() < deadline: + session.spin_some() + + assert robot.capability_state('navigation') == CapabilityState.CANCELED + finally: + fake_robot.shutdown() + + +def test_rejected_goal_reports_rejected_specifically(session): + # CapabilityState reports the real, specific outcome -- a goal the + # server itself rejects must read as REJECTED, not CANCELED (which + # is reserved for a mission script/timeout deliberately stopping an + # in-flight goal). + robot_name = _unique_name('robot') + fake_robot = FakeNavCapabilityProcess(robot_name) + try: + robot = RobotHandle(robot_name) + session.add_robot(robot) + session.discover_capabilities() + assert robot.has_capability('navigation') is True + + robot.run_capability( + 'navigation', Navigation, make_navigation_goal(REJECT_WAYPOINT_ID), timeout_sec=5.0) + + assert _wait_until( + lambda: not robot.is_capability_running('navigation'), timeout_sec=3.0) + assert robot.capability_state('navigation') == CapabilityState.REJECTED + finally: + fake_robot.shutdown() + + +def test_preempting_goal_wins_over_a_late_arriving_stale_outcome(session): + # Regression test: a second run_capability() call while the first + # goal is still in flight preempts it server-side. The *first* + # goal's own client-side outcome callback can still arrive after + # the second goal has already settled -- without the generation + # guard in RunningCapability, that late callback would stomp + # capability_state() with the superseded goal's outcome. + robot_name = _unique_name('robot') + fake_robot = FakeNavCapabilityProcess(robot_name, mock_duration_sec=1.0) + try: + robot = RobotHandle(robot_name) + session.add_robot(robot) + session.discover_capabilities() + assert robot.has_capability('navigation') is True + + robot.run_capability( + 'navigation', Navigation, make_navigation_goal('kitchen'), timeout_sec=30.0) + assert _wait_until( + lambda: robot.is_capability_running('navigation'), timeout_sec=0.5) + + # Preempt partway through the first goal's ~1s mock duration. + robot.run_capability( + 'navigation', Navigation, make_navigation_goal('charging_station'), timeout_sec=30.0) + + assert _wait_until( + lambda: not robot.is_capability_running('navigation'), timeout_sec=4.0) + assert robot.capability_state('navigation') == CapabilityState.SUCCEEDED + + # The first goal's own late-arriving outcome, if not filtered + # out, would land some time after the preempting goal already + # succeeded -- give it a window to (not) do so. + time.sleep(0.5) + assert robot.capability_state('navigation') == CapabilityState.SUCCEEDED + finally: + fake_robot.shutdown() + + +def test_is_alive_true_after_discovery_of_an_active_capability(session): + robot_name = _unique_name('robot') + fake_robot = FakeNavCapabilityProcess(robot_name) + try: + robot = RobotHandle(robot_name) + session.add_robot(robot) + session.discover_capabilities() + + assert _wait_until(lambda: robot.is_alive('navigation'), timeout_sec=2.0) + finally: + fake_robot.shutdown() + + +def test_simple_controller_forwards_to_a_working_fleet_session(): + robot_name = _unique_name('robot') + fake_robot = FakeNavCapabilityProcess(robot_name, mock_duration_sec=0.05) + controller = SimpleController() + try: + robot = RobotHandle(robot_name) + controller.add_robot(robot) + + assert len(controller.robots) == 1 + assert controller.find_robot(robot_name) is robot + + controller.discover_capabilities() + assert robot.has_capability('navigation') is True + + robot.run_capability( + 'navigation', Navigation, make_navigation_goal('kitchen'), timeout_sec=5.0) + controller.spin_for(0.5) + + assert _wait_until( + lambda: not robot.is_capability_running('navigation'), timeout_sec=3.0) + assert robot.capability_state('navigation') == CapabilityState.SUCCEEDED + assert controller.node is not None + finally: + fake_robot.shutdown() + # SimpleController wraps a FleetSession the test fixture doesn't + # own -- tear it down the same test-only way. + controller._session._teardown() diff --git a/easyfleet_mission_manager_py/test/test_fleet_session_shutdown.py b/easyfleet_mission_manager_py/test/test_fleet_session_shutdown.py new file mode 100644 index 0000000..c871f65 --- /dev/null +++ b/easyfleet_mission_manager_py/test/test_fleet_session_shutdown.py @@ -0,0 +1,38 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +""" +Test for FleetSession.shutdown(), mirroring test_fleet_session_shutdown.cpp. + +FleetSession.shutdown() calls rclpy.shutdown(), invalidating the ROS +context for the rest of the process -- kept to its own local +rclpy.init()/rclpy.shutdown() pair (this module's only interaction with +rclpy) rather than sharing a fixture with the other test modules here, +so it can't race with them when pytest runs every test file in one +process. +""" + +from easyfleet_mission_manager_py.fleet_session import FleetSession +import rclpy + + +def test_shutdown_stops_the_context(): + rclpy.init() + session = FleetSession() + assert rclpy.ok() is True + + session.shutdown() + + assert rclpy.ok() is False diff --git a/easyfleet_mission_manager_py/test/test_nav_fake_capability.py b/easyfleet_mission_manager_py/test/test_nav_fake_capability.py new file mode 100644 index 0000000..f31195c --- /dev/null +++ b/easyfleet_mission_manager_py/test/test_nav_fake_capability.py @@ -0,0 +1,132 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +""" +Minimal fake navigation capability for exercising RobotHandle end to end. + +Mirrors easyfleet_mission_manager's own test/test_nav_fake_capability.hpp +(TestNavActionServer/FakeRobotProcess): ignores goal content entirely +except to check for REJECT_WAYPOINT_ID, succeeds after +`mock_duration_sec` (0 by default -- fast tests), and honors +cancellation immediately. +""" + +import threading +import time + +from easyfleet_interfaces.action import Navigation +from easyfleet_interfaces.msg import CapabilityDescription, CapabilityStatus +import rclpy +from rclpy.action import ActionServer, CancelResponse, GoalResponse +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.executors import MultiThreadedExecutor +from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy + +# Magic waypoint id (parameters_json's goal_id) that makes the fake +# server reject the goal outright -- the only way this fake ever +# produces a genuine REJECTED (as opposed to CANCELED) outcome. +REJECT_WAYPOINT_ID = '__reject__' + + +class FakeNavCapabilityProcess: + """ + Brings up one fake navigation action server, namespaced under `robot_name`. + + Spun on its own executor/thread -- simulating a robot's own, + independent process the way discover_capabilities()/RobotHandle + actually talk to one over ROS, not a shortcut into the same process. + """ + + def __init__(self, robot_name: str, mock_duration_sec: float = 0.0): + self._robot_name = robot_name + self._mock_duration_sec = mock_duration_sec + self._busy = False + + self._node = rclpy.create_node( + 'fake_nav_capability', namespace=f'/{robot_name}', + start_parameter_services=False) + + action_name = f'/{robot_name}/navigation' + self._action_name = action_name + action_group = ReentrantCallbackGroup() + self._action_server = ActionServer( + self._node, Navigation, action_name, + execute_callback=self._execute, + goal_callback=self._on_goal, + cancel_callback=lambda goal_handle: CancelResponse.ACCEPT, + callback_group=action_group) + + capabilities_pub = self._node.create_publisher( + CapabilityDescription, '/capabilities', + QoSProfile( + depth=10, reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.TRANSIENT_LOCAL)) + description = CapabilityDescription() + description.robot = robot_name + description.capability = 'navigation' + description.action_name = action_name + description.description_json = '{"display_name": "Fake Navigation"}' + capabilities_pub.publish(description) + + self._status_pub = self._node.create_publisher( + CapabilityStatus, '/capabilities_status', + QoSProfile(depth=10, reliability=ReliabilityPolicy.RELIABLE)) + # A distinct (default, mutually-exclusive) callback group from + # the action server's own reentrant one, so the heartbeat keeps + # ticking under a MultiThreadedExecutor even while a goal is + # mid-execution. + self._status_timer = self._node.create_timer(1.0, self._publish_heartbeat) + + self._executor = MultiThreadedExecutor(num_threads=4) + self._executor.add_node(self._node) + self._spin_thread = threading.Thread(target=self._executor.spin, daemon=True) + self._spin_thread.start() + while not self._executor.is_spinning: + time.sleep(0) + + def _publish_heartbeat(self) -> None: + msg = CapabilityStatus() + msg.robot = self._robot_name + msg.capability = 'navigation' + msg.action_name = self._action_name + msg.busy = self._busy + self._status_pub.publish(msg) + + def _on_goal(self, goal_request): + if REJECT_WAYPOINT_ID in goal_request.parameters_json: + return GoalResponse.REJECT + return GoalResponse.ACCEPT + + def _execute(self, goal_handle): + self._busy = True + try: + step = 0.02 + remaining = self._mock_duration_sec + while remaining > 0.0: + if goal_handle.is_cancel_requested: + goal_handle.canceled() + return Navigation.Result() + time.sleep(step) + remaining -= step + goal_handle.succeed() + return Navigation.Result() + finally: + self._busy = False + + def shutdown(self) -> None: + self._executor.shutdown() + if self._spin_thread.is_alive(): + self._spin_thread.join() + self._node.destroy_node() diff --git a/easyfleet_mission_manager_py/test/test_pep257.py b/easyfleet_mission_manager_py/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/easyfleet_mission_manager_py/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 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 ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/easyfleet_mission_manager_py/test/test_throttle.py b/easyfleet_mission_manager_py/test/test_throttle.py new file mode 100644 index 0000000..ade0589 --- /dev/null +++ b/easyfleet_mission_manager_py/test/test_throttle.py @@ -0,0 +1,38 @@ +# Copyright 2026 Intelligent Robotics Lab +# +# This file is part of the project EasyFleet +# 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. + +"""Unit tests for Throttle. No ROS node required.""" + +import time + +from easyfleet_mission_manager_py.throttle import Throttle + + +def test_first_call_is_always_ready(): + throttle = Throttle(1.0) + assert throttle.ready() is True + + +def test_immediate_second_call_is_not_ready(): + throttle = Throttle(1.0) + assert throttle.ready() is True + assert throttle.ready() is False + + +def test_ready_again_after_interval_elapses(): + throttle = Throttle(0.05) + assert throttle.ready() is True + time.sleep(0.08) + assert throttle.ready() is True diff --git a/easyfleet_mission_manager_py/test/test_xmllint.py b/easyfleet_mission_manager_py/test/test_xmllint.py new file mode 100644 index 0000000..3e08c02 --- /dev/null +++ b/easyfleet_mission_manager_py/test/test_xmllint.py @@ -0,0 +1,23 @@ +# Copyright 2015 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 ament_xmllint.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.xmllint +def test_xmllint() -> None: + rc = main(argv=[]) + assert rc == 0, 'Found code style errors / warnings' From 78be3657668d9e69adc79d86418d08bb5569de7d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sun, 16 Aug 2026 13:29:26 +0200 Subject: [PATCH 2/2] Apply feedback MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../mission.py | 3 +++ .../capability_discovery.py | 8 ++++++-- .../easyfleet_mission_manager_py/capability_state.py | 2 +- .../detail/running_capability.py | 6 +++--- .../test/test_capability_discovery.py | 10 +--------- 5 files changed, 14 insertions(+), 15 deletions(-) diff --git a/easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/easyfleet_easynav_collaboration_simple_api_deployment_py/mission.py b/easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/easyfleet_easynav_collaboration_simple_api_deployment_py/mission.py index 15991b4..0e7e554 100644 --- a/easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/easyfleet_easynav_collaboration_simple_api_deployment_py/mission.py +++ b/easyfleet_example_deployments/easyfleet_easynav_collaboration_simple_api_deployment_py/easyfleet_easynav_collaboration_simple_api_deployment_py/mission.py @@ -115,18 +115,21 @@ def main(args=None): safe_print( f'{ansi.BOLD}{ansi.RED}Both robots must have a navigation capability for this ' f'demo to work.{ansi.RESET}') + controller.shutdown() return 1 if not robot_1.has_capability('perception'): safe_print( f'{ansi.BOLD}{ansi.RED}Robot 1 must have a perception capability for this demo ' f'to work.{ansi.RESET}') + controller.shutdown() return 1 if not robot_2.has_capability('manipulation'): safe_print( f'{ansi.BOLD}{ansi.RED}Robot 2 must have a manipulation capability for this demo ' f'to work.{ansi.RESET}') + controller.shutdown() return 1 # Phase 1: robot_1 -> "kitchen" (perceiving throughout) while diff --git a/easyfleet_mission_manager_py/easyfleet_mission_manager_py/capability_discovery.py b/easyfleet_mission_manager_py/easyfleet_mission_manager_py/capability_discovery.py index 1c26175..8c6b00e 100644 --- a/easyfleet_mission_manager_py/easyfleet_mission_manager_py/capability_discovery.py +++ b/easyfleet_mission_manager_py/easyfleet_mission_manager_py/capability_discovery.py @@ -48,8 +48,12 @@ def on_description(msg: CapabilityDescription) -> None: info.description_json_raw = msg.description_json try: parsed = json.loads(msg.description_json) - info.description_json = parsed if isinstance(parsed, dict) else {} - info.description_json_valid = True + if isinstance(parsed, dict): + info.description_json = parsed + info.description_json_valid = True + else: + info.description_json = {} + info.description_json_valid = False except (ValueError, TypeError): info.description_json = {} info.description_json_valid = False diff --git a/easyfleet_mission_manager_py/easyfleet_mission_manager_py/capability_state.py b/easyfleet_mission_manager_py/easyfleet_mission_manager_py/capability_state.py index 674c9a4..d46f06b 100644 --- a/easyfleet_mission_manager_py/easyfleet_mission_manager_py/capability_state.py +++ b/easyfleet_mission_manager_py/easyfleet_mission_manager_py/capability_state.py @@ -40,4 +40,4 @@ class CapabilityState(enum.Enum): def to_string(state: CapabilityState) -> str: - return state.value + return state.name diff --git a/easyfleet_mission_manager_py/easyfleet_mission_manager_py/detail/running_capability.py b/easyfleet_mission_manager_py/easyfleet_mission_manager_py/detail/running_capability.py index 35af6f7..ec2920a 100644 --- a/easyfleet_mission_manager_py/easyfleet_mission_manager_py/detail/running_capability.py +++ b/easyfleet_mission_manager_py/easyfleet_mission_manager_py/detail/running_capability.py @@ -20,7 +20,7 @@ from .. import ansi from ..capability_client import CapabilityClient, Outcome -from ..capability_state import CapabilityState +from ..capability_state import CapabilityState, to_string from ..output import safe_print _OUTCOME_TO_STATE = { @@ -74,10 +74,10 @@ def on_response(response): new_state = _OUTCOME_TO_STATE.get(response.outcome, CapabilityState.ABORTED) self._state = new_state self._status.set_status( - self._robot_name, f'{self._capability_type} -> {new_state.value}') + self._robot_name, f'{self._capability_type} -> {to_string(new_state)}') safe_print( f' {ansi.DIM}[{self._robot_name}/{self._capability_type}] {ansi.RESET}' - f'finished with outcome {ansi.MAGENTA}{new_state.value}{ansi.RESET}') + f'finished with outcome {ansi.MAGENTA}{to_string(new_state)}{ansi.RESET}') self._client.request(goal, on_response) diff --git a/easyfleet_mission_manager_py/test/test_capability_discovery.py b/easyfleet_mission_manager_py/test/test_capability_discovery.py index 944d298..2760898 100644 --- a/easyfleet_mission_manager_py/test/test_capability_discovery.py +++ b/easyfleet_mission_manager_py/test/test_capability_discovery.py @@ -16,10 +16,10 @@ """Integration tests for discover_capabilities(), mirroring test_capability_discovery.cpp.""" import itertools -import threading from easyfleet_interfaces.msg import CapabilityDescription, CapabilityStatus from easyfleet_mission_manager_py.capability_discovery import discover_capabilities +from easyfleet_mission_manager_py.mission_helpers import spin_in_background as _spin_in_background import pytest import rclpy from rclpy.executors import SingleThreadedExecutor @@ -32,14 +32,6 @@ def _unique_name(base: str) -> str: return f'{base}_{next(_name_counter)}' -def _spin_in_background(executor) -> threading.Thread: - thread = threading.Thread(target=executor.spin, daemon=True) - thread.start() - while not executor.is_spinning: - pass - return thread - - def _make_description(capability: str, action_name: str, description_json: str, robot: str = ''): msg = CapabilityDescription() msg.robot = robot