VTD_IO-ROS(1)—读取交通参与者信息

1 篇文章 0 订阅

系统:Ubuntu20.04

ROS版本:ROS noetic

VTD版本:VTD 2021.3

       本文根据VTD天舒的视频及代码,与大家分享如何通过ROS读取VTD中交通参与者信息,后续还会分享如何将ROS的消息再发送到VTD的过程。目的是让使用ROS的用户可以把自己的算法和VTD连接起来,起到测试的作用

一、VTD与ROS的通讯架构

VTD与ROS通讯架构
VTD与ROS通讯架构

       VTD在跑仿真时,会有TC链接和RDB消息等。普通的RDB消息是走图中红色Nerwork的路,图片和点云是走灰色SHM的路,但本质上这些都属于RDB结构。那么gateway的本质是翻译RDB消息到ROS中的msg,是ROS下面的一个package.它的作用有以下三点:1.可以通过本机的RDB网络读取VTD中的信息;2.可以在ROS中通过话题的发布者发布消息。这样,ROS里的另一个进程-话题订阅者可以得到传过来的交通参与者信息(RDB_OBJECT_STATE).。之后把这个信息用到ROS里面,就可以服务于算法。然后再用rviz,就可以显示出来。

       目前,这个程序是单向通讯的状态,仅仅是从VTD中读取信息,对于双向通讯的过程会在之后进行分享。   

二、RDB介绍 

                                

RDB结构框架

       RDB-(Runtime Data Bus)是VTD各个模块通过TC和外部进行通信的数据格式之一,其本身就是一个数据结构,每一个仿真帧都会进行RDB通信。RDB架构如上图所示,它是一个三层结构。RDB可以通过TCP/UDP/SHM三种方式在进程间通信,传递数值信息和图片、点云信息等,目前比较常用的是TCP,通过48190端口收发消息。

  1. RDB_MSG_t 即完整的RDB消息,包含Msg头和Msg体;
  2. RDB_MSG_HDR_t 消息头,包含simTime和size信息;
  3. RDB_MSG_ENTRY_HDR_t 包头 ,包括Pkg_Id和size信息。
  4. RDB_XX_t 包体,它可以有若干个body,数据结构一致。
RDB_OBJECT_STATE_t,pkgId=9

       交通参与者信息如上图所示,主要是包括了物体的Id、几何信息、位置信息、速度、加速度和里程信息,具体含义可参考VTD中自带的RDB文件查询。

三、代码解析

1.主要函数声明及对象初始化

void parseRDBMessage( RDB_MSG_t* msg, bool & isImage );
void parseRDBMessageEntry( const double & simTime, const unsigned int & simFrame, RDB_MSG_ENTRY_HDR_t* entryHdr );
void handleRDBitem( const double & simTime, const unsigned int & simFrame, RDB_OBJECT_STATE_t & item, bool isExtended );
geometry_msgs::PoseStamped msgEgoPose;
  • parseRDBMessage函数主要是解析RDB_MSG_HDR
  • parseRDBMessageEntry函数主要是解析RDB_MSG_ENTRY
  • handleRDBitem函数主要是解析RDB_OBJECT_STATE_t,对应着RDB框架图中紫色的部分
  • geometry_msgs/PoseStamped msgEgoPose定义一个叫msgEgoPose的对象,该对象可以使用下图中的header、pose两个数据成员。

 2.main函数

int  sClient;
char* szBuffer = new char[DEFAULT_BUFFER];
int  ret;

struct sockaddr_in server;
struct hostent *host = NULL;

static bool sSendData = false;
static bool sVerbose  = true;  // 改成false,就不会显示所有显示所有信息,只会看到和OBJECT_STATE相关的信息
static bool sSendTrigger = false;


ValidateArgs(argc, argv);
    

sClient = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
    
if ( sClient == -1 )
{
    fprintf( stderr, "socket() failed: %s\n", strerror( errno ) );
    return 1;
}
    
int opt = 1;
setsockopt ( sClient, IPPROTO_TCP, TCP_NODELAY, &opt, sizeof( opt ) );

server.sin_family      = AF_INET;
server.sin_port        = htons(iPort);
server.sin_addr.s_addr = inet_addr(szServer);
    
if ( server.sin_addr.s_addr == INADDR_NONE )
{
     host = gethostbyname(szServer);
     if ( host == NULL )
     {
            fprintf( stderr, "Unable to resolve server: %s\n", szServer );
            return 1;
     }
     memcpy( &server.sin_addr, host->h_addr_list[0], host->h_length );
}

