ROS集成并解包网口传输的GPS和IMU信息并控制云台相机

呵呵呵呵呵呵呵哈

已于 2025-03-12 21:09:58 修改

阅读量299

点赞数 6

文章标签：机器人

于 2025-03-12 21:08:43 首次发布

本文链接：https://blog.youkuaiyun.com/u011549111/article/details/145042125

版权

整个项目在这里https://github.com/li01233/mul_modal_ROS

一 GPS

我又来了，这次需要在采集数据的系统中加入GPS和IMU，我们这两个东西都是依靠网口传输的，所以一些通用的串口方法不适用。因此这里我使用了ros_nmea_driver包

GitHub - ros-drivers/nmea_navsat_driver: ROS package containing drivers for NMEA devices that can output satellite navigation data (e.g. GPS or GLONASS).

编写网口传输的launch文件，这个包会将网口中的NMEA语句转为ROS格式。这里我的网口设置为127.0.0.1是因为我开了opencpn转接数据，正常来说直接填你的IP即可

<launch>
 
  <!-- A simple launch file for the nmea_serial_driver node. -->
 
  <arg name="ip" default="127.0.0.1" />
  <arg name="port_gps" default="20003" />
  <arg name="port_compass" default="20001" />

  <arg name="frame_id_1" default="gps" />
  <arg name="frame_id_3" default="compass" />

  <arg name="use_GNSS_time" default="False" />
  <arg name="time_ref_source" default="gps" />
  <arg name="useRMC" default="False" />
 
  <node name="nmea_socket_driver_node_gps" pkg="nmea_navsat_driver" type="nmea_socket_driver" output="screen">
    <param name="ip" value="$(arg ip)"/>
    <param name="port" value="$(arg port_gps)" />
    <param name="frame_id" value="$(arg frame_id_1)" />
    <param name="use_GNSS_time" value="$(arg use_GNSS_time)" />
    <param name="time_ref_source" value="$(arg time_ref_source)" />
    <param name="useRMC" value="$(arg useRMC)" />
  </node>

  <node name="nmea_socket_driver_node_compass" pkg="nmea_navsat_driver" type="nmea_socket_driver" output="screen">
    <param name="ip" value="$(arg ip)"/>
    <param name="port" value="$(arg port_compass)" />
    <param name="frame_id" value="$(arg frame_id_3)" />
    <param name="use_GNSS_time" value="$(arg use_GNSS_time)" />
    <param name="time_ref_source" value="$(arg time_ref_source)" />
    <param name="useRMC" value="$(arg useRMC)" />
  </node>
 
</launch>

二 IMU

IMU信息是特殊的格式，用网口传输，其定义如下

因此我新写了一个包将IMU的信息从网口直接读取为uint8[]数组（留待后续解析）

#! /usr/bin/env python3
import rospy
import socket
import struct
from pyais import decode
from message_interface.msg import Imu

class IMUParser:
    def __init__(self):
        # 创建UDP套接字
        self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)

        # 读取param
        self.ip = rospy.get_param('~ip', '0.0.0.1')
        self.port = rospy.get_param('~port', 30002)
        # 绑定本地地址和端口
        local_address = (self.ip, self.port)
        self.sock.bind(local_address)
        
        self.pub = rospy.Publisher("/imu", Imu, queue_size=10)
        self.timer = rospy.Timer(rospy.Duration(0.1), self.run)


    def run(self, event):
        data, _ = self.sock.recvfrom(1024)
        msg = Imu()
        msg.header.stamp = rospy.Time.now()
        msg.header.frame_id = "imu"
        msg.data = data
        self.pub.publish(msg)

if __name__ == "__main__":
    rospy.init_node("imu_parser")
    parser = IMUParser()
    rospy.spin()

launch文件为，其中imu的ip是192.168.2.111

