Adamc/sub camera temp

.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
 

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

.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

.gitmodules

@@ -0,0 +1,4 @@
+[submodule "src/vision_opencv"]
+	path = src/vision_opencv
+	url = https://github.com/ros-perception/vision_opencv.git
+	branch = ros2

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

src/camera/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>camera</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="kimdavid2222@gmail.com">trickfire</maintainer>
+  <license>TODO: License declaration</license>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

src/camera/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/camera
+[install]
+install_scripts=$base/lib/camera

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"],
+    },
+)

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"

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,
         ]
     )

src/viator_launch/package.xml

@@ -11,6 +11,7 @@
   <exec_depend>ros2launch</exec_depend>
   <exec_depend>mission_control_updater</exec_depend>
   <exec_depend>arm</exec_depend>
+  <exec_depend>camera</exec_depend>
   <exec_depend>example_node</exec_depend>