JYC/RC2025_r1
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

提交一下

Browse Source
This commit is contained in:
Robofish 2025-07-11 00:48:19 +08:00
parent dcef535b33
commit ccb4a91e64
18 changed files with 486 additions and 330 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
nav.sh
View File

@ -3,18 +3,18 @@ source install/setup.bash
commands=(
# "/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/pub_aim.py"
"ros2 launch rm_nav_bringup bringup_real.launch.py \
world:=RC2026 \
world:=map2 \
mode:=nav \
lio:=fastlio \
localization:=icp \
lio_rviz:=false \
nav_rviz:=true"
"ros2 launch rm_simpal_move simple_move.launch.py"
"ros2 topic pub /move_goal rm_msgs/msg/MoveGoal '{x: 0.596, y: 4.01, angle: 0.0, max_speed: 10.0, tolerance: 0.1, rotor: false}' --once"
# "ros2 topic pub /move_goal rm_msgs/msg/MoveGoal '{x: 0.596, y: 4.01, angle: 0.0, max_speed: 10.0, tolerance: 0.1, rotor: false}' --once"
#"ros2 topic pub /move_goal rm_msgs/msg/MoveGoal '{x: 13.58, y: -3.35, angle: 0.0, max_speed: 10.0, tolerance: 0.1, rotor: false}' --once"
"ros2 topic pub /move_goal rm_msgs/msg/MoveGoal '{x: 0.54, y: -3.30, angle: 0.0, max_speed: 10.0, tolerance: 0.1, rotor: false}' --once"
"/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/siyuanshu.py"
"/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/pub_self.py"
# "ros2 topic pub /move_goal rm_msgs/msg/MoveGoal '{x: 0.54, y: -3.30, angle: 0.0, max_speed: 10.0, tolerance: 0.1, rotor: false}' --once"
"/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/receive_and_pub.py"
# "/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/pub_self.py"
)
for cmd in "${commands[@]}"; do

BIN
src.zip
View File

Binary file not shown.

4
src/rm_driver/livox_ros_driver2/README.md
View File

