ROS2 Porting: multi_object_tracker (autowarefoundation#24)

* First pass at node (exclude tracker library)
 - Rename files to match existing packages
   - node -> _core
   - main -> _node

 - Add publishers and subscribers
 - Add TF and logging
 - issue with lookupTransform from tf2::BufferCore doesn't allow for duration specification in foxy
bfe938b

* Add the base implementation of the helper class and files
 - Depends on the unique_id package which has not been release rosdep yet
 - Point to the correct packages
 - Add vehicle_tracker class
 - Missing uuid implementation for the object id creation
 - Convert bicycle_tracker class
 - Convert pedestrian_tracker to ROS2
 - Add successive_shortest_path library
   - Changed header file extension to match existing files
 - Convert data_association to ROS2
   - Return type change to resolve -Wreturn-type warning

* Add data associator and tracker method calls back into the MultiObjectTracker implementation

* Clean up
 - Align headers
 - Fix typo
 - Add back the transform with no duration;  see issue
 - Remove comments

* Clean up package.xml and CMakelists

* Clean up header order

* Remove main file

* Fix issues after rebase
 - Add wait for transform to add duration
 - Add UUID generation
 - Add -Werror

* Address PR comment:
 - Reintroduce getUUID method in tracker code

* Fix the waitForTransform implementation
 - Use the synchronous approach to getting the transform using wait_for()

* Address PR comments:
 - Add further comments in CMakelist explaining Eigen library deps
 - Use durable transient_local QoS for subscriber
 - Remove second explicit string type decl
 - Convert buffer and listener to plain objects, move initialization to initialization list
 - Make generation of random bits more idiomatic
 - Use indepedent_bit_engine instead of uniform distribution

* Address PR comments:
 - Fix eigen cmake dep

* Fix Eigen3 deps
 - find cmake module

perception/multi_object_tracker/CMakeLists.txt

@@ -1,34 +1,27 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.5)
 project(multi_object_tracker)
 
-add_compile_options(-std=c++14)
+### Compile options
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wno-unused-parameter -Wall -Wextra -Wpedantic)
+endif()
 
-find_package(catkin REQUIRED COMPONENTS
-  autoware_perception_msgs
-  roscpp
-  tf2
-  tf2_ros
-  unique_id
-  uuid_msgs
-)
+### Find Packages
+find_package(ament_cmake_auto REQUIRED)
 
+### Find Eigen Dependencies
+find_package(eigen3_cmake_module REQUIRED)
 find_package(Eigen3 REQUIRED)
 
-catkin_package(
-  CATKIN_DEPENDS
-    autoware_perception_msgs
-    roscpp
-    tf2
-    tf2_ros
-    unique_id
-    uuid_msgs
-)
-
-include_directories(include ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR})
+### Find dependencies listed in the package.xml
+ament_auto_find_build_dependencies()
 
