Simple ZED to Husky Pipeline

zed_to_husky_ws/.gitignore

@@ -0,0 +1,12 @@
+.vscode
+
+# ROS2 basic directories
+/build
+/install
+/log
+
+# Gazebo cache
+docker/cache/*
+!docker/cache/.gazebo
+docker/cache/.gazebo/*
+!docker/cache/.gazebo/.gitkeep

zed_to_husky_ws/README.md

@@ -0,0 +1,61 @@
+# zed_to_husky_ws
+
+This repo is utilized to demonstrate a straightforward end-to-end control flow with using ZED and Clearpath Husky.
+
+## 🌱 Structure 🌱
+
+```
+zed_to_husky_ws
+├── workspaces
+|   ├── dummy_controller_ws
+|   ├── husky_ws
+|   └── zed_ws
+├── .gitignore
+└── README.md
+```
+
+Since we aim to reuse the code, the repository has a different structure. It includes a folder named `workspaces`, which contains three workspaces: `husky_ws`, `zed_ws`, and `dummy_controller_ws`. The first two workspaces are almost identical to the original, with only a few differences. You can replace both of them with the latest version if you won't be using Gazebo for simulation. However, if Gazebo simulation is necessary, you may need to redo some tasks, such as reintegrating the ZED URDF into the Husky workspace. For guidance, you can refer to the git commit history.
+
+## 🚩 Testing 🚩
+
+The dummy controller offers two modes: 
+
+1. Utilizing both the left and right images from ZED to control the Husky based on the comparison of brightness levels.
+2. Utilizing depth estimation from ZED and implementing a P controller to regulate the Husky's movement.
+
+For further details, please refer to the file `dummy_controller_ws/src/dummy_controller/scripts/main.py`.
+
+> Before proceeding to the next step, please ensure that all workspaces are built. This entails opening the workspace with Docker and utilizing `colcon build` to compile the workspace.
+
+### Simulate in Gazebo
+
+```bash=
+# In dummy_controller_ws
+ros2 run dummy_controller main.py
+
+# In husky_ws
+ros2 launch husky_gazebo gazebo.launch.py
+```
+
+> Since we've successfully reintegrated ZED into the Husky URDF, there are no additional commands that need to be executed in the `zed_ws` workspace.
+
+After executing the command above, you can add a cube inside Gazebo and move it back and forth in front of the Husky. The dummy controller will then use the observation to control the Husky's behavior.
+
+### Control Real Robot
+
+```bash=
+# In dummy_controller_ws
+ros2 run dummy_controller main.py
+
+# In husky_ws
+cd /home/ros2-agv-essentials/husky_ws
+./udev_rules/install_udev_rules.sh
+./script/husky-generate.sh 
+source ~/.bashrc
+./script/husky-bringup.sh
+
+# In zed_ws
+ros2 launch zed_wrapper zed_camera.launch.py camera_model:=zed2i
+```
+
+> For the setup of the Husky and ZED, please refer to the README file in each respective repository for more detailed instructions.

zed_to_husky_ws/workspaces/dummy_controller_ws/.devcontainer/devcontainer.json

@@ -0,0 +1,25 @@
+/* Reference: https://aka.ms/devcontainer.json */
+{
+    "name": "dummy-controller-ws",
+    "dockerComposeFile": "../docker/compose.yaml",
+    "service": "dummy-controller-ws",
+    // workspace settings
+    "workspaceFolder": "/home/ros2-agv-essentials/dummy_controller_ws",
+    // Vscode extensions
+    "customizations": {
+        "vscode": {
+            "extensions": [
+                "ms-vscode.cpptools",
+                "ms-vscode.cpptools-themes",
+                "twxs.cmake",
+                "donjayamanne.python-extension-pack",
+                "eamodio.gitlens",
+                "mhutchie.git-graph",
+                "streetsidesoftware.code-spell-checker",
+                "ms-iot.vscode-ros"
+            ]
+        }
+    },
+    // Lifecycle scripts
+    "postCreateCommand": "${containerWorkspaceFolder}/.devcontainer/postCreateCommand.sh"
+}

zed_to_husky_ws/workspaces/dummy_controller_ws/.devcontainer/postCreateCommand.sh

