diff --git a/.devcontainer/Dockerfile b/.devcontainer/Dockerfile index 6fe2b1c..960b9cc 100644 --- a/.devcontainer/Dockerfile +++ b/.devcontainer/Dockerfile @@ -1,6 +1,5 @@ # Base image: Ubuntu 20.04 with ROS2 Humble installed FROM ros:humble - # Install dependencies from apt RUN apt-get update && apt-get install -y \ # To use git in the container @@ -15,6 +14,12 @@ RUN apt-get update && apt-get install -y \ can-utils \ iproute2 \ kmod \ + # Used for cv_bridge + python3-numpy \ + libboost-python-dev \ + # OpenCV + libopencv-dev \ + python3-opencv \ # Clean out the apt lists after `apt-get update` && rm -rf /var/lib/apt/lists/* @@ -24,6 +29,9 @@ RUN pip3 install moteus RUN pip3 install pyusb +# Install OpenCV for python +RUN pip3 install opencv-python + # Update pydocstyle to remove a deprecation warning when testing for PEP257 RUN pip install --upgrade pydocstyle diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 035ae15..f2069ab 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -7,7 +7,7 @@ "workspaceFolder": "/home/trickfire/${localWorkspaceFolderBasename}", "runArgs": [ "--cap-add=SYS_PTRACE", - //"--device=/dev/ttyUSB0", + "--device=/dev/video0", "--security-opt", "seccomp=unconfined" ], "customizations": { diff --git a/.github/workflows/lint.yml b/.github/workflows/lint.yml index 32f464b..7c38a3e 100644 --- a/.github/workflows/lint.yml +++ b/.github/workflows/lint.yml @@ -7,12 +7,14 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@v4 + with: + submodules: true # Black has a first-party ci/cd solution doesn't require python or setup, so use that # before running setup. - name: Run Black uses: psf/black@stable with: - options: "--check --verbose" + options: "--check --verbose --exclude vision_opencv" src: "./src" version: 24.8.0 - name: Setup Python @@ -26,7 +28,7 @@ jobs: - name: Run Pylint # Ignoring ros packages is unideal, but installing them as packages is not trivial run: | - pylint --fail-under=8 --ignored-modules=rclpy,std_msgs,ament_copyright,ament_flake8,pytest,launch ./src + pylint --fail-under=8 --ignored-modules=rclpy,std_msgs,ament_copyright,ament_flake8,pytest,launch,vision_opencv --ignore-paths=^src/vision_opencv.*$ ./src - name: Run Mypy run: | - mypy --exclude /test/ --exclude setup\.py ./src + mypy --exclude /test/ --exclude setup\.py --exclude vision_opencv ./src diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..86bd033 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,4 @@ +[submodule "src/vision_opencv"] + path = src/vision_opencv + url = https://github.com/ros-perception/vision_opencv.git + branch = ros2 diff --git a/src/camera/camera/__init__.py b/src/camera/camera/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/camera/camera/roscamera.py b/src/camera/camera/roscamera.py new file mode 100644 index 0000000..ee721a0 --- /dev/null +++ b/src/camera/camera/roscamera.py @@ -0,0 +1,111 @@ +import cv2 # OpenCV library +import rclpy # Python Client Library for ROS 2 +from cv_bridge import CvBridge # Package to convert between ROS and OpenCV Images +from rclpy.executors import MultiThreadedExecutor +from rclpy.node import Node # Handles the creation of nodes +from sensor_msgs.msg import CompressedImage # Image is the message type + + +def getCameras() -> list[int]: + """ + Function returns a list of working camera IDs to capture every camera connected to the robot + """ + + non_working_ports = 0 + dev_port = 0 + working_ports = [] + + while non_working_ports < 6: + camera = cv2.VideoCapture(dev_port) + if not camera.isOpened(): + non_working_ports += 1 + else: + is_reading, img = camera.read() + _ = camera.get(3) + _ = camera.get(4) + if is_reading: + working_ports.append(dev_port) + + dev_port += 1 + + return working_ports + + +class RosCamera(Node): + + def __init__(self, topicName: str, camera: int): + super().__init__("ros_camera") + self.get_logger().info("Launching ros_camera node") + + # Create the publisher. This publisher will publish an Image + # to the video_frames topic. The queue size is 10 messages. + self._publisher = self.create_publisher(CompressedImage, topicName, 10) + self.get_logger().info("Created Publisher " + topicName) + + # We will publish a message every 0.1 seconds + # timer_period = 0.1 # seconds + timer_period = 0.1 # seconds + + # Create the timer + self.timer = self.create_timer(timer_period, self.publishCameraFrame) + + # Create a VideoCapture object + # The argument '0' gets the default webcam. + self.cap = cv2.VideoCapture(camera) + self.get_logger().info( + "Using video ID: " + str(camera) + ", ON: " + str(self.cap.isOpened()) + ) + + # Used to convert between ROS and OpenCV images + self.br = CvBridge() + + # Declare parameters for each 'ros_camera' node thread + self.declare_parameter("cameraNumber", camera) + + def publishCameraFrame(self) -> None: + """ + Callback function publishes a frame captured from a camera to /video_framesX (X is specific + camera ID) every 0.1 seconds + """ + + # Capture frame-by-frame + # This method returns True/False as well as the video frame. + ret, frame = self.cap.read() + + if ret: + # Publish the image. + self._publisher.publish(self.br.cv2_to_compressed_imgmsg(frame)) + + +def main(args: list[str] | None = None) -> None: + """ + The entry point of the node. + """ + + rclpy.init(args=args) + try: + # We need an executor because running .spin() is a blocking function. + # using the MultiThreadedExecutor, we can control multiple nodes + executor = MultiThreadedExecutor() + nodes = [] + camera_num = 0 + + for camera_id in getCameras(): + node = RosCamera("video_frames" + str(camera_num), camera_id) + nodes.append(node) + executor.add_node(node) + camera_num += 1 + + try: + executor.spin() + finally: + executor.shutdown() + for node in nodes: + node.destroy_node() + + finally: + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/camera/package.xml b/src/camera/package.xml new file mode 100644 index 0000000..2500f65 --- /dev/null +++ b/src/camera/package.xml @@ -0,0 +1,18 @@ + + + + camera + 0.0.0 + TODO: Package description + trickfire + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + \ No newline at end of file diff --git a/src/camera/resource/camera b/src/camera/resource/camera new file mode 100644 index 0000000..e69de29 diff --git a/src/camera/setup.cfg b/src/camera/setup.cfg new file mode 100644 index 0000000..bedd1f7 --- /dev/null +++ b/src/camera/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/camera +[install] +install_scripts=$base/lib/camera \ No newline at end of file diff --git a/src/camera/setup.py b/src/camera/setup.py new file mode 100644 index 0000000..805334a --- /dev/null +++ b/src/camera/setup.py @@ -0,0 +1,23 @@ +from setuptools import find_packages, setup + +package_name = "camera" + +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="trickfire", + maintainer_email="kimdavid2222@gmail.com", + description="TODO: Package description", + license="TODO: License declaration", + tests_require=["pytest"], + entry_points={ + "console_scripts": ["roscamera = camera.roscamera:main"], + }, +) diff --git a/src/camera/test/test_copyright.py b/src/camera/test/test_copyright.py new file mode 100644 index 0000000..60c2d1e --- /dev/null +++ b/src/camera/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. + +import pytest +from ament_copyright.main import main + + +# 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/viator_launch/launch/robot.launch.py b/src/viator_launch/launch/robot.launch.py index ecd1440..ae88cfe 100644 --- a/src/viator_launch/launch/robot.launch.py +++ b/src/viator_launch/launch/robot.launch.py @@ -22,6 +22,8 @@ heartbeat_node = Node(package="heartbeat", executable="heartbeat", name="heartbeat_node") +camera_node = Node(package="camera", executable="roscamera", name="camera_node") + # This is the example node. It will show ROS timers, subscribers, and publishers # To include it in the startup, add it to the array in the generate_launch_description() method example_node = Node(package="example_node", executable="myExampleNode", name="my_example_node") @@ -44,6 +46,7 @@ def generate_launch_description() -> launch.LaunchDescription: # pylint: disabl mission_control_updater_node, arm_node, heartbeat_node, + camera_node, launch_include, ] ) diff --git a/src/viator_launch/package.xml b/src/viator_launch/package.xml index 58a78d9..7f08692 100644 --- a/src/viator_launch/package.xml +++ b/src/viator_launch/package.xml @@ -11,6 +11,7 @@ ros2launch mission_control_updater arm + camera example_node diff --git a/src/vision_opencv b/src/vision_opencv new file mode 160000 index 0000000..a34a0c2 --- /dev/null +++ b/src/vision_opencv @@ -0,0 +1 @@ +Subproject commit a34a0c261984e4ab71f267d093f6a48820801d80