[ROS] CAN communication 통신 data parsing

pc 환경 : Ubuntu18.04 melodic, ROS1

Ubuntu에서 수신받은 CAN Raw Data를 Start bit 와 Length 그리고 factor와 offset을 고려하여 data를 parsing하는 방법에 대해 설명하려고 한다.
우선 CAN gateway로 부터 CAN Raw Data를 받았다고 가정하고 진행하도록 하겠다. bitset을 사용해서 비트연산을 통해 계산하는 예제이다.
이해하기 쉽게하기 위해 복잡한 내용은 생략하고 기본적인 base code를 가지고 설명을 진행하겠다.

이 코드를 토대로 각자 상황에 맞는 코드를 구성하고,사용자 정의 메시지로 각 Signal에 맞도록 변수명을 구성하는게 나중에 한눈에 어떤 신호인지 파악하기 쉬울 것이다.

 #include <bitset>
#include <ros/ros.h>
#include <iostream>
#include <can_msgs/Frame.h>
#include <std_msgs/Float32.h>


class CAN_PARSER
{
    public:
        CAN_PARSER(ros::NodeHandle &n);
        ~CAN_PARSER();

        void CanCallback(const can_msgs::Frame &msg);

    private:
        ros::NodeHandle nh;
        ros::Subscriber raw_can_sub;
        ros::Publisher parsed_data_pub;
        std_msgs::Float32 can_signalname1;
        std_msgs::Float32 can_signalname2;
};

CAN_PARSER::CAN_PARSER(ros::Nodehandle &n)
{
    ROS_INFO("CAN_PARSER is created");
    nh = n;
    raw_can_sub = nh.subscribe("/received_messages" 10, &CAN_PARSER::CanCallback, this);
    parsed_data_pub = nh.advertise<std_msgs::Float32>("/parsed_can_msg", 5);
}
CAN_PARSER::~CAN_PARSER()
{
    ROS_INFO("CAN_PASER in destructed");
}

void CAN_PARSER::CanCallback(const can_msgs::Frame &msg)
{
    if (msg.id == 0x520)
    {
    	//star bit가 0이고 signal length가 24일 때 
        bitset<24> temp = bitset<24>(msg.data[2]) << 16;
        temp = temp | bitset<24>(msg.data[1]) << 8;
        temp = temp | bitset<24>(msg.data[0]);
        double data = (double)(temp.to_ulong());
        data = data * 0.1; //여기서 offset은 0이고 factor는 0.1이다 
        can_signalname1.data = data;
        parsed_data_pub.publish(can_signalname1);
    }
    else if (msg.id == 0x170)
    {
    	//star bit가 11이고 signal length가 4일 때 
        bitset<7> temp1 = bitset<7>(msg.data[1]) >> 3;
        double long data1 = (double)(temp1.to_ulong());
        can_signalname2.data = data; ////여기서 offset과 factor는 0이다 
        parsed_data_pub.publish(can_signalname2);
    }

    int main (int argc, char ** argv)
    {
        ros::init(argc,argv,"CAN_PARSER")
        ros::NodeHandle _nh;
        printf("Initiate CAN_PARSER\n");

        CAN_PARSER parser(_nh);
        ros::spin();

        printf("CAN_PARSER is Terminated\n");
        return 0;
    } 

1. msg_id=0x520인 경우

bitset <24>를 선언한이유는 signal length가 24이기 때문이다. msg.data[i]에는 8비트의 data가 저장된다.
startbit가 0이므로

MSB                            LSB
0000  0000  0000  0000  0000  0000
23 24 ...........................0  index는 다음과 같이 부여된다.

만약에 msg.data[0]= 0000 1001, msg.data[1]= 0000 0011, msg.data[2]=0000 0000 이 었다면

MSB                            LSB
0000  0000  0000  0001  0000  1001

십진수로 변환하면 265가 된다.
왜냐하면 msg.data[0]은 0000 1001 그대로 오른쪽 부터 들어가고
msg.data[1]은<< 8의 의미는 왼쪽으로 8칸 밀어서 인덱스 15~8 자리에 삽입한다라는 뜻이다.
msg.data[2]은<< 16 의 의미는 왼쪽으로 16칸 밀어서 인덱스 23~16자리에 삽입한다는 뜻이다.

2. msg_id=0x170인 경우

bitset <8>를 선언한이유는 star bit가 11이고 signal length가 4이기 때문이다. msg.data[i]에는 8비트의 data가 저장된다.startbit가 11이므로

                  MSB           LSB

                  0 0 0 0   0 0 0 0
msg.data[1]       0 0 0 0   0 0 0 0    
index           15 14 13 12 11 10 9 8  

start bit가 11이므로 11,12,13,14 index에 있는 data를 필요로 한다.

bitset<7> temp1 = bitset<7>(msg.data[1]) >> 3;

위 코드의 의미는 7개의 data를 가져와서 3개의 비트를 버린다는 것이다.

만약 msg.data[1]이 아래와 같았다면

msg.data[1]       1  0  1  1   1  0  0  0    
index             15 14 13 12   11 10 9  8  

즉 7개의 데이터 0 1 1 1 0 0 0 을 가져와서 오른쪽으로 3칸 밀어버니니깐 결국 0 1 1 1 이남는다.

MSB    LSB
0  1  1  1  

십진수로 변환하면 7이된다.

만약 msg.data[1]이 아래와 같았다면

msg.data[1]       1  0  1  0    1  0   0  0    
index            15 14 13 12    11 10  9   8  

즉 7개의 데이터 0 1 0 1 0 0 0 을 가져와서 오른쪽으로 3칸 밀어버니니깐 결국 0 1 0 1 이남는다.

MSB    LSB
0  1  0  1  

십진수로 변환하면 5가 된다.

출처 : https://romillion.tistory.com/101