import numpy as np

class AckermannKinematics:
    """阿克曼运动学模型 (bicycle model)"""

    def __init__(self, wheelbase: float, max_steer: float = 0.5):
        self.L = wheelbase
        self.max_steer = max_steer

    def inverse_kinematics(
        self,
        v: float,
        omega: float,
        dt: float = 0.01
    ) -> dict:
        """逆运动学: 期望速度 → 电机/舵机指令"""
        if abs(omega) < 1e-6:
            return {"steer_angle": 0.0, "linear_vel": v}

        kappa = omega / v
        steer_angle = np.arctan(kappa * self.L)
        steer_angle = np.clip(steer_angle, -self.max_steer, self.max_steer)

        return {
            "steer_angle": float(steer_angle),
            "linear_vel": float(v),
            "curvature": float(kappa),
        }

    def forward_kinematics(self, v: float, steer_angle: float) -> dict:
        """正运动学: 电机/舵机指令 → 机器人速度"""
        kappa = np.tan(steer_angle) / self.L
        omega = kappa * v
        return {"vx": v, "omega": omega}

    def compute_wheel_angles(self, steer_angle: float, W: float) -> tuple:
        """计算左右轮独立转向角 (cot(ai) - cot(ao) = L/W)"""
        tan_s = np.tan(steer_angle)
        tan_ai = (W + self.L * tan_s) / (self.L - W * tan_s)
        return float(np.arctan(tan_s)), float(np.arctan(tan_ai))

# 使用示例
kin = AckermannKinematics(wheelbase=0.5, max_steer=0.6)
cmd = kin.inverse_kinematics(v=0.5, omega=0.3)
print(f"舵机角度: {np.degrees(cmd['steer_angle']):.1f}°")

controller_manager:
  ros__parameters:
    update_rate: 100  # Hz
    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster
    ackermann_controller:
      type: diff_drive_controller/DiffDriveController

ackermann_controller:
  ros__parameters:
    wheelbase: 0.5           # 轴距 m
    wheel_radius: 0.1        # 轮子半径 m
    max_steering_angle: 0.6  # 最大转向角 rad
    linear:
      max: 2.0
      min: -0.5
    angular:
      max: 2.0
    odom_frame: odom
    base_frame_id: base_link
    enable_odom_tf: true
    velocity_rolling_window_size: 10

import numpy as np
import heapq
from typing import Optional

class ImprovedAStar:
    """改进A*算法：密度加权启发+斜向惩罚+路径平滑"""

    DIRS_5 = [
        (0, 1, 1.0),    # 上
        (0, -1, 1.0),   # 下
        (1, 0, 1.0),    # 右
        (-1, 0, 1.0),   # 左
        (1, 1, 1.414),  # 右上
        (1, -1, 1.414), # 右下
        (-1, 1, 1.414), # 左上
        (-1, -1, 1.414) # 左下
    ]

    def __init__(self, grid: np.ndarray, resolution: float = 0.1):
        self.grid = grid
        self.resolution = resolution
        self.rows, self.cols = grid.shape
        self.obstacle_density = self._compute_density()

    def _compute_density(self) -> np.ndarray:
        """计算障碍物密度图 (3x3窗口平滑)"""
        from scipy.ndimage import uniform_filter
        return uniform_filter(
            (self.grid > 0.5).astype(float),
            size=3,
            mode='constant'
        )

    def heuristic(self, node: tuple, goal: tuple, alpha: float = 0.3) -> float:
        """改进启发函数: 欧氏距离 × (1 + α × 障碍密度)"""
        dist = np.sqrt((node[0] - goal[0])**2 + (node[1] - goal[1])**2)
        density = self.obstacle_density[
            min(node[0], self.rows-1), min(node[1], self.cols-1)
        ]
        return dist * (1.0 + alpha * density)

    def search(self, start: tuple, goal: tuple, alpha: float = 0.3) -> Optional[list]:
        """A*搜索，返回路径点列表"""
        open_set = []
        heapq.heappush(open_set, (0.0, start))
        came_from = {start: None}
        g_score = {start: 0.0}
        visited = set()

        while open_set:
            _, current = heapq.heappop(open_set)
            if current in visited:
                continue
            visited.add(current)
            if current == goal:
                return self._reconstruct_path(came_from, current)

            for dr, dc, cost in self.DIRS_5:
                nr, nc = current[0] + dr, current[1] + dc
                if not (0 <= nr < self.rows and 0 <= nc < self.cols):
                    continue
                if self.grid[nr, nc] > 0.5:
                    continue
                if (nr, nc) in visited:
                    continue

                # 斜向移动遇相邻障碍则加大cost
                move_cost = cost
                if abs(dr) == 1 and abs(dc) == 1:
                    if self.grid[current[0]+dr, current[1]] > 0.5 or \
                       self.grid[current[0], current[1]+dc] > 0.5:
                        move_cost *= 2.5

                tentative_g = g_score[current] + move_cost
                if (nr, nc) not in g_score or tentative_g < g_score[(nr, nc)]:
                    came_from[(nr, nc)] = current
                    g_score[(nr, nc)] = tentative_g
                    f = tentative_g + self.heuristic((nr, nc), goal, alpha)
                    heapq.heappush(open_set, (f, (nr, nc)))
        return None

    def _reconstruct_path(self, came_from: dict, current: tuple) -> list:
        path = [current]
        while current in came_from and came_from[current] is not None:
            current = came_from[current]
            path.append(current)
        path.reverse()
        return path

    def smooth_path(self, path: list) -> list:
        """路径平滑: 移除共线点"""
        if len(path) <= 2:
            return path
        smoothed = [path[0]]
        for i in range(1, len(path) - 1):
            d1 = (path[i][0] - path[i-1][0], path[i][1] - path[i-1][1])
            d2 = (path[i+1][0] - path[i][0], path[i+1][1] - path[i][1])
            if d1 != d2:
                smoothed.append(path[i])
        smoothed.append(path[-1])
        return smoothed