-add_executable(multi_object_tracker_node
-  src/main.cpp
-  src/node.cpp
+# Generate exe file
+set(MULTI_OBJECT_TRACKER_SRC
+  src/multi_object_tracker_core.cpp
   src/utils/utils.cpp
   src/tracker/model/tracker_base.cpp
   src/tracker/model/vehicle_tracker.cpp
@@ -38,20 +31,30 @@ add_executable(multi_object_tracker_node
   src/data_association/successive_shortest_path/successive_shortest_path.cpp
 )
 
-add_dependencies(multi_object_tracker_node
-  ${${PROJECT_NAME}_EXPORTED_TARGETS}
-  ${catkin_EXPORTED_TARGETS}
+set(MULTI_OBJECT_TRACKER_HEADERS
+  src/data_association/successive_shortest_path/successive_shortest_path.hpp
+  include/multi_object_tracker/data_association/data_association.hpp
+  include/multi_object_tracker/tracker/tracker.hpp
+  include/multi_object_tracker/tracker/model/tracker_base.hpp
+  include/multi_object_tracker/tracker/model/bicycle_tracker.hpp
+  include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp
+  include/multi_object_tracker/tracker/model/vehicle_tracker.hpp
+  include/multi_object_tracker/utils/utils.hpp
+  include/multi_object_tracker/multi_object_tracker_core.hpp
 )
 
-target_link_libraries(multi_object_tracker_node ${catkin_LIBRARIES})
+ament_auto_add_library(multi_object_tracker_node SHARED 
+  ${MULTI_OBJECT_TRACKER_SRC} 
+  ${MULTI_OBJECT_TRACKER_HEADERS}
+)
 
-install(TARGETS multi_object_tracker_node
-        ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-        LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-        RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+target_link_libraries(multi_object_tracker_node Eigen3::Eigen)
 
-install(
-  DIRECTORY
-    launch
-  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+rclcpp_components_register_node(multi_object_tracker_node
+  PLUGIN "MultiObjectTracker"
+  EXECUTABLE multi_object_tracker
 )
+
+ament_auto_package(INSTALL_TO_SHARE
+  launch
+) 

perception/multi_object_tracker/include/multi_object_tracker/data_association/data_association.hpp

@@ -16,20 +16,25 @@
  * v1.0 Yukihiro Saito
  */
 
-#pragma once
-#include <autoware_perception_msgs/DynamicObjectWithFeatureArray.h>
-#include <list>
-#include <unordered_map>
-#include <vector>
+#ifndef MULTI_OBJECT_TRACKER_DATA_ASSOCIATION_HPP_
+#define MULTI_OBJECT_TRACKER_DATA_ASSOCIATION_HPP_
+
 #include "multi_object_tracker/tracker/tracker.hpp"
+#include "autoware_perception_msgs/msg/dynamic_object_with_feature_array.hpp"
+
 #define EIGEN_MPL2_ONLY
 #include <Eigen/Core>
 #include <Eigen/Geometry>
+
+#include <list>
+#include <unordered_map>
+#include <vector>
+
 class DataAssociation
 {
 private:
   double getDistance(
-    const geometry_msgs::Point & measurement, const geometry_msgs::Point & tracker);
+    const geometry_msgs::msg::Point & measurement, const geometry_msgs::msg::Point & tracker);
   Eigen::MatrixXi can_assgin_matrix_;
   Eigen::MatrixXd max_dist_matrix_;
   Eigen::MatrixXd max_area_matrix_;
@@ -38,11 +43,13 @@ class DataAssociation
 
 public:
   DataAssociation();
-  bool assign(
+  void assign(
     const Eigen::MatrixXd & src, std::unordered_map<int, int> & direct_assignment,
     std::unordered_map<int, int> & reverse_assignment);
   Eigen::MatrixXd calcScoreMatrix(
-    const autoware_perception_msgs::DynamicObjectWithFeatureArray & measurements,
+    const autoware_perception_msgs::msg::DynamicObjectWithFeatureArray & measurements,
     const std::list<std::shared_ptr<Tracker>> & trackers);
   virtual ~DataAssociation(){};
 };
+
+#endif

perception/multi_object_tracker/include/multi_object_tracker/node.hpp → perception/multi_object_tracker/include/multi_object_tracker/multi_object_tracker_core.hpp

@@ -16,44 +16,49 @@
  * v1.0 Yukihiro Saito
  */
 
-#pragma once
+#ifndef MULTI_OBJECT_TRACKER_CORE_HPP_
+#define MULTI_OBJECT_TRACKER_CORE_HPP_
+
+#include "multi_object_tracker/data_association/data_association.hpp"
+#include "multi_object_tracker/tracker/model/tracker_base.hpp"
+#include "autoware_perception_msgs/msg/dynamic_object_array.hpp"
+#include "autoware_perception_msgs/msg/dynamic_object_with_feature_array.hpp"
 
-#include <ros/ros.h>
 #include <tf2/LinearMath/Transform.h>
 #include <tf2/convert.h>
 #include <tf2/transform_datatypes.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <rclcpp/rclcpp.hpp>
+
 #include <memory>
 #include <vector>
-#include "autoware_perception_msgs/DynamicObjectArray.h"
-#include "autoware_perception_msgs/DynamicObjectWithFeatureArray.h"
-#include "geometry_msgs/PoseStamped.h"
-#include "multi_object_tracker/data_association/data_association.hpp"
-#include "multi_object_tracker/tracker/model/tracker_base.hpp"
 
-class MultiObjectTrackerNode
+class MultiObjectTracker : public rclcpp::Node
 {
-private:  // ros
-  ros::NodeHandle nh_;
-  ros::NodeHandle pnh_;
-  ros::Publisher pub_;
-  ros::Subscriber sub_;
-  ros::Timer publish_timer_;  // publish timer
+public:
+  explicit MultiObjectTracker(const rclcpp::NodeOptions & node_options);
+
+private:
+  rclcpp::Publisher<autoware_perception_msgs::msg::DynamicObjectArray>::SharedPtr
+    dynamic_object_pub_;
+  rclcpp::Subscription<autoware_perception_msgs::msg::DynamicObjectWithFeatureArray>::SharedPtr
+    dynamic_object_sub_;
+  rclcpp::TimerBase::SharedPtr publish_timer_;  // publish timer
+
   tf2_ros::Buffer tf_buffer_;
   tf2_ros::TransformListener tf_listener_;
 
   void measurementCallback(
-    const autoware_perception_msgs::DynamicObjectWithFeatureArray::ConstPtr & input_objects_msg);
-  void publishTimerCallback(const ros::TimerEvent & e);
+    const autoware_perception_msgs::msg::DynamicObjectWithFeatureArray::ConstSharedPtr
+      input_objects_msg);
+  void publishTimerCallback();
 
   std::string world_frame_id_;  // tracking frame
   std::list<std::shared_ptr<Tracker>> list_tracker_;
   DataAssociation data_association_;
-
-public:
-  MultiObjectTrackerNode();
-
-  ~MultiObjectTrackerNode(){};
 };
+
+#endif

perception/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp

@@ -16,14 +16,16 @@
  * v1.0 Yukihiro Saito
  */
 
-#pragma once
-#include "autoware_perception_msgs/DynamicObject.h"
-#include "tracker_base.hpp"
+#ifndef MULTI_OBJECT_TRACKER_BICYCLE_TRACKER_HPP_
+#define MULTI_OBJECT_TRACKER_BICYCLE_TRACKER_HPP_
+
+#include "multi_object_tracker/tracker/model/tracker_base.hpp"
+#include "autoware_perception_msgs/msg/dynamic_object.hpp"
 
 class BicycleTracker : public Tracker
 {
 private:
-  autoware_perception_msgs::DynamicObject object_;
+  autoware_perception_msgs::msg::DynamicObject object_;
   double filtered_posx_;
   double filtered_posy_;
   double pos_filter_gain_;
@@ -34,16 +36,20 @@ class BicycleTracker : public Tracker
   double area_filter_gain_;
   double last_measurement_posx_;
   double last_measurement_posy_;
-  ros::Time last_update_time_;
-  ros::Time last_measurement_time_;
+  rclcpp::Time last_update_time_;
+  rclcpp::Time last_measurement_time_;
 
 public:
-  BicycleTracker(const ros::Time & time, const autoware_perception_msgs::DynamicObject & object);
+  BicycleTracker(
+    const rclcpp::Time & time, const autoware_perception_msgs::msg::DynamicObject & object);
 
-  bool predict(const ros::Time & time) override;
+  bool predict(const rclcpp::Time & time) override;
   bool measure(
-    const autoware_perception_msgs::DynamicObject & object, const ros::Time & time) override;
+    const autoware_perception_msgs::msg::DynamicObject & object,
+    const rclcpp::Time & time) override;
   bool getEstimatedDynamicObject(
-    const ros::Time & time, autoware_perception_msgs::DynamicObject & object) override;
+    const rclcpp::Time & time, autoware_perception_msgs::msg::DynamicObject & object) override;
   virtual ~BicycleTracker(){};
 };
+
+#endif

perception/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp

@@ -16,14 +16,16 @@
  * v1.0 Yukihiro Saito
  */
 
-#pragma once
-#include "autoware_perception_msgs/DynamicObject.h"
-#include "tracker_base.hpp"
+#ifndef MULTI_OBJECT_TRACKER_PEDESTRIAN_TRACKER_HPP_
+#define MULTI_OBJECT_TRACKER_PEDESTRIAN_TRACKER_HPP_
+
+#include "multi_object_tracker/tracker/model/tracker_base.hpp"
+#include "autoware_perception_msgs/msg/dynamic_object.hpp"
 
 class PedestrianTracker : public Tracker
 {
 private:
-  autoware_perception_msgs::DynamicObject object_;
+  autoware_perception_msgs::msg::DynamicObject object_;
   double filtered_posx_;
   double filtered_posy_;
   double pos_filter_gain_;
@@ -34,16 +36,20 @@ class PedestrianTracker : public Tracker
   double area_filter_gain_;
   double last_measurement_posx_;
   double last_measurement_posy_;
-  ros::Time last_update_time_;
-  ros::Time last_measurement_time_;
+  rclcpp::Time last_update_time_;
+  rclcpp::Time last_measurement_time_;
 
 public:
-  PedestrianTracker(const ros::Time & time, const autoware_perception_msgs::DynamicObject & object);
+  PedestrianTracker(
+    const rclcpp::Time & time, const autoware_perception_msgs::msg::DynamicObject & object);
 
-  bool predict(const ros::Time & time) override;
+  bool predict(const rclcpp::Time & time) override;
   bool measure(
-    const autoware_perception_msgs::DynamicObject & object, const ros::Time & time) override;
+    const autoware_perception_msgs::msg::DynamicObject & object,
+    const rclcpp::Time & time) override;
   bool getEstimatedDynamicObject(
-    const ros::Time & time, autoware_perception_msgs::DynamicObject & object) override;
+    const rclcpp::Time & time, autoware_perception_msgs::msg::DynamicObject & object) override;
   virtual ~PedestrianTracker(){};
 };
