系统：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端口收发消息。
- RDB_MSG_t 即完整的RDB消息，包含Msg头和Msg体；
- RDB_MSG_HDR_t 消息头，包含simTime和size信息；
- RDB_MSG_ENTRY_HDR_t 包头 ，包括Pkg_Id和size信息。
- 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 );
}
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节点，运行仿真，就能在终端中看到打印的信息。
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
