chore: apply ktro2828/autoware_mtr

Signed-off-by: ktro2828 <[email protected]>

perception/autoware_mtr/.gitignore

@@ -0,0 +1,51 @@
+devel/
+logs/
+build/
+bin/
+msg_gen/
+srv_gen/
+data/*onnx
+msg/*Action.msg
+msg/*ActionFeedback.msg
+msg/*ActionGoal.msg
+msg/*ActionResult.msg
+msg/*Feedback.msg
+msg/*Goal.msg
+msg/*Result.msg
+msg/_*.py
+build_isolated/
+devel_isolated/
+
+# Generated by dynamic reconfigure
+*.cfgc
+/cfg/cpp/
+/cfg/*.py
+
+# Ignore generated docs
+*.dox
+*.wikidoc
+
+# eclipse stuff
+.project
+.cproject
+
+# qcreator stuff
+CMakeLists.txt.user
+
+srv/_*.py
+*.pcd
+*.pyc
+qtcreator-*
+*.user
+
+/planning/cfg
+/planning/docs
+/planning/src
+
+*~
+
+# Emacs
+.#*
+
+# Catkin custom files
+CATKIN_IGNORE

perception/autoware_mtr/.pre-commit-config-optional.yaml

@@ -0,0 +1,6 @@
+repos:
+  - repo: https://github.com/tcort/markdown-link-check
+    rev: v3.12.2
+    hooks:
+      - id: markdown-link-check
+        args: [--quiet, --config=.markdown-link-check.json]

perception/autoware_mtr/.pre-commit-config.yaml

@@ -0,0 +1,95 @@
+ci:
+  autofix_commit_msg: "style(pre-commit): autofix"
+
+repos:
+  - repo: https://github.com/pre-commit/pre-commit-hooks
+    rev: v4.6.0
+    hooks:
+      - id: check-json
+      - id: check-merge-conflict
+      - id: check-toml
+      - id: check-xml
+      - id: check-yaml
+        args: [--unsafe]
+      - id: detect-private-key
+      - id: end-of-file-fixer
+      - id: mixed-line-ending
+      - id: trailing-whitespace
+        args: [--markdown-linebreak-ext=md]
+
+  - repo: https://github.com/igorshubovych/markdownlint-cli
+    rev: v0.41.0
+    hooks:
+      - id: markdownlint
+        args: [-c, .markdownlint.yaml, --fix]
+
+  - repo: https://github.com/pre-commit/mirrors-prettier
+    rev: v4.0.0-alpha.8
+    hooks:
+      - id: prettier
+
+  - repo: https://github.com/adrienverge/yamllint
+    rev: v1.35.1
+    hooks:
+      - id: yamllint
+
+  - repo: https://github.com/tier4/pre-commit-hooks-ros
+    rev: v0.10.0
+    hooks:
+      - id: flake8-ros
+      - id: prettier-xacro
+      - id: prettier-launch-xml
+      - id: prettier-package-xml
+      - id: ros-include-guard
+      - id: sort-package-xml
+
+  - repo: https://github.com/shellcheck-py/shellcheck-py
+    rev: v0.10.0.1
+    hooks:
+      - id: shellcheck
+
+  - repo: https://github.com/scop/pre-commit-shfmt
+    rev: v3.9.0-1
+    hooks:
+      - id: shfmt
+        args: [-w, -s, -i=4]
+
+  - repo: https://github.com/pycqa/isort
+    rev: 5.13.2
+    hooks:
+      - id: isort
+
+  - repo: https://github.com/psf/black
+    rev: 24.8.0
+    hooks:
+      - id: black
+        args: [--line-length=100]
+
+  - repo: https://github.com/pre-commit/mirrors-clang-format
+    rev: v18.1.8
+    hooks:
+      - id: clang-format
+        types_or: [c++, c, cuda]
+
+  - repo: https://github.com/cpplint/cpplint
+    rev: 1.6.1
+    hooks:
+      - id: cpplint
+        args: [--quiet]
+        exclude: .cu
+
+  - repo: https://github.com/python-jsonschema/check-jsonschema
+    rev: 0.29.2
+    hooks:
+      - id: check-metaschema
+        files: ^.+/schema/.*schema\.json$
+
+  - repo: local
+    hooks:
+      - id: prettier-svg
+        name: prettier svg
+        description: Apply Prettier with plugin-xml to svg.
+        entry: prettier --write --list-different --ignore-unknown --print-width 200 --xml-self-closing-space false --xml-whitespace-sensitivity ignore
+        language: node
+        files: .svg$
+        additional_dependencies: [[email protected], "@prettier/[email protected]"]

perception/autoware_mtr/LICENSE

@@ -1,4 +1,3 @@
-
                                  Apache License
                            Version 2.0, January 2004
                         http://www.apache.org/licenses/

perception/autoware_mtr/README.md

@@ -1 +1 @@
-# Motion TRansformer (MTR)
+# autoware-mtr

perception/autoware_mtr/config/mtr.param.yaml

@@ -12,9 +12,6 @@
       intention_point_filepath: "$(var data_path)/intention_point.csv"
       num_intention_point_cluster: 64
       polyline_label_path: "$(var data_path)/polyline_label.txt"
-      static_inference:
-        num_target: 2
-        num_agent: 7
     build_params:
       is_dynamic: false
       precision: "FP32"

perception/autoware_mtr/include/autoware/mtr/builder.hpp

@@ -155,10 +155,10 @@ class MTRBuilder
    *
    * @return True if plugins were initialized successfully.
    */
