feat(control): track planning path with spline+slerp and PID

tinynav/core/imu_propagator_node.py

@@ -2,7 +2,6 @@
 
 import numpy as np
 import rclpy
-from message_filters import ApproximateTimeSynchronizer, Subscriber
 from nav_msgs.msg import Odometry
 from rclpy.node import Node
 from rclpy.qos import QoSProfile, ReliabilityPolicy
@@ -66,12 +65,12 @@ def imu_callback(self, imu_msg: Imu):
             return
 
         timestamp = self._stamp_to_sec(imu_msg.header.stamp)
-        print("imu: ", timestamp)
+        #print("imu: ", timestamp)
         self.imu_buffer.append((timestamp, imu_msg))
         if len(self.imu_buffer) > 2000:
             self.imu_buffer.pop(0)
 
-        if self.imu_buffer[-1][0] <= self.odom_100hz_buffer[-1][0] + 0.050:
+        if self.imu_buffer[-1][0] <= self.odom_100hz_buffer[-1][0] + 0.010:
             return
 
         start_idx = None

tinynav/core/planning_node.py

@@ -9,6 +9,7 @@
 from numba import njit
 import message_filters
 from rclpy.time import Time
+from rclpy.duration import Duration
 from sensor_msgs.msg import PointCloud2, PointCloud
 from geometry_msgs.msg import PoseStamped, Point32
 import sensor_msgs_py.point_cloud2 as pc2
@@ -591,8 +592,10 @@ def cost_function(traj, param, score, target_pose):
             path = Path()
             path.header = depth_msg.header
             path.header.frame_id = "world"
+            traj_dt = 0.1
+            base_time = Time.from_msg(depth_msg.header.stamp)
             for i in top_indices:
-                for j in range(0, len(trajectories[i]), 10):
+                for j in range(0, len(trajectories[i]), 1):
                     x,y,z,qx,qy,qz,qw = trajectories[i][j]
                     if self.poi_changed or self.target_pose is None:
                         x,y,z,qx,qy,qz,qw = trajectories[i][0]
@@ -602,7 +605,8 @@ def cost_function(traj, param, score, target_pose):
                             self.get_logger().info("target pose is None, using first point")
 
                     pose = PoseStamped()
-                    pose.header = depth_msg.header
+                    pose.header.frame_id = "world"
+                    pose.header.stamp = (base_time + Duration(seconds=float(j * traj_dt))).to_msg()
                     pose.pose.position.x = x
                     pose.pose.position.y = y
                     pose.pose.position.z = z