VTD_IO-ROS(2)—发送驾驶员控制信息

系统： Ubuntu20.04
ROS版本：ROS noetic
VTD版本： VTD 2021.3
本文将与大家分享如何把基于ROS开发的算法得到的驾驶员控制指令发到VTD中的自车，从而起到测试算法的作用。

一、代码解析

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_DRIVER_CTRL_t & item );
void sendOwnDriverCtl( int & sendSocket, const double & simTime, const unsigned int & simFrame );

这里主要声明的函数与上一篇文章基本类似，较为不同的是增加了sendOwnDriverCtl函数，作用是将订阅到的驾驶员指令通过TCP发到VTD。

2.回调函数

void VehicleCmdCallback(const common_msgs::Vehicle_Control& msg)
{
    sOwnDriverCtl.playerId       = 1; // 自车id
    sOwnDriverCtl.throttlePedal  = msg.ThrottlePedal; // 油门踏板 
    sOwnDriverCtl.steeringWheel  = msg.SteeringAngle*M_PI/180; // 方向盘转角 
    sOwnDriverCtl.brakePedal     = msg.BrakePedal;    // 制动踏板 
    sOwnDriverCtl.gear           = 4;     // 挡位

回调函数的作用是订阅由算法计算得到的油门踏板开度、制动踏板开度、方向盘转角等控制信息。VTD中自车的id默认为1，挡位设置为4的原因是在VTD中4代表D挡。需要注意的是，VTD接收的踏板开度为0-1，如果计算得到的踏板开度为0-100的范围，那么在这里需要除以100，这里默认得到的踏板开度为0-1范围。VTD中方向盘转角的单位为弧度，如果计算的方向盘是角度制，则需要转换为弧度制。

3.sendOwnDriverCtl函数

void sendOwnDriverCtl( int & sendSocket, const double & simTime, const unsigned int & simFrame )
{
    Framework::RDBHandler myHandler;
    myHandler.initMsg();
    RDB_DRIVER_CTRL_t *drvCtlState = ( RDB_DRIVER_CTRL_t* ) myHandler.addPackage( simTime, simFrame, RDB_PKG_ID_DRIVER_CTRL );
    if ( !drvCtlState )
    {
        fprintf( stderr, "sendOwnDriverCtl: could not create driver state\n" );
        return;
    }
    memcpy( drvCtlState, &sOwnDriverCtl, sizeof( RDB_DRIVER_CTRL_t ) );
    int retVal = send( sendSocket, ( const char* ) ( myHandler.getMsg() ), myHandler.getMsgTotalSize(), 0 );
    if ( !retVal )
        fprintf( stderr, "sendOwnDriverCtl: could not send object state\n" );
}

sendOwnDriverCtl函数实现主要有四步：
1.初始化一个消息；
2.将pkg 9—RDB_DRIVER_CTRL_推入该消息，并定义为该类型的指针。
3.将订阅得到的控制对象消息复制到drvCtlState指向的内存块。
4.发送到TaskControl。
在函数定义后，就可以在parseRDBMessageEntry函数中调用该函数，使控制消息不断地发送到VTD。

二、运行效果

启动VTD，然后启动launch文件，运行仿真，就能在终端中看到打印的信息。