<launch>
    <arg name="ip" default="127.0.0.1" />
    <arg name="arpa_port" default="20004" />
    <arg name="ais_port" default="20002" />
    <arg name="imu_port" default="30013" />
    <arg name="imu_ip" default="192.168.2.111" />

    <node name="arpa_parser" pkg = "opencpn2ros" output="screen" type = "arpa_parser.py">
        <param name="port" value="$(arg arpa_port)" />
        <param name="ip" value="$(arg ip)" />
    </node>

    <node name="ais_parser" pkg = "opencpn2ros" output="screen" type = "ais_parser.py">
        <param name="port" value="$(arg ais_port)" />
        <param name="ip" value="$(arg ip)" />
    </node>

    <node name="imu_parser" pkg = "opencpn2ros" output="screen" type = "imu_parser.py">
        <param name="port" value="$(arg imu_port)" />
        <param name="ip" value="$(arg imu_ip)" />
    </node>

</launch>

后续解析报文程序如下，解析后的数据以/imu/data话题发送，类型为sensor_msgs的IMU格式

#!/usr/bin/env python
import rospy
from sensor_msgs.msg import Imu
from message_interface.msg import Imu as CustomImu  # 替换为实际的自定义 Imu 消息包名
from tf.transformations import quaternion_from_euler
import math
import struct

def parse_hex_data(hex_str):
    data = bytes.fromhex(hex_str)
    if len(data) < 107:
        raise ValueError("数据长度不足")
    
    # 校验起始标志
    if data[0] != 0xAA or data[1] != 0x55:
        raise ValueError("无效的起始标志")
    
    result = {}
    
    # 协议解析
    result["协议类型"] = data[2]
    result["设备状态"] = "初始对准状态" if data[3] == 0x40 else "导航状态"
    
    # 航向（4字节无符号）
    heading = struct.unpack('>I', data[4:8])[0]
    result["航向"] = heading * 0.0001
    
    # 纵摇（4字节有符号）
    pitch = struct.unpack('>i', data[8:12])[0]
    result["纵摇"] = pitch * 0.0001
    
    # 横摇（4字节有符号）
    roll = struct.unpack('>i', data[12:16])[0]
    result["横摇"] = roll * 0.0001
    
    # 角速度（2字节有符号）
    result["WH"] = struct.unpack('>h', data[16:18])[0] * 0.0001
    result["WP"] = struct.unpack('>h', data[18:20])[0] * 0.0001
    result["WR"] = struct.unpack('>h', data[20:22])[0] * 0.0001
    
    # DZTime处理
    dztime_bytes = data[22:26]
    if data[3] == 0x40:
        result["对准剩余时间"] = struct.unpack('>I', dztime_bytes)[0]
    else:
        result["DZTime"] = struct.unpack('>i', dztime_bytes)[0]
    
    # 标志位处理
    result["多普勒标志位"] = data[26]
    result["GPS标志位"] = data[27]
    result["算法选择标志位"] = data[28]
    
    # 故障标志解析
    gyro_acc_flag = data[29]
    faults = []
    if gyro_acc_flag & 0x01: faults.append("加速度计X故障")
    if gyro_acc_flag & 0x02: faults.append("加速度计Y故障")
    if gyro_acc_flag & 0x04: faults.append("加速度计Z故障")
    if gyro_acc_flag & 0x10: faults.append("陀螺X故障")
    if gyro_acc_flag & 0x20: faults.append("陀螺Y故障")
    if gyro_acc_flag & 0x40: faults.append("陀螺Z故障")
    if gyro_acc_flag & 0x80: faults.append("温度传感器故障")
    result["元件故障标志位"] = faults
    
    # 其他标志位...
    
    # 校验计算
    checksum1 = sum(data[2:35]) & 0xFF
    result["校验1"] = "通过" if checksum1 == data[35] else "失败"
    
    checksum2 = sum(data[2:107]) & 0xFF
    result["校验2"] = "通过" if checksum2 == data[106] else "失败"
    
    return result

