clean up

Signed-off-by: Berkay Karaman <brkay54@gmail.com>

Author: brkay54

tools/reaction_analyzer/include/reaction_analyzer_node.hpp

@@ -79,7 +79,7 @@ using BufferVariant = std::variant<
   ControlCommandBuffer, TrajectoryBuffer, PointCloud2Buffer, PredictedObjectsBuffer,
   DetectedObjectsBuffer>;
 
-enum class MessageType {
+enum class SubscriberMessageType {
   Unknown = 0,
   AckermannControlCommand = 1,
   Trajectory = 2,
@@ -88,11 +88,31 @@ enum class MessageType {
   PredictedObjects = 5,
 };
 
+enum class PublisherMessageType {
+  Unknown = 0,
+  CameraInfo = 1,
+  Image = 2,
+  PointCloud2 = 3,
+  PoseWithCovarianceStamped = 4,
+  Imu = 5,
+  ControlModeReport = 6,
+  GearReport = 7,
+  HazardLightsReport = 8,
+  SteeringReport = 9,
+  TurnIndicatorsReport = 10,
+  VelocityReport = 11,
+};
+
+enum class RunningMode {
+  PerceptionPlanning = 0,
+  PlanningControl = 1,
+};
+
 struct TopicConfig
 {
   std::string node_name;
   std::string topic_address;
-  MessageType message_type;
+  SubscriberMessageType message_type;
 };
 
 using ChainModules = std::vector<TopicConfig>;
@@ -120,29 +140,32 @@ struct EntityParams
   double z_l;
 };
 
-struct NodeParams
+struct GeneralParams
 {
+  std::string running_mode;
   double timer_period;
+  double object_search_radius_offset;
+  EntityParams entity_params;
+  PoseParams goal_pose;
+};
+
+struct PerceptionPlanningModeParams
+{
+  std::string path_bag_without_object;
+  std::string path_bag_with_object;
+  bool publish_pointcloud_synchronically;
+  double synchronic_pointcloud_publishing_period;
+  double spawn_time_after_init;
+};
+
+struct PlanningControlModeParams
+{
   double spawn_distance_threshold;
+  double spawned_pointcloud_sampling_distance;
   double control_cmd_buffer_time_interval;
   double min_jerk_for_brake_cmd;
-  int min_number_descending_order_control_cmd;
-  bool run_from_bag;
-};
-
-enum class PublisherMessageType {
-  Unknown = 0,
-  CameraInfo = 1,
-  Image = 2,
-  PointCloud2 = 3,
-  PoseWithCovarianceStamped = 4,
-  Imu = 5,
-  ControlModeReport = 6,
-  GearReport = 7,
-  HazardLightsReport = 8,
-  SteeringReport = 9,
-  TurnIndicatorsReport = 10,
-  VelocityReport = 11,
+  size_t min_number_descending_order_control_cmd;
+  PoseParams initial_pose;
 };
 
 template <typename MessageType>
@@ -283,26 +306,26 @@ class ReactionAnalyzerNode : public rclcpp::Node
 
 private:
   std::mutex mutex_;
+  RunningMode node_running_mode_;
 
   // Parameters
-  EntityParams entity_params_;
-  PoseParams initial_pose_params_;
-  PoseParams goal_pose_params_;
-  NodeParams node_params_;
-  double entity_search_radius_;
+  GeneralParams general_params_;
+  PerceptionPlanningModeParams perception_planning_mode_params_;
+  PlanningControlModeParams planning_control_mode_params_;
 
   // TEMP
-  void genericMessagePublisherTimer(const std::string & topic_name);
+  void genericMessagePublisher(const std::string & topic_name);
   std::unordered_map<std::string, PublisherVariablesVariant> topic_publisher_map_;
   std::vector<PublisherVariables<PointCloud2>> sync_pointcloud_publishers_;
-  bool publish_pointcloud_sync_{true};
-  void publishAllPointClouds();
+  void syncPointcloudPublisher();
   rclcpp::TimerBase::SharedPtr sync_pointcloud_publish_timer_;
+  void initRosbagPublishers();
 
   // Initialization Variables
   geometry_msgs::msg::Pose entity_pose_;
   geometry_msgs::msg::PoseWithCovarianceStamped init_pose_;
   geometry_msgs::msg::PoseStamped goal_pose_;
+  double entity_search_radius_;
 
   // Subscribers
   rclcpp::Subscription<Odometry>::SharedPtr sub_kinematics_;
@@ -331,7 +354,7 @@ class ReactionAnalyzerNode : public rclcpp::Node
 
   bool loadChainModules();
 
-  bool initObservers(const ChainModules & modules);
+  bool initSubscribers(const reaction_analyzer::ChainModules & modules);
 
   void initAnalyzerVariables();
 
@@ -358,29 +381,29 @@ class ReactionAnalyzerNode : public rclcpp::Node
   void onTimer();
 
   // Callbacks
-  void vehiclePoseCallback(const Odometry::ConstSharedPtr msg_ptr);
+  void vehiclePoseCallback(Odometry::ConstSharedPtr msg_ptr);
 
-  void initializationStateCallback(const LocalizationInitializationState::ConstSharedPtr msg_ptr);
+  void initializationStateCallback(LocalizationInitializationState::ConstSharedPtr msg_ptr);
 
-  void routeStateCallback(const RouteState::ConstSharedPtr msg);
+  void routeStateCallback(RouteState::ConstSharedPtr msg);
 
-  void operationModeCallback(const OperationModeState::ConstSharedPtr msg_ptr);
+  void operationModeCallback(OperationModeState::ConstSharedPtr msg_ptr);
 
   // Callbacks for modules are observed
   void controlCommandOutputCallback(
-    const std::string & node_name, const AckermannControlCommand::ConstSharedPtr msg_ptr);
+    const std::string & node_name, const AckermannControlCommand::ConstSharedPtr& msg_ptr);
 
   void trajectoryOutputCallback(
-    const std::string & node_name, const Trajectory::ConstSharedPtr msg_ptr);
+    const std::string & node_name, const Trajectory::ConstSharedPtr& msg_ptr);
 
   void pointcloud2OutputCallback(
-    const std::string & node_name, const PointCloud2::ConstSharedPtr msg_ptr);
+    const std::string & node_name, const PointCloud2::ConstSharedPtr& msg_ptr);
 
   void predictedObjectsOutputCallback(
-    const std::string & node_name, const PredictedObjects::ConstSharedPtr msg_ptr);
+    const std::string & node_name, const PredictedObjects::ConstSharedPtr& msg_ptr);
 
   void detectedObjectsOutputCallback(
-    const std::string & node_name, const DetectedObjects::ConstSharedPtr msg_ptr);
+    const std::string & node_name, const DetectedObjects::ConstSharedPtr& msg_ptr);
 
   // Variables
   std::vector<rclcpp::SubscriptionBase::SharedPtr> subscribers_;

