replace update timer to FunctionTimer (#104)

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

src/planning/dwa_planner/include/dwa_planner/dwa_planner.hpp

@@ -16,6 +16,7 @@
 #define DWA_PLANNER__DWA_PLANNER_HPP_
 
 #include <rclcpp/rclcpp.hpp>
+#include <wheel_stuck_common_utils/ros/function_timer.hpp>
 #include <wheel_stuck_common_utils/ros/no_callback_subscription.hpp>
 
 #include <geometry_msgs/msg/pose_stamped.hpp>
@@ -27,6 +28,7 @@
 
 namespace dwa_planner
 {
+using FunctionTimer = wheel_stuck_common_utils::ros::FunctionTimer;
 
 using OccupancyGrid = nav_msgs::msg::OccupancyGrid;
 using Odometry = nav_msgs::msg::Odometry;
@@ -87,7 +89,7 @@ class DWAPlanner : public rclcpp::Node
   double calculate_stage_cost(const State & state);
   double calculate_terminal_cost(const State & state);
 
-  rclcpp::TimerBase::SharedPtr update_timer_;
+  FunctionTimer::SharedPtr update_timer_;
 
   OccupancyGridSubscription::SharedPtr map_sub_;
   OdometrySubscription::SharedPtr odom_sub_;

src/planning/dwa_planner/src/dwa_planner.cpp

@@ -89,13 +89,8 @@ DWAPlanner::DWAPlanner(const rclcpp::NodeOptions & options) : Node("dwa_planner"
 
   // Declare timer for update function
   {
-    auto update_callback = [this]() { this->update(); };
-    auto period =
-      std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::duration<double>(dt_));
-    update_timer_ = std::make_shared<rclcpp::GenericTimer<decltype(update_callback)>>(
-      this->get_clock(), period, std::move(update_callback),
-      this->get_node_base_interface()->get_context());
-    this->get_node_timers_interface()->add_timer(update_timer_, nullptr);
+    update_timer_ =
+      FunctionTimer::create_function_timer(this, update_rate_hz, [this]() { this->update(); });
   }
 }