diff --git a/rbs_perception/CMakeLists.txt b/rbs_perception/CMakeLists.txt new file mode 100644 index 0000000..bcbbdc0 --- /dev/null +++ b/rbs_perception/CMakeLists.txt @@ -0,0 +1,44 @@ +cmake_minimum_required(VERSION 3.8) +project(rbs_perception) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) +find_package(rclpy REQUIRED) +find_package(image_transport REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(std_msgs REQUIRED) +#find_package(opencv2 REQUIRED) + +# Install Python modules +ament_python_install_package(${PROJECT_NAME}) + +# Install Python executables +install(PROGRAMS + scripts/detection_service.py + DESTINATION lib/${PROJECT_NAME} +) + +install( + DIRECTORY config + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/rbs_perception/config/detection_config.json b/rbs_perception/config/detection_config.json new file mode 100644 index 0000000..634cd99 --- /dev/null +++ b/rbs_perception/config/detection_config.json @@ -0,0 +1,5 @@ +{ + "topicName":"image_sub", + "topicImage":"/ground_true/camera_node", + "weightsFile":"yolov4_objs2_final.weights" +} diff --git a/rbs_perception/package.xml b/rbs_perception/package.xml new file mode 100644 index 0000000..c2b3d2a --- /dev/null +++ b/rbs_perception/package.xml @@ -0,0 +1,25 @@ + + + + rbs_perception + 0.0.0 + An image publisher and subscriber node that uses OpenCV + shalenikol + Apache License 2.0 + + ament_cmake + ament_cmake_python + + rclpy + image_transport + cv_bridge + sensor_msgs + std_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/rbs_perception/rbs_perception/__init__.py b/rbs_perception/rbs_perception/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/rbs_perception/scripts/detection_service.py b/rbs_perception/scripts/detection_service.py new file mode 100755 index 0000000..27494a6 --- /dev/null +++ b/rbs_perception/scripts/detection_service.py @@ -0,0 +1,133 @@ +#!/usr/bin/env python3 +""" + detection_service + ROS 2 program for Object Detection + + 25.04.2023 @shalenikol release 0.1 +""" + +# Import the necessary libraries +import rclpy # Python library for ROS 2 +from rclpy.node import Node # Handles the creation of nodes +from sensor_msgs.msg import Image # Image is the message type +from cv_bridge import CvBridge # Package to convert between ROS and OpenCV Images +import cv2 # OpenCV library +import subprocess +import os +import json +from ament_index_python.packages import get_package_share_directory + +class ImageSubscriber(Node): + """ + Create an ImageSubscriber class, which is a subclass of the Node class. + """ + def _InitService(self): + # Initialization service data + p = os.path.join(get_package_share_directory("rbs_perception"), "config", "detection_config.json") + # load config + with open(p, "r") as f: + y = json.load(f) + + for name, val in y.items(): + if name == "topicName": + self.topicName = val + elif name == "topicImage": + self.topicImage = val + elif name == "weightsFile": + self.weightsFile = val + elif name == "topicPubName": + self.topicPubName = val + + def __init__(self): + """ + Class constructor to set up the node + """ + self.topicName = "image_subscriber" + self.topicImage = "/ground_true/camera_node" + self.weightsFile = "yolov4_objs2_final.weights" + self.topicPubName = "detection_images" + self._InitService() + + # Initiate the Node class's constructor and give it a name + super().__init__(self.topicName) + + # Create the subscriber. This subscriber will receive an Image + # from the video_frames topic. The queue size is 10 messages. + self.subscription = self.create_subscription( + Image, + self.topicImage, + self.listener_callback, + 3) + self.subscription # prevent unused variable warning + + # Create the publisher. This publisher will publish an Image + # to the detection_frames topic. The queue size is 5 messages. + self.publisher_ = self.create_publisher(Image, self.topicPubName, 5) + + # Used to convert between ROS and OpenCV images + self.br = CvBridge() + self.cnt = 0 + + def listener_callback(self, data): + """ + Callback function. + """ + # Display the message on the console + self.get_logger().info("Receiving video frame") + + # Convert ROS Image message to OpenCV image + current_frame = self.br.imgmsg_to_cv2(data) + + # Display image + #cv2.imshow("camera", current_frame) + frame_jpg = "cur_frame.jpg" + result_json = "res.json" + cv2.imwrite(frame_jpg, current_frame) + self.cnt += 1 + + # object detection + self.get_logger().info(f"darknet: begin {self.cnt}") # __file__ = <<<{__file__}>>> + subprocess.run(["darknet", "detector", "test", + "yolov4_objs2.data", + "yolov4_objs2.cfg", + self.weightsFile, + "-dont_show", "-ext_output", + "-out", result_json, frame_jpg] + ) # f"res{self.cnt}.json" + # удалим временный файл darknet + f = "bad.list" + if os.path.isfile(f): + os.remove(f) + self.get_logger().info(f"darknet: end {self.cnt}") + + f = "predictions.jpg" + if os.path.isfile(f): + frame = cv2.imread(f) + # Publish the image. + # The 'cv2_to_imgmsg' method converts an OpenCV + # image to a ROS 2 image message + self.publisher_.publish(self.br.cv2_to_imgmsg(frame, encoding="rgb8")) + + cv2.waitKey(1) + +def main(args=None): + + # Initialize the rclpy library + rclpy.init(args=args) + + # Create the node + image_subscriber = ImageSubscriber() + + # Spin the node so the callback function is called. + rclpy.spin(image_subscriber) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + image_subscriber.destroy_node() + + # Shutdown the ROS client library for Python + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/rbs_simulation/worlds/mir.sdf b/rbs_simulation/worlds/mir.sdf index 4031233..d9c8b98 100644 --- a/rbs_simulation/worlds/mir.sdf +++ b/rbs_simulation/worlds/mir.sdf @@ -245,7 +245,7 @@ 1 - 2 0 1.0 0 0.0 3.14 + -2.0 -0.55 1.44 0 0.44 0.44 0.05 0.05 0.05 0 0 0 @@ -274,8 +274,8 @@ 1.047 - 320 - 240 + 640 + 480 0.1