diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index cfd3079..7e548f2 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -47,7 +47,7 @@ jobs: source install/setup.bash export ROS_LOCALHOST_ONLY=1 export RMW_IMPLEMENTATION=${{ matrix.rmw_implementation }} - colcon test --executor sequential --packages-select rmf_path_server_test rmf_reservation_tests --event-handlers console_direct+ + colcon test --executor sequential --packages-select rmf_path_server_test rmf_reservation_tests rmf_path_server_demo --event-handlers console_direct+ colcon test-result --all shell: bash diff --git a/path_server/rmf_path_server_demo/package.xml b/path_server/rmf_path_server_demo/package.xml index 83001ff..ebb7da6 100644 --- a/path_server/rmf_path_server_demo/package.xml +++ b/path_server/rmf_path_server_demo/package.xml @@ -26,6 +26,11 @@ rmf_simple_destination_server rmf_reservation_destination_server + launch_testing + launch_testing_ros + python3-pytest + rclpy + ament_python diff --git a/path_server/rmf_path_server_demo/rmf_path_server_demo/robot_spawner.py b/path_server/rmf_path_server_demo/rmf_path_server_demo/robot_spawner.py index 2e38656..2f14c2b 100644 --- a/path_server/rmf_path_server_demo/rmf_path_server_demo/robot_spawner.py +++ b/path_server/rmf_path_server_demo/rmf_path_server_demo/robot_spawner.py @@ -180,6 +180,8 @@ def __init__(self): # Parameters self.declare_parameter('use_destination_server', False) self.use_destination_server = self.get_parameter('use_destination_server').value + self.declare_parameter('port', 8080) + self.port = int(self.get_parameter('port').value) self.current_map = None self.active_processes = {} @@ -247,7 +249,7 @@ def __init__(self): self.web_dir = "./www" # Start the HTTP server - self.start_http_server(port=8080) + self.start_http_server(port=self.port) def start_http_server(self, port): def serve(): diff --git a/path_server/rmf_path_server_demo/rmf_path_server_demo/test_client.py b/path_server/rmf_path_server_demo/rmf_path_server_demo/test_client.py new file mode 100644 index 0000000..0f8f7a6 --- /dev/null +++ b/path_server/rmf_path_server_demo/rmf_path_server_demo/test_client.py @@ -0,0 +1,155 @@ +# Copyright 2026 Open Source Robotics Foundation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import json +import threading +import time +import urllib.parse +import urllib.request +from typing import Callable, Dict, List, Optional + + +class SpawnerTestClient: + """Test client for interacting with the robot_spawner HTTP REST & SSE server.""" + + def __init__(self, host: str = "http://localhost:8080", timeout: float = 5.0): + self.host = host.rstrip("/") + self.timeout = timeout + self._sse_thread: Optional[threading.Thread] = None + self._sse_stop_event = threading.Event() + self.received_events: List[dict] = [] + self._events_lock = threading.Lock() + + def _get(self, endpoint: str, params: Optional[Dict[str, str]] = None) -> dict: + url = f"{self.host}{endpoint}" + if params: + query_str = urllib.parse.urlencode(params) + url = f"{url}?{query_str}" + req = urllib.request.Request(url, headers={"Accept": "application/json"}) + with urllib.request.urlopen(req, timeout=self.timeout) as response: + body = response.read().decode("utf-8") + return json.loads(body) + + def wait_until_ready(self, timeout: float = 10.0) -> bool: + """Poll the spawner server until it is responsive.""" + start = time.time() + while time.time() - start < timeout: + try: + res = self.get_config() + if "default_radius" in res: + return True + except Exception: + pass + time.sleep(0.2) + return False + + def spawn_robot(self, name: str, x: float, y: float) -> dict: + """Spawn a robot with initial position (x, y).""" + return self._get("/spawn", {"name": name, "x": str(x), "y": str(y)}) + + def set_destination(self, name: str, x: float, y: float) -> dict: + """Set a single destination (x, y) for a robot.""" + return self.set_destinations(name, [x], [y]) + + def set_destinations(self, name: str, xs: List[float], ys: List[float]) -> dict: + """Set multiple candidate destinations for a robot.""" + x_str = ",".join(str(val) for val in xs) + y_str = ",".join(str(val) for val in ys) + return self._get("/destination", {"name": name, "x": x_str, "y": y_str}) + + def send_scenario(self) -> dict: + """Broadcast discovery and send scenario goals to all spawned robots.""" + return self._get("/send_scenario") + + def reset(self) -> dict: + """Reset scenario and terminate spawned robot processes.""" + return self._get("/reset") + + def get_config(self) -> dict: + """Get spawner configuration.""" + return self._get("/config") + + def get_map(self) -> dict: + """Get occupancy grid map data.""" + return self._get("/map") + + def get_reservation_config(self) -> dict: + """Get reservation system configuration.""" + return self._get("/reservation_config") + + def start_sse_listener(self, on_event: Optional[Callable[[dict], None]] = None): + """Start listening to the /stream SSE endpoint in a background thread.""" + self._sse_stop_event.clear() + + def listen(): + url = f"{self.host}/stream" + req = urllib.request.Request(url, headers={"Accept": "text/event-stream"}) + try: + with urllib.request.urlopen(req, timeout=self.timeout) as response: + for line in response: + if self._sse_stop_event.is_set(): + break + line_str = line.decode("utf-8").strip() + if line_str.startswith("data: "): + raw_json = line_str[6:] + try: + data = json.loads(raw_json) + with self._events_lock: + self.received_events.append(data) + if on_event: + on_event(data) + except json.JSONDecodeError: + pass + except Exception: + pass + + self._sse_thread = threading.Thread(target=listen, daemon=True) + self._sse_thread.start() + + def stop_sse_listener(self): + """Stop the background SSE listener thread.""" + self._sse_stop_event.set() + if self._sse_thread and self._sse_thread.is_alive(): + self._sse_thread.join(timeout=2.0) + + +def main(): + import argparse + + parser = argparse.ArgumentParser(description="Test client for robot_spawner demo") + parser.add_argument("--host", default="http://localhost:8080", help="Spawner HTTP host") + parser.add_argument("--reset", action="store_true", help="Reset scenario") + args = parser.parse_args() + + client = SpawnerTestClient(host=args.host) + if args.reset: + print("Resetting scenario:", client.reset()) + return + + print("Checking server status...") + if not client.wait_until_ready(): + print("Error: Server not ready") + return + + print("Config:", client.get_config()) + print("Spawning robot_1 at (0.0, 0.0):", client.spawn_robot("robot_1", 0.0, 0.0)) + print("Spawning robot_2 at (3.0, 0.0):", client.spawn_robot("robot_2", 3.0, 0.0)) + print("Setting robot_1 destination (5.0, 0.0):", client.set_destination("robot_1", 5.0, 0.0)) + print("Setting robot_2 destination (8.0, 0.0):", client.set_destination("robot_2", 8.0, 0.0)) + print("Sending scenario:", client.send_scenario()) + print("Scenario sent successfully!") + + +if __name__ == "__main__": + main() diff --git a/path_server/rmf_path_server_demo/setup.py b/path_server/rmf_path_server_demo/setup.py index 77eb1a0..1d16c04 100644 --- a/path_server/rmf_path_server_demo/setup.py +++ b/path_server/rmf_path_server_demo/setup.py @@ -44,6 +44,7 @@ entry_points={ 'console_scripts': [ 'robot_spawner = rmf_path_server_demo.robot_spawner:main', + 'rmf_demo_test_client = rmf_path_server_demo.test_client:main', ], }, ) diff --git a/path_server/rmf_path_server_demo/test/test_spawner_e2e.py b/path_server/rmf_path_server_demo/test/test_spawner_e2e.py new file mode 100644 index 0000000..ea6a6d1 --- /dev/null +++ b/path_server/rmf_path_server_demo/test/test_spawner_e2e.py @@ -0,0 +1,170 @@ +# Copyright 2026 Open Source Robotics Foundation +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import time +import unittest + +from launch import LaunchDescription +import launch_ros.actions +import launch_testing.actions +from nav_msgs.msg import Odometry +import pytest +import rclpy + +from rmf_path_server_demo.test_client import SpawnerTestClient + + +@pytest.mark.launch_test +def generate_test_description(): + path_server = launch_ros.actions.Node( + package='rmf_path_server', + executable='rmf_path_server', + output='screen' + ) + + plan_executor = launch_ros.actions.Node( + package='rmf_plan_executor', + executable='rmf_plan_executor', + output='screen' + ) + + robot_spawner = launch_ros.actions.Node( + package='rmf_path_server_demo', + executable='robot_spawner', + output='screen', + parameters=[{ + 'port': 8088 + }] + ) + + return LaunchDescription([ + path_server, + plan_executor, + robot_spawner, + launch_testing.actions.ReadyToTest(), + ]), { + 'path_server': path_server, + 'plan_executor': plan_executor, + 'robot_spawner': robot_spawner, + } + + +class TestRobotSpawnerE2E(unittest.TestCase): + @classmethod + def setUpClass(cls): + rclpy.init() + + @classmethod + def tearDownClass(cls): + rclpy.shutdown() + + def setUp(self): + self.node = rclpy.create_node('test_robot_spawner_e2e_node') + self.client = SpawnerTestClient(host='http://localhost:8088') + + def tearDown(self): + try: + self.client.reset() + except Exception: + pass + self.node.destroy_node() + + def test_spawner_two_robot_scenario(self): + # 1. Wait until robot_spawner HTTP server is ready + server_ready = self.client.wait_until_ready(timeout=10.0) + self.assertTrue(server_ready, 'robot_spawner HTTP server failed to become ready') + + # 2. Check initial config endpoint + config = self.client.get_config() + self.assertIn('default_radius', config) + + # 3. Spawn robot_1 at (0.0, 0.0) and robot_2 at (3.0, 0.0) + spawn_res1 = self.client.spawn_robot('robot_1', 0.0, 0.0) + self.assertEqual(spawn_res1.get('status'), 'spawned') + self.assertEqual(spawn_res1.get('name'), 'robot_1') + + spawn_res2 = self.client.spawn_robot('robot_2', 3.0, 0.0) + self.assertEqual(spawn_res2.get('status'), 'spawned') + self.assertEqual(spawn_res2.get('name'), 'robot_2') + + # 4. Set destinations via HTTP client: robot_1 to (5.0, 0.0), robot_2 to (8.0, 0.0) + dest_res1 = self.client.set_destination('robot_1', 5.0, 0.0) + self.assertEqual(dest_res1.get('status'), 'goals_set') + + dest_res2 = self.client.set_destination('robot_2', 8.0, 0.0) + self.assertEqual(dest_res2.get('status'), 'goals_set') + + # 5. Connect SSE listener to monitor events sent by robot_spawner + self.client.start_sse_listener() + + # 6. Subscribe to robot odometry via ROS to double check positions + r1_odoms = [] + r2_odoms = [] + + def r1_odom_cb(msg): + r1_odoms.append(msg.pose.pose.position) + + def r2_odom_cb(msg): + r2_odoms.append(msg.pose.pose.position) + + self.node.create_subscription(Odometry, 'robot_1/odom', r1_odom_cb, qos_profile=10) + self.node.create_subscription(Odometry, 'robot_2/odom', r2_odom_cb, qos_profile=10) + + # 7. Trigger scenario execution via HTTP test client + scenario_res = self.client.send_scenario() + self.assertEqual(scenario_res.get('status'), 'scenario_sent') + + # 8. Spin and verify that robot_spawner component responded and both robots reach target positions + start_time = time.time() + timeout = 30.0 + r1_reached = False + r2_reached = False + + while time.time() - start_time < timeout: + rclpy.spin_once(self.node, timeout_sec=0.1) + + if r1_odoms: + last_pos1 = r1_odoms[-1] + if abs(last_pos1.x - 5.0) < 0.3 and abs(last_pos1.y - 0.0) < 0.3: + r1_reached = True + + if r2_odoms: + last_pos2 = r2_odoms[-1] + if abs(last_pos2.x - 8.0) < 0.3 and abs(last_pos2.y - 0.0) < 0.3: + r2_reached = True + + if r1_reached and r2_reached: + break + + time.sleep(0.1) + + self.client.stop_sse_listener() + + self.assertTrue( + r1_reached, + f'robot_1 did not reach target (5.0, 0.0). Last pos: {r1_odoms[-1] if r1_odoms else "None"}' + ) + self.assertTrue( + r2_reached, + f'robot_2 did not reach target (8.0, 0.0). Last pos: {r2_odoms[-1] if r2_odoms else "None"}' + ) + + # Check that SSE listener received odom events from robot_spawner + sse_events = self.client.received_events + odom_events = [e for e in sse_events if e.get('type') == 'odom'] + self.assertGreater(len(odom_events), 0, 'No odom events received via SSE stream') + + # 9. Verify reset endpoint works + reset_res = self.client.reset() + self.assertEqual(reset_res.get('status'), 'reset')