class ImuParser:
    def __init__(self):
        # 初始化ROS节点
        rospy.init_node('imu_parser_node', anonymous=True)
        
        # 创建发布者
        self.pub = rospy.Publisher('/imu/data', Imu, queue_size=10)
        
        # 订阅自定义 Imu 话题
        rospy.Subscriber('/imu', CustomImu, self.callback)

    def callback(self, msg):
        # 将 uint8[] 转换为十六进制字符串
        hex_str = ''.join('{:02x}'.format(byte) for byte in msg.data)
        
        try:
            # 解析十六进制数据
            parsed_data = parse_hex_data(hex_str)
            
            # 创建标准 Imu 消息对象
            imu_msg = Imu()
            
            # 设置消息头
            imu_msg.header.stamp = msg.header.stamp
            imu_msg.header.frame_id = msg.header.frame_id  # 坐标系名称
            
            # 填充角速度（ROS坐标系：x-forward, y-left, z-up）
            imu_msg.angular_velocity.x = parsed_data["WH"]
            imu_msg.angular_velocity.y = parsed_data["WP"]
            imu_msg.angular_velocity.z = parsed_data["WR"]
            
            # 将欧拉角转换为四元数（注意旋转顺序）
            yaw = math.radians(parsed_data["航向"])   # 航向角（绕Z轴）
            pitch = math.radians(parsed_data["纵摇"]) # 俯仰角（绕Y轴）
            roll = math.radians(parsed_data["横摇"])  # 横滚角（绕X轴）
            
            # 注意：quaternion_from_euler使用RPY顺序（roll, pitch, yaw）
            q = quaternion_from_euler(roll, pitch, yaw)
            imu_msg.orientation.x = q[0]
            imu_msg.orientation.y = q[1]
            imu_msg.orientation.z = q[2]
            imu_msg.orientation.w = q[3]
            
            # 设置协方差（如果未知则填-1）
            imu_msg.orientation_covariance = [-1.0]*9
            imu_msg.angular_velocity_covariance = [-1.0]*9
            imu_msg.linear_acceleration_covariance = [-1.0]*9
            
            # 发布消息
            self.pub.publish(imu_msg)
            rospy.loginfo("IMU数据已发布")
        
        except ValueError as e:
            rospy.logerr("数据解析错误: %s", str(e))

if __name__ == '__main__':
    try:
        parser = ImuParser()
        rospy.spin()
    except rospy.ROSInterruptException:
        pass

三云台相机的云台控制

云台相机的控制程序我写进了hik_tem包中，hik_tem包使用了海康的sdk，除了云台控制同时还有获取双光谱相机的温度原始矩阵的程序

云台的核心控制逻辑是订阅一个/ptz_ctrl话题，一旦返回消息则进入回调函数发送指令给相机

void HikCamera::PtzCtrlCallback(const message_interface::PtzCtrl::ConstPtr& msg)
{
    NET_DVR_PTZPOS ptz_pos;
    unsigned int errCode;

    ptz_pos.wAction = 1;
    ptz_pos.wPanPos = dec_to_hex((*msg).Pan * 10);
    ptz_pos.wTiltPos = dec_to_hex((*msg).Tilt * 10);
    
    // 不变焦
    NET_DVR_PTZPOS Pos_judge;
    DWORD tmp = 0;
    NET_DVR_GetDVRConfig(0, NET_DVR_GET_PTZPOS, 0, &Pos_judge, sizeof(NET_DVR_PTZPOS), &tmp);

    ptz_pos.wZoomPos = Pos_judge.wZoomPos;
    if(!NET_DVR_SetDVRConfig(user_id,NET_DVR_SET_PTZPOS,channel,(void*)&ptz_pos,sizeof(NET_DVR_PTZPOS))){
        errCode = NET_DVR_GetLastError();
        ROS_ERROR("******************************************\n");
        ROS_ERROR("errCode: %d\n",errCode);
        ROS_ERROR("******************************************\n");
    }
    
}

同时写了一个用键盘发送/ptz_ctrl命令的程序，也在hik_tem中。这部分写的比较粗糙，也没考虑逻辑，将就用吧

import rospy
from message_interface.msg import RadarTarget,PtzCtrl
from pynput import keyboard
import time

