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 7f2fc45396f7f..178e193dc2d99 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..df833296e3f66 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,9 @@ - + + - + @@ -218,7 +219,7 @@ - + @@ -290,7 +291,7 @@ - + @@ -299,7 +300,7 @@ - + @@ -381,7 +382,7 @@ - + @@ -389,8 +390,18 @@ - + + + + + + + + + + + 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..a4044de2f8f96 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,14 @@ - + + - + @@ -100,7 +103,7 @@ - + @@ -110,11 +113,12 @@ + - + @@ -127,11 +131,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 ccc236d9cdc94..23e0297dc5e44 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,7 @@ - + @@ -70,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..52c8aedff8ef9 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 @@ - - - - - + + + + + - - + + diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index a30b87a1068b5..36d43bab74894 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -11,6 +11,7 @@ + @@ -22,11 +23,11 @@ - + - + @@ -69,7 +70,8 @@ - + + @@ -87,15 +89,15 @@ description="options: `traffic_light_classifier_mobilenetv2_batch_*` or `traffic_light_classifier_efficientNet_b1_batch_*`. The batch number must be either one of 1, 4, 6" /> - + - + - + @@ -108,7 +110,7 @@ - + @@ -126,10 +128,10 @@ - + - + @@ -158,6 +160,7 @@ + @@ -169,14 +172,15 @@ - + - + + @@ -192,7 +196,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..a21ae8f7fa327 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_fusion_to_detected_object.cpp b/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp index 4f0933e993de8..523124078d7f2 100644 --- a/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp +++ b/perception/radar_fusion_to_detected_object/src/radar_fusion_to_detected_object.cpp @@ -293,7 +293,7 @@ TwistWithCovariance RadarFusionToDetectedObject::estimateTwist( bool RadarFusionToDetectedObject::isQualified( const DetectedObject & object, std::shared_ptr> & radars) { - if (object.classification[0].probability > param_.threshold_probability) { + if (object.existence_probability > param_.threshold_probability) { return true; } else { if (!radars || !(*radars).empty()) { diff --git a/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp b/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp index ab5e4eb5abe90..df7726ce7dad5 100644 --- a/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp +++ b/perception/radar_tracks_msgs_converter/src/radar_tracks_msgs_converter_node/radar_tracks_msgs_converter_node.cpp @@ -212,8 +212,15 @@ TrackedObjects RadarTracksMsgsConverterNode::convertRadarTrackToTrackedObjects() kinematics.orientation_availability = TrackedObjectKinematics::AVAILABLE; kinematics.is_stationary = false; - // Twist conversion - geometry_msgs::msg::Vector3 compensated_velocity = radar_track.velocity; + geometry_msgs::msg::Vector3 compensated_velocity{}; + { + double rotate_yaw = tf2::getYaw(transform_->transform.rotation); + const geometry_msgs::msg::Vector3 & vel = radar_track.velocity; + compensated_velocity.x = vel.x * std::cos(rotate_yaw) - vel.y * std::sin(rotate_yaw); + compensated_velocity.y = vel.x * std::sin(rotate_yaw) + vel.y * std::cos(rotate_yaw); + compensated_velocity.z = radar_track.velocity.z; + } + if (node_param_.use_twist_compensation) { if (odometry_data_) { compensated_velocity.x += odometry_data_->twist.twist.linear.x; @@ -233,12 +240,12 @@ TrackedObjects RadarTracksMsgsConverterNode::convertRadarTrackToTrackedObjects() double yaw = tier4_autoware_utils::normalizeRadian( std::atan2(compensated_velocity.y, compensated_velocity.x)); - radar_pose_stamped.pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); geometry_msgs::msg::PoseStamped transformed_pose_stamped{}; tf2::doTransform(radar_pose_stamped, transformed_pose_stamped, *transform_); kinematics.pose_with_covariance.pose = transformed_pose_stamped.pose; - + kinematics.pose_with_covariance.pose.orientation = + tier4_autoware_utils::createQuaternionFromYaw(yaw); { auto & pose_cov = kinematics.pose_with_covariance.covariance; auto & radar_position_cov = radar_track.position_covariance; 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(