#!/usr/bin/env python3 import rclpy from rclpy.node import Node from geometry_msgs.msg import Twist, PoseStamped, TransformStamped from nav_msgs.msg import Path from visualization_msgs.msg import Marker from tf2_ros import TransformBroadcaster import math class TShapeController(Node): """T字形轨迹控制器 - PID角度控制 + 智能动态距离计算版本""" def __init__(self): super().__init__('t_shape_controller') # 声明ROS参数 - 可通过Launch文件或命令行设置 self.declare_parameter('t_width', 3.0) # T字横杠宽度（米） self.declare_parameter('t_height', 2.0) # T字竖杠高度（米） self.declare_parameter('linear_speed', 0.3) # 直线速度（米/秒） self.declare_parameter('angular_speed', 0.5) # 最大角速度（弧度/秒） self.declare_parameter('loop_enabled', True) # 是否循环运动 # PID参数声明（角度控制） self.declare_parameter('pid_kp', 1.5) # 比例系数 self.declare_parameter('pid_ki', 0.01) # 积分系数 self.declare_parameter('pid_kd', 0.15) # 微分系数 self.declare_parameter('angle_tolerance', 0.01) # 角度容差（弧度，约0.57度） # 线性运动角度校正参数 self.declare_parameter('linear_angle_tolerance', 0.05) # 线性运动角度容差（弧度，约2.9度） self.declare_parameter('angle_correction_speed', 0.3) # 角度校正速度（弧度/秒） # 获取参数值 self.t_width = self.get_parameter('t_width').value self.t_height = self.get_parameter('t_height').value self.linear_speed = self.get_parameter('linear_speed').value self.angular_speed = self.get_parameter('angular_speed').value self.loop_enabled = self.get_parameter('loop_enabled').value # 获取PID参数 self.pid_kp = self.get_parameter('pid_kp').value self.pid_ki = self.get_parameter('pid_ki').value self.pid_kd = self.get_parameter('pid_kd').value self.angle_tolerance = self.get_parameter('angle_tolerance').value # 获取线性运动角度校正参数 self.linear_angle_tolerance = self.get_parameter('linear_angle_tolerance').value self.angle_correction_speed = self.get_parameter('angle_correction_speed').value # 创建发布器 self.cmd_vel_pub = self.create_publisher(Twist, '/cmd_vel', 10) # 速度指令 self.path_pub = self.create_publisher(Path, '/robot_path', 10) # 运动路径 self.marker_pub = self.create_publisher(Marker, '/t_shape_marker', 10) # 参考轨迹 # TF广播器 - 发布odom->base_link变换 self.tf_broadcaster = TransformBroadcaster(self) # 机器人状态变量 self.x = 0.0 # X坐标（米） self.y = 0.0 # Y坐标（米） self.theta = 0.0 # 航向角（弧度） self.current_segment = 0 # 当前运动段（0-9，共10个阶段） self.segment_start_time = None # 当前段开始时间 # 关键点坐标记录（智能动态距离计算） self.t_center_x = None # T字中心点X坐标 self.t_center_y = None # T字中心点Y坐标 self.start_x = None # 起点X坐标 self.start_y = None # 起点Y坐标 self.right_end_x = None # 右端点X坐标 self.right_end_y = None # 右端点Y坐标 # 每个阶段起点坐标（用于距离判断） self.segment_start_x = 0.0 self.segment_start_y = 0.0 # PID角度控制状态变量 self.target_angle = 0.0 # 目标角度（弧度） self.error_sum = 0.0 # 误差积分累积 self.last_error = 0.0 # 上一次误差（用于微分） self.pid_active = False # PID是否激活 # 线性运动角度校正状态 self.linear_target_angle = 0.0 # 线性运动目标角度 self.angle_correction_active = False # 角度校正是否激活 # 路径记录消息 self.path_msg = Path() self.path_msg.header.frame_id = 'odom' # 创建定时器：控制循环（50Hz） self.control_timer = self.create_timer(0.02, self.control_loop) # 创建定时器：发布路径和TF（10Hz） self.publish_timer = self.create_timer(0.1, self.publish_status) # 发布T字形轨迹参考标记 self.publish_t_shape_marker() self.get_logger().info('T字形控制器已启动（PID角度控制 + 智能距离计算 + 线性角度校正）') self.get_logger().info(f'轨迹参数: 宽度={self.t_width}m, 高度={self.t_height}m, 速度={self.linear_speed}m/s') self.get_logger().info(f'PID参数: Kp={self.pid_kp}, Ki={self.pid_ki}, Kd={self.pid_kd}') self.get_logger().info(f'线性角度校正: 容差={self.linear_angle_tolerance:.3f}rad, 校正速度={self.angle_correction_speed}rad/s') def control_loop(self): """主控制循环 - 50Hz频率执行""" current_time = self.get_clock().now() # 初始化段开始时间 if self.segment_start_time is None: self.segment_start_time = current_time twist = Twist() # T字形运动分为10个阶段（PID角度控制 + 智能距离计算 + 线性角度校正）： # 0: 向前移动到T字中心（记录中心点坐标 + 角度校正） # 1: 左转90度（PID控制） # 2: 向左移动到左端（角度校正） # 3: 右转180度（PID控制） # 4: 向右移动到右端（记录右端点坐标 + 角度校正） # 5: 右转180度（PID控制） # 6: 向左移动回中心（动态计算距离 + 角度校正）✅ # 7: 左转90度（PID控制） # 8: 向下移动回起点（动态计算距离 + 角度校正）✅ # 9: 左转180度并重置（PID控制） if self.current_segment == 0: # 阶段0: 向前移动T字高度的距离（竖杠）+ 角度校正 target_distance = self.t_height target_angle = 0.0 # 保持朝前（0度） # 计算从起点移动的实际距离 moved_distance = math.sqrt((self.x - 0.0)**2 + (self.y - 0.0)**2) if moved_distance < target_distance: twist.linear.x = self.linear_speed # 添加角度校正 twist.angular.z = self.linear_angle_correction(target_angle) self.update_position(twist, 0.02) else: # 记录T字中心点坐标（第一次到达） if self.t_center_x is None: self.t_center_x = self.x self.t_center_y = self.y self.get_logger().info(f'记录T字中心点: ({self.x:.4f}, {self.y:.4f})') # 进入下一阶段 self.current_segment += 1 self.segment_start_time = current_time elif self.current_segment == 1: # 阶段1: 左转90度（PID控制） if not self.pid_active: # 初始化PID控制 self.target_angle = math.pi / 2 self.reset_pid() self.pid_active = True # 计算角度误差 error = self.target_angle - self.theta while error > math.pi: error -= 2 * math.pi while error < -math.pi: error += 2 * math.pi # 检查是否到达目标角度 if abs(error) < self.angle_tolerance: # 到达目标，停止PID self.pid_active = False # 记录阶段2起点 self.segment_start_x = self.x self.segment_start_y = self.y self.current_segment += 1 self.segment_start_time = current_time else: # 使用PID计算角速度 twist.angular.z = self.pid_angle_control() self.update_position(twist, 0.02) elif self.current_segment == 2: # 阶段2: 向左移动T字宽度的一半（从中心到左端）+ 角度校正 target_distance = self.t_width / 2 target_angle = math.pi / 2 # 保持朝左（90度） moved_distance = math.sqrt((self.x - self.segment_start_x)**2 + (self.y - self.segment_start_y)**2) if moved_distance < target_distance: twist.linear.x = self.linear_speed # 添加角度校正 twist.angular.z = self.linear_angle_correction(target_angle) self.update_position(twist, 0.02) else: self.current_segment += 1 self.segment_start_time = current_time elif self.current_segment == 3: # 阶段3: 右转180度（PID控制） if not self.pid_active: # 初始化PID控制 self.target_angle = -math.pi / 2 self.reset_pid() self.pid_active = True # 计算角度误差 error = self.target_angle - self.theta while error > math.pi: error -= 2 * math.pi while error < -math.pi: error += 2 * math.pi # 检查是否到达目标角度 if abs(error) < self.angle_tolerance: # 到达目标，停止PID self.pid_active = False # 记录阶段4起点 self.segment_start_x = self.x self.segment_start_y = self.y self.current_segment += 1 self.segment_start_time = current_time else: # 使用PID计算角速度 twist.angular.z = self.pid_angle_control() self.update_position(twist, 0.02) elif self.current_segment == 4: # 阶段4: 向右移动整个T字宽度（横杠完整长度）+ 角度校正 target_distance = self.t_width target_angle = -math.pi / 2 # 保持朝右（-90度） moved_distance = math.sqrt((self.x - self.segment_start_x)**2 + (self.y - self.segment_start_y)**2) if moved_distance < target_distance: twist.linear.x = self.linear_speed # 添加角度校正 twist.angular.z = self.linear_angle_correction(target_angle) self.update_position(twist, 0.02) else: # 记录右端点坐标，用于动态计算返回距离 self.right_end_x = self.x self.right_end_y = self.y self.current_segment += 1 self.segment_start_time = current_time elif self.current_segment == 5: # 阶段5: 右转180度（PID控制） if not self.pid_active: # 初始化PID控制 self.target_angle = math.pi / 2 self.reset_pid() self.pid_active = True # 计算角度误差 error = self.target_angle - self.theta while error > math.pi: error -= 2 * math.pi while error < -math.pi: error += 2 * math.pi # 检查是否到达目标角度 if abs(error) < self.angle_tolerance: # 到达目标，停止PID self.pid_active = False # 记录阶段6起点 self.segment_start_x = self.x self.segment_start_y = self.y self.current_segment += 1 self.segment_start_time = current_time else: # 使用PID计算角速度 twist.angular.z = self.pid_angle_control() self.update_position(twist, 0.02) elif self.current_segment == 6: # 阶段6: 向左移动回到中心（智能动态距离计算 + 角度校正）✅ # 动态计算到T字中心的实际距离 if self.t_center_x is not None and self.right_end_x is not None: dx = self.t_center_x - self.right_end_x dy = self.t_center_y - self.right_end_y target_distance = math.sqrt(dx*dx + dy*dy) else: # 备用：使用理论距离 target_distance = self.t_width / 2 target_angle = math.pi / 2 # 保持朝左（90度） # 基于实际移动距离判断 moved_distance = math.sqrt((self.x - self.segment_start_x)**2 + (self.y - self.segment_start_y)**2) if moved_distance < target_distance: twist.linear.x = self.linear_speed # 添加角度校正 twist.angular.z = self.linear_angle_correction(target_angle) self.update_position(twist, 0.02) else: # 位置自然到达，进入下一阶段 self.current_segment += 1 self.segment_start_time = current_time elif self.current_segment == 7: # 阶段7: 左转90度（PID控制） if not self.pid_active: # 初始化PID控制 self.target_angle = math.pi self.reset_pid() self.pid_active = True # 计算角度误差 error = self.target_angle - self.theta while error > math.pi: error -= 2 * math.pi while error < -math.pi: error += 2 * math.pi # 检查是否到达目标角度 if abs(error) < self.angle_tolerance: # 到达目标，停止PID self.pid_active = False # 记录阶段8起点 self.segment_start_x = self.x self.segment_start_y = self.y self.current_segment += 1 self.segment_start_time = current_time else: # 使用PID计算角速度 twist.angular.z = self.pid_angle_control() self.update_position(twist, 0.02) elif self.current_segment == 8: # 阶段8: 向下移动回到起点（智能动态距离计算 + 角度校正）✅ # 初始化起点坐标 if self.start_x is None: self.start_x = 0.0 self.start_y = 0.0 # 动态计算到起点的实际距离 if self.t_center_x is not None: dx = self.start_x - self.t_center_x dy = self.start_y - self.t_center_y target_distance = math.sqrt(dx*dx + dy*dy) else: target_distance = self.t_height target_angle = math.pi # 保持朝下（180度） # 基于实际移动距离判断 moved_distance = math.sqrt((self.x - self.segment_start_x)**2 + (self.y - self.segment_start_y)**2) if moved_distance < target_distance: twist.linear.x = self.linear_speed # 添加角度校正 twist.angular.z = self.linear_angle_correction(target_angle) self.update_position(twist, 0.02) else: # 位置自然到达，进入下一阶段 self.current_segment += 1 self.segment_start_time = current_time elif self.current_segment == 9: # 阶段9: 左转180度回到初始朝向（PID控制） if not self.pid_active: # 初始化PID控制 self.target_angle = 0.0 self.reset_pid() self.pid_active = True # 计算角度误差 error = self.target_angle - self.theta while error > math.pi: error -= 2 * math.pi while error < -math.pi: error += 2 * math.pi # 检查是否到达目标角度 if abs(error) < self.angle_tolerance: # 到达目标，停止PID self.pid_active = False # 循环结束：重置到精确起点，确保每轮一致 self.x = 0.0 self.y = 0.0 self.theta = 0.0 # ⚠️ 只重置右端点，T字中心点保持固定（第一次记录后永久使用） self.right_end_x = None self.right_end_y = None if self.loop_enabled: self.current_segment = 0 self.segment_start_time = current_time self.get_logger().info('完成一轮T字形轨迹，开始新循环') else: self.current_segment = 10 self.get_logger().info('T字形轨迹完成') else: # 使用PID计算角速度 twist.angular.z = self.pid_angle_control() self.update_position(twist, 0.02) # 发布速度指令到/cmd_vel话题 self.cmd_vel_pub.publish(twist) def pid_angle_control(self): """PID角度控制算法 Returns: float: 计算出的角速度（弧度/秒） """ # 计算角度误差 error = self.target_angle - self.theta # 归一化误差到[-π, π]范围（处理角度跳变） while error > math.pi: error -= 2 * math.pi while error < -math.pi: error += 2 * math.pi # P: 比例项 - 误差越大，输出越大 p_output = self.pid_kp * error # I: 积分项 - 消除长期稳态误差 self.error_sum += error # 限制积分累积，防止积分饱和 max_integral = 10.0 if self.error_sum > max_integral: self.error_sum = max_integral elif self.error_sum < -max_integral: self.error_sum = -max_integral i_output = self.pid_ki * self.error_sum # D: 微分项 - 预测趋势，减少超调 error_rate = error - self.last_error d_output = self.pid_kd * error_rate # PID总输出 angular_velocity = p_output + i_output + d_output # 限制最大角速度 if angular_velocity > self.angular_speed: angular_velocity = self.angular_speed elif angular_velocity < -self.angular_speed: angular_velocity = -self.angular_speed # 保存当前误差用于下次微分计算 self.last_error = error return angular_velocity def reset_pid(self): """重置PID状态""" self.error_sum = 0.0 self.last_error = 0.0 def linear_angle_correction(self, target_angle): """线性运动角度校正 Args: target_angle: 目标角度（弧度） Returns: float: 校正角速度（弧度/秒） """ # 计算角度误差 error = target_angle - self.theta # 归一化误差到[-π, π]范围 while error > math.pi: error -= 2 * math.pi while error < -math.pi: error += 2 * math.pi # 如果误差在容差范围内，不需要校正 if abs(error) < self.linear_angle_tolerance: return 0.0 # 计算校正角速度（比例控制） correction_velocity = self.angle_correction_speed * (error / abs(error)) # 限制最大校正速度 if correction_velocity > self.angle_correction_speed: correction_velocity = self.angle_correction_speed elif correction_velocity < -self.angle_correction_speed: correction_velocity = -self.angle_correction_speed return correction_velocity def update_position(self, twist, dt): """更新机器人位置（差速驱动运动学模型） Args: twist: 速度指令（线速度和角速度） dt: 时间步长（秒） """ # 根据当前速度和朝向更新位置 self.x += twist.linear.x * math.cos(self.theta) * dt self.y += twist.linear.x * math.sin(self.theta) * dt self.theta += twist.angular.z * dt # 归一化角度到[-π, π]范围 self.theta = math.atan2(math.sin(self.theta), math.cos(self.theta)) def publish_status(self): """发布TF变换和路径信息 - 10Hz频率""" current_time = self.get_clock().now() # 发布TF变换: odom坐标系 -> base_link坐标系 t = TransformStamped() t.header.stamp = current_time.to_msg() t.header.frame_id = 'odom' t.child_frame_id = 'base_link' t.transform.translation.x = self.x t.transform.translation.y = self.y t.transform.translation.z = 0.0 # 将欧拉角转换为四元数（ROS使用四元数表示旋转） q = self.euler_to_quaternion(0, 0, self.theta) t.transform.rotation.x = q[0] t.transform.rotation.y = q[1] t.transform.rotation.z = q[2] t.transform.rotation.w = q[3] # 广播TF变换 self.tf_broadcaster.sendTransform(t) # 添加当前位姿到路径 pose = PoseStamped() pose.header.stamp = current_time.to_msg() pose.header.frame_id = 'odom' pose.pose.position.x = self.x pose.pose.position.y = self.y pose.pose.position.z = 0.0 pose.pose.orientation.x = q[0] pose.pose.orientation.y = q[1] pose.pose.orientation.z = q[2] pose.pose.orientation.w = q[3] self.path_msg.poses.append(pose) self.path_msg.header.stamp = current_time.to_msg() # 限制路径长度，避免内存无限增长（保留最近2000个点） if len(self.path_msg.poses) > 2000: self.path_msg.poses.pop(0) # 发布路径到/robot_path话题（RViz中显示为绿色轨迹） self.path_pub.publish(self.path_msg) def publish_t_shape_marker(self): """发布T字形轨迹标记（参考线）""" marker = Marker() marker.header.frame_id = 'odom' marker.header.stamp = self.get_clock().now().to_msg() marker.ns = 't_shape' marker.id = 0 marker.type = Marker.LINE_STRIP marker.action = Marker.ADD marker.scale.x = 0.05 marker.color.r = 1.0 marker.color.g = 0.0 marker.color.b = 0.0 marker.color.a = 0.5 # 定义T字形路径点 from geometry_msgs.msg import Point p1 = Point() p1.x, p1.y, p1.z = 0.0, 0.0, 0.0 p2 = Point() p2.x, p2.y, p2.z = self.t_height, 0.0, 0.0 p3 = Point() p3.x, p3.y, p3.z = self.t_height, self.t_width / 2, 0.0 p4 = Point() p4.x, p4.y, p4.z = self.t_height, 0.0, 0.0 p5 = Point() p5.x, p5.y, p5.z = self.t_height, -self.t_width / 2, 0.0 p6 = Point() p6.x, p6.y, p6.z = self.t_height, 0.0, 0.0 p7 = Point() p7.x, p7.y, p7.z = 0.0, 0.0, 0.0 marker.points = [p1, p2, p3, p4, p5, p6, p7] self.marker_pub.publish(marker) self.create_timer(5.0, lambda: self.marker_pub.publish(marker)) def euler_to_quaternion(self, roll, pitch, yaw): """将欧拉角转换为四元数""" qx = math.sin(roll/2) * math.cos(pitch/2) * math.cos(yaw/2) - math.cos(roll/2) * math.sin(pitch/2) * math.sin(yaw/2) qy = math.cos(roll/2) * math.sin(pitch/2) * math.cos(yaw/2) + math.sin(roll/2) * math.cos(pitch/2) * math.sin(yaw/2) qz = math.cos(roll/2) * math.cos(pitch/2) * math.sin(yaw/2) - math.sin(roll/2) * math.sin(pitch/2) * math.cos(yaw/2) qw = math.cos(roll/2) * math.cos(pitch/2) * math.cos(yaw/2) + math.sin(roll/2) * math.sin(pitch/2) * math.sin(yaw/2) return [qx, qy, qz, qw] def main(args=None): rclpy.init(args=args) node = TShapeController() try: rclpy.spin(node) except KeyboardInterrupt: pass finally: node.destroy_node() rclpy.shutdown() if __name__ == '__main__': main()