|
@@ -14,6 +14,7 @@
|
|
|
#include <arpa/inet.h>
|
|
|
#include <sys/socket.h>
|
|
|
#include <fstream>
|
|
|
+
|
|
|
using namespace std;
|
|
|
|
|
|
namespace robot{
|
|
@@ -23,7 +24,7 @@ std::string Localization::Name() const { return "localization"; }
|
|
|
int Localization::Init() {
|
|
|
LOG(INFO)<<Name()<<" init start ...";
|
|
|
if (!ros::param::get("car_input_mode", car_input_mode))
|
|
|
- car_input_mode = "rtk";
|
|
|
+ car_input_mode = "pci";
|
|
|
LOG(INFO)<< Name()<<": read param car_input_mode = " << car_input_mode;
|
|
|
//TODO dakai
|
|
|
if (car_input_mode == "modbus") {
|
|
@@ -65,6 +66,73 @@ int Localization::Init() {
|
|
|
LOG(ERROR)<<Name()<< " Unable to Init " << e.what();
|
|
|
return ROBOT_FAILTURE;
|
|
|
}
|
|
|
+ }
|
|
|
+ else if(car_input_mode == "pci")
|
|
|
+ {
|
|
|
+ // 第一步 创建设备对象
|
|
|
+ hDevice = ACTS2110_DEV_Create(0, 0);
|
|
|
+ if (hDevice == NULL)
|
|
|
+ {
|
|
|
+ LOG(ERROR)<<Name()<< "PCI DEV_Create Error";
|
|
|
+ return ROBOT_FAILTURE;
|
|
|
+ }
|
|
|
+ memset(&AIParam, 0, sizeof(ACTS2110_AI_PARAM));
|
|
|
+ // 通道参数
|
|
|
+ AIParam.nSampChanCount = 1;
|
|
|
+ for (nChannel = 0; nChannel < MainInfo.nAIChannelCount; nChannel++)
|
|
|
+ {
|
|
|
+ AIParam.CHParam[nChannel].nChannel = nChannel;
|
|
|
+ AIParam.CHParam[nChannel].nSampleRange = ACTS2110_AI_SAMPRANGE_N10_P10V;
|
|
|
+ AIParam.CHParam[nChannel].nRefGround = ACTS2110_AI_REFGND_RSE;
|
|
|
+ AIParam.CHParam[nChannel].nReserved0 = 0;
|
|
|
+ AIParam.CHParam[nChannel].nReserved1 = 0;
|
|
|
+ AIParam.CHParam[nChannel].nReserved2 = 0;
|
|
|
+ }
|
|
|
+ AIParam.nSampleSignal = ACTS2110_AI_SAMPSIGNAL_2D5V; // ACTS2110_AI_SAMPSIGNAL_0V
|
|
|
+
|
|
|
+ // 时钟参数
|
|
|
+ AIParam.fSampleRate = fSampleRate;
|
|
|
+ AIParam.nSampleMode = ACTS2110_AI_SAMPMODE_CONTINUOUS;
|
|
|
+ AIParam.nSampsPerChan = 1024;
|
|
|
+ AIParam.nSampClkSource = ACTS2110_AIO_SAMPCLKSRC_LOCAL;
|
|
|
+ AIParam.nClockOutput = ACTS2110_AIO_CLKOUT_DISABLE;
|
|
|
+ AIParam.StartTrig.nSyncTSOut = ACTS2110_AIO_STSO_DISABLE;
|
|
|
+
|
|
|
+ // 触发参数
|
|
|
+ AIParam.StartTrig.nTriggerType = ACTS2110_AI_START_TRIGTYPE_NONE;
|
|
|
+ AIParam.StartTrig.nTriggerSource = ACTS2110_AI_TRIG_SRC_FIRST;
|
|
|
+ AIParam.StartTrig.nTriggerDir = ACTS2110_AI_TRIGDIR_FALLING;
|
|
|
+ AIParam.StartTrig.fTriggerLevelTop = 1.0;
|
|
|
+ AIParam.StartTrig.fTriggerLevelBtm = 0.0;
|
|
|
+ AIParam.StartTrig.nTriggerSens = 0;
|
|
|
+ AIParam.StartTrig.nDelaySamps = 0;
|
|
|
+ AIParam.StartTrig.nReTrigger = 0;
|
|
|
+
|
|
|
+ AIParam.PauseTrig.nTriggerType = ACTS2110_AI_START_TRIGTYPE_NONE;
|
|
|
+ AIParam.PauseTrig.nTriggerSource = ACTS2110_AI_TRIG_SRC_FIRST;
|
|
|
+ AIParam.PauseTrig.nTriggerDir = ACTS2110_AI_TRIGDIR_FALLING;
|
|
|
+ AIParam.PauseTrig.fTriggerLevelTop = 1.0;
|
|
|
+ AIParam.PauseTrig.fTriggerLevelBtm = 0.0;
|
|
|
+ AIParam.PauseTrig.nTriggerSens = 0;
|
|
|
+
|
|
|
+
|
|
|
+ // 其他参数
|
|
|
+ AIParam.nReserved1 = 0;
|
|
|
+ AIParam.nReserved2 = 0;
|
|
|
+ AIParam.nReserved3 = 0;
|
|
|
+
|
|
|
+ if (!ACTS2110_AI_VerifyParam(hDevice, &AIParam))
|
|
|
+ {
|
|
|
+ LOG(ERROR)<<Name()<< "PCI Wrong parameter, it has been adjusted to legal value,Please check the log file acts2110.log";
|
|
|
+ return ROBOT_FAILTURE;
|
|
|
+ }
|
|
|
+
|
|
|
+ // 第三步 开始AI采集任务
|
|
|
+ if (!ACTS2110_AI_StartTask(hDevice))
|
|
|
+ {
|
|
|
+ LOG(ERROR)<<Name()<< "PCI AI_StartTask Error";
|
|
|
+ return ROBOT_FAILTURE;
|
|
|
+ }
|
|
|
}
|
|
|
//TODO
|
|
|
node_handle_.reset(new ros::NodeHandle());
|
|
@@ -173,6 +241,52 @@ if (car_input_mode == "modbus") {
|
|
|
return ROBOT_FAILTURE;
|
|
|
}
|
|
|
}
|
|
|
+else if(car_input_mode == "pci"){
|
|
|
+ ros::Rate loop_rate(rate);
|
|
|
+ while(ros::ok())
|
|
|
+ {
|
|
|
+ if (kbhit()) continue;
|
|
|
+ if (!ACTS2110_AI_ReadAnalog(hDevice, fVoltArray, nReadSampsPerChan, &nSampsPerChanRead, &nReadableSamps, fTimeout))
|
|
|
+ {
|
|
|
+ LOG(ERROR) << Name() <<"PCI Read Data Timeout nSampsPerChanRead=" << nSampsPerChanRead;
|
|
|
+ }
|
|
|
+ else{
|
|
|
+ if (nSampsPerChanRead <= 0)
|
|
|
+ {
|
|
|
+ continue;
|
|
|
+ }
|
|
|
+ LOG(INFO) << Name() << "PCI nSampsPerChanRead= " << nSampsPerChanRead;
|
|
|
+ double voltage = 0;
|
|
|
+ for (U32 nIndex = 0; nIndex < 1; nIndex++)
|
|
|
+ {
|
|
|
+ for (nChannel = 0; nChannel < AIParam.nSampChanCount; nChannel++)
|
|
|
+ {
|
|
|
+ voltage = fVoltArray[nChannel + nIndex * AIParam.nSampChanCount];
|
|
|
+ printf("AI%02d=%.4f\t\n", nChannel, fVoltArray[nChannel + nIndex * AIParam.nSampChanCount]);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ try{
|
|
|
+ localization::localMsg localization;
|
|
|
+ localization.Longitude = 0;
|
|
|
+ localization.Latitude = 0;
|
|
|
+ localization.Height = 0;
|
|
|
+ localization.AngleDeviated = 0;
|
|
|
+ localization.Speed = voltage;
|
|
|
+ if(localization.Speed < 0.4)
|
|
|
+ {
|
|
|
+ localization.Speed = 0;
|
|
|
+ }
|
|
|
+ geometry_pub.publish(localization);
|
|
|
+ LOG(INFO) << Name() << " publish speed:" << std::setprecision(4) << localization.Speed;
|
|
|
+ }
|
|
|
+ catch (ros::Exception ex) {
|
|
|
+ LOG(ERROR)<<Name()<< "PCI publish speed error: " << ex.what();
|
|
|
+ }
|
|
|
+ }
|
|
|
+ ros::spinOnce();
|
|
|
+ loop_rate.sleep();
|
|
|
+ }
|
|
|
+}
|
|
|
//TODO
|
|
|
LOG(INFO)<<Name()<<" start end.";
|
|
|
return ROBOT_SUCCESS;
|
|
@@ -245,6 +359,27 @@ void Localization::Stop() {
|
|
|
else if (car_input_mode == "rtk") {
|
|
|
serial_gnss.close();
|
|
|
}
|
|
|
+ else if(car_input_mode == "pci")
|
|
|
+ {
|
|
|
+ // 第六步 停止AI采集任务
|
|
|
+ if (!ACTS2110_AI_StopTask(hDevice))
|
|
|
+ {
|
|
|
+ LOG(ERROR)<<Name()<< "PCI AI_StopTask Error";
|
|
|
+ }
|
|
|
+
|
|
|
+ // 第七步 释放AI采集任务
|
|
|
+ if (!ACTS2110_AI_ReleaseTask(hDevice))
|
|
|
+ {
|
|
|
+ LOG(ERROR)<<Name()<< "PCI AI_ReleaseTask Error";
|
|
|
+ }
|
|
|
+
|
|
|
+ // 第八步 释放设备对象
|
|
|
+ if (!ACTS2110_DEV_Release(hDevice))
|
|
|
+ {
|
|
|
+ LOG(ERROR)<<Name()<< "PCI DEV_Release Error";
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
LOG(INFO)<<Name()<<" Stop end.";
|
|
|
}
|
|
|
|