Add node for object perception
This commit is contained in:
parent
f0f8d6ca5c
commit
0a735b87c9
6 changed files with 210 additions and 3 deletions
44
rbs_perception/CMakeLists.txt
Normal file
44
rbs_perception/CMakeLists.txt
Normal file
|
@ -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()
|
5
rbs_perception/config/detection_config.json
Normal file
5
rbs_perception/config/detection_config.json
Normal file
|
@ -0,0 +1,5 @@
|
||||||
|
{
|
||||||
|
"topicName":"image_sub",
|
||||||
|
"topicImage":"/ground_true/camera_node",
|
||||||
|
"weightsFile":"yolov4_objs2_final.weights"
|
||||||
|
}
|
25
rbs_perception/package.xml
Normal file
25
rbs_perception/package.xml
Normal file
|
@ -0,0 +1,25 @@
|
||||||
|
<?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>rbs_perception</name>
|
||||||
|
<version>0.0.0</version>
|
||||||
|
<description>An image publisher and subscriber node that uses OpenCV</description>
|
||||||
|
<maintainer email="shaniks77s@gmail.com">shalenikol</maintainer>
|
||||||
|
<license>Apache License 2.0</license>
|
||||||
|
|
||||||
|
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||||
|
<buildtool_depend>ament_cmake_python</buildtool_depend>
|
||||||
|
|
||||||
|
<depend>rclpy</depend>
|
||||||
|
<depend>image_transport</depend>
|
||||||
|
<depend>cv_bridge</depend>
|
||||||
|
<depend>sensor_msgs</depend>
|
||||||
|
<depend>std_msgs</depend>
|
||||||
|
|
||||||
|
<test_depend>ament_lint_auto</test_depend>
|
||||||
|
<test_depend>ament_lint_common</test_depend>
|
||||||
|
|
||||||
|
<export>
|
||||||
|
<build_type>ament_cmake</build_type>
|
||||||
|
</export>
|
||||||
|
</package>
|
0
rbs_perception/rbs_perception/__init__.py
Normal file
0
rbs_perception/rbs_perception/__init__.py
Normal file
133
rbs_perception/scripts/detection_service.py
Executable file
133
rbs_perception/scripts/detection_service.py
Executable file
|
@ -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()
|
|
@ -245,7 +245,7 @@
|
||||||
|
|
||||||
<model name="camera">
|
<model name="camera">
|
||||||
<static>1</static>
|
<static>1</static>
|
||||||
<pose>2 0 1.0 0 0.0 3.14</pose>
|
<pose>-2.0 -0.55 1.44 0 0.44 0.44</pose>
|
||||||
<link name="link">
|
<link name="link">
|
||||||
<pose>0.05 0.05 0.05 0 0 0</pose>
|
<pose>0.05 0.05 0.05 0 0 0</pose>
|
||||||
<inertial>
|
<inertial>
|
||||||
|
@ -274,8 +274,8 @@
|
||||||
<camera>
|
<camera>
|
||||||
<horizontal_fov>1.047</horizontal_fov>
|
<horizontal_fov>1.047</horizontal_fov>
|
||||||
<image>
|
<image>
|
||||||
<width>320</width>
|
<width>640</width>
|
||||||
<height>240</height>
|
<height>480</height>
|
||||||
</image>
|
</image>
|
||||||
<clip>
|
<clip>
|
||||||
<near>0.1</near>
|
<near>0.1</near>
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue