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文件,运行仿真,就能在终端中看到打印的信息。
在这里插入图片描述

  • 3
    点赞
  • 12
    收藏
    觉得还不错? 一键收藏
  • 2
    评论
评论 2
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值