From 969b85e70bbe5fb0c66a15f04a73b3c552462556 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Tue, 5 Aug 2025 09:44:17 +0000 Subject: [PATCH 1/3] Initial plan From a3fd50fafa57f29cd9e3f3323cc4c691051a60b6 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Tue, 5 Aug 2025 09:51:14 +0000 Subject: [PATCH 2/3] Implement trajectory follower improvements to reduce lag at high speeds Co-authored-by: pumablattlaus <78273962+pumablattlaus@users.noreply.github.com> --- ...mir_trajectory_follower_pure_pursuit_RL.py | 45 ++++++++++++++++--- 1 file changed, 39 insertions(+), 6 deletions(-) diff --git a/mir_trajectory_follower/scripts/mir_trajectory_follower_pure_pursuit_RL.py b/mir_trajectory_follower/scripts/mir_trajectory_follower_pure_pursuit_RL.py index 78cc99b..77d840e 100755 --- a/mir_trajectory_follower/scripts/mir_trajectory_follower_pure_pursuit_RL.py +++ b/mir_trajectory_follower/scripts/mir_trajectory_follower_pure_pursuit_RL.py @@ -15,14 +15,18 @@ def __init__(self): # Config self.path = [] - self.lookahead_distance = rospy.get_param("~lookahead_distance", 0.1) + self.base_lookahead_distance = rospy.get_param("~lookahead_distance", 0.1) + self.lookahead_distance = self.base_lookahead_distance # Will be adaptive + self.max_lookahead_distance = rospy.get_param("~max_lookahead_distance", 0.5) # Maximum lookahead + self.lookahead_velocity_gain = rospy.get_param("~lookahead_velocity_gain", 0.1) # Velocity scaling factor self.lateral_distance_threshold = rospy.get_param("~lateral_distance_threshold", 0.2) self.tangent_distance_threshold = rospy.get_param("~tangent_distance_threshold", 0.02) self.search_range = rospy.get_param("~search_range", 5) # Number of points to search for lookahead point self.Kv = rospy.get_param("~Kv", 1.0) # Linear speed multiplier self.K_distance = rospy.get_param("~K_distance", 0.0) # Distance error multiplier self.K_orientation = rospy.get_param("~K_orientation", 0.5) # Orientation error multiplier - self.K_idx = rospy.get_param("~K_idx", 0.01) # Index error multiplier + self.K_idx = rospy.get_param("~K_idx", 0.05) # Index error multiplier (increased from 0.01) + self.K_feedforward = rospy.get_param("~K_feedforward", 0.2) # Feedforward compensation gain self.mir_path_topic = rospy.get_param("~mir_path_topic", "/mir_path_original") self.mir_pose_topic = rospy.get_param("~mir_pose_topic", "/mur620a/mir_pose_simple") self.cmd_vel_topic = rospy.get_param("~cmd_vel_topic", "/mur620a/mobile_base_controller/cmd_vel") @@ -142,6 +146,14 @@ def calculate_sub_step_progress(self): self.current_sub_step += 1 + def update_adaptive_lookahead(self, current_velocity): + """ + Update lookahead distance based on current velocity to improve tracking at high speeds + """ + # Adaptive lookahead: increase lookahead distance with velocity + velocity_based_lookahead = self.base_lookahead_distance + (current_velocity * self.lookahead_velocity_gain) + self.lookahead_distance = min(velocity_based_lookahead, self.max_lookahead_distance) + def find_lookahead_point(self): # Suche im Pfadausschnitt search_range = self.path[self.current_mir_path_index:self.current_mir_path_index + self.search_range] @@ -182,15 +194,36 @@ def apply_control(self): distance_error = self.calculate_distance(self.current_pose.position, self.path[self.current_mir_path_index].pose.position) orientation_error = self.calculate_orientation_error(self.current_pose, self.path[self.current_mir_path_index].pose) + # Get current path velocity for adaptive lookahead (with bounds checking) + current_path_velocity = (self.path_velocities_lin[self.current_mir_path_index] + if self.current_mir_path_index < len(self.path_velocities_lin) + else 0.0) + + # Update adaptive lookahead distance based on current velocity + self.update_adaptive_lookahead(current_path_velocity) + + # Enhanced feedforward compensation for high-speed tracking + # Predict where the robot should be based on velocity and lag + feedforward_compensation = self.K_feedforward * current_path_velocity * max(0, index_error) + + # Enhanced index error compensation with non-linear scaling for large errors + enhanced_index_compensation = self.K_idx * index_error + if abs(index_error) > 2: # Significant lag + enhanced_index_compensation *= (1.0 + 0.5 * abs(index_error)) # Non-linear scaling + # broadcast target point self.broadcast_target_point(self.path[self.current_mir_path_index].pose.position) - rospy.loginfo_throttle(1, f"Current index: {self.current_mir_path_index}, Target index: {self.ur_trajectory_index}, Index error: {index_error}, Distance error: {distance_error}, Orientation error: {orientation_error}") + rospy.loginfo_throttle(1, f"Current index: {self.current_mir_path_index}, Target index: {self.ur_trajectory_index}, Index error: {index_error}, Distance error: {distance_error}, Orientation error: {orientation_error}, Lookahead: {self.lookahead_distance:.3f}") + velocity = Twist() - target_vel = self.Kv * self.path_velocities_lin[self.current_mir_path_index] + self.K_distance * distance_error + self.K_idx * index_error + target_vel = (self.Kv * current_path_velocity + + self.K_distance * distance_error + + enhanced_index_compensation + + feedforward_compensation) - velocity.linear.x = max(0.0, target_vel ) * self.override # min 0.0 to avoid negative speeds + velocity.linear.x = max(0.0, target_vel) * self.override # min 0.0 to avoid negative speeds - velocity.angular.z = self.K_orientation * orientation_error + self.Kv * (self.path_velocities_ang[self.current_mir_path_index]) + velocity.angular.z = self.K_orientation * orientation_error + self.Kv * (self.path_velocities_ang[self.current_mir_path_index]) self.cmd_vel_pub.publish(velocity) From ea353abcd5ef932f4edde726f861b42b2d6f3830 Mon Sep 17 00:00:00 2001 From: "copilot-swe-agent[bot]" <198982749+Copilot@users.noreply.github.com> Date: Tue, 5 Aug 2025 09:52:16 +0000 Subject: [PATCH 3/3] Update documentation with new high-speed tracking parameters Co-authored-by: pumablattlaus <78273962+pumablattlaus@users.noreply.github.com> --- mir_trajectory_follower/README.md | 32 ++++++++++++++++++++++++------- 1 file changed, 25 insertions(+), 7 deletions(-) diff --git a/mir_trajectory_follower/README.md b/mir_trajectory_follower/README.md index ee7ced8..7ee5e7c 100644 --- a/mir_trajectory_follower/README.md +++ b/mir_trajectory_follower/README.md @@ -28,17 +28,35 @@ You can override the default parameters using the launch file or command line sw Other tuning parameters include lookahead distance, distance threshold, and control gains: -- `~lookahead_distance` (default: 0.25) -- `~distance_threshold` (default: 0.25) -- `~search_range` (default: 20) -- ```~Kv``` (default: 1.0) -- `~control_rate` (default: 100) -- `~dT` (default: 0.2) +### Basic Parameters +- `~lookahead_distance` (default: 0.1): Base lookahead distance for pure pursuit +- `~distance_threshold` (default: 0.02): Tangential distance threshold for waypoint completion +- `~search_range` (default: 5): Number of points to search for lookahead point +- `~Kv` (default: 1.0): Linear velocity multiplier +- `~control_rate` (default: 100): Control loop frequency in Hz +- `~dT` (default: 0.5): Time step for velocity calculations + +### Advanced High-Speed Tracking Parameters (RL Version) +- `~max_lookahead_distance` (default: 0.5): Maximum adaptive lookahead distance +- `~lookahead_velocity_gain` (default: 0.1): Scaling factor for velocity-based lookahead adaptation +- `~K_idx` (default: 0.05): Index error compensation gain (increased from 0.01 for better high-speed tracking) +- `~K_feedforward` (default: 0.2): Feedforward compensation gain for predictive control +- `~K_distance` (default: 0.0): Distance error multiplier +- `~K_orientation` (default: 0.5): Orientation error multiplier ## Nodes +### mir_trajectory_follower_pure_pursuit_RL (Reinforcement Learning Enhanced) +- **File:** [mir_trajectory_follower_pure_pursuit_RL.py](scripts/mir_trajectory_follower_pure_pursuit_RL.py) +- **Description:** Advanced trajectory follower with reinforcement learning enhancements and high-speed tracking improvements. Features adaptive lookahead distance, enhanced lag compensation, and feedforward control for better performance at high speeds. +- **Key Improvements:** + - **Adaptive Lookahead:** Dynamically adjusts lookahead distance based on current velocity (0.1m to 0.5m) + - **Enhanced Lag Compensation:** Non-linear index error compensation for significant tracking lags + - **Feedforward Control:** Predictive velocity compensation to anticipate trajectory requirements + - **High-Speed Optimization:** Reduces lag between mir_target_pose and actual robot pose at high speeds + ### mir_trajectory_follower_pure_pursuit - **File:** [mir_trajectory_follower_pure_pursuit.py](scripts/mir_trajectory_follower_pure_pursuit.py) -- **Description:** Follows a trajectory using the Pure Pursuit algorithm. It processes incoming path data, identifies a lookahead point, computes curvature, and publishes velocity commands. +- **Description:** Standard pure pursuit implementation. Follows a trajectory using the Pure Pursuit algorithm. It processes incoming path data, identifies a lookahead point, computes curvature, and publishes velocity commands. - **TF Broadcasting:** Publishes transforms for the current pose, target (lookahead) point, and current position for debugging. ### mir_trajectory_follower (Custom Path Follower)