[ROS] CAN communication 통신 data parsing 비트연산

[ROS] CAN communication 통신 data parsing

 

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;
    } 

msg_id=0x520인 경우

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

startbit가 0이므로 

            MSB                                            LSB

            0000  0000  0000  0000  0000  0000

index   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자리에 삽입한다는 뜻이다.

 

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 1 1 이남는다.

MSB    LSB

 0 1 0 1  

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

 

 

★ 읽으셨던 글이 도움이 되셨다면 아래(▼) ♡ 공감 버튼 눌러주시면 감사하겠습니다! (로그인 필요 없습니다~)★