From dde420e212f5e9f834a3e26d0ac7232f902e0833 Mon Sep 17 00:00:00 2001 From: Arthuino Date: Thu, 8 Feb 2024 13:47:19 +0100 Subject: [PATCH] launch files for vision --- vision_ws/README.md | 57 ++++--- vision_ws/scripts/run_all_camera_yolo.sh | 3 + vision_ws/scripts/run_all_display.sh | 3 + vision_ws/scripts/run_camera_yolo.sh | 8 +- vision_ws/scripts/run_display.sh | 5 +- .../src/py_pubsub/launch/launch_all_camera.py | 22 +++ .../py_pubsub/launch/launch_all_display.py | 22 +++ vision_ws/src/py_pubsub/package.xml | 2 + .../src/py_pubsub/py_pubsub/camera_yolo.py | 152 ++++-------------- vision_ws/src/py_pubsub/py_pubsub/display.py | 29 ++-- vision_ws/src/py_pubsub/setup.py | 4 + 11 files changed, 125 insertions(+), 182 deletions(-) create mode 100644 vision_ws/scripts/run_all_camera_yolo.sh create mode 100644 vision_ws/scripts/run_all_display.sh create mode 100644 vision_ws/src/py_pubsub/launch/launch_all_camera.py create mode 100644 vision_ws/src/py_pubsub/launch/launch_all_display.py diff --git a/vision_ws/README.md b/vision_ws/README.md index 2e890c1..488ace9 100644 --- a/vision_ws/README.md +++ b/vision_ws/README.md @@ -1,56 +1,55 @@ # VISION_WS -Cette documentation ne prent pas en compte Docker - -## Build +## Utilisation -Premier build : +### Build ```bash -colcon build --packages-select py_pubsub_msgs --cmake-args -DPYTHON_EXECUTABLE=/usr/bin/python3 +source scripts/build_vision_ws.sh ``` -Ensuite, celle ci suffit : +### Lancement + +#### Traitement Yolo toutes les caméras ```bash -colcon build --packages-select py_pubsub_msgs +bash scripts/run_all_camera_yolo.sh +# ou +ros2 launch py_pubsub launch_all_camera.py ``` -## Utilisation +#### Display toutes les caméras -Activer la venv : ```bash -source .venv/bin/activate +bash scripts/run_all_display.sh +# ou +ros2 launch py_pubsub launch_all_camera.py ``` -Désactiver la venv : -```bash -deactivate -``` +#### Traitement Yolo sur une caméra -Lancement script traitement caméra (entre 1 et 4): ```bash -bash run_camera_yolo.sh +bash scripts/run_camera_yolo.sh +ros2 run py_pubsub camera --ros-args -p image_topic_id:= ``` -Lancement script display caméra (entre 1 et 4): +#### Display une caméra + ```bash -bash run_display.sh +bash scripts/run_display.sh +ros2 run py_pubsub display --ros-args -p image_topic_id:= ``` -## Communication - -Les messages sont : -* angles et classes + header : py_pubsub_msgs.msg ClassCoordinates -* l'image au format standard ROS : sensor_msgs.msg Image +## Prototype -# Charger l'environnement ROS -source /opt/ros/foxy/setup.bash +Activer la venv : -# Construire le package spécifié ```bash -colcon build --packages-select py_pubsub_msgs --cmake-args -DPYTHON_EXECUTABLE=/usr/bin/python3 -colcon build --packages-select py_pubsub --cmake-args -DPYTHON_EXECUTABLE=/usr/bin/python3 +source .venv/bin/activate +``` -source install/setup.bash +Désactiver la venv : + +```bash +deactivate ``` diff --git a/vision_ws/scripts/run_all_camera_yolo.sh b/vision_ws/scripts/run_all_camera_yolo.sh new file mode 100644 index 0000000..9f5eb84 --- /dev/null +++ b/vision_ws/scripts/run_all_camera_yolo.sh @@ -0,0 +1,3 @@ +source install/setup.bash + +ros2 launch py_pubsub launch_all_camera.py \ No newline at end of file diff --git a/vision_ws/scripts/run_all_display.sh b/vision_ws/scripts/run_all_display.sh new file mode 100644 index 0000000..1fab485 --- /dev/null +++ b/vision_ws/scripts/run_all_display.sh @@ -0,0 +1,3 @@ +source install/setup.bash + +ros2 launch py_pubsub launch_all_display.py \ No newline at end of file diff --git a/vision_ws/scripts/run_camera_yolo.sh b/vision_ws/scripts/run_camera_yolo.sh index 1beee93..9c5bb7d 100644 --- a/vision_ws/scripts/run_camera_yolo.sh +++ b/vision_ws/scripts/run_camera_yolo.sh @@ -7,11 +7,5 @@ source install/setup.bash cam_id=${1:-1} echo "Cam : $cam_id" -yolo_version=${2:-v8} -echo "Yolo : $yolo_version" - -tracker_enabled=${3:-False} -echo "Tracker : $tracker_enabled" - # Exécuter camera_yolo avec l'identifiant de la caméra -ros2 run py_pubsub camera_yolo --cam $cam_id --yolo $yolo_version --tracker $tracker_enabled \ No newline at end of file +ros2 run py_pubsub camera_yolo --ros-args -p cam_id:=$cam_id diff --git a/vision_ws/scripts/run_display.sh b/vision_ws/scripts/run_display.sh index f2ba8f7..88776e2 100644 --- a/vision_ws/scripts/run_display.sh +++ b/vision_ws/scripts/run_display.sh @@ -4,8 +4,7 @@ source install/setup.bash # Exécuter le noeud ROS avec l'argument de la caméra passé au script -cam_id=${1:-1} -topic_name="annotated_images_$cam_id" +topic_id=${1:-1} # Exécuter display avec l'identifiant de la caméra -ros2 run py_pubsub display --topic "$topic_name" \ No newline at end of file +ros2 run py_pubsub display --ros-args -p image_topic_id:=$topic_id diff --git a/vision_ws/src/py_pubsub/launch/launch_all_camera.py b/vision_ws/src/py_pubsub/launch/launch_all_camera.py new file mode 100644 index 0000000..a462c37 --- /dev/null +++ b/vision_ws/src/py_pubsub/launch/launch_all_camera.py @@ -0,0 +1,22 @@ +import launch +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + ld = LaunchDescription() + + for cam_id in range(1, 5): + node = Node( + package='py_pubsub', + executable='camera_yolo', + name=f'camera_yolo_{cam_id}', + parameters=[ + {"cam_id": cam_id} + ] + ) + ld.add_action(node) + + return ld + +if __name__ == '__main__': + generate_launch_description() \ No newline at end of file diff --git a/vision_ws/src/py_pubsub/launch/launch_all_display.py b/vision_ws/src/py_pubsub/launch/launch_all_display.py new file mode 100644 index 0000000..ab89f6a --- /dev/null +++ b/vision_ws/src/py_pubsub/launch/launch_all_display.py @@ -0,0 +1,22 @@ +import launch +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + ld = LaunchDescription() + + for cam_id in range(1, 5): + node = Node( + package='py_pubsub', + executable='display', + name=f'display_{cam_id}', + parameters=[ + {"image_topic_id": cam_id} + ] + ) + ld.add_action(node) + + return ld + +if __name__ == '__main__': + generate_launch_description() \ No newline at end of file diff --git a/vision_ws/src/py_pubsub/package.xml b/vision_ws/src/py_pubsub/package.xml index ce93706..8d32798 100644 --- a/vision_ws/src/py_pubsub/package.xml +++ b/vision_ws/src/py_pubsub/package.xml @@ -16,6 +16,8 @@ ament_pep257 python3-pytest + ros2launch + ament_python diff --git a/vision_ws/src/py_pubsub/py_pubsub/camera_yolo.py b/vision_ws/src/py_pubsub/py_pubsub/camera_yolo.py index e9fc733..09589e4 100644 --- a/vision_ws/src/py_pubsub/py_pubsub/camera_yolo.py +++ b/vision_ws/src/py_pubsub/py_pubsub/camera_yolo.py @@ -3,12 +3,11 @@ from sensor_msgs.msg import Image from cv_bridge import CvBridge import cv2 -import argparse + from ultralytics import YOLO # YOLOv8 import from ultralytics.utils import LOGGER # LOGGER import import torch import time -from deep_sort_realtime.deepsort_tracker import DeepSort from py_pubsub_msgs.msg import CameraDetectionStamped from py_pubsub_msgs.msg import CameraDetectionStampedArray @@ -22,7 +21,6 @@ class YOLOv8Detector: type = "YOLOv8" - def __init__(self): try: self.model = YOLO(f"{models_path}yolov8s.pt") @@ -35,80 +33,36 @@ def compute(self, image): return results -class YOLOv5Detector: - type = "YOLOv5" - - def __init__(self, model_name=f"{models_path}/yolov5l.pt"): - # Load YOLOv5 model - try: - self.model = torch.hub.load( - "ultralytics/yolov5", - "custom", - path=f"{models_path}yolv5l_custom.pt", - ) - except Exception as e: - LOGGER.error( - f"Error loading custom YOLOv5 model. {e} \nLoading default model." - ) - try: - self.model = YOLO(model_name) - except Exception as e: - LOGGER.error(f"Error loading default YOLOv5 model. {e}") - self.model = None - - def compute(self, image): - """Process a single image""" - if image is not None: - results = self.model(image) - return results - - class MinimalPublisher(Node): - def __init__(self, camera_id, yolo_version, tracker_enabled): + def __init__(self): super().__init__("minimal_publisher") - self.camera_id = camera_id + self.declare_parameter('cam_id', 1) + + self.camera_id = self.get_parameter('cam_id').get_parameter_value().integer_value - self.tracker_enabled = tracker_enabled - - self.topic_name_image = f"annotated_images_{camera_id}" + # Publishers + self.topic_name_image = f"annotated_images_{ self.camera_id}" self.publisher_annotated_image = self.create_publisher( Image, self.topic_name_image, 10 ) - self.topic_name_information = f"camera_detection_{camera_id}" + self.topic_name_information = f"camera_detection_{ self.camera_id}" self.publisher_information = self.create_publisher( CameraDetectionStampedArray, self.topic_name_information, 10 ) + # Subscribers self.subscription = self.create_subscription( - Image, f"Cam{camera_id}/image_raw", self.listener_callback, 10 + Image, f"Cam{self.camera_id}/image_raw", self.listener_callback, 10 ) self._cv_bridge = CvBridge() - if yolo_version == "v8": - self.detector = YOLOv8Detector() - elif yolo_version == "v5": - self.detector = YOLOv5Detector() - - if tracker_enabled: - self.tracker = DeepSort( - max_age=5, - n_init=2, - nms_max_overlap=1.0, - max_cosine_distance=0.3, - nn_budget=None, - override_track_class=None, - embedder="mobilenet", - half=True, - bgr=True, - embedder_gpu=True, - embedder_model_name=None, - embedder_wts=None, - polygon=False, - today=None, - ) + + self.detector = YOLOv8Detector() + self.get_logger().info( - f"Camera {camera_id} YOLO{yolo_version} Tracker {tracker_enabled} Node has started" + f"Camera { self.camera_id} Node has started" ) + def toData(self, result, classes): bounding_box_height = result[3] - result[1] @@ -158,49 +112,15 @@ def listener_callback(self, image): results = self.detector.compute(cv_image) if results: # Annotated image part - # check type of results - # YOLOv8-v5 - if isinstance(results, list): - annotated_image = results[0].plot() - new_results = results[0].boxes.data.cpu().tolist() - class_names = results[0].names - - # yoloV5 - else: - annotated_image = results.render()[0] - # For information datas - new_results = results.xyxy[0].cpu().numpy().tolist() - class_names = results.names - - if self.tracker_enabled: - # For tracker - res = results.xyxyn[0][:, -1], results.xyxyn[0][:, :-1] - detections = self.formalize_detection( - res, height=cv_image.shape[0], width=cv_image.shape[1] - ) - tracks = self.tracker.update_tracks(detections, frame=cv_image) - for track in tracks: - if not track.is_confirmed(): - continue - track_id = track.track_id - ltrb = track.to_ltrb() - bbox = ltrb - cv2.putText( - cv_image, - "ID : " + str(track_id), - (int(bbox[0]) + 10, int(bbox[1] + 30)), - cv2.FONT_HERSHEY_SIMPLEX, - 0.7, - (0, 0, 0), - thickness=2, - ) - encoded_annotated_image = self._cv_bridge.cv2_to_imgmsg( - cv_image, "rgb8" - ) - else: - encoded_annotated_image = self._cv_bridge.cv2_to_imgmsg( - annotated_image, "rgb8" - ) + # YOLOv8 + + annotated_image = results[0].plot() + new_results = results[0].boxes.data.cpu().tolist() + class_names = results[0].names + + encoded_annotated_image = self._cv_bridge.cv2_to_imgmsg( + annotated_image, "rgb8" + ) self.publisher_annotated_image.publish(encoded_annotated_image) @@ -236,29 +156,11 @@ def listener_callback(self, image): def main(args=None): - # Initialize ROS without passing args - rclpy.init() - - # Create an argument parser for your script - parser = argparse.ArgumentParser(description="ROS 2 YOLO Object Detection Node") - - # Add your custom argument - parser.add_argument("--cam", type=str, default="1", help="Camera identifier") - parser.add_argument("--yolo", type=str, default="v8", help="Yolo version") - parser.add_argument( - "--tracker", type=str, default="false", help="Enable tracker (false | true)" - ) - - # Parse the command line arguments - custom_args = parser.parse_args() - - if custom_args.tracker == "true" or custom_args.tracker == "True": - tracker = True - else: - tracker = False + # Initialize ROS with args + rclpy.init(args=args) # Create and spin your node - minimal_publisher = MinimalPublisher(custom_args.cam, custom_args.yolo, tracker) + minimal_publisher = MinimalPublisher() rclpy.spin(minimal_publisher) # Shutdown and cleanup diff --git a/vision_ws/src/py_pubsub/py_pubsub/display.py b/vision_ws/src/py_pubsub/py_pubsub/display.py index 3a380b4..59e6e2b 100644 --- a/vision_ws/src/py_pubsub/py_pubsub/display.py +++ b/vision_ws/src/py_pubsub/py_pubsub/display.py @@ -3,20 +3,24 @@ from sensor_msgs.msg import Image from cv_bridge import CvBridge import cv2 -import argparse -from rclpy.utilities import remove_ros_args -import sys class ImageDisplayNode(Node): - def __init__(self, topic_name): - self.topic_name = topic_name + def __init__(self): super().__init__("image_display_node") + self.declare_parameter('image_topic_id', 1) + + self.topic_id = self.get_parameter('image_topic_id').get_parameter_value().integer_value + + self.topic_name= "annotated_images_" + str(self.topic_id) + self.subscription = self.create_subscription( Image, self.topic_name, self.listener_callback, 10 ) self._cv_bridge = CvBridge() + self.get_logger().info(f"Displaying images from topic {self.topic_name}") + def listener_callback(self, image): cv_image = self._cv_bridge.imgmsg_to_cv2(image, desired_encoding="rgb8") cv2.imshow(f"Annotated Image {self.topic_name}", cv_image) @@ -25,21 +29,10 @@ def listener_callback(self, image): def main(args=None): # Initialize ROS without passing args - rclpy.init() - - # Create an argument parser for your script - parser = argparse.ArgumentParser(description="Image Display Node") - - # Add your custom argument - parser.add_argument( - "--topic", type=str, default="1", required=True, help="Topic to subscribe to" - ) - - # Parse the command line arguments - custom_args = parser.parse_args() + rclpy.init(args=args) # Create and spin your node - image_display_node = ImageDisplayNode(topic_name=custom_args.topic) + image_display_node = ImageDisplayNode() rclpy.spin(image_display_node) # Shutdown and cleanup diff --git a/vision_ws/src/py_pubsub/setup.py b/vision_ws/src/py_pubsub/setup.py index cb4b807..1bf1b92 100644 --- a/vision_ws/src/py_pubsub/setup.py +++ b/vision_ws/src/py_pubsub/setup.py @@ -1,4 +1,6 @@ from setuptools import setup, find_packages +from glob import glob +import os package_name = "py_pubsub" @@ -11,6 +13,7 @@ data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), ("share/" + package_name, ["package.xml"]), + (os.path.join('share/', package_name), glob('launch/*.py')) ], install_requires=["setuptools"], zip_safe=True, @@ -24,6 +27,7 @@ "display = py_pubsub.display:main", "camera_yolo = py_pubsub.camera_yolo:main", "simu_ecal = py_pubsub.simu_ecal_image:main", + "launch_all_camera = launch.launch_all_camera:main", ], }, )