|
@@ -177,6 +177,8 @@ int Canbus::Start() {
|
|
ros::ServiceServer IOModeService = node_handle_->advertiseService(robot::common::IOModeService ,
|
|
ros::ServiceServer IOModeService = node_handle_->advertiseService(robot::common::IOModeService ,
|
|
&Canbus::IOMode,this);
|
|
&Canbus::IOMode,this);
|
|
|
|
|
|
|
|
+ ros::ServiceServer SftTxtService = node_handle_->advertiseService(robot::common::SftTxtService,
|
|
|
|
+ &Canbus::SftTxtReq,this);
|
|
|
|
|
|
string str = "/home/madesheng/angle.txt";
|
|
string str = "/home/madesheng/angle.txt";
|
|
int flag = Canbus::read_txt(str,1);
|
|
int flag = Canbus::read_txt(str,1);
|
|
@@ -205,6 +207,13 @@ int Canbus::Start() {
|
|
return ROBOT_FAILTURE;
|
|
return ROBOT_FAILTURE;
|
|
}
|
|
}
|
|
|
|
|
|
|
|
+ str = "/home/madesheng/sft.txt";
|
|
|
|
+ flag = Canbus::read_txt(str,5);
|
|
|
|
+ if(flag != ROBOT_SUCCESS)
|
|
|
|
+ {
|
|
|
|
+ return ROBOT_FAILTURE;
|
|
|
|
+ }
|
|
|
|
+
|
|
//data receive
|
|
//data receive
|
|
while(ros::ok())
|
|
while(ros::ok())
|
|
{
|
|
{
|
|
@@ -263,8 +272,8 @@ void Canbus::OnTimer(const ros::TimerEvent &) {
|
|
}
|
|
}
|
|
|
|
|
|
void Canbus::Stop() {
|
|
void Canbus::Stop() {
|
|
- timer_.stop();
|
|
|
|
- sleep(0.5);
|
|
|
|
|
|
+timer_.stop();
|
|
|
|
+sleep(0.5);
|
|
TPCANMsg canIcu; //ICUMonitor Can Message
|
|
TPCANMsg canIcu; //ICUMonitor Can Message
|
|
icuMonitor.uiICUClsReq = 1;
|
|
icuMonitor.uiICUClsReq = 1;
|
|
MessageCoder::CanMsgPackage((void*)&icuMonitor, robot::common::ICUMonitor, &canIcu);
|
|
MessageCoder::CanMsgPackage((void*)&icuMonitor, robot::common::ICUMonitor, &canIcu);
|
|
@@ -332,7 +341,7 @@ int Canbus::read_data(TPCANMsg *pMsgBuff)
|
|
// char strData[16]={0};
|
|
// char strData[16]={0};
|
|
// MessageCoder::ConvertUnCharToStr(strData,pMsgBuff->DATA,8);
|
|
// MessageCoder::ConvertUnCharToStr(strData,pMsgBuff->DATA,8);
|
|
|
|
|
|
-// LOG(INFO)<<Name()<<" Receive CanData : ID: "<<std::string(strId)<<",Data: "<<std::string(strData);
|
|
|
|
|
|
+ // LOG(INFO)<<Name()<<" Receive CanData : ID: "<<std::string(strId)<<",Data: "<<std::string(strData);
|
|
|
|
|
|
//通过读取车辆OBD的数据,赋值给OBD的变量 原始代码 开始
|
|
//通过读取车辆OBD的数据,赋值给OBD的变量 原始代码 开始
|
|
// if(pMsgBuff->ID == robot::common::ObdAccPsng)
|
|
// if(pMsgBuff->ID == robot::common::ObdAccPsng)
|
|
@@ -415,11 +424,14 @@ int Canbus::read_data(TPCANMsg *pMsgBuff)
|
|
{
|
|
{
|
|
publish_data(pMsgBuff);
|
|
publish_data(pMsgBuff);
|
|
}
|
|
}
|
|
|
|
+
|
|
//通过设置相关变量,赋值给OBD的变量 测试版代码 结束
|
|
//通过设置相关变量,赋值给OBD的变量 测试版代码 结束
|
|
return ROBOT_SUCCESS;
|
|
return ROBOT_SUCCESS;
|
|
}
|
|
}
|
|
|
|
|
|
void Canbus::publish_data(TPCANMsg *pMsgBuff)
|
|
void Canbus::publish_data(TPCANMsg *pMsgBuff)
|
|
|
|
+{
|
|
|
|
+try
|
|
{
|
|
{
|
|
canbus::canMsg rosMsg;
|
|
canbus::canMsg rosMsg;
|
|
rosMsg.id = pMsgBuff->ID;
|
|
rosMsg.id = pMsgBuff->ID;
|
|
@@ -433,7 +445,18 @@ void Canbus::publish_data(TPCANMsg *pMsgBuff)
|
|
//std::string strData = chData;
|
|
//std::string strData = chData;
|
|
rosMsg.data = chData;
|
|
rosMsg.data = chData;
|
|
canRCUData_pub.publish(rosMsg);
|
|
canRCUData_pub.publish(rosMsg);
|
|
-// LOG(INFO)<<Name()<<" Publish CanData :" << "ID:" << strId << ",Data:"<<std::string(chData);
|
|
|
|
|
|
+if(pMsgBuff->ID == robot::common::SysLearnBack)
|
|
|
|
+{
|
|
|
|
+ // LOG(INFO)<<Name()<<" Publish CanData :" << "ID:" << strId << ",Data:"<<std::string(chData);
|
|
|
|
+}
|
|
|
|
+
|
|
|
|
+}
|
|
|
|
+ catch (ros::Exception ex)
|
|
|
|
+ {
|
|
|
|
+ LOG(ERROR)<< "publish failed "<< ex.what();
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+
|
|
}
|
|
}
|
|
|
|
|
|
int Canbus::write_data(TPCANMsg *pMsgBuff, bool print)
|
|
int Canbus::write_data(TPCANMsg *pMsgBuff, bool print)
|
|
@@ -465,6 +488,7 @@ bool Canbus::ActuatorEnableRequest(canbus::ActuatorEnableRequest::Request &req,
|
|
request.uiStrMotEnReq = req.uiStrMotEnReq;
|
|
request.uiStrMotEnReq = req.uiStrMotEnReq;
|
|
request.uiXSftMotEnReq = req.uiXSftMotEnReq;
|
|
request.uiXSftMotEnReq = req.uiXSftMotEnReq;
|
|
request.uiYSftMotEnReq = req.uiYSftMotEnReq;
|
|
request.uiYSftMotEnReq = req.uiYSftMotEnReq;
|
|
|
|
+ request.uiZSftMotEnReq = req.uiZSftMotEnReq;
|
|
|
|
|
|
MessageCoder::CanMsgPackage((void*)&request, robot::common::ActuatorEnableReq, &canActuaEnableReqMsg);
|
|
MessageCoder::CanMsgPackage((void*)&request, robot::common::ActuatorEnableReq, &canActuaEnableReqMsg);
|
|
if (write_data(&canActuaEnableReqMsg)!=ROBOT_SUCCESS)
|
|
if (write_data(&canActuaEnableReqMsg)!=ROBOT_SUCCESS)
|
|
@@ -666,6 +690,7 @@ bool Canbus::RBTManager(canbus::RBTManager::Request &req,
|
|
request.uiStrMotInt= req.uiStrMotInt;
|
|
request.uiStrMotInt= req.uiStrMotInt;
|
|
request.uiXSftMotInt = req.uiXSftMotInt;
|
|
request.uiXSftMotInt = req.uiXSftMotInt;
|
|
request.uiYSftMotInt = req.uiYSftMotInt;
|
|
request.uiYSftMotInt = req.uiYSftMotInt;
|
|
|
|
+ request.uiZSftMotInt =req.uiZSftMotInt;
|
|
|
|
|
|
MessageCoder::CanMsgPackage((void*)&request, robot::common::RobotManager, &canRobotManagerMsg);
|
|
MessageCoder::CanMsgPackage((void*)&request, robot::common::RobotManager, &canRobotManagerMsg);
|
|
if (write_data(&canRobotManagerMsg) != ROBOT_SUCCESS)
|
|
if (write_data(&canRobotManagerMsg) != ROBOT_SUCCESS)
|
|
@@ -700,6 +725,7 @@ bool Canbus::PowerOnRequest(canbus::PowerOnRequest::Request &req,
|
|
request.uiXSftCtrPowReq = req.uiXSftArmCtrPowReq;
|
|
request.uiXSftCtrPowReq = req.uiXSftArmCtrPowReq;
|
|
request.uiYSftCtrPowReq = req.uiYSftArmCtrPowReq;
|
|
request.uiYSftCtrPowReq = req.uiYSftArmCtrPowReq;
|
|
request.uiDriveFanReq = req.uiDriveFanReq;
|
|
request.uiDriveFanReq = req.uiDriveFanReq;
|
|
|
|
+ request.uiZSftArmCtrPowReq = req.uiZSftArmCtrPowReq;
|
|
|
|
|
|
MessageCoder::CanMsgPackage((void*)&request, robot::common::PowerOnReq, &canPowerOnReqMsg);
|
|
MessageCoder::CanMsgPackage((void*)&request, robot::common::PowerOnReq, &canPowerOnReqMsg);
|
|
|
|
|
|
@@ -1125,14 +1151,22 @@ void Canbus::StrTxtOnTimer(const ros::TimerEvent &event)
|
|
str_txt_timer_.stop();
|
|
str_txt_timer_.stop();
|
|
strTxtTimerState = false;;
|
|
strTxtTimerState = false;;
|
|
angles_count = 0;
|
|
angles_count = 0;
|
|
|
|
+LOG(INFO)<<Name()<< " StrTxt END. ";
|
|
return;
|
|
return;
|
|
}
|
|
}
|
|
|
|
|
|
TPCANMsg canStrActCtrReqMsg;
|
|
TPCANMsg canStrActCtrReqMsg;
|
|
int angle = angles[angles_count] * 100;
|
|
int angle = angles[angles_count] * 100;
|
|
-
|
|
|
|
tStrActCtrReqTxt.iStrAngCtrReq = angle;
|
|
tStrActCtrReqTxt.iStrAngCtrReq = angle;
|
|
- LOG(INFO)<<Name()<< " angle:. " <<angle;
|
|
|
|
|
|
+ //20220805zw 增加速度配套转速文件
|
|
|
|
+ if (hasSpeedFile)
|
|
|
|
+ {
|
|
|
|
+ int angle_speed = angle_speeds[angles_count] * 100;
|
|
|
|
+ tStrActCtrReqTxt.iStrSpdCtrReq = angle_speed;
|
|
|
|
+ LOG(INFO)<<Name()<< " angle:. " <<angle<<" angle_speed:"<<angle_speed;
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ LOG(INFO)<<Name()<< " angle:. " <<angle;
|
|
MessageCoder::CanMsgPackage((void*)&tStrActCtrReqTxt, robot::common::SteerActCtrReq, &canStrActCtrReqMsg);
|
|
MessageCoder::CanMsgPackage((void*)&tStrActCtrReqTxt, robot::common::SteerActCtrReq, &canStrActCtrReqMsg);
|
|
write_data(&canStrActCtrReqMsg,true);
|
|
write_data(&canStrActCtrReqMsg,true);
|
|
angles_count ++;
|
|
angles_count ++;
|
|
@@ -1195,6 +1229,7 @@ void Canbus::BrkTxtOnTimer(const ros::TimerEvent &event)
|
|
brk_txt_timer_.stop();
|
|
brk_txt_timer_.stop();
|
|
brkTxtTimerState = false;;
|
|
brkTxtTimerState = false;;
|
|
psng_count = 0;
|
|
psng_count = 0;
|
|
|
|
+LOG(INFO)<<Name()<< " BrkTxt END. ";
|
|
return;
|
|
return;
|
|
}
|
|
}
|
|
|
|
|
|
@@ -1350,6 +1385,75 @@ void Canbus::StrTxtLineOnTimer(const ros::TimerEvent &event)
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
+bool Canbus::SftTxtReq(canbus::SftTxtReq::Request &req,canbus::SftTxtReq::Response &res)
|
|
|
|
+{
|
|
|
|
+ LOG(INFO)<< "controlservice start SftTxtReq";
|
|
|
|
+ // SftTxtReq request
|
|
|
|
+ try{
|
|
|
|
+ if(req.uiSftTxtFlag == 0)
|
|
|
|
+ {
|
|
|
|
+ if(sftTxtTimerState)
|
|
|
|
+ {
|
|
|
|
+ sft_txt_timer_.stop();
|
|
|
|
+ sftTxtTimerState = false;
|
|
|
|
+ sft_psng_count = 0;
|
|
|
|
+ }
|
|
|
|
+ res.uiReturn = 1;
|
|
|
|
+ return true;
|
|
|
|
+ }
|
|
|
|
+ if(sftTxtTimerState)
|
|
|
|
+ {
|
|
|
|
+ sft_txt_timer_.stop();
|
|
|
|
+ sftTxtTimerState = false;
|
|
|
|
+ sft_psng_count = 0;
|
|
|
|
+ }
|
|
|
|
+ sft_psng_count = 0;
|
|
|
|
+ tSftCtrReqTxt.iClchSpdCtrReq = req.iClchSpdCtrReq;
|
|
|
|
+ tSftCtrReqTxt.uiSftMdReq = req.uiSftMdReq;
|
|
|
|
+ tSftCtrReqTxt.uiClchPsngCtrReq = req.uiClchPsngCtrReq;
|
|
|
|
+
|
|
|
|
+ sft_txt_timer_ = CreateTimer(ros::Duration(duration_txt), &Canbus::SftTxtOnTimer, this,false,false);
|
|
|
|
+ sft_txt_timer_.start();
|
|
|
|
+ sftTxtTimerState = true;
|
|
|
|
+ res.uiReturn = 1;
|
|
|
|
+ }
|
|
|
|
+ catch (ros::Exception ex)
|
|
|
|
+ {
|
|
|
|
+ res.uiReturn = 0;
|
|
|
|
+ LOG(ERROR)<< "controlservice SftTxtReq failed "<< ex.what();
|
|
|
|
+ return true;
|
|
|
|
+ }
|
|
|
|
+ return true;
|
|
|
|
+}
|
|
|
|
+
|
|
|
|
+void Canbus::SftTxtOnTimer(const ros::TimerEvent &event)
|
|
|
|
+{
|
|
|
|
+ try
|
|
|
|
+ {
|
|
|
|
+ if(sft_psng_count == sft_psng_number)
|
|
|
|
+ {
|
|
|
|
+ sft_txt_timer_.stop();
|
|
|
|
+ sftTxtTimerState = false;;
|
|
|
|
+ sft_psng_count = 0;
|
|
|
|
+ return;
|
|
|
|
+ }
|
|
|
|
+
|
|
|
|
+ TPCANMsg canSftCtrReqMsg;
|
|
|
|
+ int psng = sft_psngs[sft_psng_count];
|
|
|
|
+
|
|
|
|
+ tSftCtrReqTxt.uiSftPsngCtrReq = psng;
|
|
|
|
+
|
|
|
|
+ LOG(INFO)<<Name()<< "sft psng:. " <<psng;
|
|
|
|
+ MessageCoder::CanMsgPackage((void*)&tSftCtrReqTxt, robot::common::ShiftActCtrReq, &canSftCtrReqMsg);
|
|
|
|
+ write_data(&canSftCtrReqMsg,true);
|
|
|
|
+ sft_psng_count ++;
|
|
|
|
+ }
|
|
|
|
+ catch (ros::Exception ex)
|
|
|
|
+ {
|
|
|
|
+ LOG(ERROR)<<Name()<< " OnTimer error. "<< ex.what();
|
|
|
|
+ }
|
|
|
|
+}
|
|
|
|
+
|
|
int Canbus::read_txt(string &path,int flag)
|
|
int Canbus::read_txt(string &path,int flag)
|
|
{
|
|
{
|
|
//ifstream myfile("/home/madesheng/data.txt");
|
|
//ifstream myfile("/home/madesheng/data.txt");
|
|
@@ -1359,18 +1463,21 @@ int Canbus::read_txt(string &path,int flag)
|
|
LOG(ERROR)<<Name()<< "open src File Error opening file" << endl;
|
|
LOG(ERROR)<<Name()<< "open src File Error opening file" << endl;
|
|
return ROBOT_FAILTURE;
|
|
return ROBOT_FAILTURE;
|
|
}
|
|
}
|
|
- if(flag == 1)
|
|
|
|
|
|
+ //20220805zw 增加读取角度文件
|
|
|
|
+ ifstream speedfile("/home/siasun/speed.txt");
|
|
|
|
+ hasSpeedFile = speedfile.is_open();
|
|
|
|
+ //-------------------
|
|
|
|
+ if (flag == 1)
|
|
{
|
|
{
|
|
- for (int i = 0; i < 3000; i++)
|
|
|
|
|
|
+ for (int i = 0; i < 70000; i++)
|
|
{
|
|
{
|
|
-
|
|
|
|
if(myfile.eof())
|
|
if(myfile.eof())
|
|
{
|
|
{
|
|
angles_number = i;
|
|
angles_number = i;
|
|
break;
|
|
break;
|
|
}
|
|
}
|
|
myfile >> angles[i];
|
|
myfile >> angles[i];
|
|
-
|
|
|
|
|
|
+ speedfile >> angle_speeds[i];
|
|
//myfile >> temp;
|
|
//myfile >> temp;
|
|
//cout <<"char:"<< temp<< endl;
|
|
//cout <<"char:"<< temp<< endl;
|
|
//cout <<"angle:"<< angles[i] <<endl;
|
|
//cout <<"angle:"<< angles[i] <<endl;
|
|
@@ -1379,7 +1486,7 @@ int Canbus::read_txt(string &path,int flag)
|
|
}
|
|
}
|
|
else if(flag ==2)
|
|
else if(flag ==2)
|
|
{
|
|
{
|
|
- for (int i = 0; i < 6000; i++)
|
|
|
|
|
|
+ for (int i = 0; i < 50000; i++)
|
|
{
|
|
{
|
|
if(myfile.eof())
|
|
if(myfile.eof())
|
|
{
|
|
{
|
|
@@ -1396,7 +1503,7 @@ int Canbus::read_txt(string &path,int flag)
|
|
}
|
|
}
|
|
else if(flag ==3)
|
|
else if(flag ==3)
|
|
{
|
|
{
|
|
- for (int i = 0; i < 8000; i++)
|
|
|
|
|
|
+ for (int i = 0; i < 80000; i++)
|
|
{
|
|
{
|
|
if(myfile.eof())
|
|
if(myfile.eof())
|
|
{
|
|
{
|
|
@@ -1424,6 +1531,21 @@ int Canbus::read_txt(string &path,int flag)
|
|
}
|
|
}
|
|
cout << "angle line NUMBER:" <<angles_line_number<<endl;
|
|
cout << "angle line NUMBER:" <<angles_line_number<<endl;
|
|
}
|
|
}
|
|
|
|
+ else if(flag == 5)
|
|
|
|
+ {
|
|
|
|
+ for (int i = 0; i < 70000; i++)
|
|
|
|
+ {
|
|
|
|
+ if(myfile.eof())
|
|
|
|
+ {
|
|
|
|
+ sft_psng_number = i;
|
|
|
|
+ break;
|
|
|
|
+ }
|
|
|
|
+ myfile >> sft_psngs[i];
|
|
|
|
+
|
|
|
|
+ // cout <<"psngs:"<< acc_psngs[i] <<endl;
|
|
|
|
+ }
|
|
|
|
+ cout << "sft NUMBER:" <<sft_psng_number<<endl;
|
|
|
|
+ }
|
|
|
|
|
|
|
|
|
|
myfile.close();
|
|
myfile.close();
|