Feat/detection2d array

README.md

@@ -15,6 +15,7 @@ https://user-images.githubusercontent.com/53618876/145365173-29bb3ec1-b088-4ca2-
 - [YOLOX-ROS](https://github.com/Ar-Ray-code/YOLOX-ROS)
 - OpenVINO or TensorRT
 - [ros_video_player](https://github.com/fateshelled/ros_video_player)
+- [detector2d](https://github.com/StrayedCats/detector2d)
 
 ## Usage (OpenVINO)
 ### Install
@@ -44,6 +45,18 @@ ros2 launch bytetrack_cpp_node bytetrack_openvino.launch.py
 ros2 launch bytetrack_cpp_node bytetrack_openvino.launch.py video_path:={video file path}
 
 ```
+#### with detector2d(yolox_trt)
+```bash 
+# terminal 1
+ros2 launch ros_video_player ros_video_player.launch.py video_path:="your video path" loop:=True
+# terminal 2
+ros2 run bytetrack_cpp_node bytetrack_cpp_node
+# terminal 3
+ros2 run detector2d_node detector2d_node_exec --ros-args -p load_target_plugin:=detector2d_plugins::YoloxTrt  -p yolox_trt_plugin model_path:="your weight path" -p yolox_trt_plugin.imshow_isshow:=false
+# terminal 4
+ros2 run bytetrack_viewer bytetrack_viewer
+```
+
 
 ### ros graph
 ![rosgraph](https://user-images.githubusercontent.com/53618876/145340126-8c5fc081-9238-49f8-bace-de2854e546b7.png)

bytetrack_cpp_node/CMakeLists.txt

@@ -21,8 +21,8 @@ find_package(rclcpp REQUIRED)
 find_package(rclcpp_components REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(Eigen3 REQUIRED)
-find_package(bboxes_ex_msgs REQUIRED)
 find_package(bytetrack_cpp REQUIRED)
+find_package(vision_msgs REQUIRED)
 
 # add executable
 add_executable(bytetrack_cpp_node
@@ -33,7 +33,7 @@ ament_target_dependencies(bytetrack_cpp_node
   rclcpp_components
   std_msgs
   bytetrack_cpp
-  bboxes_ex_msgs
+  vision_msgs
 )
 target_include_directories(bytetrack_cpp_node PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
@@ -65,7 +65,7 @@ ament_target_dependencies(bytetrack_cpp_component
   rclcpp_components
   std_msgs
   bytetrack_cpp
-  bboxes_ex_msgs
+  vision_msgs
   )
 install(TARGETS bytetrack_cpp_component
   EXPORT export_${PROJECT_NAME}

bytetrack_cpp_node/include/bytetrack_cpp_node/bytetrack_cpp_node.hpp

@@ -9,8 +9,7 @@
 #include <dirent.h>
 
 #include <rclcpp/rclcpp.hpp>
-#include "bboxes_ex_msgs/msg/bounding_box.hpp"
-#include "bboxes_ex_msgs/msg/bounding_boxes.hpp"
+#include <vision_msgs/msg/detection2_d_array.hpp>
 
 #include "bytetrack_cpp/BYTETracker.h"
 
@@ -25,15 +24,15 @@ namespace bytetrack_cpp_node{
 
     private:
         void initializeParameter_();
-        void topic_callback_(const bboxes_ex_msgs::msg::BoundingBoxes::ConstSharedPtr msg);
+        void topic_callback_(const vision_msgs::msg::Detection2DArray::ConstSharedPtr msg);
 
         int video_fps_ = 30;
         int track_buffer_ = 30;
         std::string sub_bboxes_topic_name_;
         std::string pub_bboxes_topic_name_;
         std::unique_ptr<BYTETracker> tracker_;
 
-        rclcpp::Publisher<bboxes_ex_msgs::msg::BoundingBoxes>::SharedPtr pub_bboxes_;
-        rclcpp::Subscription<bboxes_ex_msgs::msg::BoundingBoxes>::SharedPtr sub_bboxes_;
+        rclcpp::Publisher<vision_msgs::msg::Detection2DArray>::SharedPtr pub_bboxes_;
+        rclcpp::Subscription<vision_msgs::msg::Detection2DArray>::SharedPtr sub_bboxes_;
     };
 }

bytetrack_cpp_node/launch/bytetrack_tensorrt.launch.py

@@ -0,0 +1,160 @@
+import os
+import sys
+import launch
+import launch_ros.actions
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
+
+def generate_launch_description():
+    launch_args = [
+        DeclareLaunchArgument(
+            "video_path",
+            default_value="/dev/video0",
+            description="input video file path."
+        ),
+        DeclareLaunchArgument(
+            "video_fps",
+            default_value="30",
+            description="video fps."
+        ),
+        DeclareLaunchArgument(
+            "model_path",
+            default_value="./weight/yolox_l.trt",
+            description="yolox model path."
+        ),
+        DeclareLaunchArgument(
+            'p6',
+            default_value='false',
+            description='with p6.'
+        ),
+        DeclareLaunchArgument(
+            "model_version",
+            default_value="0.1.1rc0",
+            description="yolox model version."
+        ),
+        DeclareLaunchArgument(
+            "device",
+            default_value="GPU",
+            description="model device. CPU, GPU, MYRIAD, etc..."
+        ),
+        DeclareLaunchArgument(
+            "conf",
+            default_value="0.3",
+            description="yolox confidence threshold."
+        ),
+        DeclareLaunchArgument(
+            "nms",
+            default_value="0.45",
+            description="yolox nms threshold"
+        ),
+        DeclareLaunchArgument(
+            "save_video",
+            default_value="true",
+            description="record video."
+        ),
+        DeclareLaunchArgument(
+            "save_video_name",
+            default_value="output.mp4",
+            description="record video filename."
+        ),
+        DeclareLaunchArgument(
+            "save_video_fps",
+            default_value="30",
+            description="record video fps."
+        ),
+        DeclareLaunchArgument(
+            "save_video_codec",
+            default_value="MJPG",
+            description="record video codec."
+        ),
+        DeclareLaunchArgument(
+            'num_classes',
+            default_value='80',
+            description='num classes.'
+        ),
+        DeclareLaunchArgument(
+            'tensorrt/device',
+            default_value='0',
+            description='GPU index. Set in string type. ex 0'
+        ),
+
+    ]
+    container = ComposableNodeContainer(
+                name='bytetrack_container',
+                namespace='',
+                package='rclcpp_components',
+                executable='component_container',
+                composable_node_descriptions=[
+                    ComposableNode(
+                        package='ros_video_player',
+                        plugin='ros_video_player::VideoPlayerNode',
+                        name='ros_video_player',
+                        parameters=[{
+                            "publish_topic_name": "/image_raw",
+                            "video_path": LaunchConfiguration("video_path"),
+                            "frame_id": "map",
+                            "loop": True,
+                            "speed": 1.0,
+                            "video_buffer_size": 1,
+                        }]),
+                    ComposableNode(
+                        package='yolox_ros_cpp',
+                        plugin='yolox_ros_cpp::YoloXNode',
+                        name='yolox_ros_cpp',
+                        parameters=[{
+                            "model_path": LaunchConfiguration("model_path"),
+                            "model_type": "tensorrt",
+                            "model_version": LaunchConfiguration("model_version"),
+                            "device": LaunchConfiguration("device"),
+                            "conf": LaunchConfiguration("conf"),
+                            "nms": LaunchConfiguration("nms"),
+                            "imshow_isshow": True,
+                            "src_image_topic_name": "/image_raw",
+                            "publish_image_topic_name": "/yolox/image_raw",
+                            "publish_boundingbox_topic_name": "/yolox/bounding_boxes",
+                            "p6": LaunchConfiguration("p6"),
+                            'num_classes': LaunchConfiguration('num_classes'),
+                            'tensorrt/device': LaunchConfiguration('tensorrt/device'),
+                        }],
+                        ),
+                    ComposableNode(
+                        package='bytetrack_cpp_node',
+                        plugin='bytetrack_cpp_node::ByteTrackNode',
+                        name='bytetrack_cpp_node',
+                        parameters=[{
+                            "video_fps": LaunchConfiguration("video_fps"),
+                            "track_buffer": 30,
+                            "sub_bboxes_topic_name": "/yolox/bounding_boxes",
+                            "pub_bboxes_topic_name": "/bytetrack/bounding_boxes",
+                        }],
+                        ),
+                    ComposableNode(
+                        package='bytetrack_viewer',
+                        plugin='bytetrack_viewer::ByteTrackViewer',
+                        name='bytetrack_viewer',
+                        parameters=[{
+                            "queue_size": 10,
+                            "exact_sync": False,
+                            "sub_image_topic_name": "/image_raw",
+                            "sub_bboxes_topic_name": "/bytetrack/bounding_boxes",
+                            "save_video": LaunchConfiguration("save_video"),
+                            "save_video_name": LaunchConfiguration("save_video_name"),
+                            "save_video_fps": LaunchConfiguration("save_video_fps"),
+                            "save_video_codec": LaunchConfiguration("save_video_codec"),
+                        }],
+                        ),
+                ],
+                output='screen',
+        )
+
+    # rqt_graph = launch_ros.actions.Node(
+    #     package="rqt_graph", executable="rqt_graph",
+    # )
+
+    return launch.LaunchDescription(
+        launch_args
+        + [container]
+        # + [rqt_graph]
+    )

bytetrack_cpp_node/package.xml

@@ -13,8 +13,8 @@
   <depend>rclcpp_components</depend>
   <depend>image_transport</depend>
   <depend>std_msgs</depend>
-  <depend>bboxes_ex_msgs</depend>
   <depend>bytetrack_cpp</depend>
+  <depend>vision_msgs</depend>  
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

bytetrack_cpp_node/src/bytetrack_cpp_node.cpp

@@ -4,45 +4,52 @@
 namespace bytetrack_cpp_node{
     using namespace bytetrack_cpp;
 
-    std::vector<Object> BoundingBoxes2Objects(const std::vector<bboxes_ex_msgs::msg::BoundingBox> bboxes)
+    std::vector<Object> Detection2Ds2Objects(const vector<vision_msgs::msg::Detection2D> detections)
     {
         std::vector<Object> objects;
         float scale = 1.0;
-        for(auto bbox: bboxes){
+        for(auto detect: detections){
             Object obj;
-            obj.rect.x = (bbox.xmin) / scale;
-            obj.rect.y = (bbox.ymin) / scale;
-            obj.rect.width = (bbox.xmax - bbox.xmin) / scale;
-            obj.rect.height = (bbox.ymax - bbox.ymin) / scale;
+            obj.rect.x = (detect.bbox.center.position.x - (detect.bbox.size_x / 2.)) / scale;
+            obj.rect.y = (detect.bbox.center.position.y - (detect.bbox.size_y / 2.)) / scale;
+            obj.rect.width = (detect.bbox.size_x) / scale;
+            obj.rect.height = (detect.bbox.size_y) / scale;
 
-            auto it = std::find(COCO_CLASSES, COCO_CLASSES + 80, bbox.class_id);
+            auto it = std::find(COCO_CLASSES, COCO_CLASSES + 80, detect.results[0].hypothesis.class_id);
             if (it != COCO_CLASSES + 80){
                 int idx = std::distance(COCO_CLASSES, it);
                 obj.label = idx;
             }
-            
-            obj.prob = bbox.probability;
+
+            obj.prob = detect.results[0].hypothesis.score;
             objects.push_back(obj);
         }
         return objects;
     }
-    std::vector<bboxes_ex_msgs::msg::BoundingBox> STrack2BoundingBoxes(const std::vector<STrack> trackers)
+    std::vector<vision_msgs::msg::Detection2D> STrack2Detection2Ds(const std::vector<STrack> trackers, const std::vector<vision_msgs::msg::Detection2D> & ds)
     {
-        std::vector<bboxes_ex_msgs::msg::BoundingBox> bboxes;
+        std::vector<vision_msgs::msg::Detection2D> detections;
         for(int i=0; i<trackers.size(); i++){
-            bboxes_ex_msgs::msg::BoundingBox bbox;
-            bbox.ymin = trackers[i].tlbr[0];
-            bbox.xmin = trackers[i].tlbr[1];
-            bbox.ymax = trackers[i].tlbr[2];
-            bbox.xmax = trackers[i].tlbr[3];
-            bbox.id = trackers[i].track_id;
-            bbox.class_id = COCO_CLASSES[trackers[i].label];
+            vision_msgs::msg::Detection2D detection;
+            detection.bbox.size_y = trackers[i].tlbr[3] - trackers[i].tlbr[1];
+            detection.bbox.size_x = trackers[i].tlbr[2] - trackers[i].tlbr[0];
+
+            detection.bbox.center.position.x = detection.bbox.size_x / 2 + trackers[i].tlbr[0];
+            detection.bbox.center.position.y = detection.bbox.size_y / 2 + trackers[i].tlbr[1];
+            detection.id = std::to_string(trackers[i].track_id);
+
+            vision_msgs::msg::ObjectHypothesisWithPose hypothesis;
+            hypothesis.hypothesis.class_id = COCO_CLASSES[trackers[i].label];
+            hypothesis.hypothesis.score = trackers[i].score;
+            hypothesis.pose = ds[i].results[0].pose;
+
+            detection.results.push_back(hypothesis);
             // bbox.center_dist = 0.0;
             // bbox.img_height = 0;
             // bbox.img_width = 0;
-            bboxes.push_back(bbox);
+            detections.push_back(detection);
         }
-        return bboxes;
+        return detections;
     }
 
     ByteTrackNode::ByteTrackNode(const std::string &node_name, const rclcpp::NodeOptions& options)
@@ -55,12 +62,12 @@ namespace bytetrack_cpp_node{
         this->initializeParameter_();
         this->tracker_ = std::make_unique<BYTETracker>(this->video_fps_, this->track_buffer_);
 
-        this->sub_bboxes_ = this->create_subscription<bboxes_ex_msgs::msg::BoundingBoxes>(
+        this->sub_bboxes_ = this->create_subscription<vision_msgs::msg::Detection2DArray>(
                             this->sub_bboxes_topic_name_, 10, 
                             std::bind(&ByteTrackNode::topic_callback_, 
                                       this,
                                       std::placeholders::_1));
-        this->pub_bboxes_ = this->create_publisher<bboxes_ex_msgs::msg::BoundingBoxes>(
+        this->pub_bboxes_ = this->create_publisher<vision_msgs::msg::Detection2DArray>(
             this->pub_bboxes_topic_name_,
             10
         );
@@ -69,20 +76,19 @@ namespace bytetrack_cpp_node{
     {
         this->video_fps_ = this->declare_parameter<int>("video_fps", 30);
         this->track_buffer_ = this->declare_parameter<int>("track_buffer", 30);
-        this->sub_bboxes_topic_name_ = this->declare_parameter<std::string>("sub_bboxes_topic_name", "yolox/bounding_boxes");
+        this->sub_bboxes_topic_name_ = this->declare_parameter<std::string>("sub_bboxes_topic_name", "positions");
         this->pub_bboxes_topic_name_ = this->declare_parameter<std::string>("pub_bboxes_topic_name", "bytetrack/bounding_boxes");
     }
-    void ByteTrackNode::topic_callback_(const bboxes_ex_msgs::msg::BoundingBoxes::ConstSharedPtr msg)
+    void ByteTrackNode::topic_callback_(const vision_msgs::msg::Detection2DArray::ConstSharedPtr msg)
     {
-        bboxes_ex_msgs::msg::BoundingBoxes bboxes;
-        bboxes.header = msg->header;
-        bboxes.image_header = msg->image_header;
-
-        vector<Object> objects = BoundingBoxes2Objects(msg->bounding_boxes);
+        vision_msgs::msg::Detection2DArray boxes;
+        boxes.header = msg->header;
+
+        vector<Object> objects = Detection2Ds2Objects(msg->detections);
         vector<STrack> output_stracks = this->tracker_->update(objects);
         RCLCPP_INFO(this->get_logger(), "Detect objects: %d, Output Tracker: %d", objects.size(), output_stracks.size());
-        bboxes.bounding_boxes = STrack2BoundingBoxes(output_stracks);
-        this->pub_bboxes_->publish(bboxes);
+        boxes.detections = STrack2Detection2Ds(output_stracks,msg->detections);
+        this->pub_bboxes_->publish(boxes);
     }
 }
 

bytetrack_viewer/CMakeLists.txt

@@ -24,7 +24,7 @@ find_package(image_transport REQUIRED)
 find_package(message_filters REQUIRED)
 find_package(cv_bridge REQUIRED)
 find_package(OpenCV REQUIRED)
-find_package(bboxes_ex_msgs REQUIRED)
+find_package(vision_msgs REQUIRED)
 
 # add executable
 add_executable(bytetrack_viewer
@@ -38,7 +38,7 @@ ament_target_dependencies(bytetrack_viewer
   message_filters
   cv_bridge
   OpenCV
-  bboxes_ex_msgs
+  vision_msgs
 )
 target_include_directories(bytetrack_viewer PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
@@ -71,7 +71,7 @@ ament_target_dependencies(bytetrack_viewer_component
   message_filters
   cv_bridge
   OpenCV
-  bboxes_ex_msgs
+  vision_msgs
 )
 install(TARGETS bytetrack_viewer_component
   EXPORT export_${PROJECT_NAME}