diff --git a/.devcontainer/append_to_bashrc.txt b/.devcontainer/append_to_bashrc.txt index 6fc9bf6..7c3c338 100644 --- a/.devcontainer/append_to_bashrc.txt +++ b/.devcontainer/append_to_bashrc.txt @@ -1,7 +1,8 @@ # Custom Alias alias src='source /opt/ros/jazzy/setup.bash && source /home/ws/install/setup.bash' alias build='/home/ws/docker/build.sh' -alias test='build && sudo /home/act -j test-locally' +alias test='sudo /home/act -j test-locally' +alias clc_test='build && colcon build --symlink-install && colcon test --executor sequential' # Integrate entrypoint.sh source /home/ws/docker/entrypoint.sh \ No newline at end of file diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 7780743..365b215 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -49,5 +49,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/ && sudo chown -R $(whoami) /var/run/docker.sock && python3 /home/ws/.devcontainer/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-local.yml b/.github/workflows/ci-local.yml index 462b376..b8ff418 100644 --- a/.github/workflows/ci-local.yml +++ b/.github/workflows/ci-local.yml @@ -32,7 +32,7 @@ jobs: echo "Starting virtual display (Xvfb)..." Xvfb :99 -screen 0 1024x768x24 & export DISPLAY=:99 - export ROS_DOMAIN_ID=42 + export ROS_DOMAIN_ID=50 echo "Start testing..." docker exec -e DISPLAY=$DISPLAY -e ROS_DOMAIN_ID=$ROS_DOMAIN_ID ${{ env.CONTAINER_NAME }} bash -c " diff --git a/README.md b/README.md index da8a154..8259c92 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,85 @@ # HelMoRo-software-ROS2 -Welcome to our HelMoRo repository! -We moved all our documentation to our [website](https://helbling-technik.github.io/HelMoRo/), to remove redundant and/ or conflicting information. + +Welcome to the microservices branch! This branch provides a modular and extensible software stack for robotics applications using ROS 2. The repository includes tools for simulation, control, visualization, and command-line interaction with robots. + +## Roadmap +### Current Features +* **Simulation**: Manage simulation environments with Gazebo and ROS 2 integration. +* **Control**: Implement robot joint control using ros2_control and Gazebo plugins. +* **Visualization**: Visualize robot states and environments using RViz2. +* **Command-Line Interface**: Interact with the system via a user-friendly CLI. +* **Multi-Robot Support**: Built for managing multiple robots in simulation. + +### Planned Features +* **State Estimation**: Sensor fusion for accurate odometry. +* **SLAM**: Self-localization and mapping in a dynamic environment. +* **Teleopration**: Steer your robots with joysticks. +* **Navigation**: Autonomous navigation through unknown environments. + + +## Repository Overview +* **`.devcontainer`**: Development container configuration for VS Code. +* **`.github`**: Configurations for CI pipeline using GitHub Actions. +* **`docker`**: Docker setup for development and deployment. +* **`config`**: Configuration files for middleware and other tools. +* **`src/ helmoro_cli`**: Command-line interface for managing simulations and robots. +* **`src/ helmoro_control`**: ROS 2 package for controlling robot joints and states. +* **`src/ helmoro_description`**: Robot description files (URDF/Xacro) +* **`src/ helmoro_simulation`**: Tools and configurations for simulation environments. +* **`src/ helmoro_utils`**: Utility scripts and tools for the project +* **`src/ helmoro_visualization`**: Visualization tools and RViz configurations. + +## Installation + +### Prerequisites +- Linux OS +- Docker +- VSCode with Devcontainer Extension + +### Setup +1. Clone the repository and check out the microservices branch: + ```bash +git clone https://github.com/helbling-technik/HelMoRo-software-ROS2.git +git checkout microservices + ``` + +2. Open the repo with VSCode: + ```bash +code HelMoRo-software-ROS2 + ``` + +3. Start the Devcontainer through VSCode's command palette: + ``` + Dev Containers: Reopen in Container + ``` + +### Running the Simulation +For running the simulation it's recommended to use the [CLI](/src/helmoro_cli/README.md). Start the CLI with: +```bash +ros2 run helmoro_cli open_cli +``` + +The simulation can be started with: +```bash +start_simulation depot +``` + +Robots can be spawned with: +```bash +multi_robot_spawn +``` + +## Contributing +Contributions are welcome! Create an issue or resolve an existing one. For new contributors, we recommend tackling one of the issues labeled with "good first issue", as they do not require a complete understanding of the repo. + +### Testing +For local testing type `test` in your terminal. This will start up the act container. On the first startup, you'll be asked to choose a default image to use with Act, and choose the medium one. Until now that one had all the utilities we needed. During development, the alias `clc_test` is also useful. Instead of creating a containerized environment similar to the tests running in GitHub runners, it runs the tests directly in the Devcontainer. This leads to quicker testing, but may not catch all bugs. + +### Pull Request +When starting out create a draft pull request, so others know that you're working on the issue. On completion of a successful local test push to your dev branch and change your draft pull request to a normal one. + +### Commit Messages +By following a unified commit message standard, we are able to trace bugs quickly. Please adhere to the [Conventional Commits](https://www.conventionalcommits.org/en/v1.0.0/) specification. + +## License +This project is licensed under the BSD 3-Clause License. See the [LICENSE](./LICENSE) file for details. \ No newline at end of file diff --git a/docker/Dockerfile b/docker/Dockerfile index c68dd39..6d855cc 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -54,6 +54,7 @@ RUN apt-get update && apt-get install -y \ ros-jazzy-ros-gz \ ros-jazzy-ros-gz-sim \ ros-jazzy-ros2-control \ + ros-jazzy-gz-ros2-control \ ros-jazzy-diff-drive-controller \ ros-jazzy-joint-state-broadcaster \ ros-jazzy-rviz2 \ @@ -98,7 +99,11 @@ RUN groupadd --gid $USER_GID $USERNAME \ && apt-get update \ && apt-get install -y sudo \ && echo $USERNAME ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$USERNAME \ - && chmod 0440 /etc/sudoers.d/$USERNAME + && chmod 0440 /etc/sudoers.d/$USERNAME \ + \ + # Give user privilege to start docker without sudo + && usermod -aG systemd-network $USERNAME + ENV SHELL=/bin/bash # Define entrypoint diff --git a/docker/docker-compose.yml b/docker/docker-compose.yml index ef1635a..d0dd30b 100644 --- a/docker/docker-compose.yml +++ b/docker/docker-compose.yml @@ -4,7 +4,7 @@ services: image: helmoro environment: - DISPLAY=${DISPLAY} - - ROS_DOMAIN_ID=42 + - ROS_DOMAIN_ID=${ROS_DOMAIN_ID} network_mode: host ipc: host volumes: @@ -41,7 +41,7 @@ services: gz_ros_bridge: extends: template - command: bash -c "ros2 launch helmoro_simulation ros_gz_bridge.launch.py namespace:=$${NAMESPACE} word:=$${WORLD}" + command: bash -c "ros2 launch helmoro_simulation ros_gz_bridge.launch.py namespace:=$${NAMESPACE} world:=$${WORLD}" environment: - NAMESPACE - WORLD @@ -54,3 +54,10 @@ services: - NAMESPACE - USE_SIM_TIME + ros2_control: + extends: + service: template + command: bash -c "ros2 launch helmoro_control control.launch.py namespace:=$${NAMESPACE} run_in_simulation:=$${USE_SIM_TIME}" + environment: + - NAMESPACE + - USE_SIM_TIME diff --git a/src/helmoro_cli/helmoro_cli/main.py b/src/helmoro_cli/helmoro_cli/main.py index 6a4575b..e0a6c8f 100644 --- a/src/helmoro_cli/helmoro_cli/main.py +++ b/src/helmoro_cli/helmoro_cli/main.py @@ -41,7 +41,7 @@ def do_spawn(self, arg): if len(args) != 5: print("Usage: spawn ") return - self.robot_manager.spawn_robot(*args) + self.robot_manager.add_robot(*args) self.simulation_manager.spawn_robot(*args) def do_delete(self, arg): diff --git a/src/helmoro_cli/helmoro_cli/robot_manager.py b/src/helmoro_cli/helmoro_cli/robot_manager.py index 45e9a99..8f52220 100644 --- a/src/helmoro_cli/helmoro_cli/robot_manager.py +++ b/src/helmoro_cli/helmoro_cli/robot_manager.py @@ -6,14 +6,15 @@ class RobotManager: def __init__(self): self.robots = {} - def spawn_robot(self, name, x, y, z, yaw): + def add_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"NAMESPACE={name} USE_SIM_TIME=true \ docker compose -p robot_{name} -f /home/ws/docker/docker-compose.yml up robot_description --detach --wait" + ros2_control = f"NAMESPACE={name} USE_SIM_TIME=true \ + docker compose -p robot_{name}_control -f /home/ws/docker/docker-compose.yml up ros2_control --detach --wait" print(f"Start core nodes of {name}") - print(f"Executing command: {spawn}") os.system(spawn) + os.system(ros2_control) def delete_robot(self, name): "Stops all containers spawned by the robot" @@ -23,7 +24,7 @@ def delete_robot(self, name): raw_output = ( os.popen( - f"sudo docker container ls --filter name={name} --format '{{{{.ID}}}} {{{{.Names}}}}'" + f"docker container ls --filter name={name} --format '{{{{.ID}}}} {{{{.Names}}}}'" ) .read() .strip() @@ -36,7 +37,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 && sudo docker container rm {container_id} > /dev/null" + f"docker container stop {container_id} --timeout 0 && docker container rm {container_id} > /dev/null" ) if result == 0: print(f"{container_name} Stopped") diff --git a/src/helmoro_cli/helmoro_cli/simulation_manager.py b/src/helmoro_cli/helmoro_cli/simulation_manager.py index 24adc17..dfb4c53 100644 --- a/src/helmoro_cli/helmoro_cli/simulation_manager.py +++ b/src/helmoro_cli/helmoro_cli/simulation_manager.py @@ -11,10 +11,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/docker/docker-compose.yml up spawn_robot" + spawn = f"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 --detach --wait" - ros_gz_bridge = f"sudo NAMESPACE={name} WORLD={self.world} \ + ros_gz_bridge = f"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.") @@ -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"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 WORLD={world} docker compose -f /home/ws/docker/docker-compose.yml up simulation --detach --wait" + simulation = f"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" @@ -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/docker/docker-compose.yml down simulation --timeout 1" + stop_simulation = f"docker compose -f /home/ws/docker/docker-compose.yml down simulation --timeout 0" print(f"Executing command: {stop_simulation}") os.system(stop_simulation) self.simulation_status = "stopped" diff --git a/src/helmoro_cli/test/test_simple_scenario.py b/src/helmoro_cli/test/test_simple_scenario.py new file mode 100644 index 0000000..f6246ed --- /dev/null +++ b/src/helmoro_cli/test/test_simple_scenario.py @@ -0,0 +1,95 @@ +import time +import pytest +import rclpy + +from helmoro_cli.robot_manager import RobotManager +from helmoro_cli.simulation_manager import SimulationManager +from helmoro_utils.test_helpers import wait_for_topic, wait_for_node_with_namespace, wait_for_goal_reached +from helmoro_utils.publishers import publish_twist_stamped + +@pytest.fixture(scope="module") +def robot_manager(): + """Fixture to initialize and return a RobotManager instance.""" + print("[Fixture] Creating RobotManager instance") + return RobotManager() + +@pytest.fixture(scope="module") +def simulation_manager(): + """Fixture to initialize and return a SimulationManager instance.""" + print("[Fixture] Creating SimulationManager instance") + return SimulationManager() + +def test_simple_scenario(robot_manager, simulation_manager): + """Integration test for spawning, controlling, and deleting a robot.""" + rclpy.init() + node = rclpy.create_node("test_node") + + world = "empty" + robot_name = "testbot" + + print("[Action] Starting simulation...") + simulation_manager.start_simulation(world) + + print(f"[Action] Spawning robot: {robot_name}") + robot_manager.add_robot(robot_name, 0, 0, 0, 0) + simulation_manager.spawn_robot(robot_name, 0, 0, 0, 0) + + print("[Test] Checking for robot_description topic...") + wait_for_topic(node, f"/{robot_name}/robot_description", timeout=10.0) + print("[Result] Robot description topic is active") + + print("[Test] Checking for control nodes...") + wait_for_node_with_namespace(node, "joint_state_broadcaster", f"/{robot_name}", timeout=10.0) + wait_for_node_with_namespace(node, "diff_drive_controller", f"/{robot_name}", timeout=10.0) + print("[Results] Control nodes are active") + + print("[Action] Sending move command to the robot...") + publish_twist_stamped( + node, f"/{robot_name}/cmd_vel", x_vel=1.0, rot_vel=0.0) + + print("[Test] Waiting for robot to reach the goal position...") + wait_for_goal_reached(node, robot_name, x=1.0, y=0.0, timeout=10.0) + + print("[Action] Sending stop command to the robot...") + publish_twist_stamped( + node, f"/{robot_name}/cmd_vel", x_vel=0.0, rot_vel=0.0) + + print("[Test] Verifying robot stopped...") + print("[Result] Robot control commands were successfully sent and executed") + + print(f"[Action] Deleting robot: {robot_name}") + robot_manager.delete_robot(robot_name) + + print("[Test] Verifying robot deletion...") + timeout = 2.0 + start_time = time.time() + while time.time() - start_time < timeout: + if robot_name not in robot_manager.get_robot_names(): + break + time.sleep(0.1) + + remaining_robots = robot_manager.get_robot_names() + assert robot_name not in remaining_robots, ( + f"Robot '{robot_name}' was not properly deleted. Remaining robots: {remaining_robots}" + ) + print(f"[Result] Robot '{robot_name}' deleted successfully") + + print("[Action] Stopping simulation...") + simulation_manager.stop_simulation() + + rclpy.shutdown() + +def test_cleanup(robot_manager, simulation_manager): + """Test to ensure RobotManager cleans up properly.""" + print("[Action] Cleaning up test...") + robot_manager.delete_robot("all") + + remaining_robots = robot_manager.get_robot_names() + assert not remaining_robots, ( + f"RobotManager cleanup failed. Remaining robots: {remaining_robots}" + ) + + simulation_manager.stop_simulation() + print("[Result] RobotManager cleaned up successfully") + + \ No newline at end of file diff --git a/src/helmoro_cli/test/test_spawn_delete.py b/src/helmoro_cli/test/test_spawn_delete.py deleted file mode 100644 index 9824697..0000000 --- a/src/helmoro_cli/test/test_spawn_delete.py +++ /dev/null @@ -1,46 +0,0 @@ -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_control/CMakeLists.txt b/src/helmoro_control/CMakeLists.txt new file mode 100644 index 0000000..a181813 --- /dev/null +++ b/src/helmoro_control/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 3.8) +project(helmoro_control) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) + +install( + DIRECTORY config launch test + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(launch_testing_ament_cmake) + add_launch_test(test/launch_control.test.py) +endif() + +ament_package() diff --git a/src/helmoro_control/LICENSE b/src/helmoro_control/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/src/helmoro_control/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_control/config/helmoro_controller.yaml b/src/helmoro_control/config/helmoro_controller.yaml new file mode 100644 index 0000000..c8c167a --- /dev/null +++ b/src/helmoro_control/config/helmoro_controller.yaml @@ -0,0 +1,68 @@ +/**: + ros__parameters: + update_rate: 100 # Hz + hardware_plugin: gz_ros2_control/GazeboSimSystem + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + diff_drive_controller: + type: diff_drive_controller/DiffDriveController + +/**/diff_drive_controller: + ros__parameters: + left_wheel_names: ["wheel_left_front_joint", "wheel_left_back_joint"] + right_wheel_names: ["wheel_right_front_joint", "wheel_right_back_joint"] + + wheel_separation: 0.185 + wheels_per_side: 1 + wheel_radius: 0.045 + + wheel_separation_multiplier: 1.0 + left_wheel_radius_multiplier: 1.0 + right_wheel_radius_multiplier: 1.0 + + publish_rate: 60.0 + odom_frame_id: wheel_odom + base_frame_id: base_link + pose_covariance_diagonal : [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] + twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01] + + open_loop: true + position_feedback: false + + # Publishes transform between odom_frame_id and base_frame_id + enable_odom_tf: false + + cmd_vel_timeout: 1.0 + publish_limited_velocity: true + use_stamped_vel: true + velocity_rolling_window_size: 10 + + # Publishing rate (Hz) of the odometry and TF message + publish_rate: 50.0 + + # Velocity and acceleration limits + # Whenever a min_* is unspecified, default to -max_* + linear: + x: + max_velocity: 1.3 + min_velocity: -0.5 + max_acceleration: 2.0 + max_deceleration: -2.0 + max_acceleration_reverse: .NAN + max_deceleration_reverse: .NAN + max_jerk: .NAN + min_jerk: .NAN + + angular: + z: + max_velocity: 6.2 + min_velocity: .NAN + max_acceleration: 40.0 + max_deceleration: -40.0 + max_acceleration_reverse: .NAN + max_deceleration_reverse: .NAN + max_jerk: .NAN + min_jerk: .NAN + diff --git a/src/helmoro_control/launch/control.launch.py b/src/helmoro_control/launch/control.launch.py new file mode 100644 index 0000000..e4a2f60 --- /dev/null +++ b/src/helmoro_control/launch/control.launch.py @@ -0,0 +1,51 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, RegisterEventHandler +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from launch.event_handlers import OnProcessExit + +ARGUMENTS = [ + DeclareLaunchArgument('use_sim_time', default_value='true', + choices=['true', 'false'], description='Use sim time.'), + DeclareLaunchArgument('namespace', default_value='example_robot_name', + description='Robot namespace'), +] + +def generate_launch_description(): + # Launch Description + joint_state_broadcaster = Node( + package='controller_manager', + executable='spawner', + namespace=LaunchConfiguration('namespace'), + arguments=['joint_state_broadcaster', + '-c', 'controller_manager', + '--switch-timeout', '30.0'] + ) + + diff_drive_base_controller = Node( + package='controller_manager', + executable='spawner', + namespace=LaunchConfiguration('namespace'), + arguments=['diff_drive_controller', + '-c', 'controller_manager', + '--switch-timeout', '30.0', + "--controller-ros-args", + "-r ~/cmd_vel:=cmd_vel"] + ) + + # Delay start of robot_controller after `joint_state_broadcaster` + start_diff_drive_controller_after_joint_state_broadcaster = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_broadcaster, + on_exit=[diff_drive_base_controller], + ) + ) + + # Create launch description and add actions + ld = LaunchDescription(ARGUMENTS) + ld.add_action(joint_state_broadcaster) + ld.add_action(start_diff_drive_controller_after_joint_state_broadcaster) + return ld + + \ No newline at end of file diff --git a/src/helmoro_control/package.xml b/src/helmoro_control/package.xml new file mode 100644 index 0000000..aca4b9f --- /dev/null +++ b/src/helmoro_control/package.xml @@ -0,0 +1,27 @@ + + + + helmoro_control + 0.0.0 + Controls the joints + Marc Blöchlinger + Apache-2.0 + + ament_cmake + + ros2_control + gz_ros2_control + diff_drive_controller + joint_state_broadcaster + + ament_cmake_ros + launch + launch_testing + launch_testing_ament_cmake + rclpy + helmoro_utils + + + ament_cmake + + diff --git a/src/helmoro_control/test/launch_control.test.py b/src/helmoro_control/test/launch_control.test.py new file mode 100644 index 0000000..81d6cf7 --- /dev/null +++ b/src/helmoro_control/test/launch_control.test.py @@ -0,0 +1,63 @@ +import time +import unittest +import rclpy + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, TimerAction +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import PathJoinSubstitution + +from launch_testing.actions import ReadyToTest + +import pytest + +from helmoro_utils.test_helpers import wait_for_node_with_namespace + +namespace = "test_robot" +ARGUMENTS = [ + ("use_sim_time", "true"), + ("namespace", namespace), +] + + +@pytest.mark.launch_test +def generate_test_description(): + """Generate a LaunchDescription for the test.""" + control_dir = get_package_share_directory("helmoro_control") + control_path = PathJoinSubstitution( + [control_dir, "launch", "control.launch.py"] + ) + + # Include the launch description with arguments + launch_control = IncludeLaunchDescription( + PythonLaunchDescriptionSource([control_path]), + launch_arguments=ARGUMENTS, + ) + + ready_to_test = TimerAction(period=0.5, actions=[ReadyToTest()]) + + ld = LaunchDescription() + ld.add_action(ready_to_test) + ld.add_action(launch_control) + + return ld + +class TestProcess(unittest.TestCase): + + def setUp(self): + """Initialize the ROS node before each test.""" + rclpy.init() + self.node = rclpy.create_node("test_node") + + def tearDown(self): + """Shut down the ROS node after each test.""" + self.node.destroy_node() + rclpy.shutdown() + + def test_spawner(self): + """Check if node is running in the expected namespace.""" + wait_for_node_with_namespace( + self.node, "spawner_joint_state_broadcaster", f"/{namespace}", timeout=3.0 + ) diff --git a/src/helmoro_description/urdf/helmoro.urdf.xacro b/src/helmoro_description/urdf/helmoro.urdf.xacro index 701d19f..4daf5df 100644 --- a/src/helmoro_description/urdf/helmoro.urdf.xacro +++ b/src/helmoro_description/urdf/helmoro.urdf.xacro @@ -1,6 +1,6 @@ - + diff --git a/src/helmoro_description/urdf/plugins.urdf.xacro b/src/helmoro_description/urdf/plugins.urdf.xacro index 3df14d1..0599a8d 100644 --- a/src/helmoro_description/urdf/plugins.urdf.xacro +++ b/src/helmoro_description/urdf/plugins.urdf.xacro @@ -5,8 +5,8 @@ - - + + $(find helmoro_control)/config/helmoro_controller.yaml ${namespace} /robot_description:=robot_description diff --git a/src/helmoro_utils/helmoro_utils/publishers.py b/src/helmoro_utils/helmoro_utils/publishers.py new file mode 100644 index 0000000..f0a197c --- /dev/null +++ b/src/helmoro_utils/helmoro_utils/publishers.py @@ -0,0 +1,54 @@ +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import TwistStamped +from rclpy.qos import QoSProfile + +def publish_twist_stamped( + node: Node, + topic_name: str, + x_vel: float, + rot_vel: float, + duration: float = 1.0, + frequency: int = 10, + qos_profile: QoSProfile = QoSProfile(depth=10) +): + """ + Publishes a TwistStamped message at a specified frequency for a set duration. + + This function is non-blocking: it uses a ROS 2 timer to periodically publish + the message without blocking the main event loop. + + Args: + node (Node): The ROS 2 node that owns the publisher and timer. + topic_name (str): The name of the topic to publish to (e.g., '/robot/cmd_vel'). + x_vel (float): Linear velocity in the x-direction (forward/backward). + rot_vel (float): Angular velocity around the z-axis (yaw). + duration (float): Duration (in seconds) to publish the message. + frequency (int, optional): Publishing rate in Hz. Defaults to 10. + qos_profile (QoSProfile, optional): QoS profile for the publisher. Defaults to QoSProfile(depth=10). + + Example: + publish_twist_stamped(node, '/robot/cmd_vel', 1.0, 0.2, duration=2.0) + """ + pub = node.create_publisher(TwistStamped, topic_name, qos_profile) + + # Prepare the constant part of the message + msg = TwistStamped() + msg.header.frame_id = 'base_link' + msg.twist.linear.x = x_vel + msg.twist.angular.z = rot_vel + + # Timer setup + count = 0 + max_count = int(frequency * duration) + period = 1.0 / frequency + + def timer_callback(): + nonlocal count + msg.header.stamp = node.get_clock().now().to_msg() + pub.publish(msg) + count += 1 + if count >= max_count: + timer.cancel() + + timer = node.create_timer(period, timer_callback) \ No newline at end of file diff --git a/src/helmoro_utils/helmoro_utils/test_helpers.py b/src/helmoro_utils/helmoro_utils/test_helpers.py index 27fc77e..51ca6bf 100644 --- a/src/helmoro_utils/helmoro_utils/test_helpers.py +++ b/src/helmoro_utils/helmoro_utils/test_helpers.py @@ -6,7 +6,7 @@ from rosidl_runtime_py.utilities import get_message import std_msgs.msg import rosgraph_msgs.msg -from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy +from rclpy.qos import QoSProfile def wait_for_node(node, node_name: str, timeout: float = 10.0): @@ -72,7 +72,6 @@ def wait_for_topic(node, topic_name: str, timeout: float = 10.0): 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. @@ -90,7 +89,22 @@ def wait_for_topic(node, topic_name: str, timeout: float = 10.0): 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.""" + """ + Waits for a message to be received on the specified topic within a timeout period. + + Args: + node (Node): The rclpy Node used to create the subscription and spin. + topic_name (str): The full name of the topic to subscribe to (e.g., '/robot1/cmd_vel'). + msg_type_str (str): The string type of the message (e.g., 'geometry_msgs/msg/Twist'). + timeout (float, optional): Maximum time to wait in seconds. Defaults to 10.0. + qos_profile (QoSProfile, optional): QoS profile to use for the subscription. Defaults to 10 (depth-based). + + Returns: + msg: The first message received on the topic. + + Raises: + AssertionError: If no message is received before the timeout expires. + """ received_msg = [] def callback(msg): @@ -106,3 +120,34 @@ def callback(msg): node.destroy_subscription(sub) assert received_msg, f"No message received on {topic_name} within {timeout} seconds." + return received_msg[-1] + +def wait_for_goal_reached(node: Node, robot_name: str, x: float, y: float, timeout: float = 10.0): + """ + Waits until the specified robot reaches the given (x, y) goal position based on its Odometry. + + Args: + node (Node): rclpy Node used to receive messages. + robot_name (str): Robot name used to resolve the odometry topic (e.g., 'robot1'). + x (float): Target x-coordinate. + y (float): Target y-coordinate. + timeout (float): Maximum time to wait for the robot to reach the goal (in seconds). + tolerance (float): Distance threshold to consider the goal reached. Defaults to 0.1 meters. + + Raises: + AssertionError: If the goal is not reached within the timeout. + """ + topic_name = f"/{robot_name}/diff_drive_controller/odom" + msg_type = get_message("nav_msgs/msg/Odometry") + + start = time.time() + while time.time() - start < timeout: + odom_msg = wait_for_message(node, topic_name, msg_type, timeout=5.0) + + # Check if the robot is at the goal position + pos = odom_msg.pose.pose.position + + if (abs(pos.x - x) < 0.1 and abs(pos.y - y) < 0.1): + return + print(f"Robot {robot_name} at position ({pos.x}, {pos.y})") + raise AssertionError(f"Robot {robot_name} did not reach goal ({x}, {y}) within {timeout} seconds. Robot is currently at position ({pos.x}, {pos.y}).") \ No newline at end of file