update for detected objects

update reaction analyzer

update

Author: brkay54

tools/reaction_analyzer/include/reaction_analyzer_node.hpp

@@ -117,6 +117,7 @@ struct NodeParams
   double control_cmd_buffer_time_interval;
   double min_jerk_for_brake_cmd;
   int min_number_descending_order_control_cmd;
+  bool run_from_bag;
 };
 
 class ReactionAnalyzerNode : public rclcpp::Node
@@ -137,7 +138,6 @@ class ReactionAnalyzerNode : public rclcpp::Node
   double entity_search_radius_;
 
   // TEMP
-  bool run_from_bag_{true};
   PointCloud2::SharedPtr msg_cloud_empty_;
   PointCloud2::SharedPtr msg_cloud_with_obj_;
 

tools/reaction_analyzer/param/reaction_analyzer.param.yaml

@@ -1,6 +1,7 @@
 /**:
   ros__parameters:
     use_sim_time: true
+    run_from_bag: true
     path_bag_without_object: /home/berkay/projects/bags/awsim-bag/bag-without-car-full/rosbag2_2024_01_17-16_23_07_0.db3
     path_bag_with_object: /home/berkay/projects/bags/awsim-bag/bag-with-car-full/rosbag2_2024_01_17-17_27_14_0.db3
     topic_cloud: /sensing/lidar/top/pointcloud_raw_ex
@@ -54,30 +55,30 @@
 #      vehicle_cmd_gate:
 #        topic_name: /control/command/control_cmd
 #        message_type: autoware_auto_control_msgs::msg::AckermannControlCommand
-      ground_filtered_pointcloud:
-        topic_name: /perception/obstacle_segmentation/single_frame/pointcloud_raw
-        message_type: sensor_msgs::msg::PointCloud2
-      obstacle_seg_pointcloud:
+#      ground_filtered_pointcloud:
+#        topic_name: /perception/obstacle_segmentation/single_frame/pointcloud_raw
+#        message_type: sensor_msgs::msg::PointCloud2
+      occupancy_grid_map_outlier:
         topic_name: /perception/obstacle_segmentation/pointcloud
         message_type: sensor_msgs::msg::PointCloud2
       voxel_based_compare_map_filter:
         topic_name: /perception/object_recognition/detection/pointcloud_map_filtered/pointcloud
         message_type: sensor_msgs::msg::PointCloud2
-#      cropbox_filter_self:
-#        topic_name: /sensing/lidar/top/self_cropped/pointcloud_ex
-#        message_type: sensor_msgs::msg::PointCloud2
-#      cropbox_filter_mirror:
-#        topic_name: /sensing/lidar/top/mirror_cropped/pointcloud_ex
-#        message_type: sensor_msgs::msg::PointCloud2
-#      distortion_corrector:
-#        topic_name: /sensing/lidar/top/rectified/pointcloud_ex
-#        message_type: sensor_msgs::msg::PointCloud2
-#      outlier_filter:
-#        topic_name: /sensing/lidar/top/outlier_filtered/pointcloud
-#        message_type: sensor_msgs::msg::PointCloud2
-#      concat_pointcloud:
-#        topic_name: /sensing/lidar/top/concatenated/pointcloud
-#        message_type: sensor_msgs::msg::PointCloud2
-      predicted_objects_topic:
+      lidar_centerpoint:
+        topic_name: /perception/object_recognition/detection/centerpoint/objects
+        message_type: autoware_auto_perception_msgs::msg::DetectedObjects
+      obstacle_pointcloud_based_validator:
+        topic_name: /perception/object_recognition/detection/centerpoint/validation/objects
+        message_type: autoware_auto_perception_msgs::msg::DetectedObjects
+      detected_object_feature_remover:
+        topic_name: /perception/object_recognition/detection/clustering/objects
+        message_type: autoware_auto_perception_msgs::msg::DetectedObjects
+      detection_by_tracker:
+        topic_name: /perception/object_recognition/detection/detection_by_tracker/objects
+        message_type: autoware_auto_perception_msgs::msg::DetectedObjects
+      object_lanelet_filter:
+        topic_name: /perception/object_recognition/detection/objects
+        message_type: autoware_auto_perception_msgs::msg::DetectedObjects
+      map_based_prediction:
         topic_name: /perception/object_recognition/objects
         message_type: autoware_auto_perception_msgs::msg::PredictedObjects

tools/reaction_analyzer/src/reaction_analyzer_node.cpp

@@ -110,16 +110,19 @@ void ReactionAnalyzerNode::pointcloud2OutputCallback(
   const auto current_odometry_ptr = odometry_;
   //  const auto is_spawned = spawn_cmd_time_;
   mutex_.unlock();
-  if (!current_odometry_ptr) return;
 
   if (!std::holds_alternative<PointCloud2Buffer>(variant)) {
     // If the variant doesn't hold a Trajectory message yet, initialize it
     PointCloud2Buffer buffer(std::nullopt);
     variant = buffer;
+    mutex_.lock();
+    message_buffers_[node_name] = variant;
+    mutex_.unlock();
   } else if (std::get<PointCloud2Buffer>(variant).has_value()) {
     // reacted
     return;
   }
+  if (!current_odometry_ptr) return;
 
   // transform pointcloud
   geometry_msgs::msg::TransformStamped transform_stamped{};
@@ -156,17 +159,23 @@ void ReactionAnalyzerNode::predictedObjectsOutputCallback(
 {
   mutex_.lock();
   auto variant = message_buffers_[node_name];
+  const auto current_odometry_ptr = odometry_;
   //  const auto is_spawned = spawn_cmd_time_;
   mutex_.unlock();
 
   if (!std::holds_alternative<PredictedObjectsBuffer>(variant)) {
     // If the variant doesn't hold a Trajectory message yet, initialize it
     PredictedObjectsBuffer buffer(std::nullopt);
     variant = buffer;
+    mutex_.lock();
+    message_buffers_[node_name] = variant;
+    mutex_.unlock();
   } else if (std::get<PredictedObjectsBuffer>(variant).has_value()) {
     // reacted
     return;
   }
+  if (!current_odometry_ptr || msg_ptr->objects.empty()) return;
+
   if (searchPredictedObjectsNearEntity(*msg_ptr)) {
     std::get<PredictedObjectsBuffer>(variant) = *msg_ptr;
     std::cout << "Reacted " << node_name << std::endl;
@@ -182,19 +191,45 @@ void ReactionAnalyzerNode::detectedObjectsOutputCallback(
 {
   mutex_.lock();
   auto variant = message_buffers_[node_name];
+  const auto current_odometry_ptr = odometry_;
   //  const auto is_spawned = spawn_cmd_time_;
   mutex_.unlock();
 
   if (!std::holds_alternative<DetectedObjectsBuffer>(variant)) {
     // If the variant doesn't hold a Trajectory message yet, initialize it
     DetectedObjectsBuffer buffer(std::nullopt);
     variant = buffer;
+    mutex_.lock();
+    message_buffers_[node_name] = variant;
+    mutex_.unlock();
   } else if (std::get<DetectedObjectsBuffer>(variant).has_value()) {
     // reacted
     return;
   }
-  if (searchDetectedObjectsNearEntity(*msg_ptr)) {
+
+  if (!current_odometry_ptr || msg_ptr->objects.empty()) return;
+
+  // transform pointcloud
+  geometry_msgs::msg::TransformStamped transform_stamped{};
+  try {
+    transform_stamped = tf_buffer_.lookupTransform(
+      "map", msg_ptr->header.frame_id, this->now(), rclcpp::Duration::from_seconds(0.5));
+  } catch (tf2::TransformException & ex) {
+    RCLCPP_ERROR_STREAM(
+      get_logger(), "Failed to look up transform from " << msg_ptr->header.frame_id << " to map");
+    return;
+  }
+  DetectedObjects output_objs;
+  output_objs = *msg_ptr;
+  for (auto & obj : output_objs.objects) {
+    geometry_msgs::msg::PoseStamped output_stamped, input_stamped;
+    input_stamped.pose = obj.kinematics.pose_with_covariance.pose;
+    tf2::doTransform(input_stamped, output_stamped, transform_stamped);
+    obj.kinematics.pose_with_covariance.pose = output_stamped.pose;
+  }
+  if (searchDetectedObjectsNearEntity(output_objs)) {
     std::get<DetectedObjectsBuffer>(variant) = *msg_ptr;
+    std::cout << "Reacted " << node_name << std::endl;
   }
   //  if (!is_spawned) return;
   mutex_.lock();
@@ -241,6 +276,9 @@ ReactionAnalyzerNode::ReactionAnalyzerNode(rclcpp::NodeOptions options)
     get_parameter("observer.min_jerk_for_brake_cmd").as_double();
   node_params_.min_number_descending_order_control_cmd =
     get_parameter("observer.min_number_descending_order_control_cmd").as_int();
+  node_params_.run_from_bag =
+    get_parameter("run_from_bag").as_bool();
+
 
   // prepare the object which will be spawned
   entity_params_.x = get_parameter("test_manager.entity.x").as_double();
@@ -295,15 +333,15 @@ ReactionAnalyzerNode::ReactionAnalyzerNode(rclcpp::NodeOptions options)
   pub_initial_pose_ = create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>(
     "output/initialpose", rclcpp::QoS(1));
   pub_goal_pose_ = create_publisher<geometry_msgs::msg::PoseStamped>("output/goal", rclcpp::QoS(1));
-  if (!run_from_bag_) {
+  if (!node_params_.run_from_bag) {
     pub_pointcloud_ = create_publisher<PointCloud2>(
       "/perception/obstacle_segmentation/pointcloud", rclcpp::SensorDataQoS());
   }
 
   pub_predicted_objects_ = create_publisher<PredictedObjects>("output/objects", rclcpp::QoS(1));
 
   client_change_to_autonomous_ = create_client<ChangeOperationMode>("service/change_to_autonomous");
-  if (run_from_bag_) {
+  if (node_params_.run_from_bag) {
     // init pointcloud in bags
     const std::string path_bag_without_object =
       get_parameter("path_bag_without_object").as_string();
@@ -408,7 +446,7 @@ void ReactionAnalyzerNode::onTimer()
   const auto message_buffers = message_buffers_;
   auto spawn_cmd_time = spawn_cmd_time_;
   mutex_.unlock();
-  if (!run_from_bag_) {
+  if (!node_params_.run_from_bag) {
     if (!is_test_environment_created_) {
       initEgoForTest(initialization_state_ptr, route_state_ptr);
       return;
@@ -451,7 +489,7 @@ void ReactionAnalyzerNode::onTimer()
         return;
       } else {
         rclcpp::Duration time_diff = this->now() - last_test_environment_init_time_.value();
-        if (time_diff > rclcpp::Duration::from_seconds(10.0)) {
+        if (time_diff > rclcpp::Duration::from_seconds(50.0)) {
           if (!spawn_cmd_time) {
             spawn_cmd_time = this->now();
             mutex_.lock();
@@ -591,18 +629,18 @@ void ReactionAnalyzerNode::printResults(
       const std::pair<std::string, rclcpp::Time> & b) { return a.second < b.second; });
 
   // first print spawn_cmd_time to first reaction
-  std::cout << "spawn_cmd_time to " << reaction_times.begin()->first << ": "
-            << createDurationMs(spawn_cmd_time_.value(), reaction_times.begin()->second)
-            << std::endl;
+//  std::cout << "spawn_cmd_time to " << reaction_times.begin()->first << ": "
+//            << createDurationMs(spawn_cmd_time_.value(), reaction_times.begin()->second)
+//            << std::endl;
 
-  if (reaction_times.size() < 2) return;
+//  if (reaction_times.size() < 2) return;
 
-  for (size_t i = 0; i < reaction_times.size() - 1; i++) {
+  for (size_t i = 0; i < reaction_times.size(); i++) {
     const auto & first_reaction = reaction_times.at(i);
-    const auto & second_reaction = reaction_times.at(i + 1);
+//    const auto & second_reaction = reaction_times.at(i + 1);
 
-    std::cout << first_reaction.first << " to " << second_reaction.first << ": "
-              << createDurationMs(first_reaction.second, second_reaction.second) << std::endl;
+    std::cout << "spawn_cmd_time to " << first_reaction.first << ": "
+              << createDurationMs(spawn_cmd_time_.value(), first_reaction.second) << std::endl;
   }
   is_output_printed_ = true;
 }
@@ -1049,4 +1087,5 @@ bool ReactionAnalyzerNode::searchDetectedObjectsNearEntity(const DetectedObjects
 #include <rclcpp_components/register_node_macro.hpp>
 
 #include <utility>
+
 RCLCPP_COMPONENTS_REGISTER_NODE(reaction_analyzer::ReactionAnalyzerNode)