added all planning topics

Author: brkay54

tools/reaction_analyzer/include/reaction_analyzer_node.hpp

@@ -15,6 +15,7 @@
 #ifndef REACTION_ANALYZER_NODE_HPP_
 #define REACTION_ANALYZER_NODE_HPP_
 
+#include <motion_utils/trajectory/trajectory.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/geometry/geometry.hpp>
 #include <tier4_autoware_utils/math/unit_conversion.hpp>
@@ -55,17 +56,16 @@ using autoware_auto_planning_msgs::msg::Trajectory;
 using geometry_msgs::msg::Pose;
 using nav_msgs::msg::Odometry;
 using sensor_msgs::msg::PointCloud2;
+
 using ControlBuffer =
   std::pair<std::vector<AckermannControlCommand>, std::optional<AckermannControlCommand>>;
-
-using BufferVariant =
-  std::variant<ControlBuffer
-               // , std::vector<OtherMessageType> // Add other message types here
-               >;
+using PlanningBuffer = std::pair<Trajectory, std::optional<Trajectory>>;
+using BufferVariant = std::variant<ControlBuffer, PlanningBuffer>;
 
 enum class MessageType {
   Unknown = 0,
   AckermannControlCommand = 1,
+  Trajectory = 2,
 };
 
 struct TopicConfig
@@ -120,7 +120,8 @@ class ReactionAnalyzerNode : public rclcpp::Node
   std::vector<rclcpp::SubscriptionBase::SharedPtr> subscribers_;
   void controlCommandOutputCallback(
     const std::string & node_name, const AckermannControlCommand::ConstSharedPtr msg_ptr);
-
+  void planningOutputCallback(
+    const std::string & node_name, const Trajectory::ConstSharedPtr msg_ptr);
   std::unordered_map<std::string, BufferVariant> messageBuffers_;
 
   std::mutex mutex_;
@@ -182,10 +183,6 @@ class ReactionAnalyzerNode : public rclcpp::Node
   bool all_topics_reacted_{false};
   std::optional<rclcpp::Time> last_initialize_test_time_;
 
-  // measurement for control cmd
-  //  std::optional<ControlCommandWithSeenTime> trajectory_follower_brake_cmd;
-  //  std::optional<ControlCommandWithSeenTime> vehicle_cmd_gate_brake_cmd;
-
   // Timer
   rclcpp::TimerBase::SharedPtr timer_;
 

tools/reaction_analyzer/package.xml

@@ -20,6 +20,7 @@
   <depend>autoware_auto_planning_msgs</depend>
   <depend>autoware_auto_system_msgs</depend>
   <depend>autoware_auto_vehicle_msgs</depend>
+  <depend>motion_utils</depend>
   <depend>eigen</depend>
   <depend>libpcl-all-dev</depend>
   <depend>pcl_conversions</depend>

tools/reaction_analyzer/param/reaction_analyzer.param.yaml

