系统: 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文件,运行仿真,就能在终端中看到打印的信息。