在TROS.A中跑导远ins570d

在TROS.A中跑导远ins570d

前段时间一直在忙，没空折腾天准这边的域控制器。短短两周过去，调试域控的记忆如同过眼云烟。于是写此博客，记录我调试导远ins570d以及使用TROS.A进行包装的过程

INS570D

U1S1，这款惯导的外观和天准的域控真的很配

CAN没有数据

天准域控这边的CAN这边居然没有内置120欧电阻，导致CAN的数据读不出来。只好自己并联一个上去了。。。

启动can&&can测试

/app/bin/ip link set can0 up type can bitrate 500000
/app/bin/candump can0

Publisher

#include <iostream>
#include <string>
#include <unistd.h>
#include <net/if.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <linux/can.h>
#include <linux/can/raw.h>
#include <sys/ioctl.h>
#include <cstring>

#include <iostream>
#include <thread>
#include "../protocol_msg1/imu_Message.pb.h"
#include "communication/publisher.h"

using hobot::communication::CommAttr;
using hobot::communication::ParticipantAttr;
using hobot::communication::Publisher;
using hobot::communication::PROTOCOL_ZMQ_TCP;

using ImuProtoMsg =
  hobot::message::ProtoMsg<simple_proto::ImuMessage>;
using ImuProtoSerializer =
  hobot::message::ProtobufSerializer<simple_proto::ImuMessage>;

int main() {
    // 创建套接字
    float latitude;
    float longidude;
    float altitude;
    uint32_t a;
    uint32_t b = 0x500;
    uint32_t c;
    float pitch;
    float yaw;
    float roll;
    int socket_fd = socket(PF_CAN, SOCK_RAW, CAN_RAW);
    if (socket_fd < 0) {
        perror("Error while opening socket");
        return 1;
    }

    // 绑定套接字到CAN接口
    struct sockaddr_can addr;
    struct ifreq ifr;
    std::string can_interface = "can0"; // 你的CAN接口名称，例如"can0"或"vcan0"
    strcpy(ifr.ifr_name, can_interface.c_str());
    if (ioctl(socket_fd, SIOCGIFINDEX, &ifr) < 0) {
        perror("Error while getting interface index");
        return 1;
    }
    addr.can_family = AF_CAN;
    addr.can_ifindex = ifr.ifr_ifindex;
    if (bind(socket_fd, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
        perror("Error while binding socket to interface");
        return 1;
    }

    hobot::communication::Init("communication.json");
    CommAttr comm_attr;
    comm_attr.participant_attrs_.push_back(ParticipantAttr{2});
    std::string topic = "imu_topic";
    auto publisher  =
        Publisher<ImuProtoSerializer>::New(
            comm_attr, topic, 0, PROTOCOL_ZMQ_TCP);
    
    auto imuMsg = std::make_shared<ImuProtoMsg>();

    // 接收CAN消息
    struct can_frame frame;
    while (true) {
        ssize_t nbytes = read(socket_fd, &frame, sizeof(struct can_frame));
        if (nbytes < 0) {
            perror("Error while reading CAN frame");
            break;
        }

        // 提取CAN ID和数据
        uint32_t can_id = frame.can_id;
        uint8_t data[11];
        memcpy(data, frame.data, 11);
        c = can_id % b;
        switch(c){
        	case 1:
        		break;
        	case 2:
        		a = data[0]*256+data[1];
        		pitch = a*0.010986-360;
        		a = data[2]*256+data[3];
        		roll = a*0.010986-360;
        		a = data[4]*256+data[5];
        		yaw = a*0.010986-360;
        		imuMsg->proto.set_roll(roll);
                imuMsg->proto.set_pitch(pitch);
                imuMsg->proto.set_yaw(yaw);
        		break;
        	case 3:
        		a = data[3]+data[2]*256+data[1]*65536+data[0]*16777216;
        		altitude = a*0.001-10000;
                imuMsg->proto.set_altitude(altitude);
        		break;
        	case 4:
        		a = data[3]+data[2]*256+data[1]*65536+data[0]*16777216;
        		latitude = a*1e-07-180;
        		a = data[7]+data[6]*256+data[5]*65536+data[4]*16777216;
        		longidude = a*1e-07-180;
                imuMsg->proto.set_latitude(latitude);
                imuMsg->proto.set_longitude(longidude);
                publisher->Pub(imuMsg);
        		break;
        	default:
        		break;
        }
    }
    // 关闭套接字
    close(socket_fd);
    return 0;
}

Subscriber

#include <iostream>
#include <thread>
#include <chrono>
#include "../protocol_msg1/imu_Message.pb.h"
#include "communication/subscriber.h"


using hobot::communication::CommAttr;
using hobot::communication::ParticipantAttr;
using hobot::communication::Subscriber;
using hobot::communication::PROTOCOL_ZMQ_TCP;
using hobot::communication::PROTOCOL_COMPOSITE;
using hobot::communication::ProtoMsg;
using hobot::communication::ProtobufSerializer;

using ImuProtoMsg = hobot::message::ProtoMsg<simple_proto::ImuMessage>;
using ImuProtoSerializer = hobot::message::ProtobufSerializer<simple_proto::ImuMessage>;



static void IMUSubCallback(
    const std::shared_ptr<ImuProtoMsg> &msg) {
    float roll = msg->proto.roll();
    float pitch = msg->proto.pitch();
    float yaw = msg->proto.yaw();
    float latitude = msg->proto.latitude();
    float longitude = msg->proto.longitude();
    float altitude = msg->proto.altitude();
    std::cout << "received imu msg info: \n "
        << "roll=" << roll
        << "; pitch=" << pitch
        << "; yaw=" << yaw
        << "; latitude=" << latitude
        << "; longitude=" << longitude
        << "; altitude=" << altitude << ";";
  
}

int main() {

    hobot::communication::Init("communication.json");
    CommAttr comm_attr;
    comm_attr.participant_attrs_.push_back(ParticipantAttr{2});
    std::string topic = "imu_topic";
    std::shared_ptr<Subscriber<ImuProtoSerializer>> subscriber;

    subscriber = Subscriber<ImuProtoSerializer>::New(
        comm_attr, topic, 0, IMUSubCallback, PROTOCOL_ZMQ_TCP);
        
    while (1) {
        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
        std::cout << "subscriber is running ..." << std::endl;
    }

    return 0;
}

如何编译

1. 来到TZTEK_TROS_R3.0_v1.2.0_release/samples/component_samples/communication_ex/example/pub_sub_example/CMakeLists.txt下，照猫画虎添加要编译的cpp文件
2. 在上上级目录里python build.py --config j5_conan
3. 生成的可执行文件位于TROS/TZTEK_TROS_R3.0_v1.2.0_release/samples/component_samples/commu
   nication_ex/example/build/build_subfolder/bin中

效果展示

文件目录如下

pub&&sub