tools/reaction_analyzer/launch/reaction_analyzer.launch.xml

@@ -1,9 +1,34 @@
 <launch>
-  <arg name="reaction_analyzer_param_path" default="$(find-pkg-share reaction_analyzer)/param/reaction_analyzer.param.yaml"/>
+  <arg name="reaction_analyzer_param_path"
+       default="$(find-pkg-share reaction_analyzer)/param/reaction_analyzer.param.yaml"/>
+  <arg name="running_mode" default="perception_planning"/> <!-- planning_control or perception_planning -->
+  <!-- Essential parameters -->
+  <arg name="map_path" description="point cloud and lanelet2 map directory path"/>
+  <arg name="vehicle_model" default="sample_vehicle" description="vehicle model name"/>
+  <arg name="sensor_model" default="sample_sensor_kit" description="sensor model name"/>
+
+  <group if="$(eval &quot;'$(var running_mode)'=='perception_planning'&quot;)">
+    <include file="$(find-pkg-share autoware_launch)/launch/e2e_simulator.launch.xml">
+      <arg name="map_path" value="$(var map_path)"/>
+      <arg name="vehicle_model" value="$(var vehicle_model)"/>
+      <arg name="sensor_model" value="$(var sensor_model)"/>
+      <arg name="use_sim_time" value="false"/>
+      <arg name="enable_image_decompressor" value="false"/>
+    </include>
+  </group>
+  <group if="$(eval &quot;'$(var running_mode)'=='planning_control'&quot;)">
+    <include file="$(find-pkg-share autoware_launch)/launch/planning_simulator.launch.xml">
+      <arg name="map_path" value="$(var map_path)"/>
+      <arg name="vehicle_model" value="$(var vehicle_model)"/>
+      <arg name="sensor_model" value="$(var sensor_model)"/>
+      <arg name="use_sim_time" value="false"/>
+    </include>
+  </group>
+
   <node_container pkg="rclcpp_components" exec="component_container_mt" name="reaction_analyzer_container" namespace="" args="" output="screen">
     <composable_node pkg="reaction_analyzer" plugin="reaction_analyzer::ReactionAnalyzerNode" name="reaction_analyzer" namespace="">
       <param from="$(var reaction_analyzer_param_path)"/>
