diff --git a/tinynav/core/imu_propagator_node.py b/tinynav/core/imu_propagator_node.py index a36002f4..b8d1a4f8 100644 --- a/tinynav/core/imu_propagator_node.py +++ b/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 diff --git a/tinynav/core/planning_node.py b/tinynav/core/planning_node.py index 5e5b1910..4052b931 100644 --- a/tinynav/core/planning_node.py +++ b/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 diff --git a/tinynav/platforms/cmd_vel_control.py b/tinynav/platforms/cmd_vel_control.py index 3afa4547..14f84d4b 100644 --- a/tinynav/platforms/cmd_vel_control.py +++ b/tinynav/platforms/cmd_vel_control.py @@ -3,21 +3,49 @@ from geometry_msgs.msg import Twist from nav_msgs.msg import Path from nav_msgs.msg import Odometry -from std_msgs.msg import Bool -from rclpy.qos import DurabilityPolicy, QoSProfile from scipy.spatial.transform import Rotation as R +from scipy.spatial.transform import Slerp +from scipy.interpolate import CubicSpline import numpy as np import logging import time +from tinynav.core.math_utils import np2msg # Module-level logger for cases where self.get_logger() is not available logger = logging.getLogger(__name__) + +class PIDController: + def __init__(self, kp: float, ki: float, kd: float, integral_limit: float = 1.0): + self.kp = float(kp) + self.ki = float(ki) + self.kd = float(kd) + self.integral_limit = float(abs(integral_limit)) + self.integral = 0.0 + self.prev_error = 0.0 + self.initialized = False + + def reset(self): + self.integral = 0.0 + self.prev_error = 0.0 + self.initialized = False + + def update(self, error: float, dt: float) -> float: + dt = max(1e-3, float(dt)) + self.integral += float(error) * dt + self.integral = float(np.clip(self.integral, -self.integral_limit, self.integral_limit)) + derivative = 0.0 if not self.initialized else (float(error) - self.prev_error) / dt + self.prev_error = float(error) + self.initialized = True + return self.kp * float(error) + self.ki * self.integral + self.kd * derivative + + class CmdVelControlNode(Node): def __init__(self): super().__init__('cmd_vel_control_node') self.logger = self.get_logger() # Use ROS2 logger self.cmd_pub = self.create_publisher(Twist, '/cmd_vel', 10) + self.target_pose_pub = self.create_publisher(Odometry, '/control/target_pose_pid', 10) self.pose_sub = self.create_subscription(Odometry, '/slam/odometry', self.pose_callback, 10) self.create_subscription(Path, '/planning/trajectory_path', self.path_callback, 10) self.T_robot_to_camera = np.array([ @@ -29,70 +57,216 @@ def __init__(self): self.last_path_time = 0.0 self.pose = None self.path = None + self.path_start_time = None + self.path_duration = 0.0 + self.path_pos_spline = None + self.path_ori_slerp = None + + # PID for tracking path against /slam/odometry. + self.pos_pid = PIDController(kp=1.2, ki=0.05, kd=0.08, integral_limit=1.5) + self.yaw_pid = PIDController(kp=1.8, ki=0.03, kd=0.10, integral_limit=1.0) + # Cross-track coupling: map lateral error (robot-frame y) into yaw-rate correction. + self.cross_track_gain = 1.2 # === Control loop (ported from planning_node_compare style) === - # Planner input is typically 7-10 Hz; over-driving cmd publish rate amplifies jitter. - self.cmd_rate_hz = 12.0 - # Use minima; actual stale thresholds are scaled by observed planner period. - self.path_stale_slow_s = 0.35 - self.path_stale_stop_s = 0.8 - self.path_stale_slow_factor = 3.5 - self.path_stale_stop_factor = 5.0 - self.max_linear_acc = 0.6 # m/s^2 + self.cmd_rate_hz = 20.0 + self.path_stale_slow_s = 0.3 + self.path_stale_stop_s = 0.6 + self.max_linear_acc = 0.4 # m/s^2 self.max_angular_acc = 0.8 # rad/s^2 - self.planner_dt = 0.1 # trajectory dt in planning_node - # planning_node publishes path with for j in range(..., step=10), so points are ~1.0 s apart. - self.path_pose_stride = 10 - self.path_period_ema = 0.12 - self.path_filter_tau = 0.30 - self.lookahead_steps = 1 - # Static-friction compensation: very small vx often cannot move the robot. - self.min_effective_linear_speed = 0.2 - self.linear_engage_threshold = 0.04 - self.fixed_reverse_speed = 0.2 self.latest_cmd = Twist() self.prev_cmd = Twist() - self.last_cmd_pub_time = time.monotonic() + self.last_odom_time = None self.last_path_update_time = None - self._paused = False - _latched_qos = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) - self.create_subscription(Bool, '/nav/paused', self._on_paused, _latched_qos) - self.cmd_timer = self.create_timer(1.0 / self.cmd_rate_hz, self.cmd_timer_callback) - def _on_paused(self, msg: Bool): - self._paused = msg.data - if not self._paused: - # Reset prev_cmd so resume starts from zero cleanly - self.prev_cmd = Twist() - def pose_callback(self, msg): self.pose = msg + now = time.monotonic() + if self.last_odom_time is None: + dt = 1.0 / self.cmd_rate_hz + else: + dt = max(1e-3, now - self.last_odom_time) + self.last_odom_time = now + + self._publish_pid_target_pose(msg.header.stamp) + self._publish_cmd_on_odom(now, dt) + + def _publish_pid_target_pose(self, stamp): + if self.path_start_time is None or self.path_duration <= 0.0: + return + now_mono = time.monotonic() + t_ref = now_mono - self.path_start_time + ref_pos, ref_quat = self._sample_pose_at(t_ref) + if ref_pos is None: + return + T_robot_ref = self._world_from_pose_quat(ref_pos, ref_quat) + target_msg = np2msg(T_robot_ref, stamp, "world", "camera_target") + self.target_pose_pub.publish(target_msg) def _clamp_step(self, target: float, current: float, max_delta: float) -> float: return float(np.clip(target - current, -max_delta, max_delta) + current) - def cmd_timer_callback(self): - now = time.monotonic() - dt = max(1e-3, now - self.last_cmd_pub_time) - self.last_cmd_pub_time = now + def _wrap_to_pi(self, angle: float) -> float: + return float((angle + np.pi) % (2.0 * np.pi) - np.pi) - if self._paused: - self.cmd_pub.publish(Twist()) - self.prev_cmd = Twist() - return + def _yaw_from_transform(self, T: np.ndarray) -> float: + fwd = T[:3, :3] @ np.array([1.0, 0.0, 0.0], dtype=np.float64) + return float(np.arctan2(fwd[1], fwd[0])) + + def _world_from_pose_quat(self, position: np.ndarray, quat: np.ndarray) -> np.ndarray: + T_world_cam = np.eye(4) + T_world_cam[:3, :3] = R.from_quat(quat).as_matrix() + T_world_cam[:3, 3] = np.asarray(position, dtype=np.float64) + return T_world_cam + + def _path_pose_camera_to_body(self, position: np.ndarray, quat: np.ndarray): + """Convert path pose from world-camera to world-body by right multiply.""" + T_world_cam = self._world_from_pose_quat(position, quat) + T_world_body = T_world_cam @ self.T_robot_to_camera + pos_body = T_world_body[:3, 3].copy() + quat_body = R.from_matrix(T_world_body[:3, :3]).as_quat() + return pos_body, quat_body + + def _current_world_robot_from_odom(self): + if self.pose is None: + return None + p = self.pose.pose.pose.position + q = self.pose.pose.pose.orientation + pos = np.array([p.x, p.y, p.z], dtype=np.float64) + quat = np.array([q.x, q.y, q.z, q.w], dtype=np.float64) + return self._world_from_pose_quat(pos, quat) @ self.T_robot_to_camera + + def _build_path_interpolators(self, path_msg: Path): + poses = path_msg.poses + count = len(poses) + if count < 2: + return False + + stamp_times = np.array( + [ + float(pose_stamped.header.stamp.sec) + + float(pose_stamped.header.stamp.nanosec) * 1e-9 + for pose_stamped in poses + ], + dtype=np.float64, + ) + if not np.all(np.diff(stamp_times) > 0): + self.logger.warning("Path timestamps are invalid (non-monotonic); skip this path update.") + return False + times = stamp_times - stamp_times[0] + + positions = np.empty((count, 3), dtype=np.float64) + quats = np.empty((count, 4), dtype=np.float64) + for i, pose_stamped in enumerate(poses): + p = pose_stamped.pose.position + q = pose_stamped.pose.orientation + pos_cam = np.array([p.x, p.y, p.z], dtype=np.float64) + quat_cam = np.array([q.x, q.y, q.z, q.w], dtype=np.float64) + pos_body, quat_body = self._path_pose_camera_to_body(pos_cam, quat_cam) + positions[i] = pos_body + quats[i] = quat_body + + try: + rotations = R.from_quat(quats) + self.path_pos_spline = CubicSpline(times, positions, axis=0, bc_type='natural') + self.path_ori_slerp = Slerp(times, rotations) + self.path_duration = float(times[-1]) + self.path_start_time = time.monotonic() + self.pos_pid.reset() + self.yaw_pid.reset() + return True + except ValueError as exc: + self.logger.warning(f"Failed to build path interpolators: {exc}") + self.path_pos_spline = None + self.path_ori_slerp = None + self.path_duration = 0.0 + self.path_start_time = None + return False + + def _sample_pose_at(self, t: float): + if self.path_pos_spline is None or self.path_ori_slerp is None: + return None, None + t_clamped = float(np.clip(t, 0.0, self.path_duration)) + position = self.path_pos_spline(t_clamped) + quat = self.path_ori_slerp([t_clamped]).as_quat()[0] + return position, quat + + def _compute_cmd_from_sampled_path(self, now_mono: float): + if self.path_start_time is None or self.path_duration <= 0.0: + return None, None + + t0 = now_mono - self.path_start_time + t1 = t0 + (1.0 / self.cmd_rate_hz) + + p0, q0 = self._sample_pose_at(t0) + p1, q1 = self._sample_pose_at(t1) + if p0 is None or p1 is None: + return None, None + + T1 = np.eye(4) + T1[:3, :3] = R.from_quat(q0).as_matrix() + T1[:3, 3] = p0 + + T2 = np.eye(4) + T2[:3, :3] = R.from_quat(q1).as_matrix() + T2[:3, 3] = p1 + + T_robot_2_to_1 = np.linalg.inv(T1) @ T2 + + dt = max(1e-3, t1 - t0) + linear_velocity_vec = T_robot_2_to_1[:3, 3] / dt + angular_velocity_vec = R.from_matrix(T_robot_2_to_1[:3, :3]).as_rotvec() / dt + return linear_velocity_vec, angular_velocity_vec + + def _compute_tracking_pid_cmd(self, now_mono: float, dt: float): + if self.path_start_time is None or self.path_duration <= 0.0: + return None, None + T_robot_now = self._current_world_robot_from_odom() + if T_robot_now is None: + return None, None + + t_ref = now_mono - self.path_start_time + ref_pos, ref_quat = self._sample_pose_at(t_ref) + if ref_pos is None: + return None, None + T_robot_ref = self._world_from_pose_quat(ref_pos, ref_quat) + + # Position error in current robot frame; control forward axis only. + T_ref_in_robot = np.linalg.inv(T_robot_now) @ T_robot_ref + pos_err_forward = float(T_ref_in_robot[0, 3]) + pos_err_lateral = float(T_ref_in_robot[1, 3]) + + yaw_now = self._yaw_from_transform(T_robot_now) + yaw_ref = self._yaw_from_transform(T_robot_ref) + yaw_err = self._wrap_to_pi(yaw_ref - yaw_now) + + vx_correction = self.pos_pid.update(pos_err_forward, dt) + wz_correction = self.yaw_pid.update(yaw_err, dt) + self.cross_track_gain * pos_err_lateral + return vx_correction, wz_correction + + def _publish_cmd_on_odom(self, now: float, dt: float): + pid_vx = 0.0 + pid_wz = 0.0 + pid_cmd = self._compute_tracking_pid_cmd(now, dt) + if pid_cmd[0] is not None: + pid_vx, pid_wz = pid_cmd + + vx = np.clip(pid_vx, -0.1, 0.3) + vyaw = np.clip(pid_wz, -0.8, 0.8) + self.latest_cmd.linear.x = float(vx) + self.latest_cmd.linear.y = 0.0 + self.latest_cmd.angular.z = float(vyaw) # Stale-path protection: slow down, then stop if planner has not refreshed. age = float('inf') if self.last_path_update_time is None else (now - self.last_path_update_time) - stale_slow_s = max(self.path_stale_slow_s, self.path_period_ema * self.path_stale_slow_factor) - stale_stop_s = max(self.path_stale_stop_s, self.path_period_ema * self.path_stale_stop_factor) target_cmd = Twist() target_cmd.linear.x = self.latest_cmd.linear.x target_cmd.angular.z = self.latest_cmd.angular.z - if age > stale_stop_s: + if age > self.path_stale_stop_s: target_cmd.linear.x = 0.0 target_cmd.angular.z = 0.0 - elif age > stale_slow_s: + elif age > self.path_stale_slow_s: target_cmd.linear.x *= 0.3 target_cmd.angular.z *= 0.5 @@ -103,70 +277,23 @@ def cmd_timer_callback(self): out.linear.x = self._clamp_step(target_cmd.linear.x, self.prev_cmd.linear.x, max_dv) out.angular.z = self._clamp_step(target_cmd.angular.z, self.prev_cmd.angular.z, max_dw) out.linear.y = 0.0 - # Dead-band: < 0.05 → 0; small positive → snap to min effective speed. - if abs(out.linear.x) < 0.05: - out.linear.x = 0.0 - elif 0 < out.linear.x < self.min_effective_linear_speed: - out.linear.x = self.min_effective_linear_speed - if abs(out.angular.z) < 0.05: - out.angular.z = 0.0 self.cmd_pub.publish(out) self.prev_cmd = out def path_callback(self, msg): - if msg is None or self.pose is None: + if msg is None: return if len(msg.poses) < 2: return - self.path = msg - ros_now = self.get_clock().now().to_msg() - self.last_path_time = ros_now.sec + ros_now.nanosec * 1e-9 - now_mono = time.monotonic() - if self.last_path_update_time is not None: - period = np.clip(now_mono - self.last_path_update_time, 0.05, 0.5) - self.path_period_ema = 0.85 * self.path_period_ema + 0.15 * float(period) - self.last_path_update_time = now_mono - - def msg2np(msg): - T = np.eye(4) - position = msg.pose.position - rot = msg.pose.orientation - quat = [rot.x, rot.y, rot.z, rot.w] - T[:3, :3] = R.from_quat(quat).as_matrix() - T[:3, 3] = np.array([position.x, position.y, position.z]).ravel() - return T - - T1 = msg2np(self.path.poses[0]) - step_idx = int(min(self.lookahead_steps, len(self.path.poses) - 1)) - T2 = msg2np(self.path.poses[step_idx]) - T_robot_1 = T1 @ self.T_robot_to_camera - T_robot_2 = T2 @ self.T_robot_to_camera - T_robot_2_to_1 = np.linalg.inv(T_robot_1) @ T_robot_2 - p = T_robot_2_to_1[:3, 3] - # dt must match actual spacing between published Path poses, not raw trajectory dt. - dt = self.planner_dt * self.path_pose_stride * max(1, step_idx) - linear_velocity_vec = p / dt - r = R.from_matrix(T_robot_2_to_1[:3, :3]) - angular_velocity_vec = r.as_rotvec() / dt - - vx = np.clip(linear_velocity_vec[0], -0.1, 0.5) - if vx < 0.0: - vx = -self.fixed_reverse_speed - vy = 0.0 - vyaw = np.clip(angular_velocity_vec[2], -0.8, 0.8) - - # Filter planner updates to reduce visible jitter from 7-10 Hz updates. - alpha = np.clip(self.path_period_ema / (self.path_filter_tau + self.path_period_ema), 0.15, 0.75) - self.latest_cmd.linear.x = float((1.0 - alpha) * self.latest_cmd.linear.x + alpha * vx) - self.latest_cmd.linear.y = float(vy) - self.latest_cmd.angular.z = float((1.0 - alpha) * self.latest_cmd.angular.z + alpha * vyaw) + # Save the latest path first. + self.path = msg + self.last_path_update_time = time.monotonic() + self.last_path_time = self.get_clock().now().nanoseconds * 1e-9 + self._build_path_interpolators(msg) age = 0.0 if self.last_path_update_time is None else (time.monotonic() - self.last_path_update_time) - self.logger.debug( - f"cmd vx={self.latest_cmd.linear.x:.3f} vyaw={self.latest_cmd.angular.z:.3f} " - f"path_age={age:.2f}s path_dt_ema={self.path_period_ema:.2f}s lookahead={step_idx}" - ) + self.logger.debug(f"path updated age={age:.2f}s points={len(msg.poses)}") def destroy_node(self): self.logger.info("Destroying cmd_vel_control connection.") diff --git a/tool/poi_editor.py b/tool/poi_editor.py index 3707fff4..fe121547 100755 --- a/tool/poi_editor.py +++ b/tool/poi_editor.py @@ -20,6 +20,7 @@ import rclpy import os from math_utils import msg2np, matrix_to_quat +from std_msgs.msg import String class SplatFile(TypedDict): centers: npt.NDArray[np.floating] @@ -187,6 +188,7 @@ class RelocalizationPose(Node): def __init__(self, viser_server: viser.ViserServer): super().__init__('relocalization_pose') self.viser_server = viser_server + self.cmd_pois_pub = self.create_publisher(String, "/mapping/cmd_pois", 10) self.relocalization_pose_sub = self.create_subscription(Odometry, '/map/relocalization', self.relocalization_pose_callback, 10) self.global_plan_sub = self.create_subscription(nav_msgs.msg.Path, '/mapping/global_plan', self.global_plan_callback, 10) self.planning_path_sub = self.create_subscription(nav_msgs.msg.Path, '/planning/trajectory_path', self.planning_path_callback, 10) @@ -196,6 +198,12 @@ def __init__(self, viser_server: viser.ViserServer): Odometry, "/mapping/current_pose_in_map", self.current_pose_in_map_callback, 10 ) + def publish_pois_to_map_node(self, payload: dict) -> None: + msg = String() + msg.data = json.dumps(payload, separators=(",", ":")) + self.cmd_pois_pub.publish(msg) + self.get_logger().info(f"Published {len(payload)} POIs to /mapping/cmd_pois") + def relocalization_pose_callback(self, msg: Odometry): position = np.array([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z]) self.viser_server.scene.add_icosphere( @@ -280,6 +288,7 @@ def main( server = viser.ViserServer() server.scene.world_axes.visible = True server.scene.set_up_direction("+z") + ros_bridge: dict[str, RelocalizationPose | None] = {"node": None} # POI management poi_points = {} @@ -298,12 +307,28 @@ def main( with server.gui.add_folder("Points of Interest (POI)") as _: add_poi_button = server.gui.add_button("Add POI Point") add_save_poi_button = server.gui.add_button("Save POI") + add_send_poi_button = server.gui.add_button("Send POI to map_node") @add_save_poi_button.on_click def _(_) -> None: with open(f"{tinynav_map_path}/pois.json", "w") as f: json.dump(poi_points, f, indent=2, default=lambda x: x.tolist() if isinstance(x, np.ndarray) else x) + @add_send_poi_button.on_click + def _(_) -> None: + node = ros_bridge["node"] + if node is None: + print("ROS node is not ready yet. Cannot send POIs.") + return + payload = {} + for key, poi in poi_points.items(): + payload[str(int(key))] = { + "id": int(poi.get("id", key)), + "name": str(poi.get("name", f"POI_{key}")), + "position": [float(x) for x in poi["position"]], + } + node.publish_pois_to_map_node(payload) + poi_list_container = server.gui.add_folder("POI List") for poi_id, poi_point in poi_points.items(): @@ -667,6 +692,7 @@ def _(_, pc_handle=pc_handle, remove_button=remove_button) -> None: rclpy.init() relocalization_pose_node = RelocalizationPose(server) + ros_bridge["node"] = relocalization_pose_node try: rclpy.spin(relocalization_pose_node) relocalization_pose_node.destroy_node()