|
@@ -38,8 +38,10 @@ int Decision::Init(){
|
|
|
|
|
|
int Decision::Start(){
|
|
|
LOG(INFO)<<Name()<<" start() start...";
|
|
|
+ // under ros service server
|
|
|
ros::ServiceServer CommandService = node_handle_->advertiseService(robot::common::CommandModeCmdService,
|
|
|
&Decision::CommandRequestService, this);
|
|
|
+ // under ros subscriber
|
|
|
ros::Subscriber GeometrySub = node_handle_->subscribe(robot::common::Geometry, 2000,
|
|
|
&Decision::RTKLocationCallback, this);
|
|
|
|
|
@@ -47,12 +49,20 @@ int Decision::Start(){
|
|
|
|
|
|
ros::Subscriber RCUModeSub = node_handle_->subscribe(robot::common::Si2Data, 2000, &Decision::RCUModeCallback, this);
|
|
|
|
|
|
+ // under ros publish
|
|
|
Rules_pub = node_handle_->advertise<decision::rulesMsg>(robot::common::RuleInfo, 200);
|
|
|
|
|
|
Geometry_pub = node_handle_->advertise<decision::localizationMsg>(robot::common::GeometryPlan, 200);
|
|
|
|
|
|
Control_pub = node_handle_->advertise<decision::controlMsg>(robot::common::ControlInfo, 200);
|
|
|
|
|
|
+ // under ros service of client
|
|
|
+ clientSftActCtrReq = node_handle_->serviceClient<canbus::PedalActCtrReq>(robot::common::ShiftActCtrService);
|
|
|
+
|
|
|
+ clientPedalActCtrReq = node_handle_->serviceClient<canbus::PedalActCtrReq>(robot::common::PedalActCtrService);
|
|
|
+
|
|
|
+ clientStrActCtrReq = node_handle_->serviceClient<canbus::PedalActCtrReq>(robot::common::SteerActCtrService);
|
|
|
+
|
|
|
ros::AsyncSpinner spinner(6); // Use 6 threads
|
|
|
spinner.start();
|
|
|
ros::waitForShutdown();
|
|
@@ -66,23 +76,28 @@ void Decision::Stop(){
|
|
|
return ;
|
|
|
}
|
|
|
|
|
|
-
|
|
|
+// 执行自动驾驶模式和路径规划模式,自动驾驶模式踩刹车然后挂前进当;路径规划模式删除数据库记录
|
|
|
bool Decision::CommandRequestService(decision::CommandModeCmd::Request &req,
|
|
|
decision::CommandModeCmd::Response &res)
|
|
|
{
|
|
|
- LOG(INFO)<<Name()<<"CommandRequest get command ID:."<< req.Cmd;
|
|
|
+ LOG(INFO)<<Name()<<"CommandRequestService get command ID:."<< req.Cmd;
|
|
|
if (req.Cmd == CMD_AUTODRIE_START)
|
|
|
{
|
|
|
+ LoadPlanRoute();
|
|
|
double breakCtr = -5; // 踩刹车到50%
|
|
|
ros::Rate loop_rate(1);
|
|
|
while(ros::ok())
|
|
|
{
|
|
|
if (SpeedControl(breakCtr) == 1)
|
|
|
- {
|
|
|
- LoadPlanRoute();
|
|
|
+ {
|
|
|
if (BreakCtrControl(SFT_PSNG_LRN_REQ_FORWARD_1) == 1) // 前进挡
|
|
|
break;
|
|
|
}
|
|
|
+ else
|
|
|
+ {
|
|
|
+ break;
|
|
|
+ return false;
|
|
|
+ }
|
|
|
ros::spinOnce();
|
|
|
loop_rate.sleep();
|
|
|
}
|
|
@@ -94,17 +109,20 @@ bool Decision::CommandRequestService(decision::CommandModeCmd::Request &req,
|
|
|
DecisionDatabase::DeleteLocationRuleInfo(PlanRouteName.c_str());
|
|
|
}
|
|
|
res.CmdResult = iCommandCmd = req.Cmd;
|
|
|
+ LOG(INFO)<<Name()<<"CommandRequestService end."<< res.CmdResult;
|
|
|
return true;
|
|
|
}
|
|
|
|
|
|
+
|
|
|
+// 加载规划路径,同时把规划的路径轨迹和测试的用例发送给显示界面
|
|
|
void Decision::LoadPlanRoute()
|
|
|
{
|
|
|
ilocalTotalNumbers = DecisionDatabase::countOfLocationRuleInfo();
|
|
|
iRulesTotalNumbers = DecisionDatabase::countOfActionDirective();
|
|
|
-
|
|
|
+ LOG(INFO)<<Name()<<" LoadPlanRoute select ilocalTotalNumbers: "<<ilocalTotalNumbers<<" iRulesTotalNumbers: "<<iRulesTotalNumbers;
|
|
|
+ std::vector<LOCATION_RULE_INFO> tempInfo = DecisionDatabase::QueryLocationRuleInfo(0, ilocalTotalNumbers);
|
|
|
for (int i = 0; i< ilocalTotalNumbers; i++)
|
|
|
{
|
|
|
- std::vector<LOCATION_RULE_INFO> tempInfo = DecisionDatabase::QueryLocationRuleInfo(i, 1);
|
|
|
decision::localizationMsg localMsg;
|
|
|
localMsg.Locationid = tempInfo[i].LocationID;
|
|
|
localMsg.Longitude = tempInfo[i].LocationLon;
|
|
@@ -113,20 +131,19 @@ void Decision::LoadPlanRoute()
|
|
|
localMsg.LocationSpeed = tempInfo[i].LocationSpeed;
|
|
|
Geometry_pub.publish(localMsg);
|
|
|
}
|
|
|
-
|
|
|
+ std::vector<ACTION_DIRECTIVE> tempAction = DecisionDatabase::QueryActionDirective(0, iRulesTotalNumbers);
|
|
|
for (int i = 0; i< iRulesTotalNumbers; i++)
|
|
|
{
|
|
|
- std::vector<ACTION_DIRECTIVE> tempInfo = DecisionDatabase::QueryActionDirective(i, 1);
|
|
|
decision::rulesMsg ruleMsg;
|
|
|
- ruleMsg.DirectiveID = tempInfo[i].DirectiveID;
|
|
|
- ruleMsg.DirectiveInfo = tempInfo[i].DirectiveInfo;
|
|
|
- ruleMsg.DirectiveResult = tempInfo[i].DirectiveResult;
|
|
|
+ ruleMsg.DirectiveID = tempAction[i].DirectiveID;
|
|
|
+ ruleMsg.DirectiveInfo = tempAction[i].DirectiveInfo;
|
|
|
+ ruleMsg.DirectiveResult = tempAction[i].DirectiveResult;
|
|
|
Rules_pub.publish(ruleMsg);
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
-
|
|
|
+// 接收RTK数据分为自动驾驶模式开始和结束和路径规划模式开始和结束
|
|
|
void Decision::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
|
|
|
{
|
|
|
switch (iCommandCmd) {
|
|
@@ -159,13 +176,15 @@ void Decision::RCUModeCallback(const perception::si2Msg::ConstPtr& msg)
|
|
|
LOG(INFO)<<Name()<< " car excute mode :"<<uiCarMode;
|
|
|
}
|
|
|
|
|
|
+//自动生成测试用例
|
|
|
void Decision::ExcutePlanRoute(const localization::localMsg::ConstPtr& msg)
|
|
|
{
|
|
|
- try
|
|
|
- {
|
|
|
+ try {
|
|
|
iLocalNowNumber ++;
|
|
|
- if ((iLocalNowNumber % iPredictionPointsPerNumber) == 0)
|
|
|
- {
|
|
|
+ LOG(INFO)<<Name()<<"ExcutePlanRoute iLocalNowNumber = "<< iLocalNowNumber;
|
|
|
+ LOG(INFO)<<Name()<<"Longitude:"<<std::setprecision(18)<< msg->Longitude<<"Latitude:"<<
|
|
|
+ std::setprecision(18)<<msg->Latitude;
|
|
|
+ if ((iLocalNowNumber % iPredictionPointsPerNumber) == 0) {
|
|
|
iRulesNowNumber++;
|
|
|
std::stringstream strRuleContent;
|
|
|
if (iRulesNowNumber == 1)
|
|
@@ -186,16 +205,15 @@ void Decision::ExcutePlanRoute(const localization::localMsg::ConstPtr& msg)
|
|
|
}
|
|
|
|
|
|
int iRet = DecisionDatabase::InsertLocationRuleInfoAndActionDirective(PlanRouteName.c_str(), iRulesNowNumber,
|
|
|
- strRuleContent.str().c_str(), RULE_EXCUTE_RESULT_INIT, iLocalNowNumber, msg->Longitude, msg->Latitude,
|
|
|
- msg->Height, fCarNowSpeed);
|
|
|
+ strRuleContent.str().c_str(), RULE_EXCUTE_RESULT_INIT, iLocalNowNumber, msg->Longitude,
|
|
|
+ msg->Latitude, msg->Height, fCarNowSpeed);
|
|
|
if (iRet < 0) {
|
|
|
iLocalNowNumber--;
|
|
|
iRulesNowNumber--;
|
|
|
}
|
|
|
fCarLastSpeed = fCarNowSpeed;
|
|
|
}
|
|
|
- else
|
|
|
- {
|
|
|
+ else{
|
|
|
DecisionDatabase::InsertLocationRuleInfo(PlanRouteName.c_str(),iLocalNowNumber, msg->Longitude,
|
|
|
msg->Latitude, msg->Height, fCarNowSpeed);
|
|
|
}
|
|
@@ -206,15 +224,13 @@ void Decision::ExcutePlanRoute(const localization::localMsg::ConstPtr& msg)
|
|
|
}
|
|
|
}
|
|
|
|
|
|
-
|
|
|
+// 自动生成测试用例停止
|
|
|
void Decision::StopPlanRoute(const localization::localMsg::ConstPtr& msg)
|
|
|
{
|
|
|
- if ((iLocalNowNumber % iPredictionPointsPerNumber) == 0)
|
|
|
- {
|
|
|
+ if ((iLocalNowNumber % iPredictionPointsPerNumber) == 0){
|
|
|
return;
|
|
|
}
|
|
|
- else
|
|
|
- {
|
|
|
+ else{
|
|
|
iLocalNowNumber++;
|
|
|
iRulesNowNumber++;
|
|
|
std::stringstream strRuleContent;
|
|
@@ -240,7 +256,7 @@ void Decision::StopPlanRoute(const localization::localMsg::ConstPtr& msg)
|
|
|
}
|
|
|
}
|
|
|
|
|
|
-
|
|
|
+// 执行自动驾驶MPC算法
|
|
|
void Decision::ExcuteSelfDriving(const localization::localMsg::ConstPtr& msg)
|
|
|
{
|
|
|
// 判断当前RCU是否为自动驾驶模式
|
|
@@ -249,15 +265,14 @@ void Decision::ExcuteSelfDriving(const localization::localMsg::ConstPtr& msg)
|
|
|
if (CarAutoDriveStop())
|
|
|
return;
|
|
|
int iPredictPoints;
|
|
|
- if (ilocalTotalNumbers < (iLocalNowNumber + iPredictionPointsPerNumber))
|
|
|
- {
|
|
|
+ if (ilocalTotalNumbers < (iLocalNowNumber + iPredictionPointsPerNumber)){
|
|
|
iPredictPoints = ilocalTotalNumbers - iLocalNowNumber;
|
|
|
}
|
|
|
- else
|
|
|
- {
|
|
|
+ else{
|
|
|
iPredictPoints = iPredictionPointsPerNumber;
|
|
|
}
|
|
|
- std::vector<LOCATION_RULE_INFO> tempInfo = DecisionDatabase::QueryLocationRuleInfo(iLocalNowNumber, iPredictPoints);
|
|
|
+ std::vector<LOCATION_RULE_INFO> tempInfo;
|
|
|
+ tempInfo = DecisionDatabase::QueryLocationRuleInfo(iLocalNowNumber, iPredictPoints);
|
|
|
std::vector<double> ptsy(iPredictPoints);
|
|
|
std::vector<double> ptsx(iPredictPoints);
|
|
|
for (int i= 0; i< iPredictPoints; i++)
|
|
@@ -275,17 +290,15 @@ void Decision::ExcuteSelfDriving(const localization::localMsg::ConstPtr& msg)
|
|
|
iLocalNowNumber ++;
|
|
|
}
|
|
|
|
|
|
+// 自动驾驶停止
|
|
|
bool Decision::CarAutoDriveStop()
|
|
|
{
|
|
|
- try
|
|
|
- {
|
|
|
- if (ilocalTotalNumbers = iLocalNowNumber)
|
|
|
- {
|
|
|
+ try{
|
|
|
+ if (ilocalTotalNumbers = iLocalNowNumber){
|
|
|
uiCarMode = CMD_AUTODRIE_END;
|
|
|
double breakCtr = -5; // 踩刹车到50%
|
|
|
ros::Rate loop_rate(1);
|
|
|
- while(ros::ok())
|
|
|
- {
|
|
|
+ while(ros::ok()){
|
|
|
if (SpeedControl(breakCtr) == 1)
|
|
|
{
|
|
|
if (BreakCtrControl(SFT_PSNG_LRN_REQ_PARK) == 1) // P挡
|
|
@@ -322,10 +335,8 @@ bool Decision::CarAutoDriveStop()
|
|
|
|
|
|
bool Decision::UpdataCarRuleResults()
|
|
|
{
|
|
|
- try
|
|
|
- {
|
|
|
- if (iLocalNowNumber >0 && (iLocalNowNumber % iPredictionPointsPerNumber == 0))
|
|
|
- {
|
|
|
+ try{
|
|
|
+ if (iLocalNowNumber >0 && (iLocalNowNumber % iPredictionPointsPerNumber == 0)){
|
|
|
decision::rulesMsg ruleMsg;
|
|
|
if (std::abs(fCarNowSpeed - mpc.ref_speed) < 1) //低速( <10km/h):± 0.25km/h 高速( >10km/h):± 1km/h
|
|
|
{
|
|
@@ -350,6 +361,7 @@ bool Decision::UpdataCarRuleResults()
|
|
|
}
|
|
|
}
|
|
|
|
|
|
+
|
|
|
void Decision::MPC_Controller(std::vector<double> &ptsx, std::vector<double> &ptsy,
|
|
|
double &px, double &py, double &psi, double &v)
|
|
|
{
|
|
@@ -384,7 +396,7 @@ void Decision::MPC_Controller(std::vector<double> &ptsx, std::vector<double> &pt
|
|
|
|
|
|
unsigned int Decision::SpeedControl(double &speed)
|
|
|
{
|
|
|
- ros::ServiceClient clientPedalActCtrReq = node_handle_->serviceClient<canbus::PedalActCtrReq>(robot::common::PedalActCtrService);
|
|
|
+
|
|
|
canbus::PedalActCtrReq srv;
|
|
|
if (mpc.a_prev >0){
|
|
|
srv.request.uiAccPsngCtrReq =(unsigned int)(speed *10);
|
|
@@ -408,9 +420,11 @@ unsigned int Decision::SpeedControl(double &speed)
|
|
|
}
|
|
|
return srv.response.uiReturn;
|
|
|
}
|
|
|
+
|
|
|
+
|
|
|
unsigned int Decision::StreeControl(double &streer)
|
|
|
{
|
|
|
- ros::ServiceClient clientStrActCtrReq = node_handle_->serviceClient<canbus::PedalActCtrReq>(robot::common::SteerActCtrService);
|
|
|
+
|
|
|
canbus::StrActCtrReq srv;
|
|
|
srv.request.iStrAngCtrReq =(unsigned int)(streer *100);
|
|
|
srv.request.uiStrCtrMdReq =1;
|
|
@@ -427,9 +441,9 @@ unsigned int Decision::StreeControl(double &streer)
|
|
|
return srv.response.uiReturn;
|
|
|
}
|
|
|
|
|
|
+
|
|
|
unsigned int Decision::BreakCtrControl(unsigned int breakmode)
|
|
|
-{
|
|
|
- ros::ServiceClient clientSftActCtrReq = node_handle_->serviceClient<canbus::PedalActCtrReq>(robot::common::ShiftActCtrService);
|
|
|
+{
|
|
|
canbus::SftActCtrReq srv;
|
|
|
srv.request.uiSftPsngCtrReq = breakmode;
|
|
|
srv.request.uiSftMdReq = SFT_MD_REQ_2_SECOND;
|