From 8ef15c19a88a3a6868e2d87b9528f1867630ec8c Mon Sep 17 00:00:00 2001 From: Rooholla-KhorramBakht Date: Sun, 23 Jun 2024 08:58:02 +0800 Subject: [PATCH] front camera image publisher service is added --- Makefile | 16 +++++++- deploy/docker/Dockerfile.frontcam | 28 +++++++++++++ deploy/docker/scripts/bridge_start.sh | 2 +- deploy/docker/scripts/front_cam_publisher.sh | 7 ++++ deploy/ros2_nodes/front-camera-publisher.py | 42 ++++++++++++++++++++ deploy/services/frontcam-v4l-loopback.sh | 6 +++ deploy/services/go2py-frontcam.service | 13 ++++++ scripts/front-cam-v4l-loopback.sh | 29 ++++++++++++++ 8 files changed, 141 insertions(+), 2 deletions(-) create mode 100644 deploy/docker/Dockerfile.frontcam create mode 100755 deploy/docker/scripts/front_cam_publisher.sh create mode 100644 deploy/ros2_nodes/front-camera-publisher.py create mode 100755 deploy/services/frontcam-v4l-loopback.sh create mode 100644 deploy/services/go2py-frontcam.service create mode 100755 scripts/front-cam-v4l-loopback.sh diff --git a/Makefile b/Makefile index ee4b1e1..98562d8 100644 --- a/Makefile +++ b/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 diff --git a/deploy/docker/Dockerfile.frontcam b/deploy/docker/Dockerfile.frontcam new file mode 100644 index 0000000..45f27f8 --- /dev/null +++ b/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"] diff --git a/deploy/docker/scripts/bridge_start.sh b/deploy/docker/scripts/bridge_start.sh index 5823081..820174c 100644 --- a/deploy/docker/scripts/bridge_start.sh +++ b/deploy/docker/scripts/bridge_start.sh @@ -4,4 +4,4 @@ export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp export CYCLONEDDS_URI=' ' -source /bridge_ws/install/setup.bash && ros2 launch /root/launch/bridge.launch.py \ No newline at end of file +source /bridge_ws/install/setup.bash && python3 /home/front-camera-publisher.py \ No newline at end of file diff --git a/deploy/docker/scripts/front_cam_publisher.sh b/deploy/docker/scripts/front_cam_publisher.sh new file mode 100755 index 0000000..f9a59eb --- /dev/null +++ b/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=' + + ' +python3 /home/front-camera-publisher.py \ No newline at end of file diff --git a/deploy/ros2_nodes/front-camera-publisher.py b/deploy/ros2_nodes/front-camera-publisher.py new file mode 100644 index 0000000..4a64ad2 --- /dev/null +++ b/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() diff --git a/deploy/services/frontcam-v4l-loopback.sh b/deploy/services/frontcam-v4l-loopback.sh new file mode 100755 index 0000000..3636f4e --- /dev/null +++ b/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 \ No newline at end of file diff --git a/deploy/services/go2py-frontcam.service b/deploy/services/go2py-frontcam.service new file mode 100644 index 0000000..0adc732 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/scripts/front-cam-v4l-loopback.sh b/scripts/front-cam-v4l-loopback.sh new file mode 100755 index 0000000..37bab21 --- /dev/null +++ b/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