front camera image publisher service is added

Makefile

@@ -1,3 +1,6 @@
+frontcam:
+	@cd deploy && docker build --no-cache --tag go2py_frontcam_publisher:latest -f docker/Dockerfile.frontcam .
+
 docker_start:
 	@./scripts/run_dev.sh 
 
@@ -22,6 +25,12 @@ bridge:
 robot_description:
 	@cd deploy && docker build --no-cache --tag go2py_description:latest -f docker/Dockerfile.robot_description .
 
+frontcam_install:
+	@cp deploy/services/go2py-frontcam.service /etc/systemd/system/
+	@cp deploy/services/frontcam-v4l-loopback.sh /usr/bin
+	@systemctl enable go2py-frontcam.service
+	@systemctl start go2py-frontcam.service
+
 hesai_install:
 	@cp deploy/services/go2py-hesai.service /etc/systemd/system/
 	@systemctl enable go2py-hesai.service
@@ -36,7 +45,12 @@ robot_description_install:
 	@cp deploy/services/go2py-robot-description.service /etc/systemd/system/
 	@systemctl enable go2py-robot-description.service
 	@systemctl start go2py-robot-description.service
-	
+
+frontcam_uninstall:
+	@systemctl disable go2py-frontcam.service
+	@systemctl stop go2py-frontcam.service
+	@rm /etc/systemd/system/go2py-frontcam.service
+
 hesai_uninstall:
 	@systemctl disable go2py-hesai.service
 	@systemctl stop go2py-hesai.service

deploy/docker/Dockerfile.frontcam