-  bool isInitialized() const;
+  [[nodiscard]] bool isInitialized() const;
 
   // Return true if the model supports dynamic shape inference.
-  bool isDynamic() const;
+  [[nodiscard]] bool isDynamic() const;
 
   // Set binding dimensions for specified for dynamic shape inference.
   bool setBindingDimensions(int index, nvinfer1::Dims dimensions);
@@ -184,7 +184,7 @@ class MTRBuilder
   bool loadEngine(const std::string & filepath);
 
   // Create a cache path of engine file.
-  fs::path createEngineCachePath() const;
+  [[nodiscard]] fs::path createEngineCachePath() const;
 
   /**
    * @brief Build engine from onnx file.

perception/autoware_mtr/include/autoware/mtr/cuda_helper.hpp

@@ -23,7 +23,6 @@
 
 #include <cuda_runtime_api.h>
 
-#include <iostream>
 #include <memory>
 #include <sstream>
 #include <stdexcept>

perception/autoware_mtr/include/autoware/mtr/node.hpp

@@ -22,10 +22,13 @@
 
 #include <autoware/object_recognition_utils/object_classification.hpp>
 #include <autoware/universe_utils/geometry/geometry.hpp>
+#include <autoware/universe_utils/ros/polling_subscriber.hpp>
 #include <autoware/universe_utils/ros/transform_listener.hpp>
 #include <autoware/universe_utils/ros/uuid_helper.hpp>
+#include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>
 #include <rclcpp/rclcpp.hpp>
 
+#include <autoware_map_msgs/msg/detail/lanelet_map_bin__struct.hpp>
 #include <autoware_map_msgs/msg/lanelet_map_bin.hpp>
 #include <autoware_perception_msgs/msg/object_classification.hpp>
 #include <autoware_perception_msgs/msg/predicted_object_kinematics.hpp>
@@ -47,7 +50,8 @@
 
 namespace autoware::mtr
 {
-using autoware_map_msgs::msg::LaneletMapBin;
+using HADMapBin = autoware_map_msgs::msg::LaneletMapBin;
+using autoware::vehicle_info_utils::VehicleInfo;
 using autoware_perception_msgs::msg::ObjectClassification;
 using autoware_perception_msgs::msg::PredictedObject;
 using autoware_perception_msgs::msg::PredictedObjectKinematics;
@@ -57,17 +61,6 @@ using autoware_perception_msgs::msg::TrackedObject;
 using autoware_perception_msgs::msg::TrackedObjects;
 using nav_msgs::msg::Odometry;
 
-// TODO(ktro2828): use received ego size topic
-// wheel_base: between front wheel center and rear wheel center [m]
-// wheel_tread: between left wheel center and right wheel center [m]
-// front_overhang: between front wheel center and vehicle front [m]
-// rear_overhang: between rear wheel center and vehicle rear [m]
-// left_overhang: between left wheel center and vehicle left [m]
-// right_overhang: between right wheel center and vehicle right [m]
-constexpr float EGO_LENGTH = 4.0f;
-constexpr float EGO_WIDTH = 2.0f;
-constexpr float EGO_HEIGHT = 1.0f;
-
 class PolylineTypeMap
 {
 public:
@@ -88,9 +81,9 @@ class PolylineTypeMap
     }
   }
 
-  // Return the ID corresponding to the label type. If specified type is not contained in map,
+  // Return the ID of the corresponding label type. If specified type is not contained in map,
   // return `-1`.
-  int type_to_id(const std::string & type) const
+  [[nodiscard]] int getTypeID(const std::string & type) const
   {
     return label_map_.count(type) == 0 ? -1 : label_map_.at(type);
   }
@@ -112,42 +105,47 @@ class MTRNode : public rclcpp::Node
   void callback(const TrackedObjects::ConstSharedPtr object_msg);
 
   // Callback being invoked when the HD map topic is subscribed.
-  void on_map(const LaneletMapBin::ConstSharedPtr map_msg);
+  void onMap(const HADMapBin::ConstSharedPtr map_msg);
 
-  // Callback being invoked when the Ego's odometry topic is subscribed.
-  void on_ego(const Odometry::ConstSharedPtr ego_msg);
+  // Fetch data of Ego's odometry topic.
+  bool fetchData();
 
   // Convert Lanelet to `PolylineData`.
-  bool lanelet_to_polyline();
+  bool convertLaneletToPolyline();
 
   // Remove ancient agent histories.
-  void remove_ancient_history(
+  void removeAncientAgentHistory(
     const float current_time, const TrackedObjects::ConstSharedPtr objects_msg);
 
   // Appends new states to history.
-  void update_history(const float current_time, const TrackedObjects::ConstSharedPtr objects_msg);
+  void updateAgentHistory(
+    const float current_time, const TrackedObjects::ConstSharedPtr objects_msg);
 
   // Extract ego state stored in the buffer which has the nearest timestamp from current timestamp.
-  AgentState lookup_ego_state(const float current_time) const;
+  AgentState extractNearestEgo(const float current_time) const;
+
+  [[nodiscard]] TrackedObject makeEgoTrackedObject(const Odometry::ConstSharedPtr ego_msg) const;
 
   // Extract target agents and return corresponding indices.
   // NOTE: Extract targets in order of proximity, closest first.
-  std::vector<size_t> extract_target_agents(const std::vector<AgentHistory> & histories);
+  std::vector<size_t> extractTargetAgent(const std::vector<AgentHistory> & histories);
 
   // Return the timestamps relative from the first element.Return the timestamps relative from the
   // first element.
-  std::vector<float> get_relative_timestamps() const;
+  std::vector<float> getRelativeTimestamps() const;
 
   // Generate `PredictedObject` from `PredictedTrajectory`.
-  PredictedObject to_predicted_object(
+  PredictedObject generatePredictedObject(
     const TrackedObject & object, const PredictedTrajectory & trajectory);
 
   // ROS Publisher and Subscriber
   // TODO(ktro2828): add debug publisher
   rclcpp::Publisher<PredictedObjects>::SharedPtr pub_objects_;
   rclcpp::Subscription<TrackedObjects>::SharedPtr sub_objects_;
-  rclcpp::Subscription<LaneletMapBin>::SharedPtr sub_map_;
-  rclcpp::Subscription<Odometry>::SharedPtr sub_ego_;
+  rclcpp::Subscription<HADMapBin>::SharedPtr sub_map_;
+  // polling subscriber
+  autoware::universe_utils::InterProcessPollingSubscriber<Odometry> sub_ego_{
+    this, "/localization/kinematic_state"};
 
   // Lanelet map pointers
   std::shared_ptr<lanelet::LaneletMap> lanelet_map_ptr_;
@@ -157,6 +155,8 @@ class MTRNode : public rclcpp::Node
   // Agent history
   std::map<std::string, AgentHistory> agent_history_map_;
   std::map<std::string, TrackedObject> object_msg_map_;
+  TrackedObject ego_tracked_object_;
+  VehicleInfo vehicle_info_;
 
   // Pose transform listener
   autoware::universe_utils::TransformListener transform_listener_;
@@ -168,7 +168,7 @@ class MTRNode : public rclcpp::Node
   PolylineTypeMap polyline_type_map_;
   std::shared_ptr<PolylineData> polyline_ptr_;
   std::vector<std::pair<float, AgentState>> ego_states_;
-  std::vector<float> timestamps_;
+  std::vector<double> timestamps_;
 };  // class MTRNode
 }  // namespace autoware::mtr
 #endif  // AUTOWARE__MTR__NODE_HPP_

perception/autoware_mtr/include/autoware/mtr/trt_mtr.hpp

@@ -29,6 +29,7 @@
 #include <array>
 #include <memory>
 #include <string>
+#include <utility>
 #include <vector>
 
 namespace autoware::mtr
@@ -50,12 +51,12 @@ struct MTRConfig
    * @param intention_point_filepath The path to intention points file.
    * @param num_intention_point_cluster The number of clusters for intension points.
    */
