From f58b35aef72c4a0e2ee4d61be97626080d97d6ff Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marc=20Bl=C3=B6chlinger?= Date: Fri, 16 May 2025 14:00:37 +0000 Subject: [PATCH 01/29] feat(docker): Added image build script --- .devcontainer/build.sh | 1 + 1 file changed, 1 insertion(+) create mode 100755 .devcontainer/build.sh diff --git a/.devcontainer/build.sh b/.devcontainer/build.sh new file mode 100755 index 0000000..e5b1111 --- /dev/null +++ b/.devcontainer/build.sh @@ -0,0 +1 @@ +sudo docker build -t helmoro -f .devcontainer/Dockerfile . \ No newline at end of file From 87954029e05ffc65f6b457e0a2623b9845b450a0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marc=20Bl=C3=B6chlinger?= Date: Fri, 16 May 2025 14:02:15 +0000 Subject: [PATCH 02/29] feat(cli): Added a command line interface for starting/stopping simulation and spawning/deleting of robots --- .devcontainer/docker-compose.yml | 54 ++++ src/helmoro_cli/LICENSE | 202 ++++++++++++++ src/helmoro_cli/helmoro_cli/__init__.py | 0 src/helmoro_cli/helmoro_cli/main.py | 81 ++++++ src/helmoro_cli/helmoro_cli/robot_manager.py | 63 +++++ .../helmoro_cli/simulation_manager.py | 49 ++++ src/helmoro_cli/package.xml | 15 ++ src/helmoro_cli/resource/helmoro_cli | 0 src/helmoro_cli/setup.cfg | 4 + src/helmoro_cli/setup.py | 25 ++ .../launch/ros_gz_bridge.launch.py | 246 ++++++++++++++++++ .../launch/simulation.launch.py | 81 ++++++ .../launch/spawn_robot.launch.py | 46 ++++ 13 files changed, 866 insertions(+) create mode 100644 .devcontainer/docker-compose.yml create mode 100644 src/helmoro_cli/LICENSE create mode 100644 src/helmoro_cli/helmoro_cli/__init__.py create mode 100644 src/helmoro_cli/helmoro_cli/main.py create mode 100644 src/helmoro_cli/helmoro_cli/robot_manager.py create mode 100644 src/helmoro_cli/helmoro_cli/simulation_manager.py create mode 100644 src/helmoro_cli/package.xml create mode 100644 src/helmoro_cli/resource/helmoro_cli create mode 100644 src/helmoro_cli/setup.cfg create mode 100644 src/helmoro_cli/setup.py create mode 100644 src/helmoro_simulation/launch/ros_gz_bridge.launch.py create mode 100644 src/helmoro_simulation/launch/simulation.launch.py create mode 100644 src/helmoro_simulation/launch/spawn_robot.launch.py diff --git a/.devcontainer/docker-compose.yml b/.devcontainer/docker-compose.yml new file mode 100644 index 0000000..8f6f276 --- /dev/null +++ b/.devcontainer/docker-compose.yml @@ -0,0 +1,54 @@ +services: + helmoro_template_service: + command: bash + image: helmoro + environment: + - DISPLAY=${DISPLAY} + - ROS_DOMAIN_ID=42 + network_mode: host + ipc: host + volumes: + - /tmp/.X11-unix:/tmp/.X11-unix + + cli: + extends: helmoro_template_service + command: python3 src/helmoro_cli/src/main.py + volumes: + - /var/run/docker.sock:/var/run/docker.sock + + simulation: + extends: helmoro_template_service + command: ros2 launch helmoro_simulation simulation.launch.py + + spawn_robot: + extends: helmoro_template_service + command: bash -c "ros2 launch helmoro_simulation spawn_robot.launch.py namespace:=$${NAMESPACE} x:=$${X} y:=$${Y} z:=$${Z} yaw:=$${YAW}" + environment: + - NAMESPACE + - X + - Y + - Z + - YAW + + despawn_robot: + extends: helmoro_template_service + command: bash -c "ros2 launch ros_gz_sim gz_remove_model.launch.py world:=$${WORLD} entity_name:=$${ENTITY_NAME}" + environment: + - WORLD + - ENTITY_NAME + + gz_ros_bridge: + extends: helmoro_template_service + command: bash -c "ros2 launch helmoro_simulation ros_gz_bridge.launch.py namespace:=$${NAMESPACE} word:=$${WORLD}" + environment: + - NAMESPACE + - WORLD + + description: + extends: + service: helmoro_template_service + command: bash -c "ros2 launch helmoro_description description.launch.py namespace:=$${NAMESPACE} run_in_simulation:=$${USE_SIM_TIME}" + environment: + - NAMESPACE + - USE_SIM_TIME + diff --git a/src/helmoro_cli/LICENSE b/src/helmoro_cli/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/src/helmoro_cli/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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/src/helmoro_cli/helmoro_cli/__init__.py b/src/helmoro_cli/helmoro_cli/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/helmoro_cli/helmoro_cli/main.py b/src/helmoro_cli/helmoro_cli/main.py new file mode 100644 index 0000000..a060a4a --- /dev/null +++ b/src/helmoro_cli/helmoro_cli/main.py @@ -0,0 +1,81 @@ +import cmd +import rclpy +from helmoro_cli.robot_manager import RobotManager +from helmoro_cli.simulation_manager import SimulationManager + + +class RobotCLI(cmd.Cmd): + intro = "Welcome to the Robot CLI. Type help or ? to list commands.\n" + prompt = "(robot-cli) " + + def __init__(self): + super().__init__() + rclpy.init() + self.robot_manager = RobotManager() + self.simulation_manager = SimulationManager() + self.current_robot = None + + def do_start_simulation(self, arg): + "Start the simulation: start_simulation " + if self.simulation_manager.get_status() == "running": + print("Simulation is already running.") + return + world = arg or "empty" + self.simulation_manager.start_simulation(world) + + def do_stop_simulation(self, arg): + "Stop the simulation: stop_simulation" + if self.simulation_manager.get_status() == "stopped": + print("Simulation is already stopped.") + return + self.simulation_manager.stop_simulation() + + def do_spawn(self, arg): + "Spawn a robot: spawn " + + if self.simulation_manager.get_status() == "stopped": + print("Can't spawn robot. Simulation is not running.") + return + + args = arg.split() + if len(args) != 5: + print("Usage: spawn ") + return + self.robot_manager.spawn_robot(*args) + self.simulation_manager.spawn_robot(*args) + + def do_delete(self, arg): + "Delete a robot: delete " + self.robot_manager.delete_robot(arg) + + if self.simulation_manager.get_status() == "running": + self.simulation_manager.delete_robot(arg) + + def do_exit(self, arg): + "Exit the CLI" + if self.simulation_manager.get_status() == "running": + self.simulation_manager.stop_simulation() + + print("Exiting CLI...") + return True + + def do_multi_robot_spawn(self, arg): + "Run multi-robot test" + self.do_start_simulation(arg) + arg1 = "alfred -1 0 1 0 " + arg2 = "bob 1 1 1 1" + arg3 = "charlie 3 2 1 2" + self.do_spawn(arg1) + self.do_spawn(arg2) + self.do_spawn(arg3) + + +def main(): + try: + RobotCLI().cmdloop() + finally: + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/helmoro_cli/helmoro_cli/robot_manager.py b/src/helmoro_cli/helmoro_cli/robot_manager.py new file mode 100644 index 0000000..a1d45db --- /dev/null +++ b/src/helmoro_cli/helmoro_cli/robot_manager.py @@ -0,0 +1,63 @@ +import os +import re + + +class RobotManager: + def __init__(self): + self.robots = {} + + def spawn_robot(self, name, x, y, z, yaw): + self.robots[name] = {"pos": (x, y, z), "yaw": yaw, "status": "idle"} + # Here you would trigger a ROS 2 launch or service call + spawn = f"sudo NAMESPACE={name} USE_SIM_TIME=True \ + docker compose -p robot_{name} -f /home/ws/.devcontainer/docker-compose.yml up description --detach --wait" + print(f"Start core nodes of {name}") + os.system(spawn) + + def delete_robot(self, name): + "Stops all containers spawned by the robot" + raw_output = ( + os.popen( + f"sudo docker container ls --filter name={name} --format '{{{{.ID}}}} {{{{.Names}}}}'" + ) + .read() + .strip() + ) + + if not raw_output: + print(f"No running containers found for robot: {name}") + return + + containers = [line.strip().split() for line in raw_output.splitlines()] + for container_id, container_name in containers: + result = os.system( + f"sudo docker container stop {container_id} --timeout 1 > /dev/null" + ) + if result == 0: + print(f"{container_name} Stopped") + + def list_robots(self): + "Print out all spawned robots" + robots = self.get_robot_names() + if not robots: + print("No robots spawned.") + for name in robots: + print(name) + + def get_robot_names(self): + "Return the list of robot names" + # Run the Docker command and read the output + stream = os.popen( + 'docker container ls --filter name=robot --format "{{.Names}}"' + ) + output = stream.read() + + # Extract robot names using regex + robot_names = set() + pattern = re.compile(r"^robot_([^-\s]+)-") + for line in output.strip().split("\n"): + match = pattern.match(line) + if match: + robot_names.add(match.group(1)) + + return sorted(robot_names) diff --git a/src/helmoro_cli/helmoro_cli/simulation_manager.py b/src/helmoro_cli/helmoro_cli/simulation_manager.py new file mode 100644 index 0000000..60dab67 --- /dev/null +++ b/src/helmoro_cli/helmoro_cli/simulation_manager.py @@ -0,0 +1,49 @@ +import os +from ament_index_python.packages import get_package_share_directory + + +class SimulationManager: + def __init__(self): + self.robots = {} + self.simulation_status = "stopped" + self.pkg_dir = get_package_share_directory("helmoro_cli") + self.world = "empty" + + def spawn_robot(self, name, x, y, z, yaw): + # Trigger a ROS 2 launch command to spawn the robot + spawn = f"sudo NAMESPACE={name} X={x} Y={y} Z={z} YAW={yaw} \ + docker compose -p sim_{name} -f /home/ws/.devcontainer/docker-compose.yml up spawn_robot" + + ros_gz_bridge = f"sudo NAMESPACE={name} WORLD={self.world} \ + docker compose -p robot_{name} -f /home/ws/.devcontainer/docker-compose.yml up gz_ros_bridge --detach --wait" + + print(f"Spawn {name} in gazebo simulation.") + os.system(spawn) + print(f"Attach ros_gz_bridge to {name}") + os.system(ros_gz_bridge) + + def delete_robot(self, name): + "Deletes the robot from the simulation" + os.system( + f"sudo WORLD:={self.world} ENTITY_NAME={name} \ + docker compose -p sim_{name} -f /home/ws/.devcontainer/docker-compose.yml up despawn_robot --detach --wait" + ) + + def get_status(self): + return self.simulation_status + + def start_simulation(self, world): + # Start the simulation environment + print("Starting simulation environment...") + simulation = f"sudo docker compose -f /home/ws/.devcontainer/docker-compose.yml up simulation --detach --wait" + print(f"Executing command: {simulation}") + os.system(simulation) + self.simulation_status = "running" + + def stop_simulation(self): + # Stop the simulation environment + print("Stopping simulation environment...") + stop_simulation = f"sudo docker compose -f /home/ws/.devcontainer/docker-compose.yml down simulation --timeout 1" + print(f"Executing command: {stop_simulation}") + os.system(stop_simulation) + self.simulation_status = "stopped" diff --git a/src/helmoro_cli/package.xml b/src/helmoro_cli/package.xml new file mode 100644 index 0000000..87e95f7 --- /dev/null +++ b/src/helmoro_cli/package.xml @@ -0,0 +1,15 @@ + + + + helmoro_cli + 0.0.0 + Command line interface for Helmoro + Marc Blöchlinger + Apache-2.0 + + python3-pytest + + + ament_python + + diff --git a/src/helmoro_cli/resource/helmoro_cli b/src/helmoro_cli/resource/helmoro_cli new file mode 100644 index 0000000..e69de29 diff --git a/src/helmoro_cli/setup.cfg b/src/helmoro_cli/setup.cfg new file mode 100644 index 0000000..3651c9c --- /dev/null +++ b/src/helmoro_cli/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/helmoro_cli +[install] +install_scripts=$base/lib/helmoro_cli diff --git a/src/helmoro_cli/setup.py b/src/helmoro_cli/setup.py new file mode 100644 index 0000000..7228c3f --- /dev/null +++ b/src/helmoro_cli/setup.py @@ -0,0 +1,25 @@ +from setuptools import find_packages, setup + +package_name = "helmoro_cli" + +setup( + name=package_name, + version="0.0.1", + packages=find_packages(exclude=["test"]), + data_files=[ + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="Marc Blöchlinger", + maintainer_email="mbloechli@student.ethz.ch", + description="Command line interface for Helmoro", + license="Apache-2.0", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "open_cli = helmoro_cli.main:main", + ], + }, +) diff --git a/src/helmoro_simulation/launch/ros_gz_bridge.launch.py b/src/helmoro_simulation/launch/ros_gz_bridge.launch.py new file mode 100644 index 0000000..f36c7c6 --- /dev/null +++ b/src/helmoro_simulation/launch/ros_gz_bridge.launch.py @@ -0,0 +1,246 @@ +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, GroupAction +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node, PushRosNamespace + +ARGUMENTS = [ + DeclareLaunchArgument( + "namespace", default_value="empty_namespace", description="Robot namespace" + ), + DeclareLaunchArgument( + "world", default_value="unspecified", description="Wolrd name" + ), +] + + +def generate_launch_description(): + + ros_gz_bridge = GroupAction( + [ + PushRosNamespace(LaunchConfiguration("namespace")), + # TODO: Update to ROS Kilted will add support for custom URDF frames making this file unnecessary + # Depth Camera + Node( + package="ros_gz_bridge", + executable="parameter_bridge", + name="depth_camera_bridge", + output="screen", + parameters=[{"use_sim_time": True}], + arguments=[ + [ + "/world/", + LaunchConfiguration("world"), + "/model/", + LaunchConfiguration("namespace"), + "/link/", + "base_link/sensor/depth_camera/camera_info" + + "@sensor_msgs/msg/CameraInfo" + + "[gz.msgs.CameraInfo", + ], + [ + "/world/", + LaunchConfiguration("world"), + "/model/", + LaunchConfiguration("namespace"), + "/link/", + "base_link/sensor/depth_camera/depth_image" + + "@sensor_msgs/msg/Image" + + "[gz.msgs.Image", + ], + [ + "/world/", + LaunchConfiguration("world"), + "/model/", + LaunchConfiguration("namespace"), + "/link/", + "base_link/sensor/depth_camera/depth_image/points" + + "@sensor_msgs/msg/PointCloud2" + + "[gz.msgs.PointCloudPacked", + ], + ], + remappings=[ + ( + [ + "/world/", + LaunchConfiguration("world"), + "/model/", + LaunchConfiguration("namespace"), + "/link/", + "base_link/sensor/depth_camera/camera_info", + ], + ["sensor/camera/depth/camera_info"], + ), + ( + [ + "/world/", + LaunchConfiguration("world"), + "/model/", + LaunchConfiguration("namespace"), + "/link/", + "base_link/sensor/depth_camera/depth_image", + ], + ["sensor/camera/depth/image_raw"], + ), + ( + [ + "/world/", + LaunchConfiguration("world"), + "/model/", + LaunchConfiguration("namespace"), + "/link/", + "base_link/sensor/depth_camera/depth_image/points", + ], + ["sensor/camera/depth/points"], + ), + ], + ), + # RGB Camera + Node( + package="ros_gz_bridge", + executable="parameter_bridge", + name="rgb_camera_bridge", + output="screen", + parameters=[{"use_sim_time": True}], + arguments=[ + [ + "/world/", + LaunchConfiguration("world"), + "/model/", + LaunchConfiguration("namespace"), + "/link/", + "base_link/sensor/rgb_camera/camera_info" + + "@sensor_msgs/msg/CameraInfo" + + "[gz.msgs.CameraInfo", + ], + [ + "/world/", + LaunchConfiguration("world"), + "/model/", + LaunchConfiguration("namespace"), + "/link/", + "base_link/sensor/rgb_camera/image" + + "@sensor_msgs/msg/Image" + + "[gz.msgs.Image", + ], + ], + remappings=[ + ( + [ + "/world/", + LaunchConfiguration("world"), + "/model/", + LaunchConfiguration("namespace"), + "/link/", + "base_link/sensor/rgb_camera/camera_info", + ], + ["sensor/camera/color/camera_info"], + ), + ( + [ + "/world/", + LaunchConfiguration("world"), + "/model/", + LaunchConfiguration("namespace"), + "/link/", + "base_link/sensor/rgb_camera/image", + ], + ["sensor/camera/color/image_raw"], + ), + ], + ), + # Lidar + Node( + package="ros_gz_bridge", + executable="parameter_bridge", + name="rplidar_bridge", + output="screen", + parameters=[{"use_sim_time": True}], + arguments=[ + [ + "/world/", + LaunchConfiguration("world"), + "/model/", + LaunchConfiguration("namespace"), + "/link/", + "base_link/sensor/rplidar/scan" + + "@sensor_msgs/msg/LaserScan" + + "[gz.msgs.LaserScan", + ], + [ + "/world/", + LaunchConfiguration("world"), + "/model/", + LaunchConfiguration("namespace"), + "/link/", + "base_link/sensor/rplidar/scan/points" + + "@sensor_msgs/msg/PointCloud2" + + "[gz.msgs.PointCloudPacked", + ], + ], + remappings=[ + ( + [ + "/world/", + LaunchConfiguration("world"), + "/model/", + LaunchConfiguration("namespace"), + "/link/", + "base_link/sensor/rplidar/scan", + ], + ["sensor/lidar/scan"], + ), + ( + [ + "/world/", + LaunchConfiguration("world"), + "/model/", + LaunchConfiguration("namespace"), + "/link/", + "base_link/sensor/rplidar/scan/points", + ], + ["sensor/lidar/scan/points"], + ), + ], + ), + # IMU + Node( + package="ros_gz_bridge", + executable="parameter_bridge", + name="imu_bridge", + output="screen", + parameters=[{"use_sim_time": True}], + arguments=[ + [ + "/world/", + LaunchConfiguration("world"), + "/model/", + LaunchConfiguration("namespace"), + "/link/", + "base_link/sensor/imu_sensor/imu" + + "@sensor_msgs/msg/Imu" + + "[gz.msgs.IMU", + ] + ], + remappings=[ + ( + [ + "/world/", + LaunchConfiguration("world"), + "/model/", + LaunchConfiguration("namespace"), + "/link/", + "base_link/sensor/imu_sensor/imu", + ], + ["sensor/imu/imu"], + ) + ], + ), + ] + ) + + # Create launch description and add actions + ld = LaunchDescription(ARGUMENTS) + ld.add_action(ros_gz_bridge) + return ld diff --git a/src/helmoro_simulation/launch/simulation.launch.py b/src/helmoro_simulation/launch/simulation.launch.py new file mode 100644 index 0000000..321cb72 --- /dev/null +++ b/src/helmoro_simulation/launch/simulation.launch.py @@ -0,0 +1,81 @@ +import os +from pathlib import Path + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, SetEnvironmentVariable, GroupAction +from launch.conditions import IfCondition, UnlessCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, TextSubstitution + +from launch_ros.actions import Node + + +ARGUMENTS = [ + DeclareLaunchArgument('world', default_value='empty', + description='Simulation World'), + DeclareLaunchArgument('headless', default_value='false', + choices=['false', 'False', 'true', 'True'], + description='Run the simulation headless') +] + + +def generate_launch_description(): + # Paths + pkg_helmoro_simulation = get_package_share_directory('helmoro_simulation') + pkg_helmoro_description = get_package_share_directory('helmoro_description') + pkg_ros_gz_sim = get_package_share_directory('ros_gz_sim') + + gz_sim_launch = PathJoinSubstitution([ + pkg_ros_gz_sim, 'launch', 'gz_sim.launch.py' + ]) + + # Set Gazebo resource path + gz_resource_path = SetEnvironmentVariable( + name='GZ_SIM_RESOURCE_PATH', + value=':'.join([ + os.path.join(pkg_helmoro_simulation, 'worlds'), + os.path.join(pkg_helmoro_simulation, 'models'), + str(Path(pkg_helmoro_description).parent.resolve()) + ]) + ) + + # Launch Gazebo headless or with GUI + gz_args = [ + LaunchConfiguration('world'), + '.sdf', + ' -r', + ' -v 2' + ] + + gazebo = GroupAction([ + IncludeLaunchDescription( + PythonLaunchDescriptionSource([gz_sim_launch]), + launch_arguments=[('gz_args', gz_args)], + condition=UnlessCondition(LaunchConfiguration('headless')), + ), + IncludeLaunchDescription( + PythonLaunchDescriptionSource([gz_sim_launch]), + launch_arguments=[('gz_args', gz_args + [' -s'])], + condition=IfCondition(LaunchConfiguration('headless')), + ) + ]) + + # Clock bridge node + clock_bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + name='clock_bridge', + output='screen', + arguments=[ + '/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock' + ] + ) + + # Compose launch description + ld = LaunchDescription(ARGUMENTS) + ld.add_action(gz_resource_path) + ld.add_action(gazebo) + ld.add_action(clock_bridge) + return ld diff --git a/src/helmoro_simulation/launch/spawn_robot.launch.py b/src/helmoro_simulation/launch/spawn_robot.launch.py new file mode 100644 index 0000000..7b8cb61 --- /dev/null +++ b/src/helmoro_simulation/launch/spawn_robot.launch.py @@ -0,0 +1,46 @@ +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +ARGUMENTS = [ + DeclareLaunchArgument( + "namespace", default_value="empty_namespace", description="Robot namespace" + ), + DeclareLaunchArgument("x", default_value="0.0", description="x position"), + DeclareLaunchArgument("y", default_value="0.0", description="y position"), + DeclareLaunchArgument("z", default_value="0.0", description="z position"), + DeclareLaunchArgument("yaw", default_value="0.0", description="yaw rotation"), +] + + +def generate_launch_description(): + + spawn_robot = Node( + package="ros_gz_sim", + namespace=LaunchConfiguration("namespace"), + name="create", + executable="create", + arguments=[ + "-name", + LaunchConfiguration("namespace"), + "-x", + LaunchConfiguration("x"), + "-y", + LaunchConfiguration("y"), + "-z", + LaunchConfiguration("z"), + "-Y", + LaunchConfiguration("yaw"), + "-topic", + "robot_description", + ], + output="screen", + ) + + # Create launch description and add actions + ld = LaunchDescription(ARGUMENTS) + ld.add_action(spawn_robot) + return ld From 02218b01e2228885bdf29183e9d38ff1858b8037 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marc=20Bl=C3=B6chlinger?= Date: Fri, 16 May 2025 14:03:00 +0000 Subject: [PATCH 03/29] fix(description): allow for upper and lowercase when defining boolean launch configurations --- src/helmoro_description/launch/description.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/helmoro_description/launch/description.launch.py b/src/helmoro_description/launch/description.launch.py index f9ec93f..8ec900e 100644 --- a/src/helmoro_description/launch/description.launch.py +++ b/src/helmoro_description/launch/description.launch.py @@ -13,7 +13,7 @@ DeclareLaunchArgument( "run_in_simulation", default_value="false", - choices=["true", "false"], + choices=["true", "false", "True", "False"], description="run_in_simulation", ), DeclareLaunchArgument( From c7feac99338887119669b2c49bbf3ed847ad6bbd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marc=20Bl=C3=B6chlinger?= Date: Fri, 16 May 2025 14:04:03 +0000 Subject: [PATCH 04/29] test(cli): added tests for spawn and delete function in robot_manager --- src/helmoro_cli/test/test_spawn_delete.py | 50 +++++++++++++++++++++++ 1 file changed, 50 insertions(+) create mode 100644 src/helmoro_cli/test/test_spawn_delete.py diff --git a/src/helmoro_cli/test/test_spawn_delete.py b/src/helmoro_cli/test/test_spawn_delete.py new file mode 100644 index 0000000..390abc7 --- /dev/null +++ b/src/helmoro_cli/test/test_spawn_delete.py @@ -0,0 +1,50 @@ +import subprocess +import time +import pytest +import os + +from helmoro_cli.robot_manager import RobotManager + + +@pytest.fixture(scope="module") +def robot_manager(): + print("Creating RobotManager instance...") + return RobotManager() + + +def test_spawn_delete_robot(robot_manager): + print("Spawning robot...") + name = "testbot" + robot_manager.spawn_robot(name, 0, 0, 0, 0) + + print("Checking if robot has spawned...") + start = time.time() + topic_list = os.popen("ros2 topic list").read().strip().split("\n") + while time.time() - start < 2.0 and f"/{name}/robot_description" not in topic_list: + topic_list = os.popen("ros2 topic list").read().strip().split("\n") + time.sleep(0.1) + + # Check for failure + if f"/{name}/robot_description" not in topic_list: + print("ros2 topic list output:") + print(topic_list) + assert False, f"Expected topic /{name}/robot_description not found" + + print("Robot spawned sucessfully: robot_description topic is getting advertised") + print("Deleting robot...") + robot_manager.delete_robot(name) + + print("Checking if robot is deleted...") + start = time.time() + robot_list = robot_manager.get_robot_names() + while time.time() - start < 2.0 and name in robot_list: + robot_list = robot_manager.get_robot_names() + time.sleep(0.1) + + # Check for failure + if name in robot_list: + print("Currently existing robot containers:") + print(robot_list) + assert False, f"{name}'s container still exists, robot wasn't properly deleted" + + print(f"Robot deleted sucessfully: {name}'s container was stopped") From a271202778f57ae17f665a1027323d9cd8f13cab Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marc=20Bl=C3=B6chlinger?= Date: Fri, 16 May 2025 14:54:36 +0000 Subject: [PATCH 05/29] fix(dockerfile): Install docker inside image --- .devcontainer/Dockerfile | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index f1987d6..85ea20f 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -18,8 +18,13 @@ RUN sudo apt update && sudo apt install -y \ RUN apt-get update && apt-get install -y \ xterm \ xvfb \ + curl \ && rm -rf /var/lib/apt/lists/* +RUN curl -fsSL https://get.docker.com -o get-docker.sh && \ + sh get-docker.sh && \ + rm get-docker.sh + # Install ROS debug packages RUN apt-get update && apt-get install -y \ ros-jazzy-demo-nodes-cpp \ @@ -96,5 +101,6 @@ USER root WORKDIR /home/ws COPY config config COPY src src +COPY .devcontainer .devcontainer RUN source /opt/ros/jazzy/setup.bash \ && colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release \ No newline at end of file From 2247796dea12dbbbcf3816e238ae7831c77196cf Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marc=20Bl=C3=B6chlinger?= Date: Fri, 16 May 2025 14:55:09 +0000 Subject: [PATCH 06/29] fix(ci): export logs also when tests fail --- .github/workflows/ci.yml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 72745cf..1d9f068 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -41,7 +41,7 @@ jobs: - name: Start container shell: bash - run: docker run --name test_container -v /tmp/.X11-unix:/tmp/.X11-unix --detach ghcr.io/helbling-technik/helmoro-software-ros2:${{ github.ref_name }}-latest tail -f /dev/null + run: docker run --name test_container -v /tmp/.X11-unix:/tmp/.X11-unix -v /var/run/docker.sock:/var/run/docker.sock --detach ghcr.io/helbling-technik/helmoro-software-ros2:${{ github.ref_name }}-latest tail -f /dev/null - name: Run test shell: bash @@ -62,6 +62,7 @@ jobs: - name: Export logs to runner shell: bash run: docker cp test_container:/home/ws/log ./log + if: always() # export logs even if tests fail - name: Upload logs uses: actions/upload-artifact@v4 From fa250ae1d0ab1582f4a069209b57e0ef7bcd11e4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marc=20Bl=C3=B6chlinger?= Date: Fri, 16 May 2025 15:08:28 +0000 Subject: [PATCH 07/29] fix(ci): added ros_domain env variable --- .github/workflows/ci.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 1d9f068..193fe5d 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -51,6 +51,7 @@ jobs: echo "Starting virtual display (Xvfb)..." Xvfb :99 -screen 0 1024x768x24 & export DISPLAY=:99 + export ROS_DOMAIN_ID=42 echo "Start testing..." docker exec -e DISPLAY=$DISPLAY test_container bash -c " From 102874569a1b208fbd7c33cb81ab40bd6c453cce Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marc=20Bl=C3=B6chlinger?= Date: Fri, 16 May 2025 15:16:11 +0000 Subject: [PATCH 08/29] fix(ci): execute colcon tests sequentially --- .github/workflows/ci.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 193fe5d..b6e3e2a 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -56,7 +56,7 @@ jobs: echo "Start testing..." docker exec -e DISPLAY=$DISPLAY test_container bash -c " source ./entrypoint.sh && - colcon test && + colcon test --executor sequential && colcon test-result --verbose " From 50c54c330a447eb5ff239c1acf00c468c0ad9ca6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marc=20Bl=C3=B6chlinger?= Date: Sat, 24 May 2025 19:28:38 +0000 Subject: [PATCH 09/29] fix(ci): run test_container in host net and ROS_DOMAIN_ID 42 --- .github/workflows/ci.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index b6e3e2a..8e33a4a 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -41,7 +41,7 @@ jobs: - name: Start container shell: bash - run: docker run --name test_container -v /tmp/.X11-unix:/tmp/.X11-unix -v /var/run/docker.sock:/var/run/docker.sock --detach ghcr.io/helbling-technik/helmoro-software-ros2:${{ github.ref_name }}-latest tail -f /dev/null + run: docker run --name test_container --net host -v /tmp/.X11-unix:/tmp/.X11-unix -v /var/run/docker.sock:/var/run/docker.sock --detach ghcr.io/helbling-technik/helmoro-software-ros2:${{ github.ref_name }}-latest tail -f /dev/null - name: Run test shell: bash @@ -54,7 +54,7 @@ jobs: export ROS_DOMAIN_ID=42 echo "Start testing..." - docker exec -e DISPLAY=$DISPLAY test_container bash -c " + docker exec -e DISPLAY=$DISPLAY -e ROS_DOMAIN_ID=$ROS_DOMAIN_ID test_container bash -c " source ./entrypoint.sh && colcon test --executor sequential && colcon test-result --verbose From 75f75876f09de941e787a4b923ef955ece0c70c2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marc=20Bl=C3=B6chlinger?= Date: Sat, 24 May 2025 19:49:14 +0000 Subject: [PATCH 10/29] fix(cli): delete robot spawn container after stopping --- src/helmoro_cli/helmoro_cli/robot_manager.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/helmoro_cli/helmoro_cli/robot_manager.py b/src/helmoro_cli/helmoro_cli/robot_manager.py index a1d45db..2639e5b 100644 --- a/src/helmoro_cli/helmoro_cli/robot_manager.py +++ b/src/helmoro_cli/helmoro_cli/robot_manager.py @@ -31,7 +31,7 @@ def delete_robot(self, name): containers = [line.strip().split() for line in raw_output.splitlines()] for container_id, container_name in containers: result = os.system( - f"sudo docker container stop {container_id} --timeout 1 > /dev/null" + f"sudo docker container stop {container_id} --timeout 1 && sudo docker container rm {container_id} > /dev/null" ) if result == 0: print(f"{container_name} Stopped") From bd7ff8c95120d9ff92fdc582745bdf7ff756b56d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marc=20Bl=C3=B6chlinger?= Date: Sat, 24 May 2025 19:50:06 +0000 Subject: [PATCH 11/29] fix(cli): raise timeout during testing from 2 to 10 seconds --- src/helmoro_cli/test/test_spawn_delete.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/helmoro_cli/test/test_spawn_delete.py b/src/helmoro_cli/test/test_spawn_delete.py index 390abc7..548521f 100644 --- a/src/helmoro_cli/test/test_spawn_delete.py +++ b/src/helmoro_cli/test/test_spawn_delete.py @@ -20,7 +20,7 @@ def test_spawn_delete_robot(robot_manager): print("Checking if robot has spawned...") start = time.time() topic_list = os.popen("ros2 topic list").read().strip().split("\n") - while time.time() - start < 2.0 and f"/{name}/robot_description" not in topic_list: + while time.time() - start < 10.0 and f"/{name}/robot_description" not in topic_list: topic_list = os.popen("ros2 topic list").read().strip().split("\n") time.sleep(0.1) From f00a45aa18493a1d21d263e12dac7785804f2364 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marc=20Bl=C3=B6chlinger?= Date: Sat, 24 May 2025 20:07:18 +0000 Subject: [PATCH 12/29] fix(ci): change image tag to helmoro --- .github/workflows/ci.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 8e33a4a..21c28d8 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -30,7 +30,7 @@ jobs: with: context: . file: .devcontainer/Dockerfile - tags: ghcr.io/helbling-technik/helmoro-software-ros2:${{ github.ref_name }}-latest + tags: helmoro load: true cache-from: | type=gha @@ -41,7 +41,7 @@ jobs: - name: Start container shell: bash - run: docker run --name test_container --net host -v /tmp/.X11-unix:/tmp/.X11-unix -v /var/run/docker.sock:/var/run/docker.sock --detach ghcr.io/helbling-technik/helmoro-software-ros2:${{ github.ref_name }}-latest tail -f /dev/null + run: docker run --name test_container --net host -v /tmp/.X11-unix:/tmp/.X11-unix -v /var/run/docker.sock:/var/run/docker.sock --detach helmoro tail -f /dev/null - name: Run test shell: bash From 9874169443d8ed7c15f0a87ffaf1152a69bc1066 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marc=20Bl=C3=B6chlinger?= Date: Mon, 2 Jun 2025 13:15:10 +0000 Subject: [PATCH 13/29] feat(devcontainer): mount docker.sock --- .devcontainer/devcontainer.json | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 4b088d4..1995e1e 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -43,7 +43,8 @@ "mounts": [ "source=/tmp/.X11-unix,target=/tmp/.X11-unix,type=bind,consistency=cached", "source=/dev/dri,target=/dev/dri,type=bind,consistency=cached", - "source=/dev/input,target=/dev/input,type=bind,consistency=delegated" + "source=/dev/input,target=/dev/input,type=bind,consistency=delegated", + "source=/var/run/docker.sock,target=/var/run/docker.sock,type=bind" ], "postCreateCommand": "sudo chown -R $(whoami) /home/ws/ && python3 /home/ws/config/append_to_bashrc.py" } \ No newline at end of file From bde79d2ab25678096a526a8903219fdb804f8534 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marc=20Bl=C3=B6chlinger?= Date: Mon, 2 Jun 2025 13:16:13 +0000 Subject: [PATCH 14/29] refactor(compose): change name of description service to robot_description for better readability --- .devcontainer/docker-compose.yml | 2 +- src/helmoro_cli/helmoro_cli/robot_manager.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/.devcontainer/docker-compose.yml b/.devcontainer/docker-compose.yml index 8f6f276..40d09a6 100644 --- a/.devcontainer/docker-compose.yml +++ b/.devcontainer/docker-compose.yml @@ -44,7 +44,7 @@ services: - NAMESPACE - WORLD - description: + robot_description: extends: service: helmoro_template_service command: bash -c "ros2 launch helmoro_description description.launch.py namespace:=$${NAMESPACE} run_in_simulation:=$${USE_SIM_TIME}" diff --git a/src/helmoro_cli/helmoro_cli/robot_manager.py b/src/helmoro_cli/helmoro_cli/robot_manager.py index 2639e5b..c953291 100644 --- a/src/helmoro_cli/helmoro_cli/robot_manager.py +++ b/src/helmoro_cli/helmoro_cli/robot_manager.py @@ -10,7 +10,7 @@ def spawn_robot(self, name, x, y, z, yaw): self.robots[name] = {"pos": (x, y, z), "yaw": yaw, "status": "idle"} # Here you would trigger a ROS 2 launch or service call spawn = f"sudo NAMESPACE={name} USE_SIM_TIME=True \ - docker compose -p robot_{name} -f /home/ws/.devcontainer/docker-compose.yml up description --detach --wait" + docker compose -p robot_{name} -f /home/ws/.devcontainer/docker-compose.yml up robot_description --detach --wait" print(f"Start core nodes of {name}") os.system(spawn) From 0027c4dfee38ea5b82a7e0da397f3aa22bd7ca1d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marc=20Bl=C3=B6chlinger?= Date: Mon, 2 Jun 2025 13:30:42 +0000 Subject: [PATCH 15/29] fix(cli): added newline to __init__.py to conform with python convention --- src/helmoro_cli/helmoro_cli/__init__.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/helmoro_cli/helmoro_cli/__init__.py b/src/helmoro_cli/helmoro_cli/__init__.py index e69de29..8b13789 100644 --- a/src/helmoro_cli/helmoro_cli/__init__.py +++ b/src/helmoro_cli/helmoro_cli/__init__.py @@ -0,0 +1 @@ + From d1771800b6f6b9e8008da995d61a109f4481d1af Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marc=20Bl=C3=B6chlinger?= Date: Mon, 2 Jun 2025 13:57:40 +0000 Subject: [PATCH 16/29] doc(simulation): explained in comments more clearly, how the update to ROS Kilted will make the ros_gz_bridge.launch.py redundant --- src/helmoro_simulation/launch/ros_gz_bridge.launch.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/helmoro_simulation/launch/ros_gz_bridge.launch.py b/src/helmoro_simulation/launch/ros_gz_bridge.launch.py index f36c7c6..a638771 100644 --- a/src/helmoro_simulation/launch/ros_gz_bridge.launch.py +++ b/src/helmoro_simulation/launch/ros_gz_bridge.launch.py @@ -21,6 +21,8 @@ def generate_launch_description(): [ PushRosNamespace(LaunchConfiguration("namespace")), # TODO: Update to ROS Kilted will add support for custom URDF frames making this file unnecessary + # The new launchfile for models integrates the ros_gz_bridge https://github.com/gazebosim/ros_gz/blob/kilted/ros_gz_sim/launch/ros_gz_spawn_model.launch.py + # Depth Camera Node( package="ros_gz_bridge", From 42ed48965342aa652d6dfa898ff273ce2157fa44 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marc=20Bl=C3=B6chlinger?= Date: Tue, 3 Jun 2025 11:15:45 +0000 Subject: [PATCH 17/29] fix(compose): wrong path to main.py --- .devcontainer/docker-compose.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.devcontainer/docker-compose.yml b/.devcontainer/docker-compose.yml index 40d09a6..25ce625 100644 --- a/.devcontainer/docker-compose.yml +++ b/.devcontainer/docker-compose.yml @@ -12,7 +12,7 @@ services: cli: extends: helmoro_template_service - command: python3 src/helmoro_cli/src/main.py + command: python3 src/helmoro_cli/helmoro_cli/main.py volumes: - /var/run/docker.sock:/var/run/docker.sock From 3deef219bb7de0ffcffac415d323a171d2f0e97e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marc=20Bl=C3=B6chlinger?= Date: Tue, 3 Jun 2025 11:16:45 +0000 Subject: [PATCH 18/29] fix(config): typo setub.bash -> setup.bash --- config/append_to_bashrc.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/config/append_to_bashrc.txt b/config/append_to_bashrc.txt index d3d43a2..dcc9f26 100644 --- a/config/append_to_bashrc.txt +++ b/config/append_to_bashrc.txt @@ -1,5 +1,5 @@ # Custom Alias -alias src='source /opt/ros/jazzy/setub.bash && source /home/ws/install/setup.bash' +alias src='source /opt/ros/jazzy/setup.bash && source /home/ws/install/setup.bash' # Integrate entrypoint.sh source /home/ws/.devcontainer/entrypoint.sh \ No newline at end of file From a7e037960f251d3ed178f561bafb48a2aab7ab02 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marc=20Bl=C3=B6chlinger?= Date: Tue, 3 Jun 2025 11:19:50 +0000 Subject: [PATCH 19/29] fix(Dockerfile): update outdated GPG keys --- .devcontainer/Dockerfile | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index 85ea20f..995ea1b 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -1,5 +1,17 @@ -FROM ros@sha256:9749355a0760334fa5ea3c660f2de1ecc5e8a5af8047e4bbaa9afa8a7843da80 AS dev +FROM osrf/ros@sha256:f2f03a667950570edcaa14ed414a636912acd44ea9c25c10b5a7c5d44eea5142 AS dev +RUN rm /etc/apt/sources.list.d/ros2-latest.list \ + && rm /usr/share/keyrings/ros2-latest-archive-keyring.gpg + +RUN apt-get update \ + && apt-get install -y ca-certificates curl + +RUN export ROS_APT_SOURCE_VERSION=$(curl -s https://api.github.com/repos/ros-infrastructure/ros-apt-source/releases/latest | grep -F "tag_name" | awk -F\" '{print $4}') ;\ + curl -L -s -o /tmp/ros2-apt-source.deb "https://github.com/ros-infrastructure/ros-apt-source/releases/download/${ROS_APT_SOURCE_VERSION}/ros2-apt-source_${ROS_APT_SOURCE_VERSION}.$(. /etc/os-release && echo $VERSION_CODENAME)_all.deb" \ + && apt-get update \ + && apt-get install /tmp/ros2-apt-source.deb \ + && rm -f /tmp/ros2-apt-source.deb + # ******************************************************** # * Install Dependencies * # ******************************************************** From cdd0be1a9e79607bd9a895b4309be93f5000a4c4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marc=20Bl=C3=B6chlinger?= Date: Tue, 3 Jun 2025 11:22:08 +0000 Subject: [PATCH 20/29] doc(cli): add comments --- src/helmoro_cli/helmoro_cli/main.py | 22 ++++++++++++++++---- src/helmoro_cli/helmoro_cli/robot_manager.py | 1 + 2 files changed, 19 insertions(+), 4 deletions(-) diff --git a/src/helmoro_cli/helmoro_cli/main.py b/src/helmoro_cli/helmoro_cli/main.py index a060a4a..6a4575b 100644 --- a/src/helmoro_cli/helmoro_cli/main.py +++ b/src/helmoro_cli/helmoro_cli/main.py @@ -59,16 +59,30 @@ def do_exit(self, arg): print("Exiting CLI...") return True + # TODO: This function is meant as a helper during development. Do not use this function for production. def do_multi_robot_spawn(self, arg): "Run multi-robot test" self.do_start_simulation(arg) - arg1 = "alfred -1 0 1 0 " - arg2 = "bob 1 1 1 1" - arg3 = "charlie 3 2 1 2" + arg1 = "alfred -0.5 3 0 2.2 " + arg2 = "bob -0.5 -3.5 0 2.6" + arg3 = "charlie 3 -1.2 0 0" + arg4 = "echo 5.5 -1.3 0 0" + arg5 = "foxtrott 7.4 -1.6 0 1.8" + arg6 = "golf 2.4 2.5 0 1.6" + arg7 = "hotel -2.6 2.9 0 -1.6" + arg8 = "india -1.6 -3.5 0 -0.2" + arg9 = "juliett 2.6 1 0 -0.1" + arg10 = "kilo 13 -0.6 0 -1.5" self.do_spawn(arg1) self.do_spawn(arg2) self.do_spawn(arg3) - + self.do_spawn(arg4) + # self.do_spawn(arg5) + # self.do_spawn(arg6) + # self.do_spawn(arg7) + # self.do_spawn(arg8) + # self.do_spawn(arg9) + # self.do_spawn(arg10) def main(): try: diff --git a/src/helmoro_cli/helmoro_cli/robot_manager.py b/src/helmoro_cli/helmoro_cli/robot_manager.py index c953291..8d00e46 100644 --- a/src/helmoro_cli/helmoro_cli/robot_manager.py +++ b/src/helmoro_cli/helmoro_cli/robot_manager.py @@ -12,6 +12,7 @@ def spawn_robot(self, name, x, y, z, yaw): spawn = f"sudo NAMESPACE={name} USE_SIM_TIME=True \ docker compose -p robot_{name} -f /home/ws/.devcontainer/docker-compose.yml up robot_description --detach --wait" print(f"Start core nodes of {name}") + print(f"Executing command: {spawn}") os.system(spawn) def delete_robot(self, name): From 0accd63967e66cf9beb8ed334acb807fa523cea9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marc=20Bl=C3=B6chlinger?= Date: Tue, 3 Jun 2025 13:03:48 +0000 Subject: [PATCH 21/29] refactor: created a helmoro_utils pkg containing common test cases --- src/helmoro_description/package.xml | 3 +- .../test/launch_description.test.py | 101 ++------- src/helmoro_simulation/package.xml | 1 + .../tests/launch_simulation.test.py | 24 +-- src/helmoro_utils/LICENSE | 202 ++++++++++++++++++ src/helmoro_utils/helmoro_utils/__init__.py | 0 .../helmoro_utils/test_collection.py | 108 ++++++++++ src/helmoro_utils/package.xml | 18 ++ src/helmoro_utils/resource/helmoro_utils | 0 src/helmoro_utils/setup.cfg | 4 + src/helmoro_utils/setup.py | 25 +++ src/helmoro_utils/test/test_copyright.py | 25 +++ src/helmoro_visualization/package.xml | 1 + .../tests/launch.test.py | 31 +-- 14 files changed, 421 insertions(+), 122 deletions(-) create mode 100644 src/helmoro_utils/LICENSE create mode 100644 src/helmoro_utils/helmoro_utils/__init__.py create mode 100644 src/helmoro_utils/helmoro_utils/test_collection.py create mode 100644 src/helmoro_utils/package.xml create mode 100644 src/helmoro_utils/resource/helmoro_utils create mode 100644 src/helmoro_utils/setup.cfg create mode 100644 src/helmoro_utils/setup.py create mode 100644 src/helmoro_utils/test/test_copyright.py diff --git a/src/helmoro_description/package.xml b/src/helmoro_description/package.xml index cb0e733..d2b18e4 100644 --- a/src/helmoro_description/package.xml +++ b/src/helmoro_description/package.xml @@ -18,7 +18,8 @@ launch_testing launch_testing_ament_cmake rclpy - + helmoro_utils + ament_cmake diff --git a/src/helmoro_description/test/launch_description.test.py b/src/helmoro_description/test/launch_description.test.py index 544f0a1..ba9af7c 100644 --- a/src/helmoro_description/test/launch_description.test.py +++ b/src/helmoro_description/test/launch_description.test.py @@ -19,6 +19,8 @@ from launch_testing.io_handler import ActiveIoHandler from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy +from helmoro_utils.test_collection import wait_for_node, wait_for_topic, wait_for_message + NAMESPACE = "robot_namespace" @@ -64,102 +66,41 @@ def tearDown(self): def test_robot_state_publisher_node_start(self, proc_output: ActiveIoHandler): """Test if the robot_state_publisher node has started.""" - found = False - print("Waiting for node...") - start = time.time() - - # Wait for the node to start up and become available - while time.time() - start < 10.0 and not found: - found = "robot_state_publisher" in self.node.get_node_names() - time.sleep(0.1) - - # Assert that the node was found - assert found, "Node not found!" + wait_for_node(self.node, "robot_state_publisher", timeout=2.0) def test_robot_state_publisher_advertise_topic(self, proc_output: ActiveIoHandler): """Test if the robot_description topic is advertised by the node.""" - received = False - print("Listening for topics...") - start = time.time() - - # Wait for the topic to be advertised by the robot_state_publisher node - while time.time() - start < 10.0 and not received: - # Check if the node is publishing messages - topic_names = self.node.get_topic_names_and_types() - for topic_name, types in topic_names: - if topic_name == "/" + NAMESPACE + "/robot_description": - received = True - break - time.sleep(0.1) - - # Assert that the topic was advertised - assert received, "Topic not advertised!" + wait_for_topic( + self.node, + "/" + NAMESPACE + "/robot_description", + timeout=10.0 + ) def test_robot_state_publisher_publish_msgs(self, proc_output: ActiveIoHandler): - """Test if messages are published to the correct topic.""" - msgs_rx = [] # List to store received messages - - # Set up QoS profile with 'transient local' durability settings + """Test if messages are published to the correct topic.""" qos_profile = QoSProfile( reliability=ReliabilityPolicy.RELIABLE, durability=DurabilityPolicy.TRANSIENT_LOCAL, history=HistoryPolicy.KEEP_LAST, depth=10, ) - - # Create a subscription to the robot_description topic - sub = self.node.create_subscription( - std_msgs.msg.String, + + wait_for_message( + self.node, "/" + NAMESPACE + "/robot_description", - lambda msg: msgs_rx.append(msg), - qos_profile, + std_msgs.msg.String, + timeout=3.0, + qos_profile=qos_profile ) - try: - # Wait for messages to be received on the topic for up to 10 seconds - end_time = time.time() + 10 - while time.time() < end_time and len(msgs_rx) == 0: - # Spin once to execute the subscriber callback - rclpy.spin_once(self.node, timeout_sec=1) - - # Assert that at least one message has been received - assert ( - len(msgs_rx) > 0 - ), "No messages received on the robot_description topic!" - - finally: - # Ensure that the subscription is destroyed after the test - self.node.destroy_subscription(sub) - def test_joint_state_publisher_node_start(self, proc_output: ActiveIoHandler): """Test if the joint_state_publisher node has started.""" - found = False - print("Waiting for node...") - start = time.time() - - # Wait for the node to start up and become available - while time.time() - start < 10.0 and not found: - found = "joint_state_publisher" in self.node.get_node_names() - time.sleep(0.1) - - # Assert that the node was found - assert found, "Node not found!" + wait_for_node(self.node, "joint_state_publisher", timeout=2.0) def test_joint_state_publisher_advertise_topic(self, proc_output: ActiveIoHandler): """Test if the joint_state topic is advertised by the node.""" - received = False - print("Listening for topics...") - start = time.time() - - # Wait for the topic to be advertised by the robot_state_publisher node - while time.time() - start < 10.0 and not received: - # Check if the node is publishing messages - topic_names = self.node.get_topic_names_and_types() - for topic_name, types in topic_names: - if topic_name == "/" + NAMESPACE + "/joint_states": - received = True - break - time.sleep(0.1) - - # Assert that the topic was advertised - assert received, "Topic not advertised!" + wait_for_topic( + self.node, + "/" + NAMESPACE + "/joint_states", + timeout=10.0 + ) diff --git a/src/helmoro_simulation/package.xml b/src/helmoro_simulation/package.xml index 94ce52e..19be3b6 100644 --- a/src/helmoro_simulation/package.xml +++ b/src/helmoro_simulation/package.xml @@ -20,6 +20,7 @@ launch_testing launch_testing_ament_cmake rclpy + helmoro_utils ament_cmake diff --git a/src/helmoro_simulation/tests/launch_simulation.test.py b/src/helmoro_simulation/tests/launch_simulation.test.py index b109efc..8c26375 100644 --- a/src/helmoro_simulation/tests/launch_simulation.test.py +++ b/src/helmoro_simulation/tests/launch_simulation.test.py @@ -12,9 +12,11 @@ from launch.substitutions import PathJoinSubstitution from launch_testing.actions import ReadyToTest + import launch_testing.markers +import rosgraph_msgs.msg -from rosgraph_msgs.msg import Clock +from helmoro_utils.test_collection import wait_for_node, wait_for_message ARGUMENTS = [ @@ -54,20 +56,10 @@ def tearDown(self): rclpy.shutdown() def test_clock_bridge_start(self): - """Test if the clock_bridge node started""" - assert "clock_bridge" in self.node.get_node_names(), "clock_bridge node not found!" - + """Test if the clock_bridge node started""" + wait_for_node(self.node, 'clock_bridge', timeout=10.0) + + def test_publishes_clock(self, proc_output): """Check whether clock messages are published""" - msgs_rx = [] - sub = self.node.create_subscription( - Clock, '/clock', - lambda msg: msgs_rx.append(msg), 1) - try: - end_time = time.time() + 10 - while time.time() < end_time and len(msgs_rx) < 1: - rclpy.spin_once(self.node, timeout_sec=0.1) - - assert len(msgs_rx) > 0, "No clock messages received" - finally: - self.node.destroy_subscription(sub) + wait_for_message(self.node, '/clock', rosgraph_msgs.msg.Clock, timeout=10.0) diff --git a/src/helmoro_utils/LICENSE b/src/helmoro_utils/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/src/helmoro_utils/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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/src/helmoro_utils/helmoro_utils/__init__.py b/src/helmoro_utils/helmoro_utils/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/helmoro_utils/helmoro_utils/test_collection.py b/src/helmoro_utils/helmoro_utils/test_collection.py new file mode 100644 index 0000000..d37e045 --- /dev/null +++ b/src/helmoro_utils/helmoro_utils/test_collection.py @@ -0,0 +1,108 @@ +# helmoro_utils/test_collection.py + +import time +import rclpy +from rclpy.node import Node +from rosidl_runtime_py.utilities import get_message +import std_msgs.msg +import rosgraph_msgs.msg +from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy + + +def wait_for_node(node, node_name: str, timeout: float = 10.0): + """ + Waits until the specified node name appears in the ROS graph. + + Args: + node: rclpy Node object used to query node names. + node_name: Name of the node to look for. + timeout: How long to wait before failing. + poll_interval: How often to check. + + Raises: + AssertionError: If the node is not found within the timeout. + """ + found = False + start = time.time() + + while time.time() - start < timeout: + if node_name in node.get_node_names(): + found = True + break + time.sleep(0.1) + + assert found, f"Node '{node_name}' not found within {timeout} seconds. Nodes: {node.get_node_names()}" + +def wait_for_node_with_namespace(node: Node, node_name: str, node_namespace: str, timeout: float = 10.0): + """ + Waits until the specified node with the exact name and namespace appears in the ROS graph. + + Args: + node: rclpy Node object used to query node names and namespaces. + node_name: Name of the node to look for (e.g., 'my_node'). + node_namespace: Expected namespace of the node (e.g., '/robot1'). + timeout: Max time to wait for the node. + + Raises: + AssertionError: If the node with the namespace is not found within the timeout. + """ + found = False + start = time.time() + + while time.time() - start < timeout: + node_tuples = node.get_node_names_and_namespaces() # List of (name, namespace) + for name, namespace in node_tuples: + if name == node_name and namespace == node_namespace: + found = True + break + if found: + break + time.sleep(0.1) + + assert found, ( + f"Node '{node_name}' with namespace '{node_namespace}' not found within {timeout} seconds.\n" + f"Available nodes: {node_tuples}" + ) + +def wait_for_topic(node, topic_name: str, timeout: float = 10.0): + """ + Waits until the specified topic is advertised in the ROS graph. + + Args: + node: rclpy Node object used to query topics. + topic_name: Name of the topic to look for. + timeout: How long to wait before failing. + poll_interval: How often to check. + + Raises: + AssertionError: If the topic is not found within the timeout. + """ + received = False + start = time.time() + + while time.time() - start < timeout: + topic_names = node.get_topic_names_and_types() + if any(topic_name == name for name, _ in topic_names): + received = True + break + time.sleep(0.1) + + assert received, f"Topic '{topic_name}' not advertised within {timeout} seconds. Topics: {topic_names}" + +def wait_for_message(node: Node, topic_name: str, msg_type_str: str, timeout: float = 10.0, qos_profile: QoSProfile = 10): + """Waits until a message is received on the topic within the timeout.""" + received_msg = [] + + def callback(msg): + received_msg.append(msg) + + sub = node.create_subscription(msg_type_str, topic_name, callback, qos_profile) + + start = time.time() + while time.time() - start < timeout: + rclpy.spin_once(node, timeout_sec=0.1) + if received_msg: + break + + node.destroy_subscription(sub) + assert received_msg, f"No message received on {topic_name} within {timeout} seconds." diff --git a/src/helmoro_utils/package.xml b/src/helmoro_utils/package.xml new file mode 100644 index 0000000..67a75aa --- /dev/null +++ b/src/helmoro_utils/package.xml @@ -0,0 +1,18 @@ + + + + helmoro_utils + 0.0.0 + TODO: Package description + htkz + Apache-2.0 + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/helmoro_utils/resource/helmoro_utils b/src/helmoro_utils/resource/helmoro_utils new file mode 100644 index 0000000..e69de29 diff --git a/src/helmoro_utils/setup.cfg b/src/helmoro_utils/setup.cfg new file mode 100644 index 0000000..b8e2642 --- /dev/null +++ b/src/helmoro_utils/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/helmoro_utils +[install] +install_scripts=$base/lib/helmoro_utils diff --git a/src/helmoro_utils/setup.py b/src/helmoro_utils/setup.py new file mode 100644 index 0000000..e171939 --- /dev/null +++ b/src/helmoro_utils/setup.py @@ -0,0 +1,25 @@ +from setuptools import find_packages, setup + +package_name = 'helmoro_utils' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='htkz', + maintainer_email='mbloechli@student.ethz.ch', + description='TODO: Package description', + license='Apache-2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + ], + }, +) diff --git a/src/helmoro_utils/test/test_copyright.py b/src/helmoro_utils/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/src/helmoro_utils/test/test_copyright.py @@ -0,0 +1,25 @@ +# 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 + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/helmoro_visualization/package.xml b/src/helmoro_visualization/package.xml index 723a481..9a3ba77 100644 --- a/src/helmoro_visualization/package.xml +++ b/src/helmoro_visualization/package.xml @@ -16,6 +16,7 @@ launch_testing launch_testing_ament_cmake rclpy + helmoro_utils ament_cmake diff --git a/src/helmoro_visualization/tests/launch.test.py b/src/helmoro_visualization/tests/launch.test.py index 2cdd906..ec0cb80 100644 --- a/src/helmoro_visualization/tests/launch.test.py +++ b/src/helmoro_visualization/tests/launch.test.py @@ -13,6 +13,8 @@ import pytest +from helmoro_utils.test_collection import wait_for_node_with_namespace + namespace = "test_robot" ARGUMENTS = [ ("use_sim_time", "true"), @@ -55,28 +57,7 @@ def tearDown(self): rclpy.shutdown() def test_namespace(self): - nodes_and_namespaces = self.node.get_node_names_and_namespaces() - rviz_ns = None - - node_found = False - namespace_found = False - start = time.time() - - # Look for node - while time.time() - start < 5.0 and not namespace_found: - for name, ns in nodes_and_namespaces: - if name == "rviz2": - node_found = True - rviz_ns - if ns == f"/{namespace}": - namespace_found = True - break - - # If test fails, give debug output - if not namespace_found: - print("Printing all found nodes...") - for name, ns in nodes_and_namespaces: - print("Nodename: ", name, " namespace: ", namespace) - - assert node_found, "rviz2 node not found!" - assert namespace_found, f"Expected namespace '/{namespace}', got '{rviz_ns}'" + """Check if rviz2 node is running in the expected namespace.""" + wait_for_node_with_namespace( + self.node, "rviz2", f"/{namespace}", timeout=3.0 + ) From 5d79de5a2a54132ef859251796a69ee21c454c60 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marc=20Bl=C3=B6chlinger?= Date: Tue, 3 Jun 2025 13:20:01 +0000 Subject: [PATCH 22/29] refactor(cli): exchanged topic test with test from helmoro_utils pkg --- src/helmoro_cli/test/test_spawn_delete.py | 26 ++++++++++------------- 1 file changed, 11 insertions(+), 15 deletions(-) diff --git a/src/helmoro_cli/test/test_spawn_delete.py b/src/helmoro_cli/test/test_spawn_delete.py index 548521f..4e99746 100644 --- a/src/helmoro_cli/test/test_spawn_delete.py +++ b/src/helmoro_cli/test/test_spawn_delete.py @@ -1,10 +1,10 @@ -import subprocess import time import pytest import os +import rclpy from helmoro_cli.robot_manager import RobotManager - +from helmoro_utils.test_collection import wait_for_topic @pytest.fixture(scope="module") def robot_manager(): @@ -13,28 +13,22 @@ def robot_manager(): def test_spawn_delete_robot(robot_manager): + rclpy.init() + node = rclpy.create_node("test_node") + print("Spawning robot...") name = "testbot" robot_manager.spawn_robot(name, 0, 0, 0, 0) print("Checking if robot has spawned...") - start = time.time() - topic_list = os.popen("ros2 topic list").read().strip().split("\n") - while time.time() - start < 10.0 and f"/{name}/robot_description" not in topic_list: - topic_list = os.popen("ros2 topic list").read().strip().split("\n") - time.sleep(0.1) - - # Check for failure - if f"/{name}/robot_description" not in topic_list: - print("ros2 topic list output:") - print(topic_list) - assert False, f"Expected topic /{name}/robot_description not found" - + wait_for_topic(node, f"/{name}/robot_description", timeout=10.0) + print("Robot spawned sucessfully: robot_description topic is getting advertised") print("Deleting robot...") robot_manager.delete_robot(name) - print("Checking if robot is deleted...") + print("Checking if robot is deleted...") + start = time.time() robot_list = robot_manager.get_robot_names() while time.time() - start < 2.0 and name in robot_list: @@ -48,3 +42,5 @@ def test_spawn_delete_robot(robot_manager): assert False, f"{name}'s container still exists, robot wasn't properly deleted" print(f"Robot deleted sucessfully: {name}'s container was stopped") + + rclpy.shutdown() From 59b3b16b4817b7cbedcf9febe5fc2fb00b8fa5ca Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marc=20Bl=C3=B6chlinger?= Date: Wed, 4 Jun 2025 08:57:10 +0000 Subject: [PATCH 23/29] refactor: move docker specific files into seperate folder --- {config => .devcontainer}/append_to_bashrc.py | 2 +- {config => .devcontainer}/append_to_bashrc.txt | 0 .devcontainer/build.sh | 1 - .devcontainer/devcontainer.json | 4 ++-- .github/workflows/ci.yml | 2 +- {.devcontainer => docker}/Dockerfile | 5 ++--- docker/build.sh | 1 + {.devcontainer => docker}/docker-compose.yml | 14 +++++++------- {.devcontainer => docker}/entrypoint.sh | 0 src/helmoro_cli/helmoro_cli/robot_manager.py | 2 +- src/helmoro_cli/helmoro_cli/simulation_manager.py | 10 +++++----- src/helmoro_cli/package.xml | 1 + 12 files changed, 21 insertions(+), 21 deletions(-) rename {config => .devcontainer}/append_to_bashrc.py (91%) rename {config => .devcontainer}/append_to_bashrc.txt (100%) delete mode 100755 .devcontainer/build.sh rename {.devcontainer => docker}/Dockerfile (97%) create mode 100755 docker/build.sh rename {.devcontainer => docker}/docker-compose.yml (83%) rename {.devcontainer => docker}/entrypoint.sh (100%) diff --git a/config/append_to_bashrc.py b/.devcontainer/append_to_bashrc.py similarity index 91% rename from config/append_to_bashrc.py rename to .devcontainer/append_to_bashrc.py index 79ce07d..9985297 100644 --- a/config/append_to_bashrc.py +++ b/.devcontainer/append_to_bashrc.py @@ -26,7 +26,7 @@ def append_unique_lines_to_bashrc(source_file): print("ℹ️ No new lines were added. All lines already exist.") if __name__ == "__main__": - append_unique_lines_to_bashrc(os.path.abspath("/home/ws/config/append_to_bashrc.txt")) + append_unique_lines_to_bashrc(os.path.abspath("/home/ws/.devcontainer/append_to_bashrc.txt")) # If there are arguments passed to this container (i.e., CMD), run them if len(sys.argv) > 1: diff --git a/config/append_to_bashrc.txt b/.devcontainer/append_to_bashrc.txt similarity index 100% rename from config/append_to_bashrc.txt rename to .devcontainer/append_to_bashrc.txt diff --git a/.devcontainer/build.sh b/.devcontainer/build.sh deleted file mode 100755 index e5b1111..0000000 --- a/.devcontainer/build.sh +++ /dev/null @@ -1 +0,0 @@ -sudo docker build -t helmoro -f .devcontainer/Dockerfile . \ No newline at end of file diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 1995e1e..322825d 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -3,7 +3,7 @@ "privileged": true, "remoteUser": "htkz", "build": { - "dockerfile": "Dockerfile", + "dockerfile": "../docker/Dockerfile", "context": "..", "target": "dev", "args": { @@ -46,5 +46,5 @@ "source=/dev/input,target=/dev/input,type=bind,consistency=delegated", "source=/var/run/docker.sock,target=/var/run/docker.sock,type=bind" ], - "postCreateCommand": "sudo chown -R $(whoami) /home/ws/ && python3 /home/ws/config/append_to_bashrc.py" + "postCreateCommand": "sudo chown -R $(whoami) /home/ws/ && python3 /home/ws/.devcontainer/append_to_bashrc.py" } \ No newline at end of file diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 21c28d8..202f24a 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -29,7 +29,7 @@ jobs: uses: docker/build-push-action@v6 with: context: . - file: .devcontainer/Dockerfile + file: docker/Dockerfile tags: helmoro load: true cache-from: | diff --git a/.devcontainer/Dockerfile b/docker/Dockerfile similarity index 97% rename from .devcontainer/Dockerfile rename to docker/Dockerfile index 995ea1b..abd9b0d 100644 --- a/.devcontainer/Dockerfile +++ b/docker/Dockerfile @@ -95,7 +95,7 @@ ENV SHELL=/bin/bash # Define entrypoint RUN mkdir -p /home/ws -COPY .devcontainer/entrypoint.sh /home/ws +COPY docker/entrypoint.sh /home/ws RUN chmod +x /home/ws/entrypoint.sh ENTRYPOINT [ "/home/ws/entrypoint.sh" ] CMD ["/bin/bash"] @@ -111,8 +111,7 @@ FROM dev AS prod # Copy our packages into the image and build them USER root WORKDIR /home/ws -COPY config config COPY src src -COPY .devcontainer .devcontainer +COPY docker/docker-compose.yml docker/ RUN source /opt/ros/jazzy/setup.bash \ && colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release \ No newline at end of file diff --git a/docker/build.sh b/docker/build.sh new file mode 100755 index 0000000..22575dd --- /dev/null +++ b/docker/build.sh @@ -0,0 +1 @@ +sudo docker build -t helmoro -f docker/Dockerfile . \ No newline at end of file diff --git a/.devcontainer/docker-compose.yml b/docker/docker-compose.yml similarity index 83% rename from .devcontainer/docker-compose.yml rename to docker/docker-compose.yml index 25ce625..a813650 100644 --- a/.devcontainer/docker-compose.yml +++ b/docker/docker-compose.yml @@ -1,5 +1,5 @@ services: - helmoro_template_service: + template: command: bash image: helmoro environment: @@ -11,17 +11,17 @@ services: - /tmp/.X11-unix:/tmp/.X11-unix cli: - extends: helmoro_template_service + extends: template command: python3 src/helmoro_cli/helmoro_cli/main.py volumes: - /var/run/docker.sock:/var/run/docker.sock simulation: - extends: helmoro_template_service + extends: template command: ros2 launch helmoro_simulation simulation.launch.py spawn_robot: - extends: helmoro_template_service + extends: template command: bash -c "ros2 launch helmoro_simulation spawn_robot.launch.py namespace:=$${NAMESPACE} x:=$${X} y:=$${Y} z:=$${Z} yaw:=$${YAW}" environment: - NAMESPACE @@ -31,14 +31,14 @@ services: - YAW despawn_robot: - extends: helmoro_template_service + extends: template command: bash -c "ros2 launch ros_gz_sim gz_remove_model.launch.py world:=$${WORLD} entity_name:=$${ENTITY_NAME}" environment: - WORLD - ENTITY_NAME gz_ros_bridge: - extends: helmoro_template_service + extends: template command: bash -c "ros2 launch helmoro_simulation ros_gz_bridge.launch.py namespace:=$${NAMESPACE} word:=$${WORLD}" environment: - NAMESPACE @@ -46,7 +46,7 @@ services: robot_description: extends: - service: helmoro_template_service + service: template command: bash -c "ros2 launch helmoro_description description.launch.py namespace:=$${NAMESPACE} run_in_simulation:=$${USE_SIM_TIME}" environment: - NAMESPACE diff --git a/.devcontainer/entrypoint.sh b/docker/entrypoint.sh similarity index 100% rename from .devcontainer/entrypoint.sh rename to docker/entrypoint.sh diff --git a/src/helmoro_cli/helmoro_cli/robot_manager.py b/src/helmoro_cli/helmoro_cli/robot_manager.py index 8d00e46..576ad11 100644 --- a/src/helmoro_cli/helmoro_cli/robot_manager.py +++ b/src/helmoro_cli/helmoro_cli/robot_manager.py @@ -10,7 +10,7 @@ def spawn_robot(self, name, x, y, z, yaw): self.robots[name] = {"pos": (x, y, z), "yaw": yaw, "status": "idle"} # Here you would trigger a ROS 2 launch or service call spawn = f"sudo NAMESPACE={name} USE_SIM_TIME=True \ - docker compose -p robot_{name} -f /home/ws/.devcontainer/docker-compose.yml up robot_description --detach --wait" + docker compose -p robot_{name} -f /home/ws/docker/docker-compose.yml up robot_description --detach --wait" print(f"Start core nodes of {name}") print(f"Executing command: {spawn}") os.system(spawn) diff --git a/src/helmoro_cli/helmoro_cli/simulation_manager.py b/src/helmoro_cli/helmoro_cli/simulation_manager.py index 60dab67..f8ed67b 100644 --- a/src/helmoro_cli/helmoro_cli/simulation_manager.py +++ b/src/helmoro_cli/helmoro_cli/simulation_manager.py @@ -12,10 +12,10 @@ def __init__(self): def spawn_robot(self, name, x, y, z, yaw): # Trigger a ROS 2 launch command to spawn the robot spawn = f"sudo NAMESPACE={name} X={x} Y={y} Z={z} YAW={yaw} \ - docker compose -p sim_{name} -f /home/ws/.devcontainer/docker-compose.yml up spawn_robot" + docker compose -p sim_{name} -f /home/ws/docker/docker-compose.yml up spawn_robot" ros_gz_bridge = f"sudo NAMESPACE={name} WORLD={self.world} \ - docker compose -p robot_{name} -f /home/ws/.devcontainer/docker-compose.yml up gz_ros_bridge --detach --wait" + docker compose -p robot_{name} -f /home/ws/docker/docker-compose.yml up gz_ros_bridge --detach --wait" print(f"Spawn {name} in gazebo simulation.") os.system(spawn) @@ -26,7 +26,7 @@ def delete_robot(self, name): "Deletes the robot from the simulation" os.system( f"sudo WORLD:={self.world} ENTITY_NAME={name} \ - docker compose -p sim_{name} -f /home/ws/.devcontainer/docker-compose.yml up despawn_robot --detach --wait" + docker compose -p sim_{name} -f /home/ws/docker/docker-compose.yml up despawn_robot --detach --wait" ) def get_status(self): @@ -35,7 +35,7 @@ def get_status(self): def start_simulation(self, world): # Start the simulation environment print("Starting simulation environment...") - simulation = f"sudo docker compose -f /home/ws/.devcontainer/docker-compose.yml up simulation --detach --wait" + simulation = f"sudo docker compose -f /home/ws/docker/docker-compose.yml up simulation --detach --wait" print(f"Executing command: {simulation}") os.system(simulation) self.simulation_status = "running" @@ -43,7 +43,7 @@ def start_simulation(self, world): def stop_simulation(self): # Stop the simulation environment print("Stopping simulation environment...") - stop_simulation = f"sudo docker compose -f /home/ws/.devcontainer/docker-compose.yml down simulation --timeout 1" + stop_simulation = f"sudo docker compose -f /home/ws/docker/docker-compose.yml down simulation --timeout 1" print(f"Executing command: {stop_simulation}") os.system(stop_simulation) self.simulation_status = "stopped" diff --git a/src/helmoro_cli/package.xml b/src/helmoro_cli/package.xml index 87e95f7..0f3ab64 100644 --- a/src/helmoro_cli/package.xml +++ b/src/helmoro_cli/package.xml @@ -8,6 +8,7 @@ Apache-2.0 python3-pytest + helmoro_utils ament_python From 47d8d5350358106064af58ef2c9e46f5c76cc9b3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marc=20Bl=C3=B6chlinger?= Date: Wed, 4 Jun 2025 12:55:49 +0000 Subject: [PATCH 24/29] doc(Dockerfile): added comments --- docker/Dockerfile | 2 ++ 1 file changed, 2 insertions(+) diff --git a/docker/Dockerfile b/docker/Dockerfile index abd9b0d..227f101 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -1,5 +1,6 @@ FROM osrf/ros@sha256:f2f03a667950570edcaa14ed414a636912acd44ea9c25c10b5a7c5d44eea5142 AS dev +# NOTE: this updates the GPG key for ros2 apt repository (to be deleted when new ros2 jazzy docker image is released) RUN rm /etc/apt/sources.list.d/ros2-latest.list \ && rm /usr/share/keyrings/ros2-latest-archive-keyring.gpg @@ -33,6 +34,7 @@ RUN apt-get update && apt-get install -y \ curl \ && rm -rf /var/lib/apt/lists/* +# Install Docker Engine RUN curl -fsSL https://get.docker.com -o get-docker.sh && \ sh get-docker.sh && \ rm get-docker.sh From 8ee30b92f65c840de49cd496979bd44e44ffcce7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marc=20Bl=C3=B6chlinger?= Date: Wed, 4 Jun 2025 12:56:15 +0000 Subject: [PATCH 25/29] doc(cli): added a README --- src/helmoro_cli/README.md | 28 ++++++++++++++++++++++++++++ 1 file changed, 28 insertions(+) create mode 100644 src/helmoro_cli/README.md diff --git a/src/helmoro_cli/README.md b/src/helmoro_cli/README.md new file mode 100644 index 0000000..2daf1bd --- /dev/null +++ b/src/helmoro_cli/README.md @@ -0,0 +1,28 @@ +# Helmoro CLI + +The `helmoro_cli` package provides a command-line interface (CLI) for interacting with the Helmoro robotics system. It allows users to manage simulations, spawn and delete robots, and perform other operations directly from the terminal. + +## Features + +- **Simulation Management**: Start and stop simulations with ease. +- **Robot Management**: Spawn and delete robots in the simulation environment. +- **Interactive CLI**: A user-friendly interface with commands for various tasks. +- **Multi-Robot Support**: Includes a helper function for spawning multiple robots during development. + +## Usage + +To launch the CLI, use the following command: + +```bash +ros2 run helmoro_cli open_cli +``` + +Once launched, you will be greeted with a prompt where you can type commands. Use help or ? to list available commands. + +## Commands +* start_simulation : Start the simulation with the specified world (default: empty). +* stop_simulation: Stop the currently running simulation. +* spawn : Spawn a robot with the specified arguments. +* delete : Delete a robot by its name. +* multi_robot_spawn : Helper function for spawning multiple robots (for development purposes). +* exit: Exit the CLI. \ No newline at end of file From 18a916af4c70825b919705e303f72fa5d225ddd3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marc=20Bl=C3=B6chlinger?= Date: Wed, 4 Jun 2025 13:50:58 +0000 Subject: [PATCH 26/29] refactor(description): use lowercase for true and false --- src/helmoro_cli/helmoro_cli/robot_manager.py | 2 +- src/helmoro_description/launch/description.launch.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/helmoro_cli/helmoro_cli/robot_manager.py b/src/helmoro_cli/helmoro_cli/robot_manager.py index 576ad11..f02c860 100644 --- a/src/helmoro_cli/helmoro_cli/robot_manager.py +++ b/src/helmoro_cli/helmoro_cli/robot_manager.py @@ -9,7 +9,7 @@ def __init__(self): def spawn_robot(self, name, x, y, z, yaw): self.robots[name] = {"pos": (x, y, z), "yaw": yaw, "status": "idle"} # Here you would trigger a ROS 2 launch or service call - spawn = f"sudo NAMESPACE={name} USE_SIM_TIME=True \ + spawn = f"sudo NAMESPACE={name} USE_SIM_TIME=true \ docker compose -p robot_{name} -f /home/ws/docker/docker-compose.yml up robot_description --detach --wait" print(f"Start core nodes of {name}") print(f"Executing command: {spawn}") diff --git a/src/helmoro_description/launch/description.launch.py b/src/helmoro_description/launch/description.launch.py index 8ec900e..f9ec93f 100644 --- a/src/helmoro_description/launch/description.launch.py +++ b/src/helmoro_description/launch/description.launch.py @@ -13,7 +13,7 @@ DeclareLaunchArgument( "run_in_simulation", default_value="false", - choices=["true", "false", "True", "False"], + choices=["true", "false"], description="run_in_simulation", ), DeclareLaunchArgument( From 9dbf05f177717cdf1baeac0e9254125f2b2bfc6f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marc=20Bl=C3=B6chlinger?= Date: Wed, 4 Jun 2025 14:32:05 +0000 Subject: [PATCH 27/29] feat(cli): add function to delete all robots --- src/helmoro_cli/helmoro_cli/robot_manager.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/helmoro_cli/helmoro_cli/robot_manager.py b/src/helmoro_cli/helmoro_cli/robot_manager.py index f02c860..45e9a99 100644 --- a/src/helmoro_cli/helmoro_cli/robot_manager.py +++ b/src/helmoro_cli/helmoro_cli/robot_manager.py @@ -17,6 +17,10 @@ def spawn_robot(self, name, x, y, z, yaw): def delete_robot(self, name): "Stops all containers spawned by the robot" + + # To delete all robots, change identifier + if name == "all": name = "robot" + raw_output = ( os.popen( f"sudo docker container ls --filter name={name} --format '{{{{.ID}}}} {{{{.Names}}}}'" From 476a3d8332bd4e19915fc37bb2702c0548412bb8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marc=20Bl=C3=B6chlinger?= Date: Wed, 4 Jun 2025 14:33:27 +0000 Subject: [PATCH 28/29] fix(cli): corrected env input for container starts, solves failing robot deletion inside gazebo --- docker/docker-compose.yml | 4 +++- src/helmoro_cli/helmoro_cli/simulation_manager.py | 4 ++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/docker/docker-compose.yml b/docker/docker-compose.yml index a813650..ef1635a 100644 --- a/docker/docker-compose.yml +++ b/docker/docker-compose.yml @@ -18,7 +18,9 @@ services: simulation: extends: template - command: ros2 launch helmoro_simulation simulation.launch.py + command: bash -c "ros2 launch helmoro_simulation simulation.launch.py world:=$${WORLD}" + environment: + - WORLD spawn_robot: extends: template diff --git a/src/helmoro_cli/helmoro_cli/simulation_manager.py b/src/helmoro_cli/helmoro_cli/simulation_manager.py index f8ed67b..24adc17 100644 --- a/src/helmoro_cli/helmoro_cli/simulation_manager.py +++ b/src/helmoro_cli/helmoro_cli/simulation_manager.py @@ -25,7 +25,7 @@ def spawn_robot(self, name, x, y, z, yaw): def delete_robot(self, name): "Deletes the robot from the simulation" os.system( - f"sudo WORLD:={self.world} ENTITY_NAME={name} \ + f"sudo WORLD={self.world} ENTITY_NAME={name} \ docker compose -p sim_{name} -f /home/ws/docker/docker-compose.yml up despawn_robot --detach --wait" ) @@ -35,7 +35,7 @@ def get_status(self): def start_simulation(self, world): # Start the simulation environment print("Starting simulation environment...") - simulation = f"sudo docker compose -f /home/ws/docker/docker-compose.yml up simulation --detach --wait" + simulation = f"sudo WORLD={world} docker compose -f /home/ws/docker/docker-compose.yml up simulation --detach --wait" print(f"Executing command: {simulation}") os.system(simulation) self.simulation_status = "running" From 95fbbaacfc798891fc929aa11c039d457d1df286 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marc=20Bl=C3=B6chlinger?= Date: Thu, 5 Jun 2025 10:40:25 +0000 Subject: [PATCH 29/29] refactor(utils): change filename for better readability --- src/helmoro_cli/test/test_spawn_delete.py | 2 +- src/helmoro_description/test/launch_description.test.py | 2 +- src/helmoro_simulation/tests/launch_simulation.test.py | 2 +- .../helmoro_utils/{test_collection.py => test_helpers.py} | 2 +- src/helmoro_visualization/tests/launch.test.py | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) rename src/helmoro_utils/helmoro_utils/{test_collection.py => test_helpers.py} (99%) diff --git a/src/helmoro_cli/test/test_spawn_delete.py b/src/helmoro_cli/test/test_spawn_delete.py index 4e99746..9824697 100644 --- a/src/helmoro_cli/test/test_spawn_delete.py +++ b/src/helmoro_cli/test/test_spawn_delete.py @@ -4,7 +4,7 @@ import rclpy from helmoro_cli.robot_manager import RobotManager -from helmoro_utils.test_collection import wait_for_topic +from helmoro_utils.test_helpers import wait_for_topic @pytest.fixture(scope="module") def robot_manager(): diff --git a/src/helmoro_description/test/launch_description.test.py b/src/helmoro_description/test/launch_description.test.py index ba9af7c..ae49113 100644 --- a/src/helmoro_description/test/launch_description.test.py +++ b/src/helmoro_description/test/launch_description.test.py @@ -19,7 +19,7 @@ from launch_testing.io_handler import ActiveIoHandler from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy -from helmoro_utils.test_collection import wait_for_node, wait_for_topic, wait_for_message +from helmoro_utils.test_helpers import wait_for_node, wait_for_topic, wait_for_message NAMESPACE = "robot_namespace" diff --git a/src/helmoro_simulation/tests/launch_simulation.test.py b/src/helmoro_simulation/tests/launch_simulation.test.py index 8c26375..11ba585 100644 --- a/src/helmoro_simulation/tests/launch_simulation.test.py +++ b/src/helmoro_simulation/tests/launch_simulation.test.py @@ -16,7 +16,7 @@ import launch_testing.markers import rosgraph_msgs.msg -from helmoro_utils.test_collection import wait_for_node, wait_for_message +from helmoro_utils.test_helpers import wait_for_node, wait_for_message ARGUMENTS = [ diff --git a/src/helmoro_utils/helmoro_utils/test_collection.py b/src/helmoro_utils/helmoro_utils/test_helpers.py similarity index 99% rename from src/helmoro_utils/helmoro_utils/test_collection.py rename to src/helmoro_utils/helmoro_utils/test_helpers.py index d37e045..27fc77e 100644 --- a/src/helmoro_utils/helmoro_utils/test_collection.py +++ b/src/helmoro_utils/helmoro_utils/test_helpers.py @@ -1,4 +1,4 @@ -# helmoro_utils/test_collection.py +# helmoro_utils/test_helpers.py import time import rclpy diff --git a/src/helmoro_visualization/tests/launch.test.py b/src/helmoro_visualization/tests/launch.test.py index ec0cb80..e9a3eb7 100644 --- a/src/helmoro_visualization/tests/launch.test.py +++ b/src/helmoro_visualization/tests/launch.test.py @@ -13,7 +13,7 @@ import pytest -from helmoro_utils.test_collection import wait_for_node_with_namespace +from helmoro_utils.test_helpers import wait_for_node_with_namespace namespace = "test_robot" ARGUMENTS = [