r1-7-12
This commit is contained in:
parent
ccb4a91e64
commit
2f3e6caa8d
13
nav.sh
Executable file → Normal file
13
nav.sh
Executable file → Normal file
@ -1,20 +1,19 @@
|
||||
# 备场代码
|
||||
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:=map2 \
|
||||
world:=RC2026 \
|
||||
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: 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/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/R2_Serial.py"
|
||||
"/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/receive.py"
|
||||
"/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/slect.py map"
|
||||
|
||||
)
|
||||
|
||||
for cmd in "${commands[@]}"; do
|
||||
|
||||
@ -1,16 +1,19 @@
|
||||
# 备场代码
|
||||
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:=RC2025 \
|
||||
world:=map1 \
|
||||
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.56, y: 3.960, 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/R2_Serial.py"
|
||||
"/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/receive.py"
|
||||
"/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/slect.py map1"
|
||||
|
||||
)
|
||||
|
||||
for cmd in "${commands[@]}"; do
|
||||
22
nav2.sh
Normal file
22
nav2.sh
Normal file
@ -0,0 +1,22 @@
|
||||
# 备场代码
|
||||
source install/setup.bash
|
||||
|
||||
commands=(
|
||||
"ros2 launch rm_nav_bringup bringup_real.launch.py \
|
||||
world:=map2 \
|
||||
mode:=nav \
|
||||
lio:=fastlio \
|
||||
localization:=icp \
|
||||
lio_rviz:=false \
|
||||
nav_rviz:=true"
|
||||
"ros2 launch rm_simpal_move simple_move.launch.py"
|
||||
"/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/R2_Serial.py"
|
||||
"/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/receive.py"
|
||||
"/bin/python3 /home/robofish/RC2025/src/rm_driver/rm_serial_driver/script/slect.py map2"
|
||||
|
||||
)
|
||||
|
||||
for cmd in "${commands[@]}"; do
|
||||
gnome-terminal -- bash -c "source install/setup.bash; $cmd; exec bash"
|
||||
sleep 0.5
|
||||
done
|
||||
@ -1,128 +1,117 @@
|
||||
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
|
||||
import serial
|
||||
import struct
|
||||
from rm_msgs.msg import DataAim
|
||||
|
||||
class ImuAndTfMonitor(Node):
|
||||
class R2SerialNode(Node):
|
||||
def __init__(self):
|
||||
super().__init__('imu_and_tf_monitor')
|
||||
super().__init__('r2_serial_node')
|
||||
|
||||
# 初始化TF缓冲区和监听器
|
||||
self.tf_buffer = tf2_ros.Buffer()
|
||||
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self)
|
||||
# 声明参数
|
||||
self.declare_parameter('yaw_port', '/dev/ttyUSB0')
|
||||
self.declare_parameter('distance_port', '/dev/ttyUSB1')
|
||||
self.declare_parameter('baud_rate', 115200)
|
||||
|
||||
# 订阅IMU话题
|
||||
self.imu_subscription = self.create_subscription(
|
||||
Imu,
|
||||
'/livox/imu',
|
||||
self.imu_callback,
|
||||
# 获取参数
|
||||
self.yaw_port = self.get_parameter('yaw_port').get_parameter_value().string_value
|
||||
self.distance_port = self.get_parameter('distance_port').get_parameter_value().string_value
|
||||
self.baud_rate = self.get_parameter('baud_rate').get_parameter_value().integer_value
|
||||
|
||||
# 初始化串口
|
||||
try:
|
||||
self.yaw_serial = serial.Serial(self.yaw_port, self.baud_rate, timeout=1)
|
||||
self.get_logger().info(f"Yaw serial port {self.yaw_port} opened successfully")
|
||||
except Exception as e:
|
||||
self.get_logger().error(f"Failed to open yaw serial port: {e}")
|
||||
self.yaw_serial = None
|
||||
|
||||
try:
|
||||
self.distance_serial = serial.Serial(self.distance_port, self.baud_rate, timeout=1)
|
||||
self.get_logger().info(f"Distance serial port {self.distance_port} opened successfully")
|
||||
except Exception as e:
|
||||
self.get_logger().error(f"Failed to open distance serial port: {e}")
|
||||
self.distance_serial = None
|
||||
|
||||
# 数据存储
|
||||
self.yaw1 = 0.0
|
||||
self.yaw2 = 0.0
|
||||
self.distance1 = 0.0
|
||||
self.distance2 = 0.0
|
||||
|
||||
# 订阅话题
|
||||
self.sub1 = self.create_subscription(
|
||||
DataAim,
|
||||
'/chassis/data_aim',
|
||||
self.data_aim_callback,
|
||||
10
|
||||
)
|
||||
self.sub2 = self.create_subscription(
|
||||
DataAim,
|
||||
'/data_aim_r2',
|
||||
self.data_aim_r2_callback,
|
||||
10
|
||||
)
|
||||
|
||||
# 创建定时器用于查询TF变换
|
||||
self.tf_timer = self.create_timer(0.1, self.tf_timer_callback)
|
||||
self.get_logger().info("R2 Serial Node initialized")
|
||||
|
||||
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 data_aim_callback(self, msg):
|
||||
"""处理/data_aim话题的回调"""
|
||||
self.yaw1 = msg.yaw
|
||||
self.distance1 = msg.distance
|
||||
self.get_logger().debug(f"Received data_aim: yaw={self.yaw1}, distance={self.distance1}")
|
||||
self.send_data()
|
||||
|
||||
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
|
||||
)
|
||||
def data_aim_r2_callback(self, msg):
|
||||
"""处理/data_aim_r2话题的回调"""
|
||||
self.yaw2 = msg.yaw
|
||||
self.distance2 = msg.distance
|
||||
self.get_logger().debug(f"Received data_aim_r2: yaw={self.yaw2}, distance={self.distance2}")
|
||||
self.send_data()
|
||||
|
||||
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):
|
||||
"""查询指定的坐标变换"""
|
||||
def send_data(self):
|
||||
"""发送数据到串口"""
|
||||
# 打包所有数据：包头FF + yaw1 + distance1 + yaw2 + distance2 + 包尾FE
|
||||
try:
|
||||
# 获取最新可用的变换
|
||||
transform: TransformStamped = self.tf_buffer.lookup_transform(
|
||||
target_frame,
|
||||
source_frame,
|
||||
rclpy.time.Time(),
|
||||
timeout=rclpy.duration.Duration(seconds=0.1)
|
||||
)
|
||||
# 包头FF + 四个float32值 + 包尾FE
|
||||
all_data = struct.pack('<ffff', self.yaw1, self.distance1, self.yaw2, self.distance2) # 小端序
|
||||
data_packet = b'\xFF' + all_data + b'\xFE'
|
||||
|
||||
# 提取位置和姿态信息
|
||||
translation = transform.transform.translation
|
||||
rotation = transform.transform.rotation
|
||||
# 通过yaw串口发送
|
||||
if self.yaw_serial and self.yaw_serial.is_open:
|
||||
self.yaw_serial.write(data_packet)
|
||||
print(f"Sent yaw data: {self.yaw1}, {self.yaw2}")
|
||||
|
||||
# 将四元数转换为欧拉角
|
||||
roll, pitch, yaw = self.quaternion_to_euler(
|
||||
rotation.x, rotation.y, rotation.z, rotation.w
|
||||
)
|
||||
# 通过distance串口发送相同数据
|
||||
if self.distance_serial and self.distance_serial.is_open:
|
||||
self.distance_serial.write(data_packet)
|
||||
print(f"Sent distance data: {self.distance1}, {self.distance2}")
|
||||
|
||||
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 Exception as e:
|
||||
self.get_logger().error(f"Failed to send data: {e}")
|
||||
|
||||
except TransformException as ex:
|
||||
self.get_logger().warn(f'Failed to get transform {description}: {ex}')
|
||||
def __del__(self):
|
||||
"""析构函数，关闭串口"""
|
||||
if hasattr(self, 'yaw_serial') and self.yaw_serial and self.yaw_serial.is_open:
|
||||
self.yaw_serial.close()
|
||||
self.get_logger().info("Yaw serial port closed")
|
||||
|
||||
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
|
||||
if hasattr(self, 'distance_serial') and self.distance_serial and self.distance_serial.is_open:
|
||||
self.distance_serial.close()
|
||||
self.get_logger().info("Distance serial port closed")
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = ImuAndTfMonitor()
|
||||
|
||||
try:
|
||||
node = R2SerialNode()
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
node.get_logger().info('Shutting down IMU and TF Monitor...')
|
||||
pass
|
||||
except Exception as e:
|
||||
node.get_logger().error(f'An error occurred: {e}')
|
||||
print(f"Error in main: {e}")
|
||||
finally:
|
||||
if 'node' in locals():
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
@ -1,82 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
import serial
|
||||
import struct
|
||||
from rm_msgs.msg import DataAim # 假设使用DataAim消息类型，您可以根据实际消息类型调整
|
||||
class AimDataSerial(Node):
|
||||
def __init__(self):
|
||||
super().__init__('aim_data_serial')
|
||||
|
||||
# 串口配置
|
||||
self.serial_port = '/dev/upper' # 根据实际串口设备调整
|
||||
self.baud_rate = 115200
|
||||
|
||||
try:
|
||||
self.serial_conn = serial.Serial(
|
||||
port=self.serial_port,
|
||||
baudrate=self.baud_rate,
|
||||
timeout=1
|
||||
)
|
||||
self.get_logger().info(f'Serial port {self.serial_port} opened successfully')
|
||||
except Exception as e:
|
||||
self.get_logger().error(f'Failed to open serial port: {e}')
|
||||
return
|
||||
|
||||
# 订阅话题
|
||||
self.subscription = self.create_subscription(
|
||||
DataAim, # 根据实际消息类型调整
|
||||
'/chassis/data_aim',
|
||||
self.aim_callback,
|
||||
10
|
||||
)
|
||||
|
||||
self.get_logger().info('Aim data serial node started')
|
||||
|
||||
def aim_callback(self, msg):
|
||||
try:
|
||||
# 提取yaw和distance数据
|
||||
# 根据实际消息结构调整，这里假设使用Point32的x和y字段
|
||||
# yaw = msg.yaw
|
||||
distance_r2 = msg.yaw
|
||||
distance = msg.distance
|
||||
z = 0.0
|
||||
|
||||
self.get_logger().info(f'Received - distance: {distance}, distance_r2: {distance_r2}')
|
||||
|
||||
# 构建发送数据包
|
||||
# 格式: 包头(0xFF) + yaw(4字节float) + distance(4字节float) + 包尾(0xFE)
|
||||
packet = bytearray()
|
||||
packet.append(0xFF) # 包头
|
||||
packet.extend(struct.pack('<f', distance)) # yaw (小端序float)
|
||||
packet.extend(struct.pack('<f', distance_r2))
|
||||
packet.extend(struct.pack('<f', z)) # distance (小端序float)
|
||||
packet.append(0xFE) # 包尾
|
||||
|
||||
# 发送数据
|
||||
self.serial_conn.write(packet)
|
||||
self.get_logger().debug(f'Sent packet: {packet.hex()}')
|
||||
|
||||
except Exception as e:
|
||||
self.get_logger().error(f'Error in aim_callback: {e}')
|
||||
|
||||
def __del__(self):
|
||||
if hasattr(self, 'serial_conn') and self.serial_conn.is_open:
|
||||
self.serial_conn.close()
|
||||
self.get_logger().info('Serial port closed')
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
|
||||
try:
|
||||
node = AimDataSerial()
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@ -1,80 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
import serial
|
||||
import struct
|
||||
from rm_msgs.msg import DataAim # 假设使用DataAim消息类型，您可以根据实际消息类型调整
|
||||
class AimDataSerial(Node):
|
||||
def __init__(self):
|
||||
super().__init__('aim_data_serial')
|
||||
|
||||
# 串口配置
|
||||
self.serial_port = '/dev/underpan' # 根据实际串口设备调整
|
||||
self.baud_rate = 115200
|
||||
|
||||
try:
|
||||
self.serial_conn = serial.Serial(
|
||||
port=self.serial_port,
|
||||
baudrate=self.baud_rate,
|
||||
timeout=1
|
||||
)
|
||||
self.get_logger().info(f'Serial port {self.serial_port} opened successfully')
|
||||
except Exception as e:
|
||||
self.get_logger().error(f'Failed to open serial port: {e}')
|
||||
return
|
||||
|
||||
# 订阅话题
|
||||
self.subscription = self.create_subscription(
|
||||
DataAim, # 根据实际消息类型调整
|
||||
'/chassis/data_aim',
|
||||
self.aim_callback,
|
||||
10
|
||||
)
|
||||
|
||||
self.get_logger().info('Aim data serial node started')
|
||||
|
||||
def aim_callback(self, msg):
|
||||
try:
|
||||
# 提取yaw和distance数据
|
||||
# 根据实际消息结构调整，这里假设使用Point32的x和y字段
|
||||
yaw = msg.yaw
|
||||
yaw_r2 =msg.distance
|
||||
# distance = msg.distance
|
||||
|
||||
self.get_logger().info(f'Received - yaw: {yaw},yaw_r2: {yaw_r2}')
|
||||
|
||||
# 构建发送数据包
|
||||
# 格式: 包头(0xFF) + yaw(4字节float) + distance(4字节float) + 包尾(0xFE)
|
||||
packet = bytearray()
|
||||
packet.append(0xFF) # 包头
|
||||
packet.extend(struct.pack('<f', yaw)) # yaw (小端序float)
|
||||
packet.extend(struct.pack('<f', yaw_r2)) # distance (小端序float)
|
||||
packet.append(0xFE) # 包尾
|
||||
|
||||
# 发送数据
|
||||
self.serial_conn.write(packet)
|
||||
self.get_logger().debug(f'Sent packet: {packet.hex()}')
|
||||
|
||||
except Exception as e:
|
||||
self.get_logger().error(f'Error in aim_callback: {e}')
|
||||
|
||||
def __del__(self):
|
||||
if hasattr(self, 'serial_conn') and self.serial_conn.is_open:
|
||||
self.serial_conn.close()
|
||||
self.get_logger().info('Serial port closed')
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
|
||||
try:
|
||||
node = AimDataSerial()
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@ -1,97 +0,0 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
import serial
|
||||
import struct
|
||||
import tf2_ros
|
||||
from geometry_msgs.msg import TransformStamped
|
||||
from tf2_geometry_msgs import do_transform_pose
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
import math
|
||||
|
||||
class SelfPositionSerial(Node):
|
||||
def __init__(self):
|
||||
super().__init__('self_position_serial')
|
||||
|
||||
# 串口配置
|
||||
self.serial_port = '/dev/ttyUSB2' # 根据实际串口设备调整
|
||||
self.baud_rate = 115200
|
||||
|
||||
try:
|
||||
self.serial_conn = serial.Serial(
|
||||
port=self.serial_port,
|
||||
baudrate=self.baud_rate,
|
||||
timeout=1
|
||||
)
|
||||
self.get_logger().info(f'Serial port {self.serial_port} opened successfully')
|
||||
except Exception as e:
|
||||
self.get_logger().error(f'Failed to open serial port: {e}')
|
||||
return
|
||||
|
||||
# TF监听器
|
||||
self.tf_buffer = tf2_ros.Buffer()
|
||||
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self)
|
||||
|
||||
# 定时器，定期发送位置信息
|
||||
self.timer = self.create_timer(0.1, self.send_position) # 10Hz
|
||||
|
||||
self.get_logger().info('Self position serial node started')
|
||||
|
||||
def send_position(self):
|
||||
try:
|
||||
# 获取base_link在map下的位置
|
||||
transform = self.tf_buffer.lookup_transform(
|
||||
'map', 'base_link', rclpy.time.Time())
|
||||
|
||||
# 提取位置和朝向
|
||||
x = transform.transform.translation.x
|
||||
y = transform.transform.translation.y
|
||||
z = transform.transform.translation.z
|
||||
|
||||
# 计算yaw角（从四元数）
|
||||
quat = transform.transform.rotation
|
||||
yaw = math.atan2(2.0 * (quat.w * quat.z + quat.x * quat.y),
|
||||
1.0 - 2.0 * (quat.y * quat.y + quat.z * quat.z))
|
||||
|
||||
self.get_logger().debug(f'Position - x: {x:.3f}, y: {y:.3f}, z: {z:.3f}, yaw: {yaw:.3f}')
|
||||
|
||||
# 构建发送数据包
|
||||
# 格式: 包头(0xFF) + x(4字节) + y(4字节) + z(4字节) + yaw(4字节) + 校验和(1字节) + 包尾(0xFE)
|
||||
packet = bytearray()
|
||||
packet.append(0xFF) # 包头
|
||||
packet.extend(struct.pack('<f', x)) # x坐标
|
||||
packet.extend(struct.pack('<f', y)) # y坐标
|
||||
packet.extend(struct.pack('<f', z)) # z坐标
|
||||
packet.extend(struct.pack('<f', yaw)) # yaw角
|
||||
|
||||
|
||||
|
||||
packet.append(0xFE) # 包尾
|
||||
|
||||
# 发送数据
|
||||
self.serial_conn.write(packet)
|
||||
self.get_logger().debug(f'Sent position packet: {packet.hex()}')
|
||||
|
||||
except Exception as e:
|
||||
self.get_logger().warn(f'Failed to get transform or send data: {e}')
|
||||
|
||||
def __del__(self):
|
||||
if hasattr(self, 'serial_conn') and self.serial_conn.is_open:
|
||||
self.serial_conn.close()
|
||||
self.get_logger().info('Serial port closed')
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
|
||||
try:
|
||||
node = SelfPositionSerial()
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@ -1,169 +0,0 @@
|
||||
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()
|
||||
@ -1,274 +0,0 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
import serial
|
||||
import struct
|
||||
import tf2_ros
|
||||
from geometry_msgs.msg import TransformStamped
|
||||
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
|
||||
|
||||
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 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']:
|
||||
original_yaw = self.aim_data['yaw']
|
||||
original_distance = self.aim_data['distance']
|
||||
else:
|
||||
original_yaw = 0.0
|
||||
original_distance = 0.0
|
||||
|
||||
# 获取R2目标数据
|
||||
r2_yaw, r2_distance = self.get_r2_target_data()
|
||||
|
||||
# 构建发送数据包
|
||||
# 格式: 包头(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', original_yaw)) # 原始yaw
|
||||
# packet.extend(struct.pack('<f', original_distance)) # 原始distance
|
||||
packet.extend(struct.pack('<f', r2_yaw)) # R2 yaw
|
||||
# packet.extend(struct.pack('<f', r2_distance)) # R2 distance
|
||||
|
||||
|
||||
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 target packet: original({original_yaw:.3f},{original_distance:.3f}) R2({r2_yaw:.3f},{r2_distance:.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()
|
||||
@ -1,274 +0,0 @@
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
import serial
|
||||
import struct
|
||||
import tf2_ros
|
||||
from geometry_msgs.msg import TransformStamped
|
||||
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
|
||||
|
||||
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.1, self.send_target_data) # 10Hz
|
||||
|
||||
self.get_logger().info('Receive and pub node started')
|
||||
|
||||
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']:
|
||||
original_yaw = self.aim_data['yaw']
|
||||
original_distance = self.aim_data['distance']
|
||||
else:
|
||||
original_yaw = 0.0
|
||||
original_distance = 0.0
|
||||
|
||||
# 获取R2目标数据
|
||||
r2_yaw, r2_distance = self.get_r2_target_data()
|
||||
|
||||
# 构建发送数据包
|
||||
# 格式: 包头(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', original_yaw)) # 原始yaw
|
||||
# packet.extend(struct.pack('<f', original_distance)) # 原始distance
|
||||
packet.extend(struct.pack('<f', r2_yaw)) # R2 yaw
|
||||
# packet.extend(struct.pack('<f', r2_distance)) # R2 distance
|
||||
|
||||
|
||||
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 target packet: original({original_yaw:.3f},{original_distance:.3f}) R2({r2_yaw:.3f},{r2_distance:.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()
|
||||
@ -1,294 +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/ttyUSB2' # 接收串口
|
||||
self.send_port = '/dev/ttyUSB0'
|
||||
self.send_port_upper = '/dev/ttyUSB1' # 发送串口
|
||||
self.baud_rate = 115200
|
||||
self.imu_angular_z = 0.0
|
||||
|
||||
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.01, 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.1, self.send_target_data) # 10Hz
|
||||
|
||||
self.get_logger().info('Receive and pub node started')
|
||||
|
||||
def imu_callback(self, msg):
|
||||
"""接收IMU数据，提取angular.z"""
|
||||
try:
|
||||
# self.imu_angular_z = msg.angular_velocity.z
|
||||
# self.imu_angular_z = -self.imu_angular_z # 取反
|
||||
# self.get_logger().debug(f'IMU angular.z: {self.imu_angular_z:.3f}')
|
||||
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']:
|
||||
original_yaw = self.aim_data['yaw']
|
||||
original_distance = self.aim_data['distance']
|
||||
else:
|
||||
original_yaw = 0.0
|
||||
original_distance = 0.0
|
||||
|
||||
# 获取R2目标数据
|
||||
r2_yaw, r2_distance = self.get_r2_target_data()
|
||||
|
||||
# 构建发送数据包
|
||||
# 格式: 包头(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', original_yaw)) # 原始yaw
|
||||
# packet.extend(struct.pack('<f', original_distance)) # 原始distance
|
||||
packet.extend(struct.pack('<f', 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 target packet: original({original_yaw:.3f},{original_distance:.3f}) R2({r2_yaw:.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(
|
||||
'base_link',
|
||||
'R2',
|
||||
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()
|
||||
@ -3,10 +3,21 @@ from rclpy.node import Node
|
||||
from rm_msgs.msg import MoveGoal
|
||||
from geometry_msgs.msg import TransformStamped
|
||||
import tf2_ros
|
||||
import sys
|
||||
|
||||
# 预设多组参数
|
||||
MAP_CONFIGS = {
|
||||
"map": {"split_x": 7.155, "goal1": (0.65, 3.95), "goal2": (13.66, 3.73)},
|
||||
"map1": {"split_x": 7.085, "goal1": (0.54, -3.28), "goal2": (13.63, -3.37)},
|
||||
"map2": {"split_x": 7.075, "goal1": (0.57, -3.34), "goal2": (13.58, -3.35)},
|
||||
}
|
||||
|
||||
class AimSelector(Node):
|
||||
def __init__(self):
|
||||
def __init__(self, config):
|
||||
super().__init__('aim_selector')
|
||||
self.split_x = config["split_x"]
|
||||
self.goal1_x, self.goal1_y = config["goal1"]
|
||||
self.goal2_x, self.goal2_y = config["goal2"]
|
||||
self.publisher = self.create_publisher(MoveGoal, '/move_goal', 10)
|
||||
self.tf_buffer = tf2_ros.Buffer()
|
||||
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self)
|
||||
@ -19,12 +30,12 @@ class AimSelector(Node):
|
||||
x = trans.transform.translation.x
|
||||
self.get_logger().info(f'base_link x in map: {x:.2f}')
|
||||
msg = MoveGoal()
|
||||
if x < 7:
|
||||
msg.x = 0.61
|
||||
msg.y = 3.995
|
||||
if x < self.split_x:
|
||||
msg.x = self.goal1_x
|
||||
msg.y = self.goal1_y
|
||||
else:
|
||||
msg.x = 13.66
|
||||
msg.y = 3.73
|
||||
msg.x = self.goal2_x
|
||||
msg.y = self.goal2_y
|
||||
msg.angle = 0.0
|
||||
msg.max_speed = 0.0
|
||||
msg.tolerance = 0.1
|
||||
@ -34,8 +45,18 @@ class AimSelector(Node):
|
||||
self.get_logger().warn(f'Failed to get transform: {e}')
|
||||
|
||||
def main(args=None):
|
||||
# 读取命令行参数
|
||||
if len(sys.argv) < 2:
|
||||
print("用法: python slect.py [map|map1|map2]")
|
||||
sys.exit(1)
|
||||
map_key = sys.argv[1]
|
||||
if map_key not in MAP_CONFIGS:
|
||||
print(f"未知参数: {map_key}")
|
||||
sys.exit(1)
|
||||
config = MAP_CONFIGS[map_key]
|
||||
|
||||
rclpy.init(args=args)
|
||||
node = AimSelector()
|
||||
node = AimSelector(config)
|
||||
rclpy.spin(node)
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
@ -1,3 +1,3 @@
|
||||
base_link2livox_frame:
|
||||
xyz: "\"0.246 -0.143 0.397\""
|
||||
xyz: "\"0.246 -0.135 0.397\""
|
||||
rpy: "\"0.0 0.0 0.0\""
|
||||
@ -35,7 +35,7 @@ namespace rm_simpal_move
|
||||
: Node("global_position_listener", options), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_), running_(true), goal_reached_(false)
|
||||
{
|
||||
tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
|
||||
timer_ = this->create_wall_timer(std::chrono::milliseconds(10), std::bind(&RMSimpleMove::timer_callback, this));
|
||||
timer_ = this->create_wall_timer(std::chrono::milliseconds(100), std::bind(&RMSimpleMove::timer_callback, this));
|
||||
goal_pose_sub_ = this->create_subscription<rm_msgs::msg::MoveGoal>("/move_goal", 10, std::bind(&RMSimpleMove::goal_pose_callback, this, std::placeholders::_1));
|
||||
data_ai_pub_ = this->create_publisher<rm_msgs::msg::DataAI>("/chassis/data_ai", 10);
|
||||
data_nav_pub_ = this->create_publisher<rm_msgs::msg::DataNav>("/chassis/data_nav", 10);
|
||||
@ -85,27 +85,19 @@ namespace rm_simpal_move
|
||||
|
||||
// 计算当前位置到goal_pose和base_link的坐标变换
|
||||
geometry_msgs::msg::TransformStamped goal_in_base_link;
|
||||
geometry_msgs::msg::TransformStamped goal_in_R2;
|
||||
goal_in_base_link = tf_buffer_.lookupTransform("base_link", "goal_pose", tf2::TimePointZero);
|
||||
// goal_in_R2 = tf_buffer_.lookupTransform("base_link", "R2", tf2::TimePointZero);
|
||||
|
||||
float goal_x_in_base_link = goal_in_base_link.transform.translation.x;
|
||||
float goal_y_in_base_link = goal_in_base_link.transform.translation.y;
|
||||
|
||||
// float goal_x_in_R2 = goal_in_R2.transform.translation.x;
|
||||
// float goal_y_in_R2 = goal_in_R2.transform.translation.y;
|
||||
|
||||
// 计算瞄准目标的角度差和距离
|
||||
float aim_yaw = atan2f(goal_y_in_base_link, goal_x_in_base_link);
|
||||
float aim_distance = sqrtf(goal_x_in_base_link * goal_x_in_base_link + goal_y_in_base_link * goal_y_in_base_link);
|
||||
// float aim_yaw_r2 = atan2f(goal_y_in_R2, goal_x_in_R2);
|
||||
// float aim_distance_r2 = sqrtf(goal_x_in_R2 * goal_x_in_R2 + goal_y_in_R2 * goal_y_in_R2);
|
||||
|
||||
|
||||
// 发布 DataAim 消息
|
||||
auto data_aim_msg = rm_msgs::msg::DataAim();
|
||||
data_aim_msg.yaw = aim_yaw; // x方向瞄准目标的偏差角度
|
||||
data_aim_msg.distance = aim_distance; // 到目标的距离
|
||||
|
||||
data_aim_pub_->publish(data_aim_msg);
|
||||
|
||||
// 发布 DataNav 消息
|
||||
|
||||
Loading…
Reference in New Issue
Block a user