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 61% rename from config/append_to_bashrc.txt rename to .devcontainer/append_to_bashrc.txt index d3d43a2..dcc9f26 100644 --- a/config/append_to_bashrc.txt +++ b/.devcontainer/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 diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 4b088d4..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": { @@ -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" + "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 72745cf..202f24a 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -29,8 +29,8 @@ jobs: uses: docker/build-push-action@v6 with: context: . - file: .devcontainer/Dockerfile - tags: ghcr.io/helbling-technik/helmoro-software-ros2:${{ github.ref_name }}-latest + file: docker/Dockerfile + tags: helmoro load: true cache-from: | type=gha @@ -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 --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 @@ -51,17 +51,19 @@ 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 " + docker exec -e DISPLAY=$DISPLAY -e ROS_DOMAIN_ID=$ROS_DOMAIN_ID test_container bash -c " source ./entrypoint.sh && - colcon test && + colcon test --executor sequential && colcon test-result --verbose " - 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 diff --git a/.devcontainer/Dockerfile b/docker/Dockerfile similarity index 72% rename from .devcontainer/Dockerfile rename to docker/Dockerfile index f1987d6..227f101 100644 --- a/.devcontainer/Dockerfile +++ b/docker/Dockerfile @@ -1,5 +1,18 @@ -FROM ros@sha256:9749355a0760334fa5ea3c660f2de1ecc5e8a5af8047e4bbaa9afa8a7843da80 AS dev +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 + +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 * # ******************************************************** @@ -18,8 +31,14 @@ 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/* +# Install Docker Engine +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 \ @@ -78,7 +97,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"] @@ -94,7 +113,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 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/docker/docker-compose.yml b/docker/docker-compose.yml new file mode 100644 index 0000000..ef1635a --- /dev/null +++ b/docker/docker-compose.yml @@ -0,0 +1,56 @@ +services: + template: + 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: template + command: python3 src/helmoro_cli/helmoro_cli/main.py + volumes: + - /var/run/docker.sock:/var/run/docker.sock + + simulation: + extends: template + command: bash -c "ros2 launch helmoro_simulation simulation.launch.py world:=$${WORLD}" + environment: + - WORLD + + spawn_robot: + 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 + - X + - Y + - Z + - YAW + + despawn_robot: + 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: template + command: bash -c "ros2 launch helmoro_simulation ros_gz_bridge.launch.py namespace:=$${NAMESPACE} word:=$${WORLD}" + environment: + - NAMESPACE + - WORLD + + robot_description: + extends: + service: template + 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/.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/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/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 diff --git a/src/helmoro_cli/helmoro_cli/__init__.py b/src/helmoro_cli/helmoro_cli/__init__.py new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/src/helmoro_cli/helmoro_cli/__init__.py @@ -0,0 +1 @@ + diff --git a/src/helmoro_cli/helmoro_cli/main.py b/src/helmoro_cli/helmoro_cli/main.py new file mode 100644 index 0000000..6a4575b --- /dev/null +++ b/src/helmoro_cli/helmoro_cli/main.py @@ -0,0 +1,95 @@ +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 + + # 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 -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: + 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..45e9a99 --- /dev/null +++ b/src/helmoro_cli/helmoro_cli/robot_manager.py @@ -0,0 +1,68 @@ +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/docker/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): + "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}}}}'" + ) + .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 && sudo docker container rm {container_id} > /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..24adc17 --- /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/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/docker/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/docker/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 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" + + def stop_simulation(self): + # Stop the simulation environment + print("Stopping simulation environment...") + 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 new file mode 100644 index 0000000..0f3ab64 --- /dev/null +++ b/src/helmoro_cli/package.xml @@ -0,0 +1,16 @@ + + + + helmoro_cli + 0.0.0 + Command line interface for Helmoro + Marc Blöchlinger + Apache-2.0 + + python3-pytest + helmoro_utils + + + 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_cli/test/test_spawn_delete.py b/src/helmoro_cli/test/test_spawn_delete.py new file mode 100644 index 0000000..9824697 --- /dev/null +++ b/src/helmoro_cli/test/test_spawn_delete.py @@ -0,0 +1,46 @@ +import time +import pytest +import os +import rclpy + +from helmoro_cli.robot_manager import RobotManager +from helmoro_utils.test_helpers import wait_for_topic + +@pytest.fixture(scope="module") +def robot_manager(): + print("Creating RobotManager instance...") + return RobotManager() + + +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...") + 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...") + + 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") + + rclpy.shutdown() 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..ae49113 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_helpers 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/launch/ros_gz_bridge.launch.py b/src/helmoro_simulation/launch/ros_gz_bridge.launch.py new file mode 100644 index 0000000..a638771 --- /dev/null +++ b/src/helmoro_simulation/launch/ros_gz_bridge.launch.py @@ -0,0 +1,248 @@ +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 + # 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", + 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 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..11ba585 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_helpers 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_helpers.py b/src/helmoro_utils/helmoro_utils/test_helpers.py new file mode 100644 index 0000000..27fc77e --- /dev/null +++ b/src/helmoro_utils/helmoro_utils/test_helpers.py @@ -0,0 +1,108 @@ +# helmoro_utils/test_helpers.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..e9a3eb7 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_helpers 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 + )