@@ -0,0 +1,28 @@
+# FROM isaac_ros_dev-aarch64
+FROM ros:humble
+ENV DEBIAN_FRONTEND=noninteractive
+SHELL ["/bin/bash", "-c"]
+# uodate and install dependencies 
+RUN apt-get update && apt-get install -y \
+    ros-humble-rmw-cyclonedds-cpp ros-humble-rosidl-generator-dds-idl \
+    libyaml-cpp-dev \
+    ros-humble-xacro \
+    libboost-all-dev\
+    build-essential \
+    cmake \
+    git \
+    ros-humble-cv-bridge \
+    && rm -rf /var/lib/apt/lists/*
+
+# Cheange the ROS2 RMW to CycloneDDS as instructed by Unitree
+RUN cd / && git clone https://github.com/unitreerobotics/unitree_ros2 && cd /unitree_ros2/cyclonedds_ws/src && \
+git clone https://github.com/ros2/rmw_cyclonedds -b humble && git clone https://github.com/eclipse-cyclonedds/cyclonedds -b releases/0.10.x &&\
+cd .. && colcon build --packages-select cyclonedds && source /opt/ros/humble/setup.bash && colcon build
+
+# copy the required scripts and source files
+COPY ros2_nodes/front-camera-publisher.py /home/front-camera-publisher.py
+COPY docker/scripts /root/scripts
+
+# set the entrypoint to bash
+#ENTRYPOINT ["/bin/bash"]
+ENTRYPOINT ["/bin/bash", "/root/scripts/front_cam_publisher.sh"]

deploy/docker/scripts/bridge_start.sh

@@ -4,4 +4,4 @@ export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
 export CYCLONEDDS_URI='<CycloneDDS><Domain><General><Interfaces>
                             <NetworkInterface name="eth0" priority="default" multicast="default" />
                         </Interfaces></General></Domain></CycloneDDS>'
-source /bridge_ws/install/setup.bash && ros2 launch /root/launch/bridge.launch.py
+source /bridge_ws/install/setup.bash && python3 /home/front-camera-publisher.py

deploy/docker/scripts/front_cam_publisher.sh

@@ -0,0 +1,7 @@
+source /opt/ros/humble/setup.bash
+source /unitree_ros2/cyclonedds_ws/install/setup.bash
+export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
+export CYCLONEDDS_URI='<CycloneDDS><Domain><General><Interfaces>
+                            <NetworkInterface name="eth0" priority="default" multicast="default" />
+                        </Interfaces></General></Domain></CycloneDDS>'
+python3 /home/front-camera-publisher.py

deploy/ros2_nodes/front-camera-publisher.py

@@ -0,0 +1,42 @@
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import Image
+from cv_bridge import CvBridge
+import cv2
+
+class ImagePublisher(Node):
+    def __init__(self):
+        super().__init__('image_publisher')
+        self.publisher_ = self.create_publisher(Image, '/go2/rgb/image_raw', 10)
+        self.bridge = CvBridge()
+        self.gstreamer_str = "udpsrc address=230.1.1.1 port=1720 multicast-iface=eth0 ! application/x-rtp, media=video, encoding-name=H264 ! rtph264depay ! h264parse ! avdec_h264 ! videoconvert ! video/x-raw,width=1280,height=720,format=BGR ! appsink drop=1"
+        self.cap = cv2.VideoCapture(0)
+
+    def publish_images(self):
+        while rclpy.ok():
+            ret, frame = self.cap.read()
+            if ret:
+                print('new frame')
+                msg = self.bridge.cv2_to_imgmsg(frame, encoding="bgr8")
+                self.publisher_.publish(msg)
+                self.get_logger().info('Publishing image')
+            else:
+               self.get_logger().warn('Failed to capture image')
+               break
+
+        self.cap.release()
+
+def main(args=None):
+    rclpy.init(args=args)
+    image_publisher = ImagePublisher()
+
+    try:
+        image_publisher.publish_images()
+    except KeyboardInterrupt:
+        pass
+
+    image_publisher.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

deploy/services/frontcam-v4l-loopback.sh

@@ -0,0 +1,6 @@
+# Run the GStreamer pipeline
+/usr/bin/gst-launch-1.0 udpsrc address=230.1.1.1 port=1720 multicast-iface=eth0 ! queue ! application/x-rtp, media=video, \
+encoding-name=H264 ! rtph264depay ! h264parse ! avdec_h264 ! videoconvert ! v4l2sink device=/dev/video0 &
+sleep 1
+# Run the ROS image publisher node
+/usr/bin/docker run --rm --name go2py_frontcam --privileged --network host -v /dev/*:/dev/* -v /etc/localtime:/etc/localtime:ro go2py_frontcam_publisher:latest

deploy/services/go2py-frontcam.service

@@ -0,0 +1,13 @@
+[Unit]
+Description=ROS2 device driver container
+Requires=multi-user.target
+After=multi-user.target
+
+[Service]
+Restart=always
+ExecStartPre=/usr/bin/docker rm -f go2py_frontcam || true
+ExecStart=/bin/bash -c '/usr/bin/frontcam-v4l-loopback.sh' 
+ExecStop=/usr/bin/docker stop -t 2 go2py_frontcam
+
+[Install]
+WantedBy=default.target

scripts/front-cam-v4l-loopback.sh

@@ -0,0 +1,29 @@
+#!/bin/bash
+
+# Function to check if a command exists
+command_exists() {
+    command -v "$1" >/dev/null 2>&1
+}
+
+# Check if v4l2loopback is installed and install it if not
+if ! command_exists modprobe; then
+    echo "modprobe command not found. Please install kmod package."
+    exit 1
+fi
+
+if ! lsmod | grep -q v4l2loopback; then
+    echo "v4l2loopback module not loaded. Checking installation..."
+    if ! dpkg -s v4l2loopback-dkms >/dev/null 2>&1; then
+        echo "v4l2loopback not installed. Installing..."
+        sudo apt-get update
+        sudo apt-get install -y v4l2loopback-dkms v4l2loopback-utils
+    fi
+    echo "Loading v4l2loopback module..."
+    sudo modprobe v4l2loopback
+else
+    echo "v4l2loopback module already loaded."
+fi
+
+# Run the GStreamer pipeline
+gst-launch-1.0 udpsrc address=230.1.1.1 port=1720 multicast-iface=eth0 ! queue ! application/x-rtp, media=video, \
+encoding-name=H264 ! rtph264depay ! h264parse ! avdec_h264 ! videoconvert ! v4l2sink device=/dev/video0