Implement ros2_control

.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

.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"
 }

.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 "

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.

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

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

src/helmoro_cli/helmoro_cli/main.py

@@ -41,7 +41,7 @@ def do_spawn(self, arg):
         if len(args) != 5:
             print("Usage: spawn <name> <x> <y> <z> <yaw>")
             return
-        self.robot_manager.spawn_robot(*args)
+        self.robot_manager.add_robot(*args)
         self.simulation_manager.spawn_robot(*args)
 
     def do_delete(self, arg):

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")

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,15 +35,15 @@ 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"
 
     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"

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")
+
+