bool bConnected = false;

while ( !bConnected )
{
        if (connect( sClient, (struct sockaddr *)&server, sizeof( server ) ) == -1 )
        {
            fprintf( stderr, "connect() failed: %s\n", strerror( errno ) );
            sleep( 1 );
        }
        else
            bConnected = true;
}

fprintf( stderr, "connected!\n" );
    
unsigned int  bytesInBuffer = 0;
size_t        bufferSize    = sizeof( RDB_MSG_HDR_t );
unsigned int  count         = 0;
unsigned char *pData        = ( unsigned char* ) calloc( 1, bufferSize );

       这部分是计算机网络的相关内容。就是通过socket建立了TCP的连接,能够得到TaskControl发出的buffer。

    ros::init(argc, argv, "vtd_gateway");

    ros::NodeHandle n;

    ros::Publisher publisher = n.advertise<geometry_msgs::PoseStamped>("ego_pose", 1000);

    while (ros::ok())
    {
        bool bMsgComplete = false;

        while ( !bMsgComplete )
        {
            ret = recv( sClient, szBuffer, DEFAULT_BUFFER, 0 );

            if ( ret == -1 )
            {
                printf( "recv() failed: %s\n", strerror( errno ) );
                break;
            }

            if ( ret != 0 )
            {
                
                if ( ( bytesInBuffer + ret ) > bufferSize )
                {
                    pData      = ( unsigned char* ) realloc( pData, bytesInBuffer + ret );
                    bufferSize = bytesInBuffer + ret;
                }

                memcpy( pData + bytesInBuffer, szBuffer, ret );
                bytesInBuffer += ret;
   
                if ( bytesInBuffer >= sizeof( RDB_MSG_HDR_t ) )
                {
                    RDB_MSG_HDR_t* hdr = ( RDB_MSG_HDR_t* ) pData;

                    
                    if ( hdr->magicNo != RDB_MAGIC_NO )
                    {
                        printf( "message receiving is out of sync; discarding data" );
                        bytesInBuffer = 0;
                    }

                    while ( bytesInBuffer >= ( hdr->headerSize + hdr->dataSize ) )
                    {
                        unsigned int msgSize = hdr->headerSize + hdr->dataSize;
                        bool         isImage = false;

                       
                        if ( sVerbose )
                            Framework::RDBHandler::printMessage( ( RDB_MSG_t* ) pData, true );      
                        parseRDBMessage( ( RDB_MSG_t* ) pData, isImage );
                        memmove( pData, pData + msgSize, bytesInBuffer - msgSize );
                        bytesInBuffer -= msgSize;
                        bMsgComplete = true;
                    }
                }
            }
        }

        publisher.publish(msgEgoPose);
        ros::spinOnce();

       这部分是对ROS进行初始化,然后得到TaskControl发出来的buffer后解buffer。然后将buffer按msg头转换类型,解析msg头。

3.parseRDBMessage函数

void parseRDBMessage( RDB_MSG_t* msg, bool & isImage )
{
    if ( !msg )
      return;

    if ( !msg->hdr.dataSize )
        return;
    //将指针从MSG_HDR_t移位到MSG_ENTRY_HDR_t(PKG_HDR)
    RDB_MSG_ENTRY_HDR_t* entry = ( RDB_MSG_ENTRY_HDR_t* ) ( ( ( char* ) msg ) + msg->hdr.headerSize );
    uint32_t remainingBytes    = msg->hdr.dataSize;
        
    while ( remainingBytes )
    {
        parseRDBMessageEntry( msg->hdr.simTime, msg->hdr.frameNo, entry ); // 解析蓝色和绿色的部分

        isImage |= ( entry->pkgId == RDB_PKG_ID_IMAGE );

        remainingBytes -= ( entry->headerSize + entry->dataSize );
        
        if ( remainingBytes ) // 如果还有entry,继续解析后面的entry,直到最后一个entry——EOF 
          entry = ( RDB_MSG_ENTRY_HDR_t* ) ( ( ( ( char* ) entry ) + entry->headerSize + entry->dataSize ) );
    }
}

       这部分是解析RDB_MSG_HDR,就是RDB框架图中褐色的部分。然后通过指针移位到MSG_ENTRY_HDR_t,即PKG_HDR。当解析到MSG_HDR的大小不为0时,再去解析RDB框架图中绿色和蓝色的部分,直到解析到结束帧-EOF。

4.parseRDBMessageEntry函数

void parseRDBMessageEntry( const double & simTime, const unsigned int & simFrame, RDB_MSG_ENTRY_HDR_t* entryHdr )
{
    if ( !entryHdr )
        return;
    
    int noElements = entryHdr->elementSize ? ( entryHdr->dataSize / entryHdr->elementSize ) : 0;
    
    if ( !noElements )  // some elements require special treatment
    {
        switch ( entryHdr->pkgId ) // 处理绿色的部分
        {
            case RDB_PKG_ID_START_OF_FRAME: // SOF
                fprintf( stderr, "void parseRDBMessageEntry: got start of frame\n" );
                break;
                
            case RDB_PKG_ID_END_OF_FRAME: // EOF
                fprintf( stderr, "void parseRDBMessageEntry: got end of frame\n" );
                break;
                
            default:
                return;
                break;
        }
        return;
    }

    unsigned char ident   = 6;
    char*         dataPtr = ( char* ) entryHdr;
        
    dataPtr += entryHdr->headerSize;
    // 处理蓝色的部分    
    while ( noElements-- )
    {
        bool printedMsg = true;
            
        switch ( entryHdr->pkgId )
        {
                   
            case RDB_PKG_ID_OBJECT_STATE:
                handleRDBitem( simTime, simFrame, *( ( RDB_OBJECT_STATE_t* ) dataPtr ), entryHdr->flags & RDB_PKG_FLAG_EXTENDED );
                break;                                        
            default:
                printedMsg = false;
                break;
        }
        dataPtr += entryHdr->elementSize;
     }
}

       这部分是解析RDB_MSG_ENTRY,首先处理绿色的部分-起始帧和终止帧,然后再处理蓝色的部分-PKD_ID。此时swicth-case函数中的PKD_ID只包括了RDB_PKG_ID_OBJECT_STATE,它在VTD中的ID号为9。如果还要解析其他的数据,比如图像、驾驶员指令等,则需要增加相应的case。

5.handleRDBitem函数

void handleRDBitem( const double & simTime, const unsigned int & /*simFrame*/, RDB_OBJECT_STATE_t & item, bool /*isExtended*/ )
{
    if(item.base.id != 1)
        return;

    msgEgoPose.header.seq++;
    msgEgoPose.header.stamp.sec = simTime;
    msgEgoPose.header.stamp.nsec = (simTime - msgEgoPose.header.stamp.sec)*1000000000;

    msgEgoPose.header.frame_id = "base_link";

    msgEgoPose.pose.position.x = item.base.pos.x;
    msgEgoPose.pose.position.y = item.base.pos.y;
    msgEgoPose.pose.position.z = item.base.pos.z;

    tf::Quaternion q = tf::createQuaternionFromRPY(item.base.pos.r, item.base.pos.p, item.base.pos.h);

    msgEgoPose.pose.orientation.x = q.x();
    msgEgoPose.pose.orientation.y = q.y();
    msgEgoPose.pose.orientation.z = q.z();
    msgEgoPose.pose.orientation.w = q.w();

    if(msgEgoPose.header.seq % 100 == 0) {
        fprintf(stderr, "xyz[%6.3f;%6.3f;%6.3f] hpr[%6.3f;%6.3f;%6.3f] q[%6.3f;%6.3f;%6.3f;%6.3f] stamp[%d.%d]\n",
                item.base.pos.x,
                item.base.pos.y,
                item.base.pos.z,
                item.base.pos.h * 180.0/M_PI,
                item.base.pos.p * 180.0/M_PI,
                item.base.pos.r * 180.0/M_PI,
                msgEgoPose.pose.orientation.x,
                msgEgoPose.pose.orientation.y,
                msgEgoPose.pose.orientation.z,
                msgEgoPose.pose.orientation.w,
                msgEgoPose.header.stamp.sec,
                msgEgoPose.header.stamp.nsec
                );
    }
}

      这部分是解析RDB_OBJECT_STATE_t,即图中紫色的部分,PKD中的详细信息将会打印出来,包括交通参与者的几何信息和位置信息等。

四、运行效果

      启动VTD,然后启动该ROS节点,运行仿真,就能在终端中看到打印的信息。

 

  • 9
    点赞
  • 20
    收藏
    觉得还不错? 一键收藏
  • 5
    评论
评论 5
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值