diff --git a/CMakeLists.txt b/CMakeLists.txt index 3bfbb9a..697b836 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -28,6 +28,7 @@ find_package(catkin REQUIRED COMPONENTS sensor_msgs message_generation zetton_common + zetton_stream ) catkin_python_setup() catkin_package( diff --git a/example/ros_mot_tracker.cc b/example/ros_mot_tracker.cc index dfcd0e9..99705b5 100644 --- a/example/ros_mot_tracker.cc +++ b/example/ros_mot_tracker.cc @@ -16,7 +16,7 @@ #include "zetton_inference/tracker/sort_tracker.h" void signalHandler(int sig) { - ROS_WARN("Trying to exit!"); + AWARN_F("Trying to exit!"); ros::shutdown(); } @@ -28,7 +28,7 @@ class RosMotTracker { try { cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); } catch (cv_bridge::Exception& e) { - ROS_ERROR("cv_bridge exception: %s", e.what()); + AERROR_F("cv_bridge exception: {}", e.what()); return; } @@ -40,15 +40,15 @@ class RosMotTracker { tracker_.Track(cv_ptr->image, ros::Time::now(), detections); // print detections and tracks - ROS_INFO("Detections:"); + AINFO_F("Detections:"); for (auto& detection : detections) { - ROS_INFO_STREAM(detection); + AINFO << detection; detection.Draw(cv_ptr->image); } - ROS_INFO("Trackings:"); + AINFO_F("Trackings:"); for (auto& track : tracker_.tracks()) { // if (track.tracking_fail_count <= 3) { - ROS_INFO_STREAM(track); + AINFO << track; track.Draw(cv_ptr->image); // } } @@ -57,7 +57,7 @@ class RosMotTracker { image_pub_.publish( cv_bridge::CvImage(std_msgs::Header(), "bgr8", cv_ptr->image) .toImageMsg()); - ROS_INFO("---"); + AINFO_F("---"); } ros::NodeHandle* nh_; @@ -74,7 +74,8 @@ class RosMotTracker { RosMotTracker(ros::NodeHandle* nh) : nh_(nh), it_(*nh_) { // load params // hardcoded or using GPARAM - // std::string image_topic_sub = "/uvds_communication/image_streaming/mavic_0"; + // std::string image_topic_sub = + // "/uvds_communication/image_streaming/mavic_0"; std::string image_topic_sub = "/camera/image"; // subscribe to input video feed diff --git a/example/ros_yolo_object_detector.cc b/example/ros_yolo_object_detector.cc index 7287f97..7e4c173 100644 --- a/example/ros_yolo_object_detector.cc +++ b/example/ros_yolo_object_detector.cc @@ -11,7 +11,7 @@ #include "zetton_inference/detector/yolo_object_detector.h" void signalHandler(int sig) { - ROS_WARN("Trying to exit!"); + AWARN_F("Trying to exit!"); ros::shutdown(); } @@ -23,7 +23,7 @@ class RosYoloObjectDetector { try { cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); } catch (cv_bridge::Exception& e) { - ROS_ERROR("cv_bridge exception: %s", e.what()); + AERROR_F("cv_bridge exception: {}", e.what()); return; } @@ -33,7 +33,7 @@ class RosYoloObjectDetector { // print results for (const auto& result : results) { - ROS_INFO_STREAM(result); + AINFO << result; } } diff --git a/example/rtsp_yolo_object_detector.cc b/example/rtsp_yolo_object_detector.cc index b110b67..65c2115 100644 --- a/example/rtsp_yolo_object_detector.cc +++ b/example/rtsp_yolo_object_detector.cc @@ -8,8 +8,8 @@ #include "opencv2/opencv.hpp" #include "ros/package.h" #include "ros/ros.h" -#include "zetton_common/stream/cv_gst_stream_source.h" #include "zetton_inference/detector/yolo_object_detector.h" +#include "zetton_stream/stream/cv_gst_stream_source.h" #define SHOW_GUI false @@ -35,7 +35,7 @@ class RtspYoloObjectDetector { std::string url_; std::shared_ptr detector_; - std::shared_ptr streamer_; + std::shared_ptr streamer_; std::atomic stop_flag_{false}; std::shared_ptr thread_; @@ -68,11 +68,11 @@ bool RtspYoloObjectDetector::Open() { detector_ = std::make_shared(); detector_->Init(config_); // init streamer - zetton::common::StreamOptions options; + zetton::stream::StreamOptions options; options.resource = url_; - options.codec = zetton::common::StreamCodec::CODEC_H264; - options.platform = zetton::common::StreamPlatformType::PLATFORM_CPU; - streamer_ = std::make_shared(); + options.codec = zetton::stream::StreamCodec::CODEC_H264; + options.platform = zetton::stream::StreamPlatformType::PLATFORM_CPU; + streamer_ = std::make_shared(); streamer_->Init(options); thread_ = std::make_shared([&]() { Process(); }); @@ -135,7 +135,7 @@ int main(int argc, char** argv) { detector.SetHeightLimitation(50, 1920); // start - ROS_INFO("Starting detection"); + AINFO_F("Starting detection"); while (detector.IsOpened()) { cv::Mat frame; zetton::inference::ObjectDetectionResults results; @@ -144,7 +144,7 @@ int main(int argc, char** argv) { detector.Capture(frame, results); // print results for (const auto& result : results) { - ROS_INFO_STREAM(result); + AINFO << result; } // show results in GUI if (SHOW_GUI) { @@ -154,7 +154,7 @@ int main(int argc, char** argv) { cv::imshow("Results", frame); char key = cv::waitKey(10); if (key == 27) { - ROS_INFO("Stopping detection"); + AINFO_F("Stopping detection"); break; } } diff --git a/include/zetton_inference/tracker/mot/association_type.h b/include/zetton_inference/tracker/mot/association_type.h index 21e1349..852fca5 100644 --- a/include/zetton_inference/tracker/mot/association_type.h +++ b/include/zetton_inference/tracker/mot/association_type.h @@ -2,6 +2,8 @@ #include +#include "zetton_common/util/log.h" + namespace zetton { namespace inference { namespace tracker { @@ -50,10 +52,11 @@ class AssociationVector { } void report() { - ROS_INFO("Data Association Report:"); - for (auto ass : ass_vector) - ROS_INFO_STREAM("id: " << ass.id << " | score: " << ass.score); - ROS_INFO(" "); + AINFO_F("Data Association Report:"); + for (auto ass : ass_vector) { + AINFO_F("id: {} | score: {}", ass.id, ass.score); + } + AINFO_F(" "); } std::vector ass_vector; diff --git a/include/zetton_inference/tracker/mot/util.h b/include/zetton_inference/tracker/mot/util.h index 2ed24e5..eba75db 100644 --- a/include/zetton_inference/tracker/mot/util.h +++ b/include/zetton_inference/tracker/mot/util.h @@ -7,6 +7,8 @@ #include #include +#include "zetton_common/util/log.h" + namespace zetton { namespace inference { namespace tracker { @@ -50,8 +52,7 @@ inline std::vector feature_vector_to_eigen( } inline void print_bbox(cv::Rect2d bbox) { - ROS_INFO_STREAM("Bbox Info: " << bbox.x << ", " << bbox.y << ", " - << bbox.width << ", " << bbox.height); + AINFO_F("Bbox Info: {},{},{},{}", bbox.x, bbox.y, bbox.width, bbox.height); } inline double cal_bbox_overlap_ratio(cv::Rect2d track_bbox, diff --git a/include/zetton_inference/tracker/mot_tracker.h b/include/zetton_inference/tracker/mot_tracker.h index fd1edb2..9e44449 100644 --- a/include/zetton_inference/tracker/mot_tracker.h +++ b/include/zetton_inference/tracker/mot_tracker.h @@ -2,6 +2,7 @@ #include +#include "zetton_common/util/log.h" #include "zetton_inference/interface/base_object_detector.h" #include "zetton_inference/interface/base_object_tracker.h" #include "zetton_inference/tracker/mot/association_type.h" diff --git a/include/zetton_inference/tracker/sort_tracker.h b/include/zetton_inference/tracker/sort_tracker.h index 7e1b751..dd62693 100644 --- a/include/zetton_inference/tracker/sort_tracker.h +++ b/include/zetton_inference/tracker/sort_tracker.h @@ -1,5 +1,6 @@ #pragma once +#include "ros/ros.h" #include "zetton_inference/interface/base_object_detector.h" #include "zetton_inference/interface/base_object_tracker.h" #include "zetton_inference/tracker/sort/hungarian.h" diff --git a/package.xml b/package.xml index c92838c..1e05143 100644 --- a/package.xml +++ b/package.xml @@ -15,6 +15,7 @@ sensor_msgs message_generation zetton_common + zetton_stream roscpp rospy roslib @@ -24,4 +25,5 @@ message_generation message_runtime zetton_common + zetton_stream \ No newline at end of file diff --git a/src/zetton_inference/detector/yolo_object_detector.cc b/src/zetton_inference/detector/yolo_object_detector.cc index a3a28da..417475a 100644 --- a/src/zetton_inference/detector/yolo_object_detector.cc +++ b/src/zetton_inference/detector/yolo_object_detector.cc @@ -23,7 +23,7 @@ bool YoloObjectDetector::Init(const std::string& param_uri, } else if (net_type == "YOLOV4_TINY") { config_.net_type = yolo_trt::ModelType::YOLOV4_TINY; } else { - ROS_ERROR_STREAM("Unsupported net type: " << net_type); + AERROR_F("Unsupported net type: {}", net_type); return false; } auto precision = static_cast(params["inference_precision"]); @@ -37,7 +37,7 @@ bool YoloObjectDetector::Init(const std::string& param_uri, package_path + static_cast(params["calibration_image_list_file_txt"]); } else { - ROS_ERROR_STREAM("Unsupported inference precision: " << precision); + AERROR_F("Unsupported inference precision: {}", precision); return false; } diff --git a/src/zetton_inference/tracker/mot/local_object.cc b/src/zetton_inference/tracker/mot/local_object.cc index a1fbf68..ca29ef7 100644 --- a/src/zetton_inference/tracker/mot/local_object.cc +++ b/src/zetton_inference/tracker/mot/local_object.cc @@ -41,7 +41,7 @@ void LocalObject::track_bbox_by_optical_flow(const ros::Time &time_now) { tracking_fail_count = 0; } else { tracking_fail_count++; - ROS_INFO_STREAM("Object " << id << " tracking failure detected!"); + AINFO_F("Object {} tracking failure detected!", id); } // update the bbox last updated time diff --git a/src/zetton_inference/tracker/mot_tracker.cc b/src/zetton_inference/tracker/mot_tracker.cc index f4b3c2d..fdf6307 100644 --- a/src/zetton_inference/tracker/mot_tracker.cc +++ b/src/zetton_inference/tracker/mot_tracker.cc @@ -35,7 +35,7 @@ std::vector MotTracker::update_bbox_by_tracker( void MotTracker::update_bbox_by_detector( const cv::Mat &img, const std::vector &bboxes, const std::vector features_detector, const ros::Time &update_time) { - ROS_INFO_STREAM("******Update Bbox by Detector******"); + AINFO_F("******Update Bbox by Detector******"); // maximum block size, to perform augumentation and rectification of the // tracker block @@ -156,7 +156,7 @@ bool MotTracker::update_local_database(tracker::LocalObject &local_object, local_object.database_update_timer.toc() > record_interval) { local_object.img_blocks.push_back(img_block); local_object.database_update_timer.tic(); - ROS_INFO_STREAM("Adding an image to the datebase id: " << local_object.id); + AINFO_F("Adding an image to the datebase id: ", local_object.id); return true; } else { return false; @@ -236,14 +236,13 @@ std::vector MotTracker::remove_dead_trackers() { void MotTracker::report_local_object() { // lock_guard lk(mtx); //lock the thread - ROS_INFO("------Local Object List Summary------"); - ROS_INFO_STREAM("Local Object Num: " << local_objects_list.size()); + AINFO_F("------Local Object List Summary------"); + AINFO_F("Local Object Num: {}", local_objects_list.size()); for (auto lo : local_objects_list) { - ROS_INFO_STREAM( - "id: " << lo.id << "| database images num: " << lo.img_blocks.size()); + AINFO_F("id: {} | database images num: {}", lo.id, lo.img_blocks.size()); std::cout << lo.bbox << std::endl; } - ROS_INFO("------Summary End------"); + AINFO_F("------Summary End------"); } void MotTracker::visualize_tracking(cv::Mat &img) { @@ -271,13 +270,13 @@ void MotTracker::detector_and_tracker_association( std::vector &all_detected_bbox_ass_vec) { // lock_guard lk(mtx); //lock if (!local_objects_list.empty()) { - ROS_INFO_STREAM("SUMMARY:" << bboxes.size() << " bboxes detected!"); + AINFO_F("SUMMARY: {} bboxes detected!", bboxes.size()); // perform matching from detector to tracker for (int i = 0; i < bboxes.size(); i++) { tracker::AssociationVector one_detected_object_ass_vec; cv::Rect2d detector_bbox = tracker::BboxPadding(bboxes[i], block_max, detector_bbox_padding); - std::cout << "Detector" << i << " bbox: " << bboxes[i] << std::endl; + AINFO << "Detector" << i << " bbox: " << bboxes[i] << std::endl; /*Data Association part: - two critertion: - augemented bboxes(bbox with padding) have enough overlap @@ -286,9 +285,9 @@ void MotTracker::detector_and_tracker_association( for (int j = 0; j < local_objects_list.size(); j++) { double bbox_overlap_ratio = tracker::cal_bbox_overlap_ratio( local_objects_list[j].bbox, detector_bbox); - ROS_INFO_STREAM("Bbox overlap ratio: " << bbox_overlap_ratio); - std::cout << "Tracker " << local_objects_list[j].id - << " bbox: " << local_objects_list[j].bbox << std::endl; + AINFO_F("Bbox overlap ratio: {}", bbox_overlap_ratio); + AINFO << "Tracker " << local_objects_list[j].id + << " bbox: " << local_objects_list[j].bbox; if (bbox_overlap_ratio > bbox_overlap_ratio_threshold) { // TODO might speed up in here float min_query_score = @@ -306,17 +305,17 @@ void MotTracker::detector_and_tracker_association( if (one_detected_object_ass_vec.ass_vector.size() > 1) one_detected_object_ass_vec.reranking(); one_detected_object_ass_vec.report(); - ROS_INFO("---------------------------------"); + AINFO_F("---------------------------------"); all_detected_bbox_ass_vec.push_back(one_detected_object_ass_vec); } uniquify_detector_association_vectors(all_detected_bbox_ass_vec, local_objects_list.size()); - ROS_INFO("---Report after uniquification---"); + AINFO_F("---Report after uniquification---"); for (auto ass : all_detected_bbox_ass_vec) { ass.report(); } - ROS_INFO("---Report finished---"); + AINFO_F("---Report finished---"); } else { // create empty association vectors to indicate all the detected objects are // new @@ -333,8 +332,7 @@ void MotTracker::manage_local_objects_list_by_detector( for (int i = 0; i < all_detected_bbox_ass_vec.size(); i++) { if (all_detected_bbox_ass_vec[i].ass_vector.empty()) { // this detected object is a new object - ROS_INFO_STREAM( - "Adding Tracking Object with ID:" << local_id_not_assigned); + AINFO_F("Adding Tracking Object with ID: {}", local_id_not_assigned); cv::Mat example_img; cv::resize(img(bboxes[i]), example_img, cv::Size(128, 256)); // hard code in here @@ -349,8 +347,7 @@ void MotTracker::manage_local_objects_list_by_detector( } else { // re-detect a previous tracking object int matched_id = all_detected_bbox_ass_vec[i].ass_vector[0].id; - ROS_INFO_STREAM("Object " << local_objects_list[matched_id].id - << " re-detected!"); + AINFO_F("Object {} re-detected!", local_objects_list[matched_id].id); local_objects_list[matched_id].track_bbox_by_detector(bboxes[i], update_time); @@ -381,8 +378,7 @@ void MotTracker::manage_local_objects_list_by_reid_detector( for (int i = 0; i < all_detected_bbox_ass_vec.size(); i++) { if (all_detected_bbox_ass_vec[i].ass_vector.empty()) { // this detected object is a new object - ROS_INFO_STREAM( - "Adding Tracking Object with ID:" << local_id_not_assigned); + AINFO_F("Adding Tracking Object with ID: {}", local_id_not_assigned); cv::Mat example_img; cv::resize(img(bboxes[i]), example_img, cv::Size(128, 256)); // hard code in here @@ -399,8 +395,7 @@ void MotTracker::manage_local_objects_list_by_reid_detector( } else { // re-detect a previous tracking object int matched_id = all_detected_bbox_ass_vec[i].ass_vector[0].id; - ROS_INFO_STREAM("Object " << local_objects_list[matched_id].id - << " re-detected!"); + AINFO_F("Object {} re-detected!", local_objects_list[matched_id].id); local_objects_list[matched_id].track_bbox_by_detector(bboxes[i], update_time); @@ -477,7 +472,7 @@ bool MotTracker::Track(const cv::Mat &frame, const ros::Time ×tamp, if (one_detected_object_ass_vec.ass_vector.size() > 1) one_detected_object_ass_vec.reranking(); // one_detected_object_ass_vec.report(); - // ROS_INFO("---------------------------------"); + // AINFO_F("---------------------------------"); all_detected_bbox_ass_vec.push_back(one_detected_object_ass_vec); } uniquify_detector_association_vectors(all_detected_bbox_ass_vec, @@ -493,8 +488,7 @@ bool MotTracker::Track(const cv::Mat &frame, const ros::Time ×tamp, for (size_t i = 0; i < all_detected_bbox_ass_vec.size(); i++) { if (all_detected_bbox_ass_vec[i].ass_vector.empty()) { // this detected object is a new object - ROS_INFO_STREAM( - "Adding Tracking Object with ID:" << local_id_not_assigned); + AINFO_F("Adding Tracking Object with ID: {}", local_id_not_assigned); tracker::LocalObject new_object(local_id_not_assigned, detections[i].bbox, kf_param, timestamp); local_id_not_assigned++; @@ -502,8 +496,7 @@ bool MotTracker::Track(const cv::Mat &frame, const ros::Time ×tamp, } else { // re-detect a previous tracking object int matched_id = all_detected_bbox_ass_vec[i].ass_vector[0].id; - ROS_INFO_STREAM("Object " << local_objects_list[matched_id].id - << " re-detected!"); + AINFO_F("Object {} re-detected!", local_objects_list[matched_id].id); local_objects_list[matched_id].track_bbox_by_detector(detections[i].bbox, timestamp); } diff --git a/src/zetton_inference/tracker/sort_tracker.cc b/src/zetton_inference/tracker/sort_tracker.cc index f57cf66..463b09f 100644 --- a/src/zetton_inference/tracker/sort_tracker.cc +++ b/src/zetton_inference/tracker/sort_tracker.cc @@ -135,7 +135,7 @@ bool SortTracker::Track(const cv::Mat &frame, const ros::Time ×tamp, std::sort(duplicated_tracks.begin(), duplicated_tracks.end()); for (auto it = duplicated_tracks.rbegin(); it != duplicated_tracks.rend(); ++it) { - // ROS_INFO_STREAM("Remove duplicated track " << *it); + // AINFO_F("Remove duplicated track {}", *it); trackers_.erase(trackers_.begin() + *it); }