fix(gyro_odometer): use all data

localization/gyro_odometer/CMakeLists.txt

@@ -12,3 +12,5 @@ ament_auto_add_executable(${PROJECT_NAME}
 ament_auto_package(INSTALL_TO_SHARE
   launch
 )
+
+target_link_libraries(gyro_odometer fmt)

localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp

@@ -31,41 +31,50 @@
 #include <tf2_ros/transform_listener.h>
 
 #include <string>
+#include <vector>
 
 class GyroOdometer : public rclcpp::Node
 {
+  using TwistStamped = geometry_msgs::msg::TwistStamped;
+  using TwistWithCovarianceStamped = geometry_msgs::msg::TwistWithCovarianceStamped;
+
 public:
   GyroOdometer();
   ~GyroOdometer();
 
 private:
+  void timerCallback();
+
   void callbackTwistWithCovariance(
-    const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_with_cov_msg_ptr);
+    const TwistWithCovarianceStamped::ConstSharedPtr twist_with_cov_msg_ptr);
   void callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr);
   bool getTransform(
     const std::string & target_frame, const std::string & source_frame,
     const geometry_msgs::msg::TransformStamped::SharedPtr transform_stamped_ptr);
 
-  rclcpp::Subscription<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr
-    vehicle_twist_with_cov_sub_;
+  rclcpp::Subscription<TwistWithCovarianceStamped>::SharedPtr vehicle_twist_with_cov_sub_;
   rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;
+  rclcpp::TimerBase::SharedPtr timer_;
 
-  rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_raw_pub_;
-  rclcpp::Publisher<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr
-    twist_with_covariance_raw_pub_;
+  rclcpp::Publisher<TwistStamped>::SharedPtr twist_raw_pub_;
+  rclcpp::Publisher<TwistWithCovarianceStamped>::SharedPtr twist_with_covariance_raw_pub_;
 
-  rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_pub_;
-  rclcpp::Publisher<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr
-    twist_with_covariance_pub_;
+  rclcpp::Publisher<TwistStamped>::SharedPtr twist_pub_;
+  rclcpp::Publisher<TwistWithCovarianceStamped>::SharedPtr twist_with_covariance_pub_;
 
   tf2_ros::Buffer tf_buffer_;
   tf2_ros::TransformListener tf_listener_;
 
+  std::vector<TwistWithCovarianceStamped> vel_buffer_;
+  std::vector<TwistWithCovarianceStamped> gyro_buffer_;
+  rclcpp::Time latest_vel_timestamp_;
+  rclcpp::Time latest_imu_timestamp_;
+
   std::string output_frame_;
   double message_timeout_sec_;
 
-  geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_with_cov_msg_ptr_;
-  sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr_;
+  bool is_velocity_arrived_;
+  bool is_imu_arrived_;
 };
 
 #endif  // GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_

localization/gyro_odometer/launch/gyro_odometer.launch.xml

@@ -11,6 +11,7 @@
 
   <arg name="output_frame" default="base_link" description="output frame id"/>
   <arg name="message_timeout_sec" default="0.2" description="delay tolerance time for message"/>
+  <arg name="output_rate" default="50.0" description="output rate for twist"/>
 
   <node pkg="gyro_odometer" exec="gyro_odometer" name="gyro_odometer" output="screen">
     <remap from="vehicle/twist_with_covariance" to="$(var input_vehicle_twist_with_covariance_topic)"/>
@@ -25,5 +26,6 @@
 
     <param name="output_frame" value="$(var output_frame)"/>
     <param name="message_timeout_sec" value="$(var message_timeout_sec)"/>
+    <param name="output_rate" value="$(var output_rate)"/>
   </node>
 </launch>

localization/gyro_odometer/package.xml

@@ -12,16 +12,15 @@
 
   <build_depend>autoware_cmake</build_depend>
 
+  <depend>fmt</depend>
   <depend>geometry_msgs</depend>
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
   <depend>tf2</depend>
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
-
   <exec_depend>rclcpp</exec_depend>
   <exec_depend>rclcpp_components</exec_depend>
-
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>