diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index cf18f6c691878..dfa276856321d 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -1,9 +1,9 @@ - + - + @@ -18,7 +18,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml index 2bd007b658978..38d141241c215 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml @@ -1,15 +1,15 @@ - + - + - + @@ -17,7 +17,7 @@ - + @@ -44,8 +44,11 @@ - + + + + - + @@ -102,6 +105,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -113,6 +158,8 @@ + + @@ -141,54 +188,12 @@ - + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - @@ -218,7 +223,7 @@ - + @@ -290,17 +295,20 @@ - + - + - + + + + @@ -327,7 +335,7 @@ - + @@ -381,7 +389,7 @@ - + @@ -389,8 +397,19 @@ - + + + + + + + + + + + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml index ab55047132482..77a3e345ec5e8 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml @@ -1,15 +1,16 @@ + - + - + @@ -28,14 +29,15 @@ - + + + + + - - - - + @@ -63,13 +65,16 @@ - + + + + - + @@ -100,7 +105,7 @@ - + @@ -109,12 +114,14 @@ - + + + - + @@ -127,11 +134,12 @@ - + + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml index ebe96200b6883..c122cac5031e3 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml @@ -8,7 +8,14 @@ - + + + + + + + + @@ -63,7 +70,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml index 4756fc9338e46..cee6829ea3b4a 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml @@ -1,17 +1,18 @@ - + - + - + + - + @@ -22,11 +23,11 @@ - + - + @@ -42,7 +43,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml index 8e2e23b510a07..5b5646a061ac7 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml @@ -2,14 +2,14 @@ - - - - - + + + + + - - + + @@ -20,14 +20,8 @@ - - - - - - - + @@ -40,8 +34,14 @@ + + + + + + - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml index 1ba1e8de9d42e..77de1e5995ee0 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml @@ -15,6 +15,7 @@ + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index acc14b1dcbe24..6b4877b771f20 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -11,6 +11,7 @@ + @@ -21,11 +22,11 @@ - + - + @@ -66,20 +67,36 @@ /> - + + + + - + + + + + + - + - + @@ -92,7 +109,7 @@ - + @@ -110,10 +127,10 @@ - + - + @@ -142,6 +159,7 @@ + @@ -153,14 +171,15 @@ - + - + + @@ -176,7 +195,7 @@ - + diff --git a/perception/object_merger/include/object_association_merger/node.hpp b/perception/object_merger/include/object_association_merger/node.hpp index 1e5b9fad9c9ca..6815b59894083 100644 --- a/perception/object_merger/include/object_association_merger/node.hpp +++ b/perception/object_merger/include/object_association_merger/node.hpp @@ -59,14 +59,16 @@ class ObjectAssociationMergerNode : public rclcpp::Node tf2_ros::TransformListener tf_listener_; rclcpp::Publisher::SharedPtr merged_object_pub_; - message_filters::Subscriber object0_sub_; - message_filters::Subscriber object1_sub_; - typedef message_filters::sync_policies::ApproximateTime< + message_filters::Subscriber object0_sub_{}; + message_filters::Subscriber object1_sub_{}; + + using SyncPolicy = message_filters::sync_policies::ApproximateTime< autoware_auto_perception_msgs::msg::DetectedObjects, - autoware_auto_perception_msgs::msg::DetectedObjects> - SyncPolicy; - typedef message_filters::Synchronizer Sync; - Sync sync_; + autoware_auto_perception_msgs::msg::DetectedObjects>; + using Sync = message_filters::Synchronizer; + typename std::shared_ptr sync_ptr_; + + int sync_queue_size_; std::unique_ptr data_association_; std::string base_link_frame_id_; // associated with the base_link frame diff --git a/perception/object_merger/launch/object_association_merger.launch.xml b/perception/object_merger/launch/object_association_merger.launch.xml index 3418f7d5a5e61..5754c256fef3c 100644 --- a/perception/object_merger/launch/object_association_merger.launch.xml +++ b/perception/object_merger/launch/object_association_merger.launch.xml @@ -6,6 +6,7 @@ + @@ -14,6 +15,7 @@ + diff --git a/perception/object_merger/src/object_association_merger/node.cpp b/perception/object_merger/src/object_association_merger/node.cpp index 4f600ce8a4948..22d0ac273a140 100644 --- a/perception/object_merger/src/object_association_merger/node.cpp +++ b/perception/object_merger/src/object_association_merger/node.cpp @@ -77,20 +77,13 @@ ObjectAssociationMergerNode::ObjectAssociationMergerNode(const rclcpp::NodeOptio tf_buffer_(get_clock()), tf_listener_(tf_buffer_), object0_sub_(this, "input/object0", rclcpp::QoS{1}.get_rmw_qos_profile()), - object1_sub_(this, "input/object1", rclcpp::QoS{1}.get_rmw_qos_profile()), - sync_(SyncPolicy(10), object0_sub_, object1_sub_) + object1_sub_(this, "input/object1", rclcpp::QoS{1}.get_rmw_qos_profile()) { - // Create publishers and subscribers - using std::placeholders::_1; - using std::placeholders::_2; - sync_.registerCallback(std::bind(&ObjectAssociationMergerNode::objectsCallback, this, _1, _2)); - merged_object_pub_ = create_publisher( - "output/object", rclcpp::QoS{1}); - // Parameters base_link_frame_id_ = declare_parameter("base_link_frame_id", "base_link"); priority_mode_ = static_cast( declare_parameter("priority_mode", static_cast(PriorityMode::Confidence))); + sync_queue_size_ = declare_parameter("sync_queue_size", 20); remove_overlapped_unknown_objects_ = declare_parameter("remove_overlapped_unknown_objects", true); overlapped_judge_param_.precision_threshold = @@ -115,6 +108,16 @@ ObjectAssociationMergerNode::ObjectAssociationMergerNode(const rclcpp::NodeOptio const auto min_iou_matrix = this->declare_parameter>("min_iou_matrix"); data_association_ = std::make_unique( can_assign_matrix, max_dist_matrix, max_rad_matrix, min_iou_matrix); + + // Create publishers and subscribers + using std::placeholders::_1; + using std::placeholders::_2; + sync_ptr_ = std::make_shared(SyncPolicy(sync_queue_size_), object0_sub_, object1_sub_); + sync_ptr_->registerCallback( + std::bind(&ObjectAssociationMergerNode::objectsCallback, this, _1, _2)); + + merged_object_pub_ = create_publisher( + "output/object", rclcpp::QoS{1}); } void ObjectAssociationMergerNode::objectsCallback( diff --git a/perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp b/perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp index 8e909e4fc08c8..a65302aea866d 100644 --- a/perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp +++ b/perception/radar_fusion_to_detected_object/src/radar_object_fusion_to_detected_object_node/radar_object_fusion_to_detected_object_node.cpp @@ -97,7 +97,7 @@ RadarObjectFusionToDetectedObjectNode::RadarObjectFusionToDetectedObjectNode( using std::placeholders::_1; using std::placeholders::_2; - sync_ptr_ = std::make_shared(SyncPolicy(10), sub_object_, sub_radar_); + sync_ptr_ = std::make_shared(SyncPolicy(20), sub_object_, sub_radar_); sync_ptr_->registerCallback( std::bind(&RadarObjectFusionToDetectedObjectNode::onData, this, _1, _2)); diff --git a/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp b/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp index 506a40e672fb6..1f3e4eb3792ac 100644 --- a/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp +++ b/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp @@ -168,7 +168,7 @@ void SimpleObjectMergerNode::onTimer() output_objects.header.frame_id = node_param_.new_frame_id; for (size_t i = 0; i < input_topic_size; i++) { - double time_diff = (this->get_clock()->now()).seconds() - + double time_diff = rclcpp::Time(objects_data_.at(i)->header.stamp).seconds() - rclcpp::Time(objects_data_.at(0)->header.stamp).seconds(); if (std::abs(time_diff) < node_param_.timeout_threshold) { transform_ = transform_listener_->getTransform(