From 16060df85c343d936cbf1072adf525535a368f01 Mon Sep 17 00:00:00 2001 From: Ar-Ray-code Date: Mon, 12 Aug 2024 10:21:34 +0900 Subject: [PATCH 1/5] update yolox_params for switching pub-sub interface --- .../yolox_param/src/yolox_parameters.yaml | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/yolox_ros_cpp/yolox_param/src/yolox_parameters.yaml b/yolox_ros_cpp/yolox_param/src/yolox_parameters.yaml index f136641..a972b85 100644 --- a/yolox_ros_cpp/yolox_param/src/yolox_parameters.yaml +++ b/yolox_ros_cpp/yolox_param/src/yolox_parameters.yaml @@ -6,7 +6,7 @@ yolox_parameters: model_path: type: string description: "Path to the model file." - default_value: "./src/YOLOX-ROS/weights/tflite/model.tflite" + default_value: "example.onnx" class_labels_path: type: string description: "Path to the class labels file." @@ -38,9 +38,9 @@ yolox_parameters: openvino_device: type: string description: "OpenVINO device." - default_value: "CPU" + default_value: "AUTO" validation: { - one_of<>: [["CPU", "GPU", "MYRIAD"]] + one_of<>: [["AUTO", "CPU", "GPU", "NPU", "MYRIAD"]] } onnxruntime_use_cuda: type: bool @@ -86,3 +86,11 @@ yolox_parameters: type: string description: "Publish bounding box topic name." default_value: "yolox/bounding_boxes" + use_bbox_ex_msgs: + type: bool + description: "Enable or disable bbox_ex_msgs. If true, disable vision_msgs::Detection2DArray." + default_value: false + publish_resized_image: + type: bool + description: "Enable or disable resized image." + default_value: false \ No newline at end of file From 23755b957cc8edce7c535dc7dc9ddd42b6a71238 Mon Sep 17 00:00:00 2001 From: Ar-Ray-code Date: Mon, 12 Aug 2024 10:22:29 +0900 Subject: [PATCH 2/5] add new params and support usb_cam instead of v4l2_camera --- .../launch/yolox_onnxruntime.launch.py | 20 ++++++++++++---- .../launch/yolox_openvino.launch.py | 20 ++++++++++++---- .../launch/yolox_tensorrt.launch.py | 23 ++++++++++++++----- .../launch/yolox_tflite.launch.py | 20 ++++++++++++---- 4 files changed, 65 insertions(+), 18 deletions(-) diff --git a/yolox_ros_cpp/yolox_ros_cpp/launch/yolox_onnxruntime.launch.py b/yolox_ros_cpp/yolox_ros_cpp/launch/yolox_onnxruntime.launch.py index 49b1538..df34cdb 100644 --- a/yolox_ros_cpp/yolox_ros_cpp/launch/yolox_onnxruntime.launch.py +++ b/yolox_ros_cpp/yolox_ros_cpp/launch/yolox_onnxruntime.launch.py @@ -105,6 +105,16 @@ def generate_launch_description(): default_value='/yolox/bounding_boxes', description='topic name for publishing bounding box message.' ), + DeclareLaunchArgument( + 'use_bbox_ex_msgs', + default_value='false', + description='use BoundingBoxArray message type.' + ), + DeclareLaunchArgument( + 'publish_resized_image', + default_value='false', + description='use BoundingBoxArray message type.' + ), ] container = ComposableNodeContainer( name='yolox_container', @@ -113,12 +123,12 @@ def generate_launch_description(): executable='component_container', composable_node_descriptions=[ ComposableNode( - package='v4l2_camera', - plugin='v4l2_camera::V4L2Camera', - name='v4l2_camera', + package='usb_cam', + plugin='usb_cam::UsbCamNode', + name='usb_cam_node', parameters=[{ 'video_device': LaunchConfiguration('video_device'), - 'image_size': [640, 480] + 'brightness': 100 }]), ComposableNode( package='yolox_ros_cpp', @@ -142,6 +152,8 @@ def generate_launch_description(): 'src_image_topic_name': LaunchConfiguration('src_image_topic_name'), 'publish_image_topic_name': LaunchConfiguration('publish_image_topic_name'), 'publish_boundingbox_topic_name': LaunchConfiguration('publish_boundingbox_topic_name'), + 'publish_resized_image': LaunchConfiguration('publish_resized_image'), + 'use_bbox_ex_msgs': LaunchConfiguration('use_bbox_ex_msgs'), }], ), ], diff --git a/yolox_ros_cpp/yolox_ros_cpp/launch/yolox_openvino.launch.py b/yolox_ros_cpp/yolox_ros_cpp/launch/yolox_openvino.launch.py index 3342aa9..1395507 100644 --- a/yolox_ros_cpp/yolox_ros_cpp/launch/yolox_openvino.launch.py +++ b/yolox_ros_cpp/yolox_ros_cpp/launch/yolox_openvino.launch.py @@ -71,6 +71,16 @@ def generate_launch_description(): default_value='/yolox/bounding_boxes', description='topic name for publishing bounding box message.' ), + DeclareLaunchArgument( + 'use_bbox_ex_msgs', + default_value='false', + description='use BoundingBoxArray message type.' + ), + DeclareLaunchArgument( + 'publish_resized_image', + default_value='false', + description='use BoundingBoxArray message type.' + ), ] container = ComposableNodeContainer( name='yolox_container', @@ -79,12 +89,12 @@ def generate_launch_description(): executable='component_container', composable_node_descriptions=[ ComposableNode( - package='v4l2_camera', - plugin='v4l2_camera::V4L2Camera', - name='v4l2_camera', + package='usb_cam', + plugin='usb_cam::UsbCamNode', + name='usb_cam_node', parameters=[{ 'video_device': LaunchConfiguration('video_device'), - 'image_size': [640, 480] + 'brightness': 100 }]), ComposableNode( package='yolox_ros_cpp', @@ -104,6 +114,8 @@ def generate_launch_description(): 'src_image_topic_name': LaunchConfiguration('src_image_topic_name'), 'publish_image_topic_name': LaunchConfiguration('publish_image_topic_name'), 'publish_boundingbox_topic_name': LaunchConfiguration('publish_boundingbox_topic_name'), + 'publish_resized_image': LaunchConfiguration('publish_resized_image'), + 'use_bbox_ex_msgs': LaunchConfiguration('use_bbox_ex_msgs'), }], ), ], diff --git a/yolox_ros_cpp/yolox_ros_cpp/launch/yolox_tensorrt.launch.py b/yolox_ros_cpp/yolox_ros_cpp/launch/yolox_tensorrt.launch.py index fabc144..c011aeb 100644 --- a/yolox_ros_cpp/yolox_ros_cpp/launch/yolox_tensorrt.launch.py +++ b/yolox_ros_cpp/yolox_ros_cpp/launch/yolox_tensorrt.launch.py @@ -72,6 +72,16 @@ def generate_launch_description(): default_value='/yolox/bounding_boxes', description='topic name for publishing bounding box message.' ), + DeclareLaunchArgument( + 'use_bbox_ex_msgs', + default_value='false', + description='use BoundingBoxArray message type.' + ), + DeclareLaunchArgument( + 'publish_resized_image', + default_value='false', + description='use BoundingBoxArray message type.' + ), ] container = ComposableNodeContainer( name='yolox_container', @@ -80,14 +90,13 @@ def generate_launch_description(): executable='component_container', composable_node_descriptions=[ ComposableNode( - package='v4l2_camera', - plugin='v4l2_camera::V4L2Camera', - name='v4l2_camera', + package='usb_cam', + plugin='usb_cam::UsbCamNode', + name='usb_cam_node', parameters=[{ 'video_device': LaunchConfiguration('video_device'), - 'image_size': [640, 480] - }] - ), + 'brightness': 100 + }]), ComposableNode( package='yolox_ros_cpp', plugin='yolox_ros_cpp::YoloXNode', @@ -106,6 +115,8 @@ def generate_launch_description(): 'src_image_topic_name': LaunchConfiguration('src_image_topic_name'), 'publish_image_topic_name': LaunchConfiguration('publish_image_topic_name'), 'publish_boundingbox_topic_name': LaunchConfiguration('publish_boundingbox_topic_name'), + 'publish_resized_image': LaunchConfiguration('publish_resized_image'), + 'use_bbox_ex_msgs': LaunchConfiguration('use_bbox_ex_msgs'), }], ), ], diff --git a/yolox_ros_cpp/yolox_ros_cpp/launch/yolox_tflite.launch.py b/yolox_ros_cpp/yolox_ros_cpp/launch/yolox_tflite.launch.py index 278fb59..0f3781f 100644 --- a/yolox_ros_cpp/yolox_ros_cpp/launch/yolox_tflite.launch.py +++ b/yolox_ros_cpp/yolox_ros_cpp/launch/yolox_tflite.launch.py @@ -77,6 +77,16 @@ def generate_launch_description(): default_value='/yolox/bounding_boxes', description='topic name for publishing bounding box message.' ), + DeclareLaunchArgument( + 'use_bbox_ex_msgs', + default_value='false', + description='use BoundingBoxArray message type.' + ), + DeclareLaunchArgument( + 'publish_resized_image', + default_value='false', + description='use BoundingBoxArray message type.' + ), ] container = ComposableNodeContainer( name='yolox_container', @@ -85,12 +95,12 @@ def generate_launch_description(): executable='component_container', composable_node_descriptions=[ ComposableNode( - package='v4l2_camera', - plugin='v4l2_camera::V4L2Camera', - name='v4l2_camera', + package='usb_cam', + plugin='usb_cam::UsbCamNode', + name='usb_cam_node', parameters=[{ 'video_device': LaunchConfiguration('video_device'), - 'image_size': [640, 480] + 'brightness': 100 }]), ComposableNode( package='yolox_ros_cpp', @@ -111,6 +121,8 @@ def generate_launch_description(): 'src_image_topic_name': LaunchConfiguration('src_image_topic_name'), 'publish_image_topic_name': LaunchConfiguration('publish_image_topic_name'), 'publish_boundingbox_topic_name': LaunchConfiguration('publish_boundingbox_topic_name'), + 'publish_resized_image': LaunchConfiguration('publish_resized_image'), + 'use_bbox_ex_msgs': LaunchConfiguration('use_bbox_ex_msgs'), }], ), ], From c1831f3a32f69e13b69abc18b91c96446250dd17 Mon Sep 17 00:00:00 2001 From: Ar-Ray-code Date: Mon, 12 Aug 2024 10:22:53 +0900 Subject: [PATCH 3/5] update yolox_cpp interface --- yolox_ros_cpp/README.md | 3 +- .../yolox_cpp/src/yolox_openvino.cpp | 6 ++ .../include/yolox_ros_cpp/yolox_ros_cpp.hpp | 23 +++--- yolox_ros_cpp/yolox_ros_cpp/package.xml | 3 + .../yolox_ros_cpp/src/yolox_ros_cpp.cpp | 76 +++++++++++++++---- 5 files changed, 86 insertions(+), 25 deletions(-) diff --git a/yolox_ros_cpp/README.md b/yolox_ros_cpp/README.md index d368dae..8e06517 100644 --- a/yolox_ros_cpp/README.md +++ b/yolox_ros_cpp/README.md @@ -195,8 +195,7 @@ ros2 launch yolox_ros_cpp yolox_tflite.launch.py - See [here](https://github.com/fateshelled/YOLOX-ROS/blob/dev_cpp/yolox_ros_cpp/yolox_ros_cpp/labels/coco_names.txt) for label format. - `num_classes`: 80 - `model_version`: 0.1.1rc0 -- `openvino/device`: CPU -- `conf`: 0.3 +- `openvino/device`: AUTO - `nms`: 0.45 - `imshow_isshow`: true - `src_image_topic_name`: /image_raw diff --git a/yolox_ros_cpp/yolox_cpp/src/yolox_openvino.cpp b/yolox_ros_cpp/yolox_cpp/src/yolox_openvino.cpp index 3a4aad5..d0a63aa 100644 --- a/yolox_ros_cpp/yolox_cpp/src/yolox_openvino.cpp +++ b/yolox_ros_cpp/yolox_cpp/src/yolox_openvino.cpp @@ -17,6 +17,12 @@ namespace yolox_cpp{ const auto network = ie.read_model(path_to_model); // Step 3. Loading a model to the device + std::vector available_devices = ie.get_available_devices(); + std::cout << "======= AVAILABLE DEVICES FOR OPENVINO =======" << std::endl; + for (auto device : available_devices) { + std::cout << "- " << device << std::endl; + } + std::cout << "==============================================" << std::endl; std::cout << "Loading a model to the device: " << device_name_ << std::endl; auto compiled_model = ie.compile_model(network, device_name); diff --git a/yolox_ros_cpp/yolox_ros_cpp/include/yolox_ros_cpp/yolox_ros_cpp.hpp b/yolox_ros_cpp/yolox_ros_cpp/include/yolox_ros_cpp/yolox_ros_cpp.hpp index 06d1739..43e0511 100644 --- a/yolox_ros_cpp/yolox_ros_cpp/include/yolox_ros_cpp/yolox_ros_cpp.hpp +++ b/yolox_ros_cpp/yolox_ros_cpp/include/yolox_ros_cpp/yolox_ros_cpp.hpp @@ -3,11 +3,13 @@ #include #include +#include #include -#include #include #include - +#include +#include +#include #include "bboxes_ex_msgs/msg/bounding_box.hpp" #include "bboxes_ex_msgs/msg/bounding_boxes.hpp" @@ -17,28 +19,29 @@ #include "yolox_param/yolox_param.hpp" namespace yolox_ros_cpp{ - class YoloXNode : public rclcpp::Node { public: - YoloXNode(const rclcpp::NodeOptions&); + YoloXNode(const rclcpp::NodeOptions &); + private: + void onInit(); + void colorImageCallback(const sensor_msgs::msg::Image::ConstSharedPtr &); + + static bboxes_ex_msgs::msg::BoundingBoxes objects_to_bboxes(const cv::Mat &, const std::vector &, const std_msgs::msg::Header &); + static vision_msgs::msg::Detection2DArray objects_to_detection2d(const std::vector &, const std_msgs::msg::Header &); protected: std::shared_ptr param_listener_; yolox_parameters::Params params_; private: - void onInit(); - rclcpp::TimerBase::SharedPtr init_timer_; - std::unique_ptr yolox_; std::vector class_names_; + rclcpp::TimerBase::SharedPtr init_timer_; image_transport::Subscriber sub_image_; - void colorImageCallback(const sensor_msgs::msg::Image::ConstSharedPtr&); rclcpp::Publisher::SharedPtr pub_bboxes_; + rclcpp::Publisher::SharedPtr pub_detection2d_; image_transport::Publisher pub_image_; - - bboxes_ex_msgs::msg::BoundingBoxes objects_to_bboxes(const cv::Mat&, const std::vector&, const std_msgs::msg::Header&); }; } diff --git a/yolox_ros_cpp/yolox_ros_cpp/package.xml b/yolox_ros_cpp/yolox_ros_cpp/package.xml index 4c80eb1..4ae869e 100644 --- a/yolox_ros_cpp/yolox_ros_cpp/package.xml +++ b/yolox_ros_cpp/yolox_ros_cpp/package.xml @@ -18,9 +18,12 @@ std_msgs sensor_msgs bboxes_ex_msgs + vision_msgs yolox_cpp yolox_param + usb_cam + ament_lint_auto ament_lint_common diff --git a/yolox_ros_cpp/yolox_ros_cpp/src/yolox_ros_cpp.cpp b/yolox_ros_cpp/yolox_ros_cpp/src/yolox_ros_cpp.cpp index 70f6e19..e501cb4 100644 --- a/yolox_ros_cpp/yolox_ros_cpp/src/yolox_ros_cpp.cpp +++ b/yolox_ros_cpp/yolox_ros_cpp/src/yolox_ros_cpp.cpp @@ -95,10 +95,20 @@ namespace yolox_ros_cpp this, this->params_.src_image_topic_name, std::bind(&YoloXNode::colorImageCallback, this, std::placeholders::_1), "raw"); - this->pub_bboxes_ = this->create_publisher( - this->params_.publish_boundingbox_topic_name, - 10); - this->pub_image_ = image_transport::create_publisher(this, this->params_.publish_image_topic_name); + + if (this->params_.use_bbox_ex_msgs) { + this->pub_bboxes_ = this->create_publisher( + this->params_.publish_boundingbox_topic_name, + 10); + } else { + this->pub_detection2d_ = this->create_publisher( + this->params_.publish_boundingbox_topic_name, + 10); + } + + if (this->params_.publish_resized_image) { + this->pub_image_ = image_transport::create_publisher(this, this->params_.publish_image_topic_name); + } } void YoloXNode::colorImageCallback(const sensor_msgs::msg::Image::ConstSharedPtr &ptr) @@ -108,10 +118,10 @@ namespace yolox_ros_cpp auto now = std::chrono::system_clock::now(); auto objects = this->yolox_->inference(frame); - auto end = std::chrono::system_clock::now(); + auto elapsed = std::chrono::duration_cast(end - now); - RCLCPP_INFO(this->get_logger(), "Inference: %f FPS", 1000.0f / elapsed.count()); + RCLCPP_INFO(this->get_logger(), "Inference time: %5ld ms", elapsed.count()); yolox_cpp::utils::draw_objects(frame, objects, this->class_names_); if (this->params_.imshow_isshow) @@ -124,12 +134,32 @@ namespace yolox_ros_cpp } } - auto boxes = objects_to_bboxes(frame, objects, img->header); - this->pub_bboxes_->publish(boxes); + if (this->params_.use_bbox_ex_msgs) + { + if (this->pub_bboxes_ == nullptr) + { + RCLCPP_ERROR(this->get_logger(), "pub_bboxes_ is nullptr"); + return; + } + auto boxes = objects_to_bboxes(frame, objects, img->header); + this->pub_bboxes_->publish(boxes); + } + else + { + if (this->pub_detection2d_ == nullptr) + { + RCLCPP_ERROR(this->get_logger(), "pub_detection2d_ is nullptr"); + return; + } + vision_msgs::msg::Detection2DArray detections = objects_to_detection2d(objects, img->header); + this->pub_detection2d_->publish(detections); + } - sensor_msgs::msg::Image::SharedPtr pub_img; - pub_img = cv_bridge::CvImage(img->header, "bgr8", frame).toImageMsg(); - this->pub_image_.publish(pub_img); + if (this->params_.publish_resized_image) { + sensor_msgs::msg::Image::SharedPtr pub_img = + cv_bridge::CvImage(img->header, "bgr8", frame).toImageMsg(); + this->pub_image_.publish(pub_img); + } } bboxes_ex_msgs::msg::BoundingBoxes YoloXNode::objects_to_bboxes( @@ -140,8 +170,8 @@ namespace yolox_ros_cpp for (const auto &obj : objects) { bboxes_ex_msgs::msg::BoundingBox box; - box.probability = obj.prob; - box.class_id = this->class_names_[obj.label]; + box.probability = obj.prob;; + box.class_id = std::to_string(obj.label); box.xmin = obj.rect.x; box.ymin = obj.rect.y; box.xmax = (obj.rect.x + obj.rect.width); @@ -152,6 +182,26 @@ namespace yolox_ros_cpp } return boxes; } + + vision_msgs::msg::Detection2DArray YoloXNode::objects_to_detection2d(const std::vector &objects, const std_msgs::msg::Header &header) + { + vision_msgs::msg::Detection2DArray detection2d; + detection2d.header = header; + for (const auto &obj : objects) + { + vision_msgs::msg::Detection2D det; + det.bbox.center.position.x = obj.rect.x + obj.rect.width / 2; + det.bbox.center.position.y = obj.rect.y + obj.rect.height / 2; + det.bbox.size_x = obj.rect.width; + det.bbox.size_y = obj.rect.height; + + det.results.resize(1); + det.results[0].hypothesis.class_id = std::to_string(obj.label); + det.results[0].hypothesis.score = obj.prob; + detection2d.detections.emplace_back(det); + } + return detection2d; + } } RCLCPP_COMPONENTS_REGISTER_NODE(yolox_ros_cpp::YoloXNode) From 52002efc6e48a03e9a4306a58c90ddfb42cb7c9a Mon Sep 17 00:00:00 2001 From: Ar-Ray-code Date: Mon, 12 Aug 2024 10:36:42 +0900 Subject: [PATCH 4/5] cv_bridge.h -> cv_bridge.hpp --- .../yolox_ros_cpp/include/yolox_ros_cpp/yolox_ros_cpp.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/yolox_ros_cpp/yolox_ros_cpp/include/yolox_ros_cpp/yolox_ros_cpp.hpp b/yolox_ros_cpp/yolox_ros_cpp/include/yolox_ros_cpp/yolox_ros_cpp.hpp index 43e0511..2bde92a 100644 --- a/yolox_ros_cpp/yolox_ros_cpp/include/yolox_ros_cpp/yolox_ros_cpp.hpp +++ b/yolox_ros_cpp/yolox_ros_cpp/include/yolox_ros_cpp/yolox_ros_cpp.hpp @@ -3,7 +3,7 @@ #include #include -#include +#include #include #include #include From 6a43456720be64bda6dc5451a17d64a6ec7defd5 Mon Sep 17 00:00:00 2001 From: Ar-Ray <67567093+Ar-Ray-code@users.noreply.github.com> Date: Mon, 12 Aug 2024 13:41:23 +0900 Subject: [PATCH 5/5] v0.4.0 --- yolox_ros_cpp/yolox_cpp/package.xml | 2 +- yolox_ros_cpp/yolox_param/package.xml | 2 +- yolox_ros_cpp/yolox_ros_cpp/package.xml | 12 ++++++------ 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/yolox_ros_cpp/yolox_cpp/package.xml b/yolox_ros_cpp/yolox_cpp/package.xml index 923d4d8..895a02a 100644 --- a/yolox_ros_cpp/yolox_cpp/package.xml +++ b/yolox_ros_cpp/yolox_cpp/package.xml @@ -2,7 +2,7 @@ yolox_cpp - 0.3.3 + 0.4.0 The yolox_cpp package fateshelled Apache-2.0 License diff --git a/yolox_ros_cpp/yolox_param/package.xml b/yolox_ros_cpp/yolox_param/package.xml index b341c95..385808e 100644 --- a/yolox_ros_cpp/yolox_param/package.xml +++ b/yolox_ros_cpp/yolox_param/package.xml @@ -2,7 +2,7 @@ yolox_param - 0.3.2 + 0.4.0 YOLOX-ROS Parameter Package Ar-Ray-code Apache-2.0 diff --git a/yolox_ros_cpp/yolox_ros_cpp/package.xml b/yolox_ros_cpp/yolox_ros_cpp/package.xml index 4ae869e..8b3bf8f 100644 --- a/yolox_ros_cpp/yolox_ros_cpp/package.xml +++ b/yolox_ros_cpp/yolox_ros_cpp/package.xml @@ -2,7 +2,7 @@ yolox_ros_cpp - 0.3.2 + 0.4.0 The yolox_ros_cpp package fateshelled Apache-2.0 License @@ -10,14 +10,14 @@ ament_cmake_auto - rclcpp - rclcpp_components - OpenCV + bboxes_ex_msgs cv_bridge image_transport - std_msgs + libopencv-dev + rclcpp + rclcpp_components sensor_msgs - bboxes_ex_msgs + std_msgs vision_msgs yolox_cpp yolox_param