diff --git a/README.md b/README.md index 8781fc4..bc1ff11 100644 --- a/README.md +++ b/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) diff --git a/bytetrack_cpp_node/CMakeLists.txt b/bytetrack_cpp_node/CMakeLists.txt index e726517..c983f97 100644 --- a/bytetrack_cpp_node/CMakeLists.txt +++ b/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 $ @@ -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} diff --git a/bytetrack_cpp_node/include/bytetrack_cpp_node/bytetrack_cpp_node.hpp b/bytetrack_cpp_node/include/bytetrack_cpp_node/bytetrack_cpp_node.hpp index eaf8609..87cc39c 100644 --- a/bytetrack_cpp_node/include/bytetrack_cpp_node/bytetrack_cpp_node.hpp +++ b/bytetrack_cpp_node/include/bytetrack_cpp_node/bytetrack_cpp_node.hpp @@ -9,8 +9,7 @@ #include #include -#include "bboxes_ex_msgs/msg/bounding_box.hpp" -#include "bboxes_ex_msgs/msg/bounding_boxes.hpp" +#include #include "bytetrack_cpp/BYTETracker.h" @@ -25,7 +24,7 @@ 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; @@ -33,7 +32,7 @@ namespace bytetrack_cpp_node{ std::string pub_bboxes_topic_name_; std::unique_ptr tracker_; - rclcpp::Publisher::SharedPtr pub_bboxes_; - rclcpp::Subscription::SharedPtr sub_bboxes_; + rclcpp::Publisher::SharedPtr pub_bboxes_; + rclcpp::Subscription::SharedPtr sub_bboxes_; }; } \ No newline at end of file diff --git a/bytetrack_cpp_node/launch/bytetrack_tensorrt.launch.py b/bytetrack_cpp_node/launch/bytetrack_tensorrt.launch.py new file mode 100644 index 0000000..2c7fac9 --- /dev/null +++ b/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] + ) diff --git a/bytetrack_cpp_node/package.xml b/bytetrack_cpp_node/package.xml index 1f857be..f7a8793 100644 --- a/bytetrack_cpp_node/package.xml +++ b/bytetrack_cpp_node/package.xml @@ -13,8 +13,8 @@ rclcpp_components image_transport std_msgs - bboxes_ex_msgs bytetrack_cpp + vision_msgs ament_lint_auto ament_lint_common diff --git a/bytetrack_cpp_node/src/bytetrack_cpp_node.cpp b/bytetrack_cpp_node/src/bytetrack_cpp_node.cpp index 33cb460..3b43535 100644 --- a/bytetrack_cpp_node/src/bytetrack_cpp_node.cpp +++ b/bytetrack_cpp_node/src/bytetrack_cpp_node.cpp @@ -4,45 +4,52 @@ namespace bytetrack_cpp_node{ using namespace bytetrack_cpp; - std::vector BoundingBoxes2Objects(const std::vector bboxes) + std::vector Detection2Ds2Objects(const vector detections) { std::vector 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 STrack2BoundingBoxes(const std::vector trackers) + std::vector STrack2Detection2Ds(const std::vector trackers, const std::vector & ds) { - std::vector bboxes; + std::vector detections; for(int i=0; iinitializeParameter_(); this->tracker_ = std::make_unique(this->video_fps_, this->track_buffer_); - this->sub_bboxes_ = this->create_subscription( + this->sub_bboxes_ = this->create_subscription( this->sub_bboxes_topic_name_, 10, std::bind(&ByteTrackNode::topic_callback_, this, std::placeholders::_1)); - this->pub_bboxes_ = this->create_publisher( + this->pub_bboxes_ = this->create_publisher( this->pub_bboxes_topic_name_, 10 ); @@ -69,20 +76,19 @@ namespace bytetrack_cpp_node{ { this->video_fps_ = this->declare_parameter("video_fps", 30); this->track_buffer_ = this->declare_parameter("track_buffer", 30); - this->sub_bboxes_topic_name_ = this->declare_parameter("sub_bboxes_topic_name", "yolox/bounding_boxes"); + this->sub_bboxes_topic_name_ = this->declare_parameter("sub_bboxes_topic_name", "positions"); this->pub_bboxes_topic_name_ = this->declare_parameter("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 objects = BoundingBoxes2Objects(msg->bounding_boxes); + vision_msgs::msg::Detection2DArray boxes; + boxes.header = msg->header; + + vector objects = Detection2Ds2Objects(msg->detections); vector 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); } } diff --git a/bytetrack_viewer/CMakeLists.txt b/bytetrack_viewer/CMakeLists.txt index 6280f9e..01ce0c6 100644 --- a/bytetrack_viewer/CMakeLists.txt +++ b/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 $ @@ -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} diff --git a/bytetrack_viewer/include/bytetrack_viewer/bytetrack_viewer.hpp b/bytetrack_viewer/include/bytetrack_viewer/bytetrack_viewer.hpp index ea87edd..7e47fc6 100644 --- a/bytetrack_viewer/include/bytetrack_viewer/bytetrack_viewer.hpp +++ b/bytetrack_viewer/include/bytetrack_viewer/bytetrack_viewer.hpp @@ -9,9 +9,7 @@ #include #include #include - -#include "bboxes_ex_msgs/msg/bounding_box.hpp" -#include "bboxes_ex_msgs/msg/bounding_boxes.hpp" +#include namespace bytetrack_viewer{ @@ -24,15 +22,14 @@ class ByteTrackViewer : public rclcpp::Node private: using Image = sensor_msgs::msg::Image; - using BoundingBoxes = bboxes_ex_msgs::msg::BoundingBoxes; // Subscriptions image_transport::SubscriberFilter sub_image_; - message_filters::Subscriber sub_bboxes_; + message_filters::Subscriber sub_bboxes_; using SyncPolicy = - message_filters::sync_policies::ApproximateTime; + message_filters::sync_policies::ApproximateTime; using ExactSyncPolicy = - message_filters::sync_policies::ExactTime; + message_filters::sync_policies::ExactTime; using Synchronizer = message_filters::Synchronizer; using ExactSynchronizer = message_filters::Synchronizer; std::shared_ptr sync_; @@ -54,6 +51,6 @@ class ByteTrackViewer : public rclcpp::Node void imageCallback( const Image::ConstSharedPtr & image_msg, - const BoundingBoxes::ConstSharedPtr & trackers_msg); + const vision_msgs::msg::Detection2DArray::ConstSharedPtr & trackers_msg); }; } \ No newline at end of file diff --git a/bytetrack_viewer/package.xml b/bytetrack_viewer/package.xml index 6b6fd52..bed7daa 100644 --- a/bytetrack_viewer/package.xml +++ b/bytetrack_viewer/package.xml @@ -16,7 +16,7 @@ message_filters cv_bridge OpenCV - bboxes_ex_msgs + vision_msgs ament_lint_auto ament_lint_common diff --git a/bytetrack_viewer/src/bytetrack_viewer.cpp b/bytetrack_viewer/src/bytetrack_viewer.cpp index 1a36c56..3569833 100644 --- a/bytetrack_viewer/src/bytetrack_viewer.cpp +++ b/bytetrack_viewer/src/bytetrack_viewer.cpp @@ -5,12 +5,13 @@ namespace bytetrack_viewer{ int idx = id + 3; return cv::Scalar(37 * idx % 255, 17 * idx % 255, 29 * idx % 255); } - void drawObject(cv::Mat frame, bboxes_ex_msgs::msg::BoundingBox bbox){ + void drawObject(cv::Mat frame, vision_msgs::msg::Detection2D box){ // draw bbox - auto color = getColor(bbox.id); + auto color = getColor(static_cast(std::stoi(box.id))); + double bbox_xmin = box.bbox.center.position.x - (box.bbox.size_x / 2); + double bbox_ymin = box.bbox.center.position.y - (box.bbox.size_y / 2); cv::rectangle(frame, - cv::Rect(bbox.ymin, bbox.xmin, - bbox.ymax - bbox.ymin, bbox.xmax - bbox.xmin), + cv::Rect( bbox_xmin, bbox_ymin, box.bbox.size_x, box.bbox.size_y), color, 2); // draw ID @@ -22,15 +23,15 @@ namespace bytetrack_viewer{ txt_color = cv::Scalar(255, 255, 255); } - std::string txt = cv::format("ID:%d %s", bbox.id, bbox.class_id.c_str()); + std::string txt = cv::format("ID:%s %s", box.id.c_str(), box.results[0].hypothesis.class_id.c_str()); int baseLine = 0; cv::Size label_size = cv::getTextSize(txt, cv::FONT_HERSHEY_SIMPLEX, 0.6, 1, &baseLine); cv::rectangle(frame, - cv::Rect(cv::Point(bbox.ymin, bbox.xmin - label_size.height), + cv::Rect(cv::Point(bbox_xmin, bbox_ymin - label_size.height), cv::Size(label_size.width, label_size.height + baseLine)), color, -1); cv::putText(frame, txt, - cv::Point(bbox.ymin, bbox.xmin), + cv::Point(bbox_xmin, bbox_ymin), cv::FONT_HERSHEY_SIMPLEX, 0.6, txt_color, 1, cv::LINE_AA); } @@ -72,7 +73,7 @@ namespace bytetrack_viewer{ ByteTrackViewer::~ByteTrackViewer(){ if(this->video_.isOpened()){ this->video_.release(); - RCLCPP_INFO(this->get_logger(), "save as " + this->save_video_name_ + "."); + //RCLCPP_INFO(this->get_logger(), "save as " + this->save_video_name_ + "."); } } void ByteTrackViewer::initializeParameter_() @@ -94,8 +95,10 @@ namespace bytetrack_viewer{ } void ByteTrackViewer::imageCallback( const sensor_msgs::msg::Image::ConstSharedPtr & image_msg, - const bboxes_ex_msgs::msg::BoundingBoxes::ConstSharedPtr & trackers_msg) + const vision_msgs::msg::Detection2DArray::ConstSharedPtr & trackers_msg) { + RCLCPP_INFO(this->get_logger(), "Received image and bboxes."); + auto img = cv_bridge::toCvCopy(image_msg, "bgr8"); cv::Mat frame = img->image; @@ -112,7 +115,7 @@ namespace bytetrack_viewer{ true); } - auto bboxes = trackers_msg->bounding_boxes; + auto bboxes = trackers_msg->detections; for(auto bbox: bboxes){ drawObject(frame, bbox); } @@ -138,4 +141,4 @@ int main(int argc, char * argv[]) } #include -RCLCPP_COMPONENTS_REGISTER_NODE(bytetrack_viewer::ByteTrackViewer) \ No newline at end of file +RCLCPP_COMPONENTS_REGISTER_NODE(bytetrack_viewer::ByteTrackViewer)