-  MTRConfig(
+  explicit MTRConfig(
     const std::vector<std::string> & target_labels = {"VEHICLE", "PEDESTRIAN", "CYCLIST"},
     const size_t num_past = 11, const size_t num_mode = 6, const size_t num_future = 80,
     const size_t max_num_polyline = 768, const size_t max_num_point = 20,
     const float point_break_distance = 1.0f,
-    const std::string & intention_point_filepath = "./data/intention_point.csv",
+    std::string intention_point_filepath = "./data/intention_point.csv",
     const size_t num_intention_point_cluster = 64)
   : target_labels(target_labels),
     num_class(target_labels.size()),
@@ -65,7 +66,7 @@ struct MTRConfig
     max_num_polyline(max_num_polyline),
     max_num_point(max_num_point),
     point_break_distance(point_break_distance),
-    intention_point_filepath(intention_point_filepath),
+    intention_point_filepath(std::move(intention_point_filepath)),
     num_intention_point_cluster(num_intention_point_cluster)
   {
   }
@@ -97,7 +98,7 @@ class TrtMTR
    * @param max_workspace_size The max size of workspace.
    * @param build_config The configuration of build.
    */
-  TrtMTR(
+  explicit TrtMTR(
     const std::string & model_path, const MTRConfig & config = MTRConfig(),
     const BuildConfig & build_config = BuildConfig(),
     const size_t max_workspace_size = (1ULL << 30));

perception/autoware_mtr/launch/mtr.launch.xml

@@ -6,7 +6,7 @@
   <arg name="input/objects" default="/perception/object_recognition/tracking/objects"/>
   <arg name="input/vector_map" default="/map/vector_map"/>
   <arg name="input/ego" default="/localization/kinematic_state"/>
-  <arg name="output/objects" default="objects"/>
+  <arg name="output/objects" default="mtr/objects"/>
   <arg name="build_only" default="false"/>
 
   <node pkg="autoware_mtr" exec="autoware_mtr" name="mtr" output="screen">