diff --git a/.gitignore b/.gitignore index 6cc824d..9089f8c 100644 --- a/.gitignore +++ b/.gitignore @@ -27,3 +27,5 @@ qtcreator-* # Colcon custom files COLCON_IGNORE AMENT_IGNORE + +.venv diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..0a4e1aa --- /dev/null +++ b/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. diff --git a/README.md b/README.md new file mode 100644 index 0000000..d233947 --- /dev/null +++ b/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 +``` +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 + + diff --git a/install_cv_packages.sh b/install_cv_packages.sh new file mode 100755 index 0000000..78bb3c6 --- /dev/null +++ b/install_cv_packages.sh @@ -0,0 +1,4 @@ +#!/bin/bash + +source .venv/bin/activate +pip3 install roboflow ultralytics diff --git a/install_ros.sh b/install_ros.sh index a29b461..91c0075 100755 --- a/install_ros.sh +++ b/install_ros.sh @@ -1,10 +1,13 @@ #!/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 @@ -12,12 +15,14 @@ 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 diff --git a/install_workspace.sh b/install_workspace.sh new file mode 100755 index 0000000..c0812fb --- /dev/null +++ b/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 diff --git a/rebuild.sh b/rebuild.sh new file mode 100644 index 0000000..b2a52b5 --- /dev/null +++ b/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 diff --git a/setup.sh b/setup.sh new file mode 100644 index 0000000..097b169 --- /dev/null +++ b/setup.sh @@ -0,0 +1,7 @@ +#!/bin/bash + +# ROS2 stuff +source ./install/local_setup.bash + +# python stuff +source .venv/bin/activate diff --git a/src/computer_vision/.gitignore b/src/computer_vision/.gitignore new file mode 100644 index 0000000..253685b --- /dev/null +++ b/src/computer_vision/.gitignore @@ -0,0 +1,4 @@ +.roboflow_key + +cam_params.yaml +camera_info.yaml diff --git a/src/computer_vision/README.md b/src/computer_vision/README.md new file mode 100644 index 0000000..9938d0d --- /dev/null +++ b/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 diff --git a/src/computer_vision/computer_vision/board_detection.py b/src/computer_vision/computer_vision/board_detection.py new file mode 100755 index 0000000..7b56e06 --- /dev/null +++ b/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() diff --git a/src/computer_vision/computer_vision/video_process_test.py b/src/computer_vision/computer_vision/video_process_test.py new file mode 100644 index 0000000..14102a2 --- /dev/null +++ b/src/computer_vision/computer_vision/video_process_test.py @@ -0,0 +1,199 @@ +#!/usr/bin/env python +# Temporary file storing the prototype for locating the boundary box + +import rospy +from cv_bridge import CvBridge +from sensor_msgs.msg import Image +import cv2 +import numpy as np +from random import random +from matplotlib.path import Path + + +# 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): + 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 + + +# Turns image gray scale, finds chessboard corners and draws on the corners +# Logs: Chessboard detected, type of corners, grid positions +def process_image(cv_image): + # 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: + rospy.loginfo("Chessboard corners detected.") + rospy.loginfo(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, + ) + rospy.loginfo(type(corners)) + # Print the grid positions + rospy.loginfo("Grid positions:") + rospy.loginfo(corners) + return corners + else: + rospy.loginfo("Chessboard corners not detected.") + + +# Takes a rectange and calculates which square its bottom middle is in +def point_locator(points, corners, cv_image): + low1 = [0, 0] + low2 = [0, 0] + for i in range(0, 4): + if points[i][1] > low1[1]: + low1 = points[i] + lowindex = i + for i in range(0, 4): + if points[i][1] > low2[1] and i != lowindex: + low2 = points[i] + mid_point = ((low1[0] + low2[0]) / 2, (low1[1] + low2[1]) / 2) + column_translation = ["a", "b", "c", "d", "e", "f", "g", "h"] + cv2.drawMarker(cv_image, (int(mid_point[0]), int(mid_point[1])), (0, 0, 255)) + # Iterate over each column first + for j in range(0, 8): + for i in range(0, 8): + path = Path( + [ + corners[i, j], + corners[i + 1, j], + corners[i + 1, j + 1], + corners[i, j + 1], + corners[i, j], + ], + [Path.MOVETO, Path.LINETO, Path.LINETO, Path.LINETO, Path.CLOSEPOLY], + closed=True, + ) + if path.contains_point(mid_point): + cv2.circle( + cv_image, + (int(corners[i, j][0]), int(corners[i, j][1])), + 15, + (255, 0, 255), + 1, + ) + cv2.circle( + cv_image, + (int(corners[i + 1, j][0]), int(corners[i + 1, j][1])), + 15, + (255, 0, 255), + 1, + ) + cv2.circle( + cv_image, + (int(corners[i + 1, j + 1][0]), int(corners[i + 1, j + 1][1])), + 15, + (255, 0, 255), + 1, + ) + cv2.circle( + cv_image, + (int(corners[i, j + 1][0]), int(corners[i, j + 1][1])), + 15, + (255, 0, 255), + 1, + ) + rospy.loginfo(column_translation[j] + str(8 - i)) + + +def video_publisher(): + # Initiate the node + rospy.init_node("video_publisher_node") + # Create a publisher + pub = rospy.Publisher("video_topic", Image, queue_size=10) + # Create an instance of CvBridge + bridge = CvBridge() + # Load the video + video = cv2.VideoCapture(0) # change this to your video file path + rate = rospy.Rate(30) # 30 Hz to match video frame rate + (x1, y1) = (640 * random(), 480 * random()) + (x3, y3) = (640 * random(), 480 * random()) + (x2, y2) = (x1, y3) + (x4, y4) = (x3, y1) + while not rospy.is_shutdown(): + # Read a frame from the video + ret, frame = video.read() + if ret: + corners = process_image(frame) + if frame is None: + print("Could not open or find the image") + return + # Convert the frame to a ROS image message + ros_image = bridge.cv2_to_imgmsg(frame, "bgr8") + # Publish the frame + pub.publish(ros_image) + if corners is not None: + point_locator([(x1, y1), (x2, y2), (x3, y3), (x4, y4)], corners, frame) + rate.sleep() + else: + break + cv2.line(frame, (int(x1), int(y1)), (int(x2), int(y2)), (0, 0, 255), 2) + cv2.line(frame, (int(x3), int(y3)), (int(x2), int(y2)), (0, 0, 255), 2) + cv2.line(frame, (int(x3), int(y3)), (int(x4), int(y4)), (0, 0, 255), 2) + cv2.line(frame, (int(x1), int(y1)), (int(x4), int(y4)), (0, 0, 255), 2) + cv2.imshow("GRID1", frame) + cv2.waitKey(1) + + video.release() + + +if __name__ == "__main__": + try: + video_publisher() + except rospy.ROSInterruptException: + pass + diff --git a/src/computer_vision/config/cam_params_template.yaml b/src/computer_vision/config/cam_params_template.yaml new file mode 100644 index 0000000..3e00113 --- /dev/null +++ b/src/computer_vision/config/cam_params_template.yaml @@ -0,0 +1,23 @@ +/**: + ros__parameters: + video_device: "/dev/video0" + framerate: 30.0 + io_method: "mmap" + frame_id: "camera" + pixel_format: "mjpeg2rgb" # see usb_cam/supported_formats for list of supported formats + av_device_format: "YUV422P" + image_width: 640 + image_height: 480 + camera_name: "main_camera" + camera_info_url: "package://computer_vision/config/camera_info.yaml" + brightness: -1 + contrast: -1 + saturation: -1 + sharpness: -1 + gain: -1 + auto_white_balance: true + white_balance: 4000 + autoexposure: true + exposure: 100 + autofocus: false + focus: -1 diff --git a/src/computer_vision/config/camera_info_template.yaml b/src/computer_vision/config/camera_info_template.yaml new file mode 100644 index 0000000..c084f49 --- /dev/null +++ b/src/computer_vision/config/camera_info_template.yaml @@ -0,0 +1,26 @@ +image_width: 640 +image_height: 480 +camera_name: main_camera +camera_matrix: + rows: 3 + cols: 3 + data: [796.62207, 0. , 674.57298, + 0. , 856.057 , 586.88776, + 0. , 0. , 1. ] +distortion_model: plumb_bob +distortion_coefficients: + rows: 1 + cols: 5 + data: [-0.274258, 0.040343, -0.054555, -0.014826, 0.000000] +rectification_matrix: + rows: 3 + cols: 3 + data: [1., 0., 0., + 0., 1., 0., + 0., 0., 1.] +projection_matrix: + rows: 3 + cols: 4 + data: [609.51172, 0. , 633.486 , 0. , + 0. , 844.12219, 583.22598, 0. , + 0. , 0. , 1. , 0. ] diff --git a/src/computer_vision/package.xml b/src/computer_vision/package.xml index 6646a52..646819f 100644 --- a/src/computer_vision/package.xml +++ b/src/computer_vision/package.xml @@ -3,10 +3,18 @@ computer_vision 0.0.0 - TODO: Package description + Camera-based detection of chess pieces, chessboard, etc mic MIT + usb_cam + + + python3-pydantic + ament_copyright ament_flake8 ament_pep257 diff --git a/src/computer_vision/setup.py b/src/computer_vision/setup.py index 3d56f6f..c0ef6dc 100644 --- a/src/computer_vision/setup.py +++ b/src/computer_vision/setup.py @@ -1,25 +1,24 @@ from setuptools import find_packages, setup -package_name = 'computer_vision' +package_name = "computer_vision" setup( name=package_name, - version='0.0.0', - packages=find_packages(exclude=['test']), + version="0.0.0", + packages=find_packages(exclude=["test", "train"]), data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), + ("share/ament_index/resource_index/packages", ["resource/" + package_name]), + ("share/" + package_name, ["package.xml"]), + (f"share/{package_name}/config", ["config/camera_info.yaml"]), ], - install_requires=['setuptools'], + install_requires=["setuptools"], zip_safe=True, - maintainer='mic', - maintainer_email='misha.nekrasov@gmail.com', - description='TODO: Package description', - license='MIT', - tests_require=['pytest'], + maintainer="mic", + maintainer_email="misha.nekrasov@gmail.com", + description="Camera-based detection of chess pieces, chessboard, etc", + license="MIT", + tests_require=["pytest"], entry_points={ - 'console_scripts': [ - ], + "console_scripts": ["board_detection = computer_vision.board_detection:main"], }, ) diff --git a/src/computer_vision/test/test_images/chessboards/1.jpeg b/src/computer_vision/test/test_images/chessboards/1.jpeg new file mode 100644 index 0000000..23c8bf7 Binary files /dev/null and b/src/computer_vision/test/test_images/chessboards/1.jpeg differ diff --git a/src/computer_vision/test/test_images/chessboards/2.jpg b/src/computer_vision/test/test_images/chessboards/2.jpg new file mode 100644 index 0000000..17adc3a Binary files /dev/null and b/src/computer_vision/test/test_images/chessboards/2.jpg differ diff --git a/src/computer_vision/test/test_images/chessboards/3.jpg b/src/computer_vision/test/test_images/chessboards/3.jpg new file mode 100644 index 0000000..ba090b1 Binary files /dev/null and b/src/computer_vision/test/test_images/chessboards/3.jpg differ diff --git a/src/computer_vision/train/.gitignore b/src/computer_vision/train/.gitignore new file mode 100644 index 0000000..9677cb7 --- /dev/null +++ b/src/computer_vision/train/.gitignore @@ -0,0 +1,4 @@ +data/** + +# The model +yolov8n.pt diff --git a/src/computer_vision/train/.roboflow_key.example b/src/computer_vision/train/.roboflow_key.example new file mode 100644 index 0000000..71ef4b8 --- /dev/null +++ b/src/computer_vision/train/.roboflow_key.example @@ -0,0 +1 @@ +YOUR_KEY_HERE diff --git a/src/computer_vision/train/chess_piece_detection.py b/src/computer_vision/train/chess_piece_detection.py new file mode 100644 index 0000000..2449b7f --- /dev/null +++ b/src/computer_vision/train/chess_piece_detection.py @@ -0,0 +1,32 @@ +# Temporary files for loading the model +from roboflow import Roboflow +import yaml +from ultralytics import YOLO + +try: + with open(".roboflow_key", "r") as f: + ROBOFLOW_KEY = f.read().strip() +except FileNotFoundError: + from sys import exit, stderr + + print("Please create a `.roboflow_key` file with the API key", file=stderr) + exit(1) + +# Export dataset +rf = Roboflow(api_key=ROBOFLOW_KEY) +project = rf.workspace("oxram").project("chess_pieces-e6ln1") +# Download last version in a specific format +dataset = project.versions()[0].download(model_format="yolov8-obb", location="data") + +# Allows us to open the train dataset +with open(f"{dataset.location}/data.yaml", "r") as file: + data = yaml.safe_load(file) + +data["path"] = dataset.location + +with open(f"{dataset.location}/data.yaml", "w") as file: + yaml.dump(data, file, sort_keys=False) + +model = YOLO("yolov8n.pt") + +results = model.train(data=f"{dataset.location}/data.yaml", epochs=100, imgsz=640) diff --git a/src/motion_planning/motion_planning/hardware_interface.py b/src/motion_planning/motion_planning/hardware_interface.py new file mode 100644 index 0000000..4c25c2b --- /dev/null +++ b/src/motion_planning/motion_planning/hardware_interface.py @@ -0,0 +1,6 @@ +def main(): + print('Hi from motion_planning.') + + +if __name__ == '__main__': + main() diff --git a/src/motion_planning/package.xml b/src/motion_planning/package.xml index 0dbd946..3811b6c 100644 --- a/src/motion_planning/package.xml +++ b/src/motion_planning/package.xml @@ -3,7 +3,7 @@ motion_planning 0.0.0 - TODO: Package description + Controlling motion of the motors based on high-level commands mic MIT diff --git a/src/motion_planning/setup.py b/src/motion_planning/setup.py index 5c27ca4..4fe3b9d 100644 --- a/src/motion_planning/setup.py +++ b/src/motion_planning/setup.py @@ -20,6 +20,7 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ + 'hardware_interface = motion_planning.hardware_interface:main' ], }, ) diff --git a/src/stub_scripts/LICENSE b/src/stub_scripts/LICENSE new file mode 100644 index 0000000..30e8e2e --- /dev/null +++ b/src/stub_scripts/LICENSE @@ -0,0 +1,17 @@ +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. diff --git a/src/stub_scripts/README.md b/src/stub_scripts/README.md new file mode 100644 index 0000000..08cb08c --- /dev/null +++ b/src/stub_scripts/README.md @@ -0,0 +1,7 @@ +# Scripts that emulate robot packages + +For example, creating a "fake camera" that always publishes the same image. + +## List of executables +- compressed_image_publisher publishes a compressed image repeatedly on a topic, simulating a camera +Usage example: `ros2 run stub_scripts compressed_image_publisher --ros-args -p input_path:="../computer_vision/test/test_images/chessboards/1.jpeg"` diff --git a/src/stub_scripts/package.xml b/src/stub_scripts/package.xml new file mode 100644 index 0000000..66d2e9d --- /dev/null +++ b/src/stub_scripts/package.xml @@ -0,0 +1,18 @@ + + + + stub_scripts + 0.0.0 + TODO: Package description + mic + MIT + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/stub_scripts/resource/stub_scripts b/src/stub_scripts/resource/stub_scripts new file mode 100644 index 0000000..e69de29 diff --git a/src/stub_scripts/setup.cfg b/src/stub_scripts/setup.cfg new file mode 100644 index 0000000..e284ece --- /dev/null +++ b/src/stub_scripts/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/stub_scripts +[install] +install_scripts=$base/lib/stub_scripts diff --git a/src/stub_scripts/setup.py b/src/stub_scripts/setup.py new file mode 100644 index 0000000..2e82edd --- /dev/null +++ b/src/stub_scripts/setup.py @@ -0,0 +1,25 @@ +from setuptools import find_packages, setup + +package_name = "stub_scripts" + +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="mic", + maintainer_email="misha.nekrasov@gmail.com", + description="TODO: Package description", + license="MIT", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "compressed_image_publisher = stub_scripts.compressed_image_publisher:main" + ], + }, +) diff --git a/src/stub_scripts/stub_scripts/__init__.py b/src/stub_scripts/stub_scripts/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/stub_scripts/stub_scripts/compressed_image_publisher.py b/src/stub_scripts/stub_scripts/compressed_image_publisher.py new file mode 100644 index 0000000..380a5f1 --- /dev/null +++ b/src/stub_scripts/stub_scripts/compressed_image_publisher.py @@ -0,0 +1,52 @@ +import rclpy +from rclpy.node import Node + +from cv_bridge import CvBridge +from sensor_msgs.msg import CompressedImage +import cv2 + + +class CompressedImagePublisher(Node): + def __init__(self, frequency=30): + super().__init__("compressed_image_publisher") + + # Default value + self.declare_parameter("output_topic", "/image_raw/compressed") + output_topic = self.get_parameter("output_topic").value + + self.declare_parameter("input_path", rclpy.Parameter.Type.STRING) + input_path = self.get_parameter("input_path").value + # Loads the image as a NumPy Array + image = cv2.imread(input_path) + assert image is not None, "Image could not be read" + + # Processes it into a compressed message format + self.message = CvBridge().cv2_to_compressed_imgmsg(image, "jpeg") + self.message.format = "rgb8; jpeg compressed bgr8" + + self.publisher = self.create_publisher(CompressedImage, output_topic, 10) + + self.timer = self.create_timer(1.0 / frequency, self.publish_new) + + self.get_logger().info(f"Publishing {input_path} on topic {output_topic}") + + def publish_new(self): + self.publisher.publish(self.message) + + +def main(args=None): + # Initiate the node + rclpy.init(args=args) + publisher_node = CompressedImagePublisher() + + 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() diff --git a/src/stub_scripts/test/test_copyright.py b/src/stub_scripts/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/src/stub_scripts/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/stub_scripts/test/test_flake8.py b/src/stub_scripts/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/src/stub_scripts/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 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_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/src/stub_scripts/test/test_pep257.py b/src/stub_scripts/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/src/stub_scripts/test/test_pep257.py @@ -0,0 +1,23 @@ +# 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_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/src/task_planning/package.xml b/src/task_planning/package.xml index 749a9fe..1fba2c4 100644 --- a/src/task_planning/package.xml +++ b/src/task_planning/package.xml @@ -3,7 +3,7 @@ task_planning 0.0.0 - TODO: Package description + High-level logic of robot arm mic MIT diff --git a/src/task_planning/setup.py b/src/task_planning/setup.py index f75379b..ab11b00 100644 --- a/src/task_planning/setup.py +++ b/src/task_planning/setup.py @@ -20,6 +20,7 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ + 'state_machine = task_planning.state_machine:main' ], }, ) diff --git a/src/task_planning/task_planning/state_machine.py b/src/task_planning/task_planning/state_machine.py new file mode 100644 index 0000000..2259bf2 --- /dev/null +++ b/src/task_planning/task_planning/state_machine.py @@ -0,0 +1,6 @@ +def main(): + print('Hi from task_planning.') + + +if __name__ == '__main__': + main() diff --git a/test.sh b/test.sh new file mode 100755 index 0000000..48265b1 --- /dev/null +++ b/test.sh @@ -0,0 +1,4 @@ +#!/bin/bash + +colcon test +colcon test-result --verbose