8

.gitignore

@@ -27,3 +27,5 @@ qtcreator-*
 # Colcon custom files
 COLCON_IGNORE
 AMENT_IGNORE
+
+.venv

LICENSE

@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2025 OxRAM Society
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

README.md

@@ -0,0 +1,58 @@
+# RobotArmPackages
+ROS Packages related to the OxRAM Robot Arm project.
+
+To use, `git clone --recursive https://github.com/OxRAMSociety/RobotArmPackages2.git`. Use the newly created folder as a workspace.
+
+## Installation:
+We recommend that you use Ubuntu 24.04 with ROS2 Jazzy Jalisco
+
+1. Install Ubuntu 24.04 (we recommend using a VM, such as [VirtualBox](https://www.virtualbox.org/))
+2. If you do not have ROS, run `./install_ros.sh` from the terminal. This script is designed to not break if run multiple times.
+3. Reboot
+4. Follow README.md for packages you will be running/developing. For example, you will need the camera configuration files sorted out to be able to compile `computer_vison`
+5. Then, run `./install_workspace.sh` at the root of this repository. This script is designed to not break if it is run multiple times
+6. (Optional) If you want to run computer vision, install libraries using `./install_cv_packages.sh`
+
+## Usage
+Run `source ./setup.sh` when first opening in a new terminal
+
+Run `source ./rebuild.sh` in the terminal to rebuild code
+
+Run `./test.sh` to run the tests for the packages
+
+The packages have a README.md file which explains what they do and how to get them set up.
+
+## Helpful resources for ROS2 and git
+There is an [introduction document](https://github.com/OxRAMSociety/RobotArm/blob/main/resources.md) in the RobotArm repository.
+
+## Contribution workflow:
+See [this comment](https://github.com/OxRAMSociety/RobotArm/issues/14#issuecomment-2613048568) for an explanation how to figure out what tasks to work on.
+See [this comment](https://github.com/OxRAMSociety/RobotArm/issues/14#issuecomment-2614327934) for a high-level overview of the workflow, i.e. how to use git productively.
+
+Here are the specific commands that need to be run, for each of the points:
+
+1. Use Github
+2. First, make sure that you are in the main branch. If this command returns an error, you probably have uncommitted changes.
+
+```
+git checkout main
+```
+
+Create a new branch and move to it
+```
+git checkout -b <number>
+```
+3. While developing your code, don't forget to periodically create commits with messages explaining what you did in the commit. It might be easier to use a git helper in your IDE.
+
+When you are ready, make sure you have committed everything you want to push and push your changes to Github
+```
+git push
+```
+
+Then use the Github website to open a pull request.
+
+4. Use Github
+5. Same commands as in point 3, but don't forget to `git pull` and `git push` regularly to reduce the probability of conflicts
+6. Ask one of us to approve your pull request
+
+

install_cv_packages.sh

@@ -0,0 +1,4 @@
+#!/bin/bash
+
+source .venv/bin/activate
+pip3 install roboflow ultralytics

install_ros.sh

@@ -1,23 +1,28 @@
 #!/bin/bash
 
+set -e
+
 # Install needed packages
 sudo apt install git build-essential -y
 
 # Set up locales
-sudo apt update && sudo apt install locales
+sudo apt update -y
+sudo apt install locales -y
 sudo locale-gen en_US en_US.UTF-8
 sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8
 export LANG=en_US.UTF-8
 
 # Enable repositories
 sudo apt install software-properties-common -y
 sudo add-apt-repository universe -y
-sudo apt update && sudo apt install curl -y
+sudo apt install curl -y
 sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -o /usr/share/keyrings/ros-archive-keyring.gpg
 echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null
 
-# Update
-sudo apt update 
+# Update repos after a new one was added
+sudo apt update -y
+
+# Update the system
 sudo apt upgrade -y
 
 # Install ros
@@ -26,8 +31,10 @@ sudo apt install ros-jazzy-desktop -y
 
 # Run setup script on shell start
 grep -qF "source /opt/ros/jazzy/setup.bash" ~/.bashrc || echo "source /opt/ros/jazzy/setup.bash" >> ~/.bashrc
+# Tell the user to not forget to run the setup script
+grep -qF "Remember to run 'source setup.sh'" ~/.bashrc || echo "echo -e \"\033[0;31mRemember to run 'source setup.sh'\033[0;0m\"" >> ~/.bashrc
+
+# Install python-venv
+sudo apt install python3.12-venv -y
 
-# Update submodules
-git submodule update --init --recursive
 
-reboot

install_workspace.sh

@@ -0,0 +1,18 @@
+#!/bin/bash
+
+set -e
+
+# Update submodules
+git submodule update --init --recursive
+
+# Create the workspace
+colcon build --symlink-install
+
+# Setup rosdep
+# Do not fail of rosdep already set up
+sudo rosdep init 2> /dev/null || true
+rosdep update
+
+# Venv, used mostly for CV
+python3 -m venv .venv
+source .venv/bin/activate

rebuild.sh

@@ -0,0 +1,15 @@
+#!/bin/bash
+
+# Setup general ROS2 stuff
+source /opt/ros/jazzy/setup.bash
+
+colcon build --symlink-install
+
+# Setup repo-specific things
+source ./install/local_setup.bash
+
+
+### OPTIONAL ###
+# Update rosdep index and install dependencies
+rosdep update
+rosdep install --from-paths src -y --ignore-src

setup.sh

@@ -0,0 +1,7 @@
+#!/bin/bash
+
+# ROS2 stuff
+source ./install/local_setup.bash
+
+# python stuff
+source .venv/bin/activate

src/computer_vision/.gitignore

@@ -0,0 +1,4 @@
+.roboflow_key
+
+cam_params.yaml
+camera_info.yaml

src/computer_vision/README.md

@@ -0,0 +1,18 @@
+# Setup
+
+1. (Optional) If you are planning on training the YOLO model, copy `.roboflow_key.example` to `.roboflow_key`, get your Roboflow key and write it into the file
+2. Get camera calibration and parameter files and put them into `config/cam_params.yaml` and `config/camera_info.yaml`. This can be done in 2 ways:
+- Copy over the templates `config/cam_params_template.yaml` and `config/camera_info_template.yaml`
+- Calibrate the camera by yourself (See the section below).
+
+## TODO: add documentation for how to calibrate the camera
+TODO: calibrate camera
+TODO: optimise params
+
+# Usage
+## Debugging
+To start the camera node directly, run `ros2 run usb_cam usb_cam_node_exe --ros-args --params-file config/cam_params.yaml`
+
+To simulate a camera with an image, run `ros2 run stub_scripts compressed_image_publisher --ros-args -p input_path:="test/test_images/chessboards/1.jpeg"`, where the image path can be replaced for other images
+
+To view camera output, run `rqt`, select "Plugins/visualization/Image view" and select the correct topic

src/computer_vision/computer_vision/board_detection.py

@@ -0,0 +1,142 @@
+#!/usr/bin/env python
+
+import rclpy
+from rclpy.node import Node
+
+from cv_bridge import CvBridge
+from sensor_msgs.msg import Image, CompressedImage
+import cv2
+import numpy as np
+
+
+# Takes locations of the inner corners, and the board size and calculates all corners
+def compute_grid_positions(corners, board_size):
+    grid_positions = np.zeros(
+        (board_size[0] + 2, board_size[1] + 2, 2), dtype=np.float32
+    )
+    # Inner corners
+    for i in range(1, board_size[0] + 1):
+        for j in range(1, board_size[1] + 1):
+            grid_positions[i, j] = corners[(j - 1) + (i - 1) * board_size[1]].ravel()
+    # corners [0,1] to [0,7]
+    for j in range(1, board_size[1] + 1):
+        diffh = grid_positions[2, j][1] - grid_positions[1, j][1]
+        diffw = grid_positions[2, j][0] - grid_positions[1, j][0]
+        grid_positions[0, j] = [
+            grid_positions[1, j][0] - diffw,
+            grid_positions[1, j][1] - diffh,
+        ]
+    # corners [0,0] to [7,0]
+    for j in range(0, board_size[0] + 1):
+        diffh = grid_positions[j, 2][1] - grid_positions[j, 1][1]
+        diffw = grid_positions[j, 2][0] - grid_positions[j, 1][0]
+        grid_positions[j, 0] = [
+            grid_positions[j, 1][0] - diffw,
+            grid_positions[j, 1][1] - diffh,
+        ]
+    # corners [8,1] to [8,7]
+    for j in range(0, board_size[1] + 1):
+        diffh = grid_positions[6, j][1] - grid_positions[7, j][1]
+        diffw = grid_positions[6, j][0] - grid_positions[7, j][0]
+        grid_positions[8, j] = [
+            grid_positions[7, j][0] - diffw,
+            grid_positions[7, j][1] - diffh,
+        ]
+    # corners [1,0] to [7,0]
+    for j in range(0, board_size[0] + 2):  # corners [1,0] to [7,0]
+        diffh = grid_positions[j, 6][1] - grid_positions[j, 7][1]
+        diffw = grid_positions[j, 6][0] - grid_positions[j, 7][0]
+        grid_positions[j, 8] = [
+            grid_positions[j, 7][0] - diffw,
+            grid_positions[j, 7][1] - diffh,
+        ]
+    return grid_positions
+
+
+class ChessboardPublisher(Node):
+    def __init__(self):
+        # Create a publisher
+        super().__init__("chessboard_publisher")
+
+        # Parameters which can be passed to the node
+        # Topic to listen for camera events on
+        self.declare_parameter("camera_topic", "/image_raw/compressed")
+        self.camera_topic = self.get_parameter("camera_topic").value
+
+        self.chessboard_publisher = self.create_publisher(Image, "chessboard_topic", 10)
+
+        # Create an instance of CvBridge
+        self.cv_bridge = CvBridge()
+        # Subscribe to the video
+        self.subscription = self.create_subscription(
+            CompressedImage, self.camera_topic, self.on_new_camera_frame, 10
+        )
+
+    # Turns image gray scale, finds chessboard corners and draws on the corners
+    # Logs: Chessboard detected, type of corners, grid positions
+    def process_image(self, cv_image):
+        logger = self.get_logger()
+        # Convert the image to grayscale
+        gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
+        # Chessboard size in squares(width and height)
+        board_size = (7, 7)
+        # Find the chessboard corners
+        ret, corners = cv2.findChessboardCorners(gray, board_size, None)  # MatLike
+        # If the chessboard is detected, draw the grid and compute the grid positions
+        if ret:
+            logger.info("Chessboard corners detected.")
+            logger.info(str(type(corners)))
+            corners = cv2.cornerSubPix(
+                gray, corners, (3, 3), (-1, -1), (cv2.TERM_CRITERIA_EPS, 0, 0.00001)
+            )  # More accurate corner detection
+            corners = compute_grid_positions(corners, board_size)
+            # Draw on circle
+            for i in range(0, 9):
+                for j in range(0, 9):
+                    cv2.circle(
+                        cv_image,
+                        (int(corners[i, j][0]), int(corners[i, j][1])),
+                        5,
+                        (255, 0, 0),
+                        1,
+                    )
+            logger.info(str(type(corners)))
+            # Print the grid positions
+            logger.info("Grid positions:")
+            # logger.info(corners)
+        else:
+            logger.info("Chessboard corners not detected.")
+        return cv_image
+
+    def on_new_camera_frame(self, msg):
+        frame = self.cv_bridge.compressed_imgmsg_to_cv2(msg)
+        assert frame is not None, "Error reading frame"
+        frame = self.process_image(frame)
+        assert frame is not None, "Error processing frame"
+
+        img_msg = self.cv_bridge.cv2_to_imgmsg(frame, "bgr8")
+        self.chessboard_publisher.publish(img_msg)
+
+        # waitKey is a workaround - allows OpenCV to display result of imshow
+        cv2.imshow("GRID1", frame)
+        cv2.waitKey(1)
+
+        # TODO: run video.release() when exiting
+
+
+def main(args=None):
+    # Initiate the node
+    rclpy.init(args=args)
+    publisher_node = ChessboardPublisher()
+
+    rclpy.spin(publisher_node)
+
+    # Destroy the node explicitly
+    # (optional - otherwise it will be done automatically
+    # when the garbage collector destroys the node object)
+    publisher_node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()