class PTZController:
    def __init__(self, mode= "Manual"):
        self.p = PtzCtrl()
        self.p.Pan = 150
        self.p.Tilt = 0
        self.p.Zoom = 0
        self.mode = mode

        self.pub = rospy.Publisher("/ptz_ctrl", PtzCtrl, queue_size=10)
        self.listener = keyboard.Listener(on_press=self.on_press,on_release=self.on_release)
        self.sub = rospy.Subscriber("/radar_target", RadarTarget, self.target_pub)
        
        self.init_ptz() # 2s初始化云台
        self.listener.start()
        
        self.cruise_flag = False
        if self.mode == "Fix":
            self.cruise_flag = True
        self.timer = rospy.Timer(rospy.Duration(2), self.cruise)

        self.key_pressed_time = 0

    def init_ptz(self):
        start_time = time.time()
        while time.time() - start_time < 2:
            self.pub.publish(self.p)
        rospy.loginfo(f"PTZ Initialized")

    def target_pub(self, msg):
        if self.mode == "Auto":
            # 在这里写处理导航目标检测数据的逻辑
            self.p.Pan = int(msg.pitch) + 110
            self.p.Pan %= 360
            rospy.loginfo("Pan:%s, Tilt:%d, Zoom:%d",self.p.Pan, self.p.Tilt, self.p.Zoom)
            self.pub.publish(self.p)

    def on_press(self, key):
        try:
            # 若是巡航模式或自动，则不响应按键
            if self.mode == "Fix" or self.mode == "Auto":
                return
            # 手动模式下，响应按键
            if key.char == 'a':
                self.p.Pan -= 10
                self.p.Pan %= 360
                rospy.loginfo("Turn Left, Pan: %d", self.p.Pan)
            elif key.char == 'd':
                self.p.Pan += 10
                self.p.Pan %= 360
                rospy.loginfo("Turn Right, Pan: %d", self.p.Pan)
            elif key.char == 'w':
                self.p.Tilt += 5
                self.p.Tilt = 90 if self.p.Tilt > 90 else self.p.Tilt
                rospy.loginfo("Turn Up, Tilt: %d", self.p.Tilt)
            elif key.char == 's':
                self.p.Tilt -= 5
                self.p.Tilt = 0 if self.p.Tilt < -0 else self.p.Tilt
                rospy.loginfo("Turn Down, Tilt: %d", self.p.Tilt)
            self.pub.publish(self.p)

            if key.char == 'f':
                self.key_pressed_time = time.time()
        except AttributeError:
            pass

    def on_release(self, key):
        press_duration = time.time() - self.key_pressed_time
        if key.char == 'f' and press_duration > 1: # 如果是c键，且按下时间超过2秒，则切换模式
            if self.mode == "Manual" or self.mode == "Fix":
                self.mode = "Auto"
                self.cruise_flag = False
            else:
                self.mode = "Manual" # 自动模型会切换为手动模式，需要再短按进入巡航模式
                self.close_subscriber() # 关闭订阅者
                self.cruise_flag = False
        elif key.char == 'f' and press_duration < 2: # 如果是c键，且按下时间小于2秒，则切换手动或巡航模式
            if self.mode == "Manual":
                self.mode = "Fix"
                self.cruise_flag = True
            elif self.mode == "Fix":
                self.mode = "Manual"
                self.cruise_flag = False

    def cruise(self, event):
        while self.cruise_flag:
            self.p.Pan += 10
            self.p.Pan %= 360
            self.pub.publish(self.p)
            rospy.sleep(2)
            rospy.loginfo("Pan:%s, Tilt:%d, Zoom:%d",self.p.Pan, self.p.Tilt, self.p.Zoom)

if __name__ == "__main__":
    rospy.init_node("ptz_ctrl")
    controller = PTZController()
    rospy.spin()

最后大致就这样，三大部分全部做完，如果有什么没提到的细节可以参考我的github文档

总结一下功能：完成激光雷达，全景相机，双光谱相机，GPS，IMU，AIS，导航雷达的数据解析与采集，完成激光雷达和全景相机的时间同步和激光雷达的去Nan值，能对这几种数据进行重播，能将录制的bag进行按每个bag进行解包