@@ -0,0 +1,6 @@
+sudo apt-get update
+sudo rosdep update
+# Note: The following commands are commented out to prevent unintended install/builds.
+# sudo rosdep install --from-paths src --ignore-src -y
+# sudo chown -R user /home/ros2-agv-essentials/
+# colcon build

zed_to_husky_ws/workspaces/dummy_controller_ws/.gitignore

@@ -0,0 +1,12 @@
+.vscode
+
+# ROS2 basic directories
+/build
+/install
+/log
+
+# Gazebo cache
+docker/cache/*
+!docker/cache/.gazebo
+docker/cache/.gazebo/*
+!docker/cache/.gazebo/.gitkeep

zed_to_husky_ws/workspaces/dummy_controller_ws/README.md

@@ -0,0 +1 @@
+# dummy_controller_ws

zed_to_husky_ws/workspaces/dummy_controller_ws/docker/.bashrc

@@ -0,0 +1,5 @@
+# Source global ROS2 environment
+source /opt/ros/$ROS_DISTRO/setup.bash
+# Source workspace environment
+# Note: If you have not built your workspace yet, the following command will fail
+source $ROS2_WS/install/setup.bash

zed_to_husky_ws/workspaces/dummy_controller_ws/docker/.dockerignore

@@ -0,0 +1,2 @@
+*
+!.bashrc

zed_to_husky_ws/workspaces/dummy_controller_ws/docker/Dockerfile

@@ -0,0 +1,52 @@
+# Base Image : https://hub.docker.com/r/osrf/ros/tags?page=1&name=humble
+FROM osrf/ros:humble-desktop-full
+
+LABEL org.opencontainers.image.authors="[email protected]"
+
+ARG USERNAME=user
+ARG USER_UID=1000
+ARG USER_GID=$USER_UID
+
+# Create the user
+RUN groupadd --gid $USER_GID $USERNAME \
+    && useradd --uid $USER_UID --gid $USER_GID -m $USERNAME \
+    #
+    # [Optional] Add sudo support. Omit if you don't need to install software after connecting.
+    && apt-get update \
+    && apt-get install -y sudo \
+    && echo $USERNAME ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$USERNAME \
+    && chmod 0440 /etc/sudoers.d/$USERNAME \
+    && rm -rf /var/lib/apt/lists/*
+RUN apt-get update && apt-get upgrade -y \
+    && rm -rf /var/lib/apt/lists/*
+RUN apt-get update && apt-get install -y python3-pip \
+    && rm -rf /var/lib/apt/lists/*
+ENV SHELL /bin/bash
+
+# ********************************************************
+# * Anything else you want to do like clean up goes here *
+# ********************************************************
+
+# Install common tools
+RUN apt-get update && apt-get install -y \
+    curl \
+    git \
+    git-extras \
+    htop \
+    net-tools \
+    tmux \
+    vim \
+    wget \
+    && rm -rf /var/lib/apt/lists/*
+
+# Install ROS2 RVIZ and Gazebo
+RUN apt-get update && apt-get install -y \
+    ros-$ROS_DISTRO-gazebo-ros-pkgs \
+    ros-$ROS_DISTRO-rviz2 \
+    && rm -rf /var/lib/apt/lists/*
+
+COPY .bashrc /home/$USERNAME/.bashrc
+
+# [Optional] Set the default user. Omit if you want to keep the default as root.
+USER $USERNAME
+CMD ["/bin/bash"]

zed_to_husky_ws/workspaces/dummy_controller_ws/docker/compose.yaml

@@ -0,0 +1,55 @@
+version: '3'
+services:
+  dummy-controller-ws:
+    build: .
+    image: j3soon/ros2-dummy-controller-ws
+    container_name: ros2-dummy-controller-ws
+    stdin_open: true
+    tty: true
+    privileged: true
+    command: /bin/bash
+    network_mode: host
+    working_dir: /home/ros2-agv-essentials/dummy_controller_ws
+    environment:
+      - DISPLAY=${DISPLAY}
+      # Set ros2 environment variables.
+      # References:
+      # - https://docs.ros.org/en/humble/Concepts/Intermediate/About-Domain-ID.html
+      # - https://docs.ros.org/en/humble/Tutorials/Beginner-CLI-Tools/Configuring-ROS2-Environment.html
+      # - https://docs.ros.org/en/humble/Tutorials/Demos/Logging-and-logger-configuration.html#console-output-colorizing
+      # - ROS_LOCALHOST_ONLY=1
+      - ROS_DOMAIN_ID=42
+      - ROS2_WS=/home/ros2-agv-essentials/dummy_controller_ws
+      - RCUTILS_COLORIZED_OUTPUT=1
+      # If you want to access GPU, please uncomment the lines below.
+      # Reference : https://docs.docker.com/compose/gpu-support/
+      # deploy:
+      #   resources:
+      #     reservations:
+      #       devices:
+      #         - driver: nvidia
+      #           count: all
+      #           capabilities: [ gpu ]
+    volumes:
+      # Mount local timezone into container. ( Readonly )
+      # Reference: https://stackoverflow.com/questions/57607381/how-do-i-change-timezone-in-a-docker-container
+      - /etc/timezone:/etc/timezone:ro
+      - /etc/localtime:/etc/localtime:ro
+      # Mount X11 server
+      - /tmp/.X11-unix:/tmp/.X11-unix
+      # Direct Rendering Infrastructure
+      - /dev/dri:/dev/dri
+      # Mount sound card to prevent Gazebo warning.
+      - /dev/snd:/dev/snd
+      # Mount shared memory
+      - /dev/shm:/dev/shm
+      # Mount Gazebo models directory to reuse models downloaded during first launch.
+      # Reference: https://answers.ros.org/question/365658
+      - ./cache/.gazebo:/home/user/.gazebo
+      # Mounting the following directories will forbid direct deletion.
+      # Consider mount these directories only if the build process is slow.
+      # "source=${localWorkspaceFolder}/../cache/humble/build,target=/home/ws/build,type=bind",
+      # "source=${localWorkspaceFolder}/../cache/humble/install,target=/home/ws/install,type=bind",
+      # "source=${localWorkspaceFolder}/../cache/humble/log,target=/home/ws/log,type=bind"
+      # Mount workspace
+      - ../..:/home/ros2-agv-essentials

zed_to_husky_ws/workspaces/dummy_controller_ws/src/dummy_controller/CMakeLists.txt

@@ -0,0 +1,44 @@
+cmake_minimum_required(VERSION 3.8)
+project(dummy_controller)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+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)
+find_package(ament_cmake_python REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(sensor_msgs REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+# Install Python modules
+ament_python_install_package(${PROJECT_NAME})
+
+# Install Python executables
+install(PROGRAMS
+  scripts/main.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+ament_package()

zed_to_husky_ws/workspaces/dummy_controller_ws/src/dummy_controller/dummy_controller/publisher.py

@@ -0,0 +1,55 @@
+# Copyright 2016 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.
+
+import rclpy
+from rclpy.node import Node
+
+from geometry_msgs.msg import Twist
+
+
+class CmdVelPublisher(Node):
+
+    def __init__(self, topic_name: str, node_name="cmd_vel_publisher"):
+        super().__init__(node_name)
+
+        self.cmd_vel_speed = 0.5 # m/s
+
+        self.publisher_ = self.create_publisher(Twist, topic_name, 10)
+        self.status = "stop"
+        self.msg = Twist()
+
+    def forward(self, speed):
+        self.status = "forward"
+        self.msg.linear.y = self.msg.linear.z = 0.0
+        self.msg.angular.x = self.msg.angular.y = self.msg.angular.z = 0.0
+        self.msg.linear.x = speed
+
+        self.publish()
+        self.get_logger().info('Publishing speed: %lf' % speed)
+
+    def update_status(self):
+        self.msg.linear.x = self.msg.linear.y = self.msg.linear.z = 0.0
+        self.msg.angular.x = self.msg.angular.y = self.msg.angular.z = 0.0
+
+        if self.status == "right":
+            self.msg.angular.z = self.cmd_vel_speed
+        elif self.status == "left":
+            self.msg.angular.z = -self.cmd_vel_speed
+
+        self.publish()
+        self.get_logger().info('Publishing: "%s"' % self.status)
+
+    def publish(self):
+        self.publisher_.publish(self.msg)
+