refactor

Signed-off-by: satoshi-ota <[email protected]>

planning/autoware_planning_data_analyzer/CMakeLists.txt

@@ -4,42 +4,6 @@ project(autoware_planning_data_analyzer)
 find_package(autoware_cmake REQUIRED)
 autoware_package()
 
-if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 17)
-endif()
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-w)
-endif()
-
-# Library target
-add_library(matplotlib_cpp INTERFACE)
-target_include_directories(matplotlib_cpp
-  INTERFACE
-    $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/src>
-    $<INSTALL_INTERFACE:include>
-)
-target_compile_features(matplotlib_cpp INTERFACE
-  cxx_std_11
-)
-# TODO: Use `Development.Embed` component when requiring cmake >= 3.18
-find_package(Python3 COMPONENTS Interpreter Development REQUIRED)
-target_link_libraries(matplotlib_cpp INTERFACE
-  Python3::Python
-  Python3::Module
-)
-find_package(Python3 COMPONENTS NumPy)
-if(Python3_NumPy_FOUND)
-  target_link_libraries(matplotlib_cpp INTERFACE
-    Python3::NumPy
-  )
-else()
-  target_compile_definitions(matplotlib_cpp INTERFACE WITHOUT_NUMPY)
-endif()
-install(
-  TARGETS matplotlib_cpp
-  EXPORT install_targets
-)
-
 ament_auto_add_library(${PROJECT_NAME} SHARED
   src/node.cpp
   src/utils.cpp
@@ -48,25 +12,11 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src/data_structs.cpp
 )
 
-target_link_libraries(${PROJECT_NAME} matplotlib_cpp)
-
 rclcpp_components_register_node(${PROJECT_NAME}
   PLUGIN "autoware::behavior_analyzer::BehaviorAnalyzerNode"
   EXECUTABLE ${PROJECT_NAME}_node
 )
 
-ament_auto_add_executable(minimal
-  src/main.cpp
-  src/node.cpp
-  src/utils.cpp
-  src/bag_handler.cpp
-  src/evaluation.cpp
-  src/data_structs.cpp
-  )
-
-target_link_libraries(minimal matplotlib_cpp)
-# target_link_libraries(minimal PRIVATE matplotlib_cpp)
-
 ament_auto_package(
   INSTALL_TO_SHARE
   config

planning/autoware_planning_data_analyzer/config/behavior_analyzer.param.yaml

@@ -13,7 +13,14 @@
       longitudinal_accelerations: [-0.4, -0.3, -0.2, -0.1, 0.0, 0.1, 0.2, 0.3, 0.4]
       # longitudinal_accelerations: [-0.2, -0.1, 0.0, 0.1, 0.2]
 
-    weight: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
+    score_weight: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
+    time_decay_weight:
+      s0: [1.0, 0.8, 0.64, 0.51, 0.41, 0.33, 0.26, 0.21, 0.17, 0.13]
+      s1: [1.0, 0.8, 0.64, 0.51, 0.41, 0.33, 0.26, 0.21, 0.17, 0.13]
+      s2: [1.0, 0.8, 0.64, 0.51, 0.41, 0.33, 0.26, 0.21, 0.17, 0.13]
+      s3: [1.0, 0.8, 0.64, 0.51, 0.41, 0.33, 0.26, 0.21, 0.17, 0.13]
+      s4: [1.0, 0.8, 0.64, 0.51, 0.41, 0.33, 0.26, 0.21, 0.17, 0.13]
+      s5: [1.0, 0.8, 0.64, 0.51, 0.41, 0.33, 0.26, 0.21, 0.17, 0.13]
 
     grid_seach:
       min: 0.1

planning/autoware_planning_data_analyzer/src/bag_handler.hpp

@@ -93,9 +93,7 @@ auto Buffer<TFMessage>::get(const rcutils_time_point_value_t now) const -> TFMes
 
 struct BagData
 {
-  explicit BagData(
-    const rcutils_time_point_value_t timestamp, std::shared_ptr<RouteHandler> & route_handler)
-  : timestamp{timestamp}, route_handler{route_handler}
+  explicit BagData(const rcutils_time_point_value_t timestamp) : timestamp{timestamp}
   {
     buffers.emplace(TOPIC::TF, std::make_shared<Buffer<TFMessage>>());
     buffers.emplace(TOPIC::ODOMETRY, std::make_shared<Buffer<Odometry>>());
@@ -109,8 +107,6 @@ struct BagData
 
   std::map<std::string, std::shared_ptr<BufferBase>> buffers{};
 
-  std::shared_ptr<RouteHandler> route_handler;
-
   void update(const rcutils_time_point_value_t dt)
   {
     timestamp += dt;

planning/autoware_planning_data_analyzer/src/data_structs.hpp

@@ -79,15 +79,26 @@ struct GridSearchParameters
   size_t thread_num{4};
 };
 
-struct Parameters
+struct EvaluatorParameters
 {
   size_t resample_num{20};
   double time_resolution{0.5};
-  std::vector<double> weight;
   GridSearchParameters grid_seach{};
   TargetStateParameters target_state{};
 };
 
+struct SelectorParameters
+{
+  explicit SelectorParameters(const size_t sample_num)
+  : time_decay_weight(static_cast<size_t>(METRIC::SIZE), std::vector<double>(sample_num, 0.0)),
+    score_weight(static_cast<size_t>(SCORE::SIZE), 0.0)
+  {
+  }
+
+  std::vector<std::vector<double>> time_decay_weight;
+  std::vector<double> score_weight;
+};
+
 struct Result
 {
   Result(
@@ -96,10 +107,35 @@ struct Result
   : weight{w0, w1, w2, w3, w4, w5}
   {
   }
+  std::shared_ptr<TrajectoryPoints> previous_points{nullptr};
   std::vector<double> weight;
   double loss{0.0};
 };
 
+struct CoreData
+{
+  CoreData(
+    const std::shared_ptr<TrajectoryPoints> & points,
+    const std::shared_ptr<TrajectoryPoints> & previous_points,
+    const std::shared_ptr<PredictedObjects> & objects, const std::string & tag)
+  : points{points}, previous_points{previous_points}, objects{objects}, tag{tag}
+  {
+  }
+
+  std::shared_ptr<TrajectoryPoints> points;
+
+  std::shared_ptr<TrajectoryPoints> previous_points;
+
+  std::shared_ptr<PredictedObjects> objects;
+
+  std::string tag;
+};
+
+struct DataSet
+{
+  std::shared_ptr<TrajectoryPoints> points;
+};
+
 }  // namespace autoware::behavior_analyzer
 
 #endif  // DATA_STRUCTS_HPP_