(tmp) convert reaction_analyzer to run with rosbag

update

update

update

update

tools/reaction_analyzer/CMakeLists.txt

@@ -5,6 +5,7 @@ find_package(autoware_cmake REQUIRED)
 autoware_package()
 
 find_package(PCL REQUIRED)
+find_package(Eigen3 REQUIRED)
 
 ament_auto_add_library(reaction_analyzer SHARED
   include/reaction_analyzer_node.hpp
@@ -14,6 +15,7 @@ ament_auto_add_library(reaction_analyzer SHARED
 target_include_directories(reaction_analyzer
   SYSTEM PUBLIC
   ${PCL_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIR}
 )
 
 target_link_libraries(reaction_analyzer

tools/reaction_analyzer/include/reaction_analyzer_node.hpp

@@ -36,6 +36,7 @@
 #include <pcl_conversions/pcl_conversions.h>
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
+#include <rosbag2_cpp/reader.hpp>
 
 #include <memory>
 #include <mutex>
@@ -135,6 +136,11 @@ class ReactionAnalyzerNode : public rclcpp::Node
   NodeParams node_params_;
   double entity_search_radius_;
 
+  // TEMP
+  bool run_from_bag_{true};
+  PointCloud2::SharedPtr msg_cloud_empty_;
+  PointCloud2::SharedPtr msg_cloud_with_obj_;
+
   // Initialization Variables
   geometry_msgs::msg::Pose entity_pose_;
   geometry_msgs::msg::PoseWithCovarianceStamped init_pose_;

tools/reaction_analyzer/launch/reaction_analyzer.launch.xml

@@ -3,7 +3,7 @@
   <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/points_raw" 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"/>

tools/reaction_analyzer/package.xml

@@ -23,12 +23,16 @@
   <depend>eigen</depend>
   <depend>libpcl-all-dev</depend>
   <depend>motion_utils</depend>
+  <depend>rosbag2_cpp</depend>
   <depend>pcl_conversions</depend>
+  <depend>pcl_ros</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>sensor_msgs</depend>
   <depend>tf2</depend>
   <depend>tf2_ros</depend>
+  <depend>tf2_eigen</depend>
+  <depend>tf2_geometry_msgs</depend>
   <depend>tier4_autoware_utils</depend>
 
   <test_depend>ament_cmake_ros</test_depend>

tools/reaction_analyzer/param/reaction_analyzer.param.yaml

@@ -1,22 +1,26 @@
 /**:
   ros__parameters:
+    use_sim_time: 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
     observer:
-      timer_period: 0.033 # s
+      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:
-        x: 1.5
-        y: 345.0
-        z: 101.25
+        x: 81389.51599364934
+        y: 49924.1119766238
+        z: 42.240638732910156
         roll: 0.0
         pitch: 0.0
         yaw: 90.0
-        x_dimension: 3.0
-        y_dimension: 3.0
-        z_dimension: 2.5
+        x_dimension: 4.118675972722859
+        y_dimension: 1.7809072588403219
+        z_dimension: 0.8328610206872963
       initial_pose:
         x: 1.5
         y: 200.0
@@ -32,27 +36,48 @@
         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
-      pointcloud_topic:
+#      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
+      ground_filtered_pointcloud:
+        topic_name: /perception/obstacle_segmentation/single_frame/pointcloud_raw
+        message_type: sensor_msgs::msg::PointCloud2
+      obstacle_seg_pointcloud:
         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:
         topic_name: /perception/object_recognition/objects
         message_type: autoware_auto_perception_msgs::msg::PredictedObjects