import numpy as np

class ImprovedAPF:
    """改进人工势场法：分段斥力+目标可达修复+速度平滑"""

    def __init__(
        self,
        k_att: float = 5.0,
        k_rep_max: float = 15.0,
        d0: float = 0.5,
        d_threshold: float = 2.0
    ):
        self.k_att = k_att
        self.k_rep_max = k_rep_max
        self.d0 = d0
        self.d_threshold = d_threshold
        self.prev_force = None

    def attractive_force(self, pos: np.ndarray, goal: np.ndarray) -> np.ndarray:
        """引力: F_att = k_att × (goal - pos)"""
        return self.k_att * (goal - pos)

    def repulsive_force(self, pos: np.ndarray, obs: np.ndarray) -> np.ndarray:
        """分段改进斥力：d>d_threshold时为0，d0<d<=d_threshold平滑衰减"""
        diff = pos - obs
        d = np.linalg.norm(diff) + 1e-8
        if d > self.d_threshold:
            return np.zeros(2)
        if d <= self.d0:
            factor = self.k_rep_max * (1.0/d - 1.0/self.d0)**2
        else:
            d_ratio = (self.d_threshold - d) / (self.d_threshold - self.d0)
            factor = self.k_rep_max * d_ratio**2 * (1.0/d - 1.0/self.d0)
        return factor * diff / d

    def compute_force(
        self,
        pos: np.ndarray,
        goal: np.ndarray,
        obstacles: list,
        prev_force: np.ndarray = None,
        smooth_alpha: float = 0.3
    ) -> tuple:
        """合力计算（含速度平滑）"""
        F_att = self.attractive_force(pos, goal)
        F_rep = np.zeros(2)
        for obs in obstacles:
            F_rep += self.repulsive_force(pos, np.array(obs))
        F_total = F_att + F_rep
        if prev_force is not None and smooth_alpha > 0:
            F_total = (1 - smooth_alpha) * F_total + smooth_alpha * prev_force
        return F_total, np.linalg.norm(F_total)

    def navigation_step(
        self,
        pos: np.ndarray,
        goal: np.ndarray,
        obstacles: list,
        dt: float = 0.1,
        max_speed: float = 0.5
    ) -> np.ndarray:
        """导航一步，返回更新后的位置"""
        F, mag = self.compute_force(pos, goal, obstacles, self.prev_force)
        if mag > 1e-6:
            direction = F / mag
            velocity = min(max_speed, mag * 0.1)
            new_pos = pos + direction * velocity * dt
        else:
            new_pos = pos.copy()
        self.prev_force = F
        return new_pos

amcl:
  ros__parameters:
    use_sim_time: False
    alpha1: 0.2
    alpha2: 0.2
    alpha3: 0.2
    alpha4: 0.2
    alpha5: 0.2
    base_frame_id: base_link
    beam_skip_distance: 0.5

bt_navigator:
  ros__parameters:
    use_sim_time: True
    global_frame_id: map
    robot_base_frame: base_link
    default_nav_to_pose_bt_xml: $(find-pkg-share nav2_bt_navigator)/behavior_trees/navigate_to_pose_via_points.xml
    plugin_lib_names:
      - nav2_compute_path_to_pose_action_bt_node
      - nav2_follow_path_action_bt_node
      - nav2_rate_controller_bt_node
      - nav2_recovery_node_bt_node
      - nav2_back_up_action_bt_node
      - nav2_spin_action_bt_node
      - nav2_wait_action_bt_node

controller_server:
  ros__parameters:
    use_sim_time: True
    controller_frequency: 20.0
    FollowPath:
      plugin: dwb_plugins::FollowPedestrianPath
    min_vel_x: -0.5
    max_vel_x: 0.5
    max_vel_theta: 1.0
    cost_weight: 1.0
    heading_weight: 0.5

planner_server:
  ros__parameters:
    use_sim_time: True
    planner_plugin_types: ["nav2_navfn_planner/NavfnPlanner"]
    planner_plugin_names: ["grid_based"]
    use_astar: true
    allow_unknown: true
    tolerance: 0.25