clean up

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

Author: pre-commit-ci[bot]

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,76 +140,83 @@ 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>
 struct PublisherVariables
 {
-  double period_ns;
+  double period_ns{0.0};
   typename MessageType::SharedPtr empty_area_message;
   typename MessageType::SharedPtr object_spawned_message;
   typename rclcpp::Publisher<MessageType>::SharedPtr publisher;
   rclcpp::TimerBase::SharedPtr timer;
 };
 
-template <typename T>
-struct PublisherVariablesMessageTypeExtractor;
-
-template <typename MessageType>
-struct PublisherVariablesMessageTypeExtractor<PublisherVariables<MessageType>>
-{
-  using Type = MessageType;
-};
-
-template <typename T, typename = std::void_t<>>
-struct has_header : std::false_type
+struct PublisherVarAccessor
 {
-};
+  template <typename T, typename = std::void_t<>>
+  struct has_header : std::false_type
+  {
+  };
 
-template <typename T>
-struct has_header<T, std::void_t<decltype(T::header)>> : std::true_type
-{
-};
+  template <typename T>
+  struct has_header<T, std::void_t<decltype(T::header)>> : std::true_type
+  {
+  };
+  template <typename T, typename = std::void_t<>>
+  struct has_stamp : std::false_type
+  {
+  };
 
-struct PublisherVarAccessor
-{
-  // Method to set header time if available
   template <typename T>
-  void setHeaderTimeIfAvailable(T & publisherVar, const rclcpp::Time & time)
+  struct has_stamp<T, std::void_t<decltype(T::stamp)>> : std::true_type
   {
-    if constexpr (has_header<typename T::MessageType>::value) {
-      if (publisherVar.empty_area_message) {
-        publisherVar.empty_area_message->header.stamp = time;
-      }
-      if (publisherVar.object_spawned_message) {
-        publisherVar.object_spawned_message->header.stamp = time;
-      }
+  };
+  template <typename MessageType>
+  void publishWithCurrentTime(
+    const PublisherVariables<MessageType> & publisherVar, const rclcpp::Time & current_time,
+    const bool is_object_spawned) const
+  {
+    std::unique_ptr<MessageType> msg_to_be_published = std::make_unique<MessageType>();
+
+    if (is_object_spawned) {
+      *msg_to_be_published = *publisherVar.object_spawned_message;
+    } else {
+      *msg_to_be_published = *publisherVar.empty_area_message;
     }
+    if constexpr (has_header<MessageType>::value) {
+      msg_to_be_published->header.stamp = current_time;
+    } else if constexpr (has_stamp<MessageType>::value) {
+      msg_to_be_published->stamp = current_time;
+    }
+    publisherVar.publisher->publish(std::move(msg_to_be_published));
   }
-
   // Set Period
   template <typename T>
   void setPeriod(T & publisherVar, double newPeriod)
@@ -204,62 +231,19 @@ struct PublisherVarAccessor
     return publisherVar.period_ns;
   }
 
-  // Set Empty Area Message
-  template <typename T, typename Message>
-  void setEmptyAreaMessage(T & publisherVar, typename Message::SharedPtr message)
-  {
-    publisherVar.empty_area_message = message;
-  }
-
   // Get Empty Area Message
   template <typename T>
   std::shared_ptr<void> getEmptyAreaMessage(const T & publisherVar) const
   {
     return std::static_pointer_cast<void>(publisherVar.empty_area_message);
   }
 
-  // Set Object Spawned Message
-  template <typename T, typename Message>
-  void setObjectSpawnedMessage(T & publisherVar, typename Message::SharedPtr message)
-  {
-    publisherVar.object_spawned_message = message;
-  }
-
   // Get Object Spawned Message
   template <typename T>
   std::shared_ptr<void> getObjectSpawnedMessage(const T & publisherVar) const
   {
     return std::static_pointer_cast<void>(publisherVar.object_spawned_message);
   }
-
-  // Set Publisher
-  template <typename T, typename PublisherType>
-  void setPublisher(
-    T & publisherVar, typename rclcpp::Publisher<PublisherType>::SharedPtr publisher)
-  {
-    publisherVar.publisher = publisher;
-  }
-
-  // Get Publisher
-  template <typename T>
-  std::shared_ptr<void> getPublisher(const T & publisherVar) const
-  {
-    return std::static_pointer_cast<void>(publisherVar.publisher);
-  }
-
-  // Set Timer
-  template <typename T>
-  void setTimer(T & publisherVar, rclcpp::TimerBase::SharedPtr newTimer)
-  {
-    publisherVar.timer = newTimer;
-  }
-
-  // Get Timer
-  template <typename T>
-  std::shared_ptr<void> getTimer(const T & publisherVar) const
-  {
-    return std::static_pointer_cast<void>(publisherVar.timer);
-  }
 };
 
 using PublisherVariablesVariant = std::variant<
@@ -283,26 +267,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 +315,7 @@ class ReactionAnalyzerNode : public rclcpp::Node
 
   bool loadChainModules();
 
-  bool initObservers(const ChainModules & modules);
+  bool initSubscribers(const reaction_analyzer::ChainModules & modules);
 
   void initAnalyzerVariables();
 
@@ -358,35 +342,36 @@ 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_;
   std::unordered_map<std::string, BufferVariant> message_buffers_;
   std::optional<rclcpp::Time> last_test_environment_init_time_;
   std::optional<rclcpp::Time> spawn_cmd_time_;
+  std::atomic<bool> is_object_spawned_{false};
   bool is_test_environment_created_{false};
   bool is_ego_initialized_{false};
   bool is_route_set_{false};

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="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>