|
@@ -274,12 +274,17 @@ if (car_input_mode == "modbus") {
|
|
}
|
|
}
|
|
}
|
|
}
|
|
else if(car_input_mode == "pci"){
|
|
else if(car_input_mode == "pci"){
|
|
- if (!ros::param::get("car_input_volspeed_max", car_input_volspeed_max))
|
|
|
|
- car_input_volspeed_max = 100;
|
|
|
|
|
|
+ if (!ros::param::get("car_input_volspeed_max", car_input_volspeed_max)){
|
|
|
|
+ car_input_volspeed_max = 100;
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+ rack_pci_timer_ = CreateTimer(ros::Duration(duration_pci),&Localization::RackPciOnTimer,this,false,false);
|
|
|
|
+ rack_pci_timer_.start();
|
|
|
|
+
|
|
ros::Rate loop_rate(rate);
|
|
ros::Rate loop_rate(rate);
|
|
|
|
+
|
|
while(ros::ok())
|
|
while(ros::ok())
|
|
{
|
|
{
|
|
-// if (kbhit()) continue;
|
|
|
|
if (!ACTS2110_AI_ReadAnalog(hDevice, fVoltArray, nReadSampsPerChan, &nSampsPerChanRead, &nReadableSamps, fTimeout))
|
|
if (!ACTS2110_AI_ReadAnalog(hDevice, fVoltArray, nReadSampsPerChan, &nSampsPerChanRead, &nReadableSamps, fTimeout))
|
|
{
|
|
{
|
|
LOG(ERROR) << Name() <<"PCI Read Data Timeout nSampsPerChanRead=" << nSampsPerChanRead;
|
|
LOG(ERROR) << Name() <<"PCI Read Data Timeout nSampsPerChanRead=" << nSampsPerChanRead;
|
|
@@ -287,10 +292,9 @@ else if(car_input_mode == "pci"){
|
|
else{
|
|
else{
|
|
if (nSampsPerChanRead <= 0)
|
|
if (nSampsPerChanRead <= 0)
|
|
{
|
|
{
|
|
|
|
+ //LOG(INFO) << Name() << "PCI nSampsPerChanRead= " << nSampsPerChanRead;
|
|
continue;
|
|
continue;
|
|
- }
|
|
|
|
- //LOG(INFO) << Name() << "PCI nSampsPerChanRead= " << nSampsPerChanRead;
|
|
|
|
- double voltage = 0;
|
|
|
|
|
|
+ }
|
|
for (U32 nIndex = 0; nIndex < 1; nIndex++)
|
|
for (U32 nIndex = 0; nIndex < 1; nIndex++)
|
|
{
|
|
{
|
|
for (nChannel = 0; nChannel < AIParam.nSampChanCount; nChannel++)
|
|
for (nChannel = 0; nChannel < AIParam.nSampChanCount; nChannel++)
|
|
@@ -299,28 +303,9 @@ else if(car_input_mode == "pci"){
|
|
printf("AI%02d=%.4f\t\n", nChannel, fVoltArray[nChannel + nIndex * AIParam.nSampChanCount]);
|
|
printf("AI%02d=%.4f\t\n", nChannel, fVoltArray[nChannel + nIndex * AIParam.nSampChanCount]);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
- LOG(INFO) << Name() << "PCI voltage= " << voltage;
|
|
|
|
- try{
|
|
|
|
- localization::localMsg localization;
|
|
|
|
- localization.Longitude = 0;
|
|
|
|
- localization.Latitude = 0;
|
|
|
|
- localization.Height = 0;
|
|
|
|
- localization.AngleDeviated = 0;
|
|
|
|
- int max = car_input_volspeed_max;
|
|
|
|
- localization.Speed = max / 10 * 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();
|
|
|
|
- }
|
|
|
|
|
|
+ //LOG(INFO) << Name() << "PCI voltage= " << voltage;
|
|
}
|
|
}
|
|
ros::spinOnce();
|
|
ros::spinOnce();
|
|
- loop_rate.sleep();
|
|
|
|
}
|
|
}
|
|
}
|
|
}
|
|
//TODO
|
|
//TODO
|
|
@@ -372,6 +357,29 @@ int Localization::read_txt(string &path)
|
|
myfile.close();
|
|
myfile.close();
|
|
return ROBOT_SUCCESS;
|
|
return ROBOT_SUCCESS;
|
|
}
|
|
}
|
|
|
|
+
|
|
|
|
+void Localization::RackPciOnTimer(const ros::TimerEvent &event)
|
|
|
|
+{
|
|
|
|
+ try{
|
|
|
|
+ localization::localMsg localization;
|
|
|
|
+ localization.Longitude = 0;
|
|
|
|
+ localization.Latitude = 0;
|
|
|
|
+ localization.Height = 0;
|
|
|
|
+ localization.AngleDeviated = 0;
|
|
|
|
+ int max = car_input_volspeed_max;
|
|
|
|
+ localization.Speed = max / 10 * 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();
|
|
|
|
+ }
|
|
|
|
+}
|
|
|
|
+
|
|
bool Localization::IsOpen()
|
|
bool Localization::IsOpen()
|
|
{
|
|
{
|
|
//检测串口是否已经打开,并给出提示信息
|
|
//检测串口是否已经打开,并给出提示信息
|