update

brkay54 · Dec 28, 2023 · 9f51559 · 9f51559
1 parent 0a5ba7b
commit 9f51559
Show file tree

Hide file tree

Showing 3 changed files with 137 additions and 134 deletions.
diff --git a/tools/reaction_analyzer/include/reaction_analyzer_node.hpp b/tools/reaction_analyzer/include/reaction_analyzer_node.hpp
@@ -57,10 +57,10 @@ using geometry_msgs::msg::Pose;
 using nav_msgs::msg::Odometry;
 using sensor_msgs::msg::PointCloud2;
 
-using ControlBuffer =
+using ControlCommandBuffer =
   std::pair<std::vector<AckermannControlCommand>, std::optional<AckermannControlCommand>>;
-using PlanningBuffer = std::pair<Trajectory, std::optional<Trajectory>>;
-using BufferVariant = std::variant<ControlBuffer, PlanningBuffer>;
+using TrajectoryBuffer = std::pair<Trajectory, std::optional<Trajectory>>;
+using BufferVariant = std::variant<ControlCommandBuffer, TrajectoryBuffer>;
 
 enum class MessageType {
   Unknown = 0,
@@ -116,17 +116,7 @@ class ReactionAnalyzerNode : public rclcpp::Node
   ~ReactionAnalyzerNode() = default;
 
 private:
-  void initObservers(const ChainModules & modules);
-  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_;
-  // Module list in chain
-  ChainModules chain_modules;
 
   // Parameters
   EntityParams entity_params_;
@@ -139,7 +129,7 @@ class ReactionAnalyzerNode : public rclcpp::Node
   geometry_msgs::msg::PoseWithCovarianceStamped init_pose_;
   geometry_msgs::msg::PoseStamped goal_pose_;
 
-  // Core Subscribers
+  // Subscribers
   rclcpp::Subscription<Odometry>::SharedPtr sub_kinematics_;
   rclcpp::Subscription<RouteState>::SharedPtr sub_route_state_;
   rclcpp::Subscription<LocalizationInitializationState>::SharedPtr sub_localization_init_state_;
@@ -156,44 +146,52 @@ class ReactionAnalyzerNode : public rclcpp::Node
   tf2_ros::TransformListener tf_listener_{tf_buffer_};
 
   // Functions
-  void loadChainModules();
-  void initTestVariables();
+  bool loadChainModules();
+  bool initObservers(const ChainModules & modules);
+  void initAnalyzerVariables();
   void initPointcloud();
   void initPredictedObjects();
   void initEgoForTest(
     LocalizationInitializationState::ConstSharedPtr initialization_state_ptr,
     RouteState::ConstSharedPtr route_state_ptr);
-  void onTimer();
   void setControlCommandToBuffer(
     std::vector<AckermannControlCommand> & buffer, const AckermannControlCommand & cmd);
   std::optional<size_t> findFirstBrakeIdx(const std::vector<AckermannControlCommand> & cmd_array);
+  void onTimer();
 
   // Callbacks
   void vehiclePoseCallback(const Odometry::ConstSharedPtr msg_ptr);
   void initializationStateCallback(const LocalizationInitializationState::ConstSharedPtr msg_ptr);
   void routeStateCallback(const RouteState::ConstSharedPtr msg);
   void operationModeCallback(const OperationModeState::ConstSharedPtr msg_ptr);
 
+  // Callbacks for modules are observed
+  void controlCommandOutputCallback(
+    const std::string & node_name, const AckermannControlCommand::ConstSharedPtr msg_ptr);
+  void planningOutputCallback(
+    const std::string & node_name, const Trajectory::ConstSharedPtr msg_ptr);
+
   // Variables
-  std::optional<rclcpp::Time> spawn_cmd_time;
+  std::vector<rclcpp::SubscriptionBase::SharedPtr> subscribers_;
+  std::unordered_map<std::string, BufferVariant> messageBuffers_;
+  std::optional<rclcpp::Time> last_test_environment_init_time_;
+  std::optional<rclcpp::Time> spawn_cmd_time_;
   bool is_test_environment_created_{false};
   bool is_ego_initialized_{false};
   bool is_route_set_{false};
-  bool is_output_printed{false};
+  bool is_output_printed_{false};
   bool all_topics_reacted_{false};
-  std::optional<rclcpp::Time> last_initialize_test_time_;
 
   // Timer
   rclcpp::TimerBase::SharedPtr timer_;
 
   // Client
   rclcpp::Client<ChangeOperationMode>::SharedPtr client_change_to_autonomous_;
-  template <typename T>
-  void callServiceWithoutResponse(const typename rclcpp::Client<T>::SharedPtr client);
+  void callOperationModeServiceWithoutResponse(const rclcpp::Client<ChangeOperationMode>::SharedPtr client);
 
   // Pointers
   PointCloud2::SharedPtr entity_pointcloud_ptr_;
-  PredictedObjects::SharedPtr predicted_objects_ptr;
+  PredictedObjects::SharedPtr predicted_objects_ptr_;
   Odometry::ConstSharedPtr odometry_;
   LocalizationInitializationState::ConstSharedPtr initialization_state_ptr_;
   RouteState::ConstSharedPtr current_route_state_ptr_;

diff --git a/tools/reaction_analyzer/param/reaction_analyzer.param.yaml b/tools/reaction_analyzer/param/reaction_analyzer.param.yaml
@@ -1,24 +1,5 @@
 /**:
   ros__parameters:
-    chain:
-      planning_validator:
-        topic_name: /planning/scenario_planning/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
-      obstacle_stop_planner:
-        topic_name: /planning/scenario_planning/lane_driving/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
-      vehicle_cmd_gate:
-        topic_name: /control/command/control_cmd
-        message_type: autoware_auto_control_msgs::msg::AckermannControlCommand
     observer:
       timer_period: 0.033 # s
       spawn_distance_threshold: 25.0
@@ -50,3 +31,22 @@
         roll: 0.0
         pitch: 0.0
         yaw: 90.0
+    chain:
+      planning_validator:
+        topic_name: /planning/scenario_planning/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
+      obstacle_stop_planner:
+        topic_name: /planning/scenario_planning/lane_driving/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
+      vehicle_cmd_gate:
+        topic_name: /control/command/control_cmd
+        message_type: autoware_auto_control_msgs::msg::AckermannControlCommand