-      <!--      <remap from="output/points_raw" to="/perception/obstacle_segmentation/pointcloud"/>-->
+      <remap from="output/pointcloud" to="/perception/obstacle_segmentation/pointcloud"/>
       <remap from="output/objects" to="/perception/object_recognition/objects"/>
       <remap from="output/initialpose" to="/initialpose"/>
       <remap from="output/goal" to="/planning/mission_planning/goal"/>
@@ -12,6 +37,7 @@
       <remap from="input/localization_initialization_state" to="/api/localization/initialization_state"/>
       <remap from="input/routing_state" to="/api/routing/state"/>
       <remap from="input/operation_mode_state" to="/api/operation_mode/state"/>
+      <param name="running_mode" value="$(var running_mode)"/>
       <extra_arg name="use_intra_process_comms" value="false"/>
     </composable_node>
   </node_container>

tools/reaction_analyzer/param/reaction_analyzer.param.yaml

@@ -1,19 +1,19 @@
 /**:
   ros__parameters:
-    use_sim_time: false
-    #running_mode: "perception_planning" # or planning_control
-    run_from_bag: true
-    path_bag_without_object: /home/berkay/projects/bags/awsim-bag/bag/lexus_bag_without_car/rosbag2_2024_01_30-13_50_45_0.db3
-    path_bag_with_object: /home/berkay/projects/bags/awsim-bag/bag/lexus_bag_with_car/rosbag2_2024_01_30-13_50_17_0.db3
-    topic_cloud: /sensing/lidar/top/pointcloud_raw_ex
-    observer:
+    general:
       timer_period: 0.1 # s
-      spawn_distance_threshold: 25.0
-      control_cmd_buffer_time_interval: 1.0
-      min_number_descending_order_control_cmd: 8
-      min_jerk_for_brake_cmd: 0.3
-    test_manager:
-      entity:
+      object_search_radius_offset: 0.1 # m
+#      entity_params:
+#        x: 1.5
+#        y: 345.0
+#        z: 101.25
+#        roll: 0.0
+#        pitch: 0.0
+#        yaw: 90.0
+#        x_dimension: 3.0
+#        y_dimension: 3.0
+#        z_dimension: 2.5
+      entity_params:
         x: 81392.97671487389
         y: 49927.361443601316
         z: 42.13605499267578
@@ -23,21 +23,41 @@
         x_dimension: 4.118675972722859
         y_dimension: 1.7809072588403219
         z_dimension: 0.8328610206872963
-      initial_pose:
-        x: 1.5
-        y: 200.0
-        z: 101.25
-        roll: 0.0
-        pitch: 0.0
-        yaw: 90.0
       goal_pose:
         x: 81462.78125
         y: 49978.484375
         z: 0.0
         roll: 0.0
         pitch: 0.0
         yaw: 35.0
-    chain:
+#      goal_pose:
+#        x: 1.5
+#        y: 400.0
+#        z: 101.25
+#        roll: 0.0
+#        pitch: 0.0
+#        yaw: 90.0
+    perception_planning:
+      path_bag_without_object: /home/berkay/projects/bags/awsim-bag/bag/lexus_bag_without_car/rosbag2_2024_01_30-13_50_45_0.db3
+      path_bag_with_object: /home/berkay/projects/bags/awsim-bag/bag/lexus_bag_with_car/rosbag2_2024_01_30-13_50_17_0.db3
+      publish_pointcloud_synchronically: false
+      synchronic_pointcloud_publishing_period: 0.1 # s only used if publish_pointcloud_synchronically is true
+      spawn_time_after_init: 10.0 # s
+    planning_control:
+      spawn_distance_threshold: 25.0 # m
+      spawned_pointcloud_sampling_distance: 0.1
+      first_brake_params:
+        control_cmd_buffer_time_interval: 1.0 # s
+        min_number_descending_order_control_cmd: 8
+        min_jerk_for_brake_cmd: 0.3 # m/s^3
+      initialization_pose:
+        x: 1.5
+        y: 200.0
+        z: 101.25
+        roll: 0.0
+        pitch: 0.0
+        yaw: 90.0
+    reaction_chain:
       planning_validator:
         topic_name: /planning/scenario_planning/trajectory
         message_type: autoware_auto_planning_msgs::msg::Trajectory
@@ -83,3 +103,21 @@
       map_based_prediction:
         topic_name: /perception/object_recognition/objects
         message_type: autoware_auto_perception_msgs::msg::PredictedObjects
+#      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
+#      vehicle_cmd_gate:
+#        topic_name: /control/command/control_cmd
+#        message_type: autoware_auto_control_msgs::msg::AckermannControlCommand