@ -327,3 +327,7 @@ Please add '/usr/local/lib' to the env LD_LIBRARY_PATH.
export LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:/usr/local/lib
source ~/.bashrc
```
KERNELS=="3-2:1.0", SUBSYSTEMS=="usb", MODE:="0666", SYMLINK+="underpan"
KERNELS=="3-1:1.0", SUBSYSTEMS=="usb", MODE:="0666", SYMLINK+="upper"
KERNELS=="3-2:1.0", SUBSYSTEMS=="usb", MODE:="0666", SYMLINK+="r2"

130
src/rm_driver/rm_serial_driver/script/R2_Serial.py Normal file
View File

@ -0,0 +1,130 @@
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Imu
import tf2_ros
from tf2_ros import TransformException
from geometry_msgs.msg import TransformStamped
import math
class ImuAndTfMonitor(Node):
def __init__(self):
super().__init__('imu_and_tf_monitor')
# 初始化TF缓冲区和监听器
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self)
# 订阅IMU话题
self.imu_subscription = self.create_subscription(
Imu,
'/livox/imu',
self.imu_callback,
10
)
# 创建定时器用于查询TF变换
self.tf_timer = self.create_timer(0.1, self.tf_timer_callback)
self.get_logger().info('IMU and TF Monitor started successfully.')
self.get_logger().info('Monitoring /livox/imu topic and TF transforms:')
self.get_logger().info('- base_link -> map')
self.get_logger().info('- r2_base_link -> map')
def imu_callback(self, msg: Imu):
"""处理IMU数据回调"""
# 将四元数转换为欧拉角
orientation = msg.orientation
roll, pitch, yaw = self.quaternion_to_euler(
orientation.x, orientation.y, orientation.z, orientation.w
)
self.get_logger().info(
f'[IMU Data] Frame: {msg.header.frame_id}\n'
f' Orientation (quaternion): x={orientation.x:.4f}, y={orientation.y:.4f}, '
f'z={orientation.z:.4f}, w={orientation.w:.4f}\n'
f' Orientation (euler): roll={math.degrees(roll):.2f}°, '
f'pitch={math.degrees(pitch):.2f}°, yaw={math.degrees(yaw):.2f}°\n'
f' Angular velocity: x={msg.angular_velocity.x:.4f}, '
f'y={msg.angular_velocity.y:.4f}, z={msg.angular_velocity.z:.4f} rad/s\n'
f' Linear acceleration: x={msg.linear_acceleration.x:.4f}, '
f'y={msg.linear_acceleration.y:.4f}, z={msg.linear_acceleration.z:.4f} m/s²'
)
def tf_timer_callback(self):
"""定时器回调，查询TF变换"""
# 查询 base_link -> map 的变换
self.query_transform('map', 'base_link', 'base_link -> map')
# 查询 r2_base_link -> map 的变换
self.query_transform('map', 'r2_base_link', 'r2_base_link -> map')
def query_transform(self, target_frame: str, source_frame: str, description: str):
"""查询指定的坐标变换"""
try:
# 获取最新可用的变换
transform: TransformStamped = self.tf_buffer.lookup_transform(
target_frame,
source_frame,
rclpy.time.Time(),
timeout=rclpy.duration.Duration(seconds=0.1)
)
# 提取位置和姿态信息
translation = transform.transform.translation
rotation = transform.transform.rotation
# 将四元数转换为欧拉角
roll, pitch, yaw = self.quaternion_to_euler(
rotation.x, rotation.y, rotation.z, rotation.w
)
self.get_logger().info(
f'[TF Transform] {description}:\n'
f' Position: x={translation.x:.4f}, y={translation.y:.4f}, z={translation.z:.4f}\n'
f' Orientation (quaternion): x={rotation.x:.4f}, y={rotation.y:.4f}, '
f'z={rotation.z:.4f}, w={rotation.w:.4f}\n'
f' Orientation (euler): roll={math.degrees(roll):.2f}°, '
f'pitch={math.degrees(pitch):.2f}°, yaw={math.degrees(yaw):.2f}°\n'
f' Timestamp: {transform.header.stamp.sec}.{transform.header.stamp.nanosec:09d}'
)
except TransformException as ex:
self.get_logger().warn(f'Failed to get transform {description}: {ex}')
def quaternion_to_euler(self, x, y, z, w):
"""将四元数转换为欧拉角 (roll, pitch, yaw)"""
# Roll (x-axis rotation)
sinr_cosp = 2 * (w * x + y * z)
cosr_cosp = 1 - 2 * (x * x + y * y)
roll = math.atan2(sinr_cosp, cosr_cosp)
# Pitch (y-axis rotation)
sinp = 2 * (w * y - z * x)
if abs(sinp) >= 1:
pitch = math.copysign(math.pi / 2, sinp) # 使用90度如果超出范围
else:
pitch = math.asin(sinp)
# Yaw (z-axis rotation)
siny_cosp = 2 * (w * z + x * y)
cosy_cosp = 1 - 2 * (y * y + z * z)
yaw = math.atan2(siny_cosp, cosy_cosp)
return roll, pitch, yaw
def main(args=None):
rclpy.init(args=args)
node = ImuAndTfMonitor()
try:
rclpy.spin(node)
except KeyboardInterrupt:
node.get_logger().info('Shutting down IMU and TF Monitor...')
except Exception as e:
node.get_logger().error(f'An error occurred: {e}')
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()

323
src/rm_driver/rm_serial_driver/script/r1_serial.py
View File

@ -1,323 +0,0 @@
import rclpy
from rclpy.node import Node
import serial
import struct
import tf2_ros
from geometry_msgs.msg import TransformStamped
from sensor_msgs.msg import Imu
from rm_msgs.msg import DataAim
import math
import threading
from rclpy.executors import MultiThreadedExecutor
class ReceiveAndPubNode(Node):
def __init__(self):
super().__init__('receive_and_pub')
# 串口配置
self.receive_port = '/dev/r2' # 接收串口
self.send_port = '/dev/underpan'
self.send_port_upper = '/dev/upper' # 发送串口
self.baud_rate = 115200
self.imu_angular_z = 0.0
# 角度平滑滤波器参数
self.smooth_alpha = 0.7 # 平滑系数，0-1之间，越小越平滑
self.filtered_original_yaw = 0.0
self.filtered_r2_yaw = 0.0
self.filter_initialized = False
self.imu_subscription = self.create_subscription(
Imu,
'/livox/imu',
self.imu_callback,
10
)
try:
# 接收串口
self.receive_serial = serial.Serial(
port=self.receive_port,
baudrate=self.baud_rate,
timeout=0.1
)
# 发送串口
self.send_serial = serial.Serial(
port=self.send_port,
baudrate=self.baud_rate,
timeout=1
)
self.send_serial_upper = serial.Serial(
port=self.send_port_upper,
baudrate=self.baud_rate,
timeout=1
)
self.get_logger().info(f'Serial ports opened: {self.receive_port}, {self.send_port},{self.send_port_upper}')
except Exception as e:
self.get_logger().error(f'Failed to open serial ports: {e}')
return
# TF广播器和监听器
self.tf_broadcaster = tf2_ros.TransformBroadcaster(self)
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self)
# 订阅瞄准数据
self.subscription = self.create_subscription(
DataAim,
'/chassis/data_aim',
self.aim_callback,
10
)
# 存储接收到的位置信息和瞄准数据
self.received_position = {'x': 0.0, 'y': 0.0, 'z': 0.0, 'yaw': 0.0, 'valid': False}
self.aim_data = {'yaw': 0.0, 'distance': 0.0, 'valid': False}
# R1->base_link坐标监听，结果存储在self.transform_position
self.transform_position = {'x': 0.0, 'y': 0.0}
self.r1_tf_timer = self.create_timer(0.05, self._update_r1_position)
# 启动接收线程
self.receive_thread = threading.Thread(target=self.receive_position_thread)
self.receive_thread.daemon = True
self.receive_thread.start()
# 定时发送数据
self.send_timer = self.create_timer(0.005, self.send_target_data) # 10Hz
self.get_logger().info('Receive and pub node started')
def angle_smooth_filter(self, new_angle, filtered_angle):
"""角度平滑滤波器，处理角度跳跃问题"""
if not self.filter_initialized:
return new_angle
# 处理角度跳跃问题（-π到π的边界）
diff = new_angle - filtered_angle
if diff > math.pi:
diff -= 2 * math.pi
elif diff < -math.pi:
diff += 2 * math.pi
# 指数移动平均滤波
return filtered_angle + self.smooth_alpha * diff
def imu_callback(self, msg):
"""接收IMU数据，提取angular.z"""
try:
# self.imu_angular_z = msg.angular_velocity.z
self.imu_angular_z = 0.0
except Exception as e:
self.get_logger().error(f'Error in imu callback: {e}')
def receive_position_thread(self):
buffer = bytearray()
while rclpy.ok():
try:
if self.receive_serial.in_waiting > 0:
data = self.receive_serial.read(self.receive_serial.in_waiting)
buffer.extend(data)
# 查找完整的数据包
while len(buffer) >= 18: # 包头(1) + 4个float(16) + 包尾(1) = 18字节
# 查找包头
start_idx = buffer.find(0xFF)
if start_idx == -1:
break
# 移除包头前的数据
if start_idx > 0:
buffer = buffer[start_idx:]
# 检查包长度
if len(buffer) < 18:
break
# 查找包尾
if buffer[17] == 0xFE:
# 解析数据
try:
x = struct.unpack('<f', buffer[1:5])[0]
y = struct.unpack('<f', buffer[5:9])[0]
z = struct.unpack('<f', buffer[9:13])[0]
yaw = struct.unpack('<f', buffer[13:17])[0]
self.received_position = {
'x': x, 'y': y, 'z': z, 'yaw': yaw, 'valid': True
}
# 发布tf变换
self.publish_r2_transform(x, y, z, yaw)
# self.get_logger().info(f' Received R2 position: x={x:.3f}, y={y:.3f}, z={z:.3f}, yaw={yaw:.3f}')
except struct.error as e:
self.get_logger().warn(f'⚠️ Data parsing error: {e}')
# 移除已处理的数据包
buffer = buffer[18:]
else:
# 包尾不匹配，移除包头继续查找
buffer = buffer[1:]
else:
# 避免忙等待
import time
time.sleep(0.001)
except Exception as e:
self.get_logger().error(f' Error in receive thread: {e}')
def publish_r2_transform(self, x, y, z, yaw):
"""发布R2到map的tf变换"""
try:
t = TransformStamped()
t.header.stamp = self.get_clock().now().to_msg()
t.header.frame_id = 'map'
t.child_frame_id = 'R2'
t.transform.translation.x = x
t.transform.translation.y = y
t.transform.translation.z = z
# 将yaw角转换为四元数
t.transform.rotation.x = 0.0
t.transform.rotation.y = 0.0
t.transform.rotation.z = math.sin(yaw / 2.0)
t.transform.rotation.w = math.cos(yaw / 2.0)
self.tf_broadcaster.sendTransform(t)
except Exception as e:
self.get_logger().error(f'Error publishing tf: {e}')
def aim_callback(self, msg):
"""接收瞄准数据"""
try:
self.aim_data = {
'yaw': msg.yaw,
'distance': msg.distance,
'valid': True
}
self.get_logger().debug(f'Received aim data: yaw={msg.yaw:.3f}, distance={msg.distance:.3f}')
except Exception as e:
self.get_logger().error(f'Error in aim callback: {e}')
def get_r2_target_data(self):
"""获取R2的瞄准数据"""
try:
if not self.received_position['valid']:
return 0.0, 0.0
# 这里可以根据R2的位置计算目标数据
# 简化处理：返回R2的yaw角和到原点的距离
r2_yaw = math.atan2(self.transform_position['y'], self.transform_position['x'])
if r2_yaw > 1.57:
r2_yaw = r2_yaw - 3.14
if r2_yaw < -1.57:
r2_yaw = r2_yaw + 3.14
r2_distance = math.sqrt(
(self.transform_position['x'])**2 +
self.transform_position['y']**2
)
return r2_yaw, r2_distance
except Exception as e:
self.get_logger().error(f'Error getting R2 target data: {e}')
return 0.0, 0.0
def send_target_data(self):
"""发送目标数据"""
try:
# 获取原始目标数据
if self.aim_data['valid']:
raw_original_yaw = self.aim_data['yaw']
original_distance = self.aim_data['distance']
else:
raw_original_yaw = 0.0
original_distance = 0.0
# 获取R2目标数据
raw_r2_yaw, r2_distance = self.get_r2_target_data()
# 初始化滤波器
if not self.filter_initialized:
self.filtered_original_yaw = raw_original_yaw
self.filtered_r2_yaw = raw_r2_yaw
self.filter_initialized = True
# 应用平滑滤波
self.filtered_original_yaw = self.angle_smooth_filter(raw_original_yaw, self.filtered_original_yaw)
self.filtered_r2_yaw = self.angle_smooth_filter(raw_r2_yaw, self.filtered_r2_yaw)
# 构建发送数据包
# 格式: 包头(0xFF) + original_yaw(4字节) + original_distance(4字节) + r2_yaw(4字节) + r2_distance(4字节) + 校验和(1字节) + 包尾(0xFE)
packet = bytearray()
packet.append(0xFF) # 包头
packet.extend(struct.pack('<f', self.filtered_original_yaw)) # 滤波后的原始yaw
# packet.extend(struct.pack('<f', original_distance)) # 原始distance
packet.extend(struct.pack('<f', self.filtered_r2_yaw)) # 滤波后的R2 yaw
# packet.extend(struct.pack('<f', r2_distance)) # R2 distance
# 添加IMU angular.z 到 underpan 的第三个数据
packet.extend(struct.pack('<f', self.imu_angular_z)) # 新增：IMU angular.z
packet.append(0xFE) # 包尾
packet_upper = bytearray()
packet_upper.append(0xFF) # 包头
# packet.extend(struct.pack('<f', original_yaw)) # 原始yaw
packet_upper.extend(struct.pack('<f', original_distance)) # 原始distance
# packet.extend(struct.pack('<f', r2_yaw)) # R2 yaw
packet_upper.extend(struct.pack('<f', r2_distance)) # R2 distance
# 计算校验和
checksum = 0
packet_upper.append(0xFE) # 包尾
# 发送数据
self.send_serial.write(packet)
self.send_serial_upper.write(packet_upper)
self.get_logger().info(f'Sent filtered angles - original({self.filtered_original_yaw:.3f}→{raw_original_yaw:.3f}) R2({self.filtered_r2_yaw:.3f}→{raw_r2_yaw:.3f}) dist({original_distance:.3f},{r2_distance:.3f}) IMU_z({self.imu_angular_z:.3f})')
except Exception as e:
self.get_logger().error(f'Error sending target data: {e}')
def _update_r1_position(self):
try:
trans = self.tf_buffer.lookup_transform(
'R2', # 目标坐标系（修正）
'base_link', # 源坐标系（修正）
rclpy.time.Time()
)
self.transform_position['x'] = trans.transform.translation.x
self.transform_position['y'] = trans.transform.translation.y
except Exception:
pass
def __del__(self):
if hasattr(self, 'receive_serial') and self.receive_serial.is_open:
self.receive_serial.close()
if hasattr(self, 'send_serial') and self.send_serial.is_open:
self.send_serial.close()
if hasattr(self, 'send_serial_upper') and self.send_serial_upper.is_open:
self.send_serial_upper.close()
self.get_logger().info('Serial ports closed')
def main(args=None):
rclpy.init(args=args)
try:
node = ReceiveAndPubNode()
executor = MultiThreadedExecutor()
executor.add_node(node)
executor.spin()
except KeyboardInterrupt:
pass
finally:
if rclpy.ok():
rclpy.shutdown()
if __name__ == '__main__':
main()

169
src/rm_driver/rm_serial_driver/script/receive.py Normal file
View File

@ -0,0 +1,169 @@
import serial
import struct
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PointStamped, TransformStamped
from rm_msgs.msg import DataAim
import tf2_ros
import tf2_geometry_msgs
import math
# 串口参数
SERIAL_PORT = '/dev/ttyACM0' # 根据实际情况修改
BAUDRATE = 115200
# 数据包格式
PACKET_FORMAT = '<BfffB' # 1字节int8头，3个float，1字节int8尾
PACKET_SIZE = struct.calcsize(PACKET_FORMAT)
HEADER = 0xAA
TAIL = 0xBB
class SerialReceiver(Node):
def __init__(self):
super().__init__('serial_receiver')
# 创建发布者
self.point_publisher = self.create_publisher(PointStamped, 'r2_map_point', 10)
self.data_aim_publisher = self.create_publisher(DataAim, 'data_aim_r2', 10)
# 创建TF监听器和广播器
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self)
self.tf_broadcaster = tf2_ros.TransformBroadcaster(self)
# 串口初始化
self.ser = serial.Serial(SERIAL_PORT, BAUDRATE, timeout=1)
self.get_logger().info(f"打开串口 {SERIAL_PORT}，波特率 {BAUDRATE}")
# 创建定时器用于读取串口数据
self.timer = self.create_timer(0.01, self.read_serial_data) # 10ms
self.buffer = bytearray()
def read_serial_data(self):
try:
data = self.ser.read(1)
if data:
self.buffer += data
# 保证缓冲区长度不超过一个包
if len(self.buffer) > PACKET_SIZE:
self.buffer = self.buffer[-PACKET_SIZE:]
# 检查是否有完整包
if len(self.buffer) >= PACKET_SIZE:
# 检查包头和包尾
if self.buffer[0] == HEADER and self.buffer[PACKET_SIZE-1] == TAIL:
# 解析数据
_, x, y, z, _ = struct.unpack(PACKET_FORMAT, self.buffer[:PACKET_SIZE])
self.get_logger().info(f"接收到数据: x={x:.3f}, y={y:.3f}, z={z:.3f}")
# 发布map坐标点
self.publish_map_point(x, y, z)
# 发布R2的TF变换
self.publish_r2_tf(x, y, z)
# 计算相对于base_link的角度和距离
self.calculate_and_publish_data_aim(x, y, z)
self.buffer = self.buffer[PACKET_SIZE:] # 移除已处理数据
else:
# 移除第一个字节，继续查找包头
self.buffer = self.buffer[1:]
except Exception as e:
self.get_logger().error(f"读取串口数据错误: {e}")
def publish_map_point(self, x, y, z):
"""发布地图坐标点"""
point_msg = PointStamped()
point_msg.header.frame_id = 'map'
point_msg.header.stamp = self.get_clock().now().to_msg()
point_msg.point.x = x
point_msg.point.y = y
point_msg.point.z = z
self.point_publisher.publish(point_msg)
def publish_r2_tf(self, x, y, z):
"""发布R2的TF变换"""
transform = TransformStamped()
# 设置header
transform.header.stamp = self.get_clock().now().to_msg()
transform.header.frame_id = 'map'
transform.child_frame_id = 'r2_robot'
# 设置位置
transform.transform.translation.x = x
transform.transform.translation.y = y
transform.transform.translation.z = z
# 设置旋转（假设R2面向x轴正方向，无旋转）
transform.transform.rotation.x = 0.0
transform.transform.rotation.y = 0.0
transform.transform.rotation.z = 0.0
transform.transform.rotation.w = 1.0
# 发布TF变换
self.tf_broadcaster.sendTransform(transform)
def calculate_and_publish_data_aim(self, x, y, z):
"""计算相对于base_link的角度和距离，并发布DataAim消息"""
try:
# 创建map坐标系下的点
point_stamped = PointStamped()
point_stamped.header.frame_id = 'map'
point_stamped.header.stamp = self.get_clock().now().to_msg()
point_stamped.point.x = x
point_stamped.point.y = y
point_stamped.point.z = z
# 获取从map到base_link的变换
transform = self.tf_buffer.lookup_transform(
'base_link', 'map', rclpy.time.Time())
# 将点变换到base_link坐标系
point_base_link = tf2_geometry_msgs.do_transform_point(point_stamped, transform)
# 计算距离
distance = math.sqrt(
point_base_link.point.x**2 +
point_base_link.point.y**2 +
point_base_link.point.z**2
)
# 计算yaw角度（绕z轴旋转角度）
yaw = math.atan2(point_base_link.point.y, point_base_link.point.x)
# 发布DataAim消息
data_aim_msg = DataAim()
data_aim_msg.yaw = yaw
data_aim_msg.distance = distance
self.data_aim_publisher.publish(data_aim_msg)
self.get_logger().info(f"发布DataAim: yaw={yaw:.3f} rad, distance={distance:.3f} m")
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e:
self.get_logger().warn(f"TF变换失败: {e}")
def destroy_node(self):
"""节点销毁时关闭串口"""
if hasattr(self, 'ser') and self.ser.is_open:
self.ser.close()
super().destroy_node()
def main(args=None):
rclpy.init(args=args)
receiver = SerialReceiver()
try:
rclpy.spin(receiver)
except KeyboardInterrupt:
print("接收中断，退出程序。")
finally:
receiver.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()

169
src/rm_driver/rm_serial_driver/script/receive1.py Normal file
View File

@ -0,0 +1,169 @@
import serial
import struct
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PointStamped, TransformStamped
from rm_msgs.msg import DataAim
import tf2_ros
import tf2_geometry_msgs
import math
# 串口参数
SERIAL_PORT = '/dev/ttyACM0' # 根据实际情况修改
BAUDRATE = 115200
# 数据包格式
PACKET_FORMAT = '<BfffB' # 1字节int8头，3个float，1字节int8尾
PACKET_SIZE = struct.calcsize(PACKET_FORMAT)
HEADER = 0xAA
TAIL = 0xBB
class SerialReceiver(Node):
def __init__(self):
super().__init__('serial_receiver')
# 创建发布者
self.point_publisher = self.create_publisher(PointStamped, 'r2_map_point', 10)
self.data_aim_publisher = self.create_publisher(DataAim, 'data_aim_r2', 10)
# 创建TF监听器和广播器
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self)
self.tf_broadcaster = tf2_ros.TransformBroadcaster(self)
# 串口初始化
self.ser = serial.Serial(SERIAL_PORT, BAUDRATE, timeout=1)
self.get_logger().info(f"打开串口 {SERIAL_PORT}，波特率 {BAUDRATE}")
# 创建定时器用于读取串口数据
self.timer = self.create_timer(0.01, self.read_serial_data) # 10ms
self.buffer = bytearray()
def read_serial_data(self):
try:
data = self.ser.read(1)
if data:
self.buffer += data
# 保证缓冲区长度不超过一个包
if len(self.buffer) > PACKET_SIZE:
self.buffer = self.buffer[-PACKET_SIZE:]
# 检查是否有完整包
if len(self.buffer) >= PACKET_SIZE:
# 检查包头和包尾
if self.buffer[0] == HEADER and self.buffer[PACKET_SIZE-1] == TAIL:
# 解析数据
_, x, y, z, _ = struct.unpack(PACKET_FORMAT, self.buffer[:PACKET_SIZE])
self.get_logger().info(f"接收到数据: x={x:.3f}, y={y:.3f}, z={z:.3f}")
# 发布map坐标点
self.publish_map_point(x, y, z)
# 发布R2的TF变换
self.publish_r2_tf(x, y, z)
# 计算相对于base_link的角度和距离
self.calculate_and_publish_data_aim(x, y, z)
self.buffer = self.buffer[PACKET_SIZE:] # 移除已处理数据
else:
# 移除第一个字节，继续查找包头
self.buffer = self.buffer[1:]
except Exception as e:
self.get_logger().error(f"读取串口数据错误: {e}")
def publish_map_point(self, x, y, z):
"""发布地图坐标点"""
point_msg = PointStamped()
point_msg.header.frame_id = 'map'
point_msg.header.stamp = self.get_clock().now().to_msg()
point_msg.point.x = x
point_msg.point.y = y
point_msg.point.z = z
self.point_publisher.publish(point_msg)
def publish_r2_tf(self, x, y, z):
"""发布R2的TF变换"""
transform = TransformStamped()
# 设置header
transform.header.stamp = self.get_clock().now().to_msg()
transform.header.frame_id = 'map'
transform.child_frame_id = 'r2_robot'
# 设置位置
transform.transform.translation.x = x
transform.transform.translation.y = y
transform.transform.translation.z = z
# 设置旋转（假设R2面向x轴正方向，无旋转）
transform.transform.rotation.x = 0.0
transform.transform.rotation.y = 0.0
transform.transform.rotation.z = 0.0
transform.transform.rotation.w = 1.0
# 发布TF变换
self.tf_broadcaster.sendTransform(transform)
def calculate_and_publish_data_aim(self, x, y, z):
"""计算相对于base_link的角度和距离，并发布DataAim消息"""
try:
# 创建map坐标系下的点
point_stamped = PointStamped()
point_stamped.header.frame_id = 'map'
point_stamped.header.stamp = self.get_clock().now().to_msg()
point_stamped.point.x = x
point_stamped.point.y = y
point_stamped.point.z = z
# 获取从map到base_link的变换
transform = self.tf_buffer.lookup_transform(
'base_link', 'map', rclpy.time.Time())
# 将点变换到base_link坐标系
point_base_link = tf2_geometry_msgs.do_transform_point(point_stamped, transform)
# 计算距离
distance = math.sqrt(
point_base_link.point.x**2 +
point_base_link.point.y**2 +
point_base_link.point.z**2
)
# 计算yaw角度（绕z轴旋转角度）
yaw = math.atan2(point_base_link.point.y, point_base_link.point.x)
# 发布DataAim消息
data_aim_msg = DataAim()
data_aim_msg.yaw = yaw
data_aim_msg.distance = distance
self.data_aim_publisher.publish(data_aim_msg)
self.get_logger().info(f"发布DataAim: yaw={yaw:.3f} rad, distance={distance:.3f} m")
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e:
self.get_logger().warn(f"TF变换失败: {e}")
def destroy_node(self):
"""节点销毁时关闭串口"""
if hasattr(self, 'ser') and self.ser.is_open:
self.ser.close()
super().destroy_node()
def main(args=None):
rclpy.init(args=args)
receiver = SerialReceiver()
try:
rclpy.spin(receiver)
except KeyboardInterrupt:
print("接收中断，退出程序。")
finally:
receiver.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()

BIN
src/rm_nav_bringup/PCD/map1.pcd Executable file
View File

Binary file not shown.

BIN
src/rm_nav_bringup/PCD/map2.pcd Executable file
View File

Binary file not shown.

2
src/rm_nav_bringup/config/reality/icp_registration_real.yaml
View File

@ -3,7 +3,7 @@
use_sim_time: false
rough_leaf_size: 0.4
refine_leaf_size: 0.1
pcd_path: "RC2026"
pcd_path: "map2"
map_frame_id: "map"
odom_frame_id: "odom"
range_odom_frame_id: "lidar_odom"

BIN
src/rm_nav_bringup/map/map1.data Executable file
View File

Binary file not shown.

BIN
src/rm_nav_bringup/map/map1.pgm Executable file
View File

Binary file not shown.

BIN
src/rm_nav_bringup/map/map1.posegraph Executable file
View File

Binary file not shown.

7
src/rm_nav_bringup/map/map1.yaml Executable file
View File

@ -0,0 +1,7 @@
image: map1.pgm
mode: trinary
resolution: 0.05
origin: [-9.04, -13.8, 0]
negate: 0
occupied_thresh: 0.65
free_thresh: 0.25

0
test.data → src/rm_nav_bringup/map/map2.data Normal file → Executable file
View File

0
test.pgm → src/rm_nav_bringup/map/map2.pgm Normal file → Executable file
View File

0
test.posegraph → src/rm_nav_bringup/map/map2.posegraph Normal file → Executable file
View File

2
test.yaml → src/rm_nav_bringup/map/map2.yaml Normal file → Executable file
View File

@ -1,4 +1,4 @@
image: test.pgm
image: map2.pgm
mode: trinary
resolution: 0.05
origin: [-7.08, -15.4, 0]