|
|
#!/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() |
