提交一下
This commit is contained in:
parent
dcef535b33
commit
ccb4a91e64
10
nav.sh
10
nav.sh
@ -3,18 +3,18 @@ source install/setup.bash
|
|||||||
commands=(
|
commands=(
|
||||||
# "/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/pub_aim.py"
|
# "/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/pub_aim.py"
|
||||||
"ros2 launch rm_nav_bringup bringup_real.launch.py \
|
"ros2 launch rm_nav_bringup bringup_real.launch.py \
|
||||||
world:=RC2026 \
|
world:=map2 \
|
||||||
mode:=nav \
|
mode:=nav \
|
||||||
lio:=fastlio \
|
lio:=fastlio \
|
||||||
localization:=icp \
|
localization:=icp \
|
||||||
lio_rviz:=false \
|
lio_rviz:=false \
|
||||||
nav_rviz:=true"
|
nav_rviz:=true"
|
||||||
"ros2 launch rm_simpal_move simple_move.launch.py"
|
"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: 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"
|
# "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/receive_and_pub.py"
|
||||||
"/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/pub_self.py"
|
# "/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/pub_self.py"
|
||||||
)
|
)
|
||||||
|
|
||||||
for cmd in "${commands[@]}"; do
|
for cmd in "${commands[@]}"; do
|
||||||
|
|||||||
@ -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
|
export LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:/usr/local/lib
|
||||||
source ~/.bashrc
|
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
130
src/rm_driver/rm_serial_driver/script/R2_Serial.py
Normal 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()
|
||||||
@ -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
169
src/rm_driver/rm_serial_driver/script/receive.py
Normal 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
169
src/rm_driver/rm_serial_driver/script/receive1.py
Normal 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
BIN
src/rm_nav_bringup/PCD/map1.pcd
Executable file
Binary file not shown.
BIN
src/rm_nav_bringup/PCD/map2.pcd
Executable file
BIN
src/rm_nav_bringup/PCD/map2.pcd
Executable file
Binary file not shown.
@ -3,7 +3,7 @@
|
|||||||
use_sim_time: false
|
use_sim_time: false
|
||||||
rough_leaf_size: 0.4
|
rough_leaf_size: 0.4
|
||||||
refine_leaf_size: 0.1
|
refine_leaf_size: 0.1
|
||||||
pcd_path: "RC2026"
|
pcd_path: "map2"
|
||||||
map_frame_id: "map"
|
map_frame_id: "map"
|
||||||
odom_frame_id: "odom"
|
odom_frame_id: "odom"
|
||||||
range_odom_frame_id: "lidar_odom"
|
range_odom_frame_id: "lidar_odom"
|
||||||
|
|||||||
BIN
src/rm_nav_bringup/map/map1.data
Executable file
BIN
src/rm_nav_bringup/map/map1.data
Executable file
Binary file not shown.
BIN
src/rm_nav_bringup/map/map1.pgm
Executable file
BIN
src/rm_nav_bringup/map/map1.pgm
Executable file
Binary file not shown.
BIN
src/rm_nav_bringup/map/map1.posegraph
Executable file
BIN
src/rm_nav_bringup/map/map1.posegraph
Executable file
Binary file not shown.
7
src/rm_nav_bringup/map/map1.yaml
Executable file
7
src/rm_nav_bringup/map/map1.yaml
Executable 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
0
test.data → src/rm_nav_bringup/map/map2.data
Normal file → Executable file
0
test.pgm → src/rm_nav_bringup/map/map2.pgm
Normal file → Executable file
0
test.pgm → src/rm_nav_bringup/map/map2.pgm
Normal file → Executable file
0
test.posegraph → src/rm_nav_bringup/map/map2.posegraph
Normal file → Executable file
0
test.posegraph → src/rm_nav_bringup/map/map2.posegraph
Normal file → Executable file
2
test.yaml → src/rm_nav_bringup/map/map2.yaml
Normal file → Executable file
2
test.yaml → src/rm_nav_bringup/map/map2.yaml
Normal file → Executable file
@ -1,4 +1,4 @@
|
|||||||
image: test.pgm
|
image: map2.pgm
|
||||||
mode: trinary
|
mode: trinary
|
||||||
resolution: 0.05
|
resolution: 0.05
|
||||||
origin: [-7.08, -15.4, 0]
|
origin: [-7.08, -15.4, 0]
|
||||||
Loading…
Reference in New Issue
Block a user