+
+#endif

perception/multi_object_tracker/include/multi_object_tracker/tracker/model/tracker_base.hpp

@@ -16,50 +16,56 @@
  * v1.0 Yukihiro Saito
  */
 
-#pragma once
-#include <autoware_perception_msgs/DynamicObject.h>
-#include <geometry_msgs/Point.h>
-#include <ros/ros.h>
-#include <unique_id/unique_id.h>
+#ifndef MULTI_OBJECT_TRACKER_TRACKER_BASE_HPP_
+#define MULTI_OBJECT_TRACKER_TRACKER_BASE_HPP_
+
+#include "autoware_perception_msgs/msg/dynamic_object.hpp"
+
+#include <geometry_msgs/msg/point.hpp>
+#include <unique_identifier_msgs/msg/uuid.hpp>
+#include <rclcpp/rclcpp.hpp>
 
 class Tracker
 {
 protected:
-  boost::uuids::uuid getUUID() { return uuid_; }
+  unique_identifier_msgs::msg::UUID getUUID() { return uuid_; }
   void setType(int type) { type_ = type; }
 
 private:
-  boost::uuids::uuid uuid_;
+  unique_identifier_msgs::msg::UUID uuid_;
   int type_;
   int no_measurement_count_;
   int total_measurement_count_;
-  ros::Time last_update_with_measurement_time_;
+  rclcpp::Time last_update_with_measurement_time_;
 
 public:
-  Tracker(const ros::Time & time, const int type);
+  Tracker(const rclcpp::Time & time, const int type);
   virtual ~Tracker(){};
   bool updateWithMeasurement(
-    const autoware_perception_msgs::DynamicObject & object, const ros::Time & measurement_time);
+    const autoware_perception_msgs::msg::DynamicObject & object,
+    const rclcpp::Time & measurement_time);
   bool updateWithoutMeasurement();
   int getType() { return type_; }
   int getNoMeasurementCount() { return no_measurement_count_; }
   int getTotalMeasurementCount() { return total_measurement_count_; }
-  double getElapsedTimeFromLastUpdate()
+  double getElapsedTimeFromLastUpdate(const rclcpp::Time & current_time)
   {
-    return (ros::Time::now() - last_update_with_measurement_time_).toSec();
+    return (current_time - last_update_with_measurement_time_).seconds();
   }
-  virtual geometry_msgs::Point getPosition(const ros::Time & time);
-  virtual double getArea(const ros::Time & time);
+  virtual geometry_msgs::msg::Point getPosition(const rclcpp::Time & time);
+  virtual double getArea(const rclcpp::Time & time);
 
   /*
    *　Pure virtual function
    */
 protected:
   virtual bool measure(
-    const autoware_perception_msgs::DynamicObject & object, const ros::Time & time) = 0;
+    const autoware_perception_msgs::msg::DynamicObject & object, const rclcpp::Time & time) = 0;
 
 public:
   virtual bool getEstimatedDynamicObject(
-    const ros::Time & time, autoware_perception_msgs::DynamicObject & object) = 0;
-  virtual bool predict(const ros::Time & time) = 0;
+    const rclcpp::Time & time, autoware_perception_msgs::msg::DynamicObject & object) = 0;
+  virtual bool predict(const rclcpp::Time & time) = 0;
 };
+
+#endif