系统：Ubuntu20.04

ROS版本：ROS noetic

VTD版本：VTD 2021.3

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

一、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-(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，数据结构一致。

       交通参与者信息如上图所示，主要是包括了物体的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 );}elsebConnected = 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("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: // SOFfprintf( stderr, "void parseRDBMessageEntry: got start of frame\n" );break;case RDB_PKG_ID_END_OF_FRAME: // EOFfprintf( 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节点，运行仿真，就能在终端中看到打印的信息。

本文来自互联网用户投稿，文章观点仅代表作者本人，不代表本站立场，不承担相关法律责任。如若转载，请注明出处。 如若内容造成侵权/违法违规/事实不符，请点击【内容举报】进行投诉反馈！