@@ -1,6 +1,18 @@
 /**:
   ros__parameters:
     chain:
+      obstacle_stop_planner:
+        topic_name: /planning/scenario_planning/lane_driving/trajectory
+        message_type: autoware_auto_planning_msgs::msg::Trajectory
+      scenario_selector:
+        topic_name: /planning/scenario_planning/scenario_selector/trajectory
+        message_type: autoware_auto_planning_msgs::msg::Trajectory
+      motion_velocity_smoother:
+        topic_name: /planning/scenario_planning/motion_velocity_smoother/trajectory
+        message_type: autoware_auto_planning_msgs::msg::Trajectory
+      planning_validator:
+        topic_name: /planning/scenario_planning/trajectory
+        message_type: autoware_auto_planning_msgs::msg::Trajectory
       trajectory_follower:
         topic_name: /control/trajectory_follower/control_cmd
         message_type: autoware_auto_control_msgs::msg::AckermannControlCommand

tools/reaction_analyzer/src/reaction_analyzer_node.cpp

@@ -82,6 +82,8 @@ MessageType stringToMessageType(const std::string & input)
 {
   if (input == "autoware_auto_control_msgs::msg::AckermannControlCommand") {
     return MessageType::AckermannControlCommand;
+  } else if (input == "autoware_auto_planning_msgs::msg::Trajectory") {
+    return MessageType::Trajectory;
   } else {
     return MessageType::Unknown;
   }
@@ -305,6 +307,28 @@ void ReactionAnalyzerNode::onTimer()
             mutex_.unlock();
           }
         }
+      } else if (auto * message = std::get_if<PlanningBuffer>(&variant)) {
+        if (!message->second) {
+          all_topics_reacted_ = false;
+          const auto nearest_idx = motion_utils::findNearestIndex(
+            message->first.points, current_odometry_ptr->pose.pose.position);
+          //
+          //          constexpr double lookahead_distance = 15.0;
+          //          const auto destination_idx = motion_utils::calcLongitudinalOffsetPoint(
+          //            message->first.points, nearest_idx, lookahead_distance);
+          const auto zero_vel_idx = motion_utils::searchZeroVelocityIndex(
+            message->first.points, nearest_idx, message->first.points.size() - 1);
+
+          if (zero_vel_idx) {
+            std::cout << key << " found: nearest_idx: " << nearest_idx
+                      << " zero_vel_idx: " << zero_vel_idx.value()
+                      << " size: " << message->first.points.size() << " stamp: " << message->first.header.stamp.nanosec << std::endl;
+            mutex_.lock();
+            auto * tmp = std::get_if<PlanningBuffer>(&messageBuffers_[key]);
+            if (tmp) tmp->second = message->first;
+            mutex_.unlock();
+          }
+        }
       }
       // Add similar blocks for other types
     }
@@ -317,6 +341,10 @@ void ReactionAnalyzerNode::onTimer()
         if (message->second) {
           reaction_times.emplace_back(key, rclcpp::Time(message->second->stamp));
         }
+      } else if (auto * message = std::get_if<PlanningBuffer>(&variant)) {
+        if (message->second) {
+          reaction_times.emplace_back(key, rclcpp::Time(message->second->header.stamp));
+        }
       }
     }
 
@@ -450,6 +478,20 @@ void ReactionAnalyzerNode::controlCommandOutputCallback(
   setControlCommandToBuffer(std::get<ControlBuffer>(variant).first, *msg_ptr);
 }
 
+void ReactionAnalyzerNode::planningOutputCallback(
+  const std::string & node_name, const Trajectory::ConstSharedPtr msg_ptr)
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  auto & variant = messageBuffers_[node_name];
+  if (!std::holds_alternative<PlanningBuffer>(variant)) {
+    // If the variant doesn't hold a vector of AckermannControlCommand yet, initialize it
+    PlanningBuffer buffer(*msg_ptr, std::nullopt);
+    variant = buffer;
+  } else {
+    std::get<PlanningBuffer>(variant).first = *msg_ptr;
+  }
+}
+
 void ReactionAnalyzerNode::initObservers(const reaction_analyzer::ChainModules & modules)
 {
   for (const auto & module : modules) {
@@ -463,6 +505,15 @@ void ReactionAnalyzerNode::initObservers(const reaction_analyzer::ChainModules &
         subscribers_.push_back(subscriber);
         break;
       }
+      case MessageType::Trajectory: {
+        auto callback = [this, module](const Trajectory::ConstSharedPtr msg) {
+          this->planningOutputCallback(module.node_name, msg);
+        };
+        auto subscriber = this->create_subscription<Trajectory>(
+          module.topic_address, 1, callback, createSubscriptionOptions(this));
+        subscribers_.push_back(subscriber);
+        break;
+      }
       case MessageType::Unknown:
         RCLCPP_WARN(
           this->get_logger(), "Unknown message type for topic: %s", module.topic_address.c_str());