|
@@ -25,11 +25,11 @@ int Decision::Init(){
|
|
|
LOG(INFO)<<Name()<<": init() start...";
|
|
|
node_handle_.reset(new ros::NodeHandle());
|
|
|
DecisionDatabase::InitDB();
|
|
|
- iCommandCmd = CMD_UNDEFIED;// set undefied mode
|
|
|
- iRulesTotalNumbers = ilocalTotalNumbers =fCarNowSpeed = iLocalNowNumber = iRulesNowNumber = 0;
|
|
|
+ iCommandCmd = CMD_AUTODRIE_START;// set undefied mode
|
|
|
+ iAbandon = iRulesTotalNumbers = ilocalTotalNumbers =fCarNowSpeed = iLocalNowNumber = iRulesNowNumber = 0;
|
|
|
iPredictionPointsPerNumber = 5;
|
|
|
uiCarMode =6; // undefied
|
|
|
-
|
|
|
+ fCarNowSpeed = iLocalNowNumber = iRulesNowNumber = 0;
|
|
|
PlanRouteName = "siasun";
|
|
|
|
|
|
LOG(INFO)<<Name()<<" init() end.";
|
|
@@ -43,12 +43,12 @@ int Decision::Start(){
|
|
|
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,
|
|
|
+ ros::Subscriber GeometrySub = node_handle_->subscribe(robot::common::Geometry, 200,
|
|
|
&Decision::RTKLocationCallback, this);
|
|
|
|
|
|
- ros::Subscriber OBDSub = node_handle_->subscribe(robot::common::ObdData, 2000, &Decision::OBDCallback, this);
|
|
|
+ ros::Subscriber OBDSub = node_handle_->subscribe(robot::common::ObdData, 200, &Decision::OBDCallback, this);
|
|
|
|
|
|
- ros::Subscriber RCUModeSub = node_handle_->subscribe(robot::common::Si2Data, 2000, &Decision::RCUModeCallback, this);
|
|
|
+ ros::Subscriber RCUModeSub = node_handle_->subscribe(robot::common::Si2Data, 200, &Decision::RCUModeCallback, this);
|
|
|
|
|
|
// under ros publish
|
|
|
Rules_pub = node_handle_->advertise<decision::rulesMsg>(robot::common::RuleInfo, 200);
|
|
@@ -103,13 +103,19 @@ bool Decision::CommandRequestService(decision::CommandModeCmd::Request &req,
|
|
|
ros::spinOnce();
|
|
|
loop_rate.sleep();
|
|
|
}
|
|
|
+ ptsx.push_back(0);
|
|
|
+ ptsy.push_back(0);
|
|
|
+ ptsx.push_back(0);
|
|
|
+ ptsy.push_back(0);
|
|
|
+ fCarNowSpeed = iLocalNowNumber = iRulesNowNumber = 0;
|
|
|
}
|
|
|
else if (req.Cmd == CMD_PLAN_START)
|
|
|
{
|
|
|
DecisionDatabase::DeleteActionDirective(PlanRouteName.c_str());
|
|
|
- DecisionDatabase::DeleteLocationRuleInfo(PlanRouteName.c_str());
|
|
|
+ DecisionDatabase::DeleteLocationRuleInfo(PlanRouteName.c_str());
|
|
|
+ iAbandon = fCarNowSpeed = iLocalNowNumber = iRulesNowNumber = 0;
|
|
|
}
|
|
|
- fCarNowSpeed = iLocalNowNumber = iRulesNowNumber = 0;
|
|
|
+
|
|
|
res.CmdResult = iCommandCmd = req.Cmd;
|
|
|
LOG(INFO)<<Name()<<"CommandRequestService end."<< res.CmdResult;
|
|
|
return true;
|
|
@@ -182,7 +188,14 @@ void Decision::RCUModeCallback(const perception::si2Msg::ConstPtr& msg)
|
|
|
//自动生成测试用例
|
|
|
void Decision::ExcutePlanRoute(const localization::localMsg::ConstPtr& msg)
|
|
|
{
|
|
|
- double secsStart =ros::Time::now().toSec();
|
|
|
+ if (iAbandon< 30){
|
|
|
+ iAbandon ++;
|
|
|
+ return;
|
|
|
+ }
|
|
|
+ else{
|
|
|
+ iAbandon = 0;
|
|
|
+ }
|
|
|
+
|
|
|
try {
|
|
|
iLocalNowNumber ++;
|
|
|
LOG(INFO)<<Name()<<"ExcutePlanRoute iLocalNowNumber = "<< iLocalNowNumber;
|
|
@@ -223,13 +236,12 @@ void Decision::ExcutePlanRoute(const localization::localMsg::ConstPtr& msg)
|
|
|
Geometry_pub.publish(localMsg);
|
|
|
}
|
|
|
fCarLastSpeed = fCarNowSpeed;
|
|
|
+
|
|
|
}
|
|
|
catch (ros::Exception ex)
|
|
|
{
|
|
|
LOG(ERROR)<<Name()<<":ExcutePlanRoute error."<< ex.what();
|
|
|
}
|
|
|
- double secsEnd =ros::Time::now().toSec();
|
|
|
- ros::Duration(2 - secsEnd + secsStart).sleep();
|
|
|
}
|
|
|
|
|
|
// 自动生成测试用例停止
|
|
@@ -266,32 +278,24 @@ void Decision::StopPlanRoute(const localization::localMsg::ConstPtr& msg)
|
|
|
void Decision::ExcuteSelfDriving(const localization::localMsg::ConstPtr& msg)
|
|
|
{
|
|
|
// 判断当前RCU是否为自动驾驶模式
|
|
|
-// if (uiCarMode != CTR_MD_REQ_AUTO_DRIVE)
|
|
|
-// {
|
|
|
-// LOG(INFO)<<Name()<<" ExcuteSelfDriving uiCarMode is not auto drive.";
|
|
|
-// return;
|
|
|
-// }
|
|
|
+ if (uiCarMode != CTR_MD_REQ_AUTO_DRIVE)
|
|
|
+ {
|
|
|
+ LOG(INFO)<<Name()<<" ExcuteSelfDriving uiCarMode is not auto drive.";
|
|
|
+ return;
|
|
|
+ }
|
|
|
LOG(INFO)<<Name()<<" ilocalTotalNumbers:"<<ilocalTotalNumbers << "iLocalNowNumber:"<<iLocalNowNumber;
|
|
|
if (CarAutoDriveStop())
|
|
|
return;
|
|
|
- int iPredictPoints;
|
|
|
- if (ilocalTotalNumbers < (iLocalNowNumber + iPredictionPointsPerNumber)){
|
|
|
- iPredictPoints = ilocalTotalNumbers - iLocalNowNumber;
|
|
|
- }
|
|
|
- else{
|
|
|
- iPredictPoints = iPredictionPointsPerNumber;
|
|
|
- }
|
|
|
- if ((iLocalNowNumber > 0)&&((((msg->Longitude - ptsx[0])>0) && ((msg->Longitude - ptsx[1])< 0) ||
|
|
|
- ((msg->Longitude - ptsx[0])<0) && ((msg->Longitude - ptsx[1])> 0))&&
|
|
|
- (((msg->Latitude - ptsy[0])>0) && ((msg->Latitude - ptsy[1])< 0) ||
|
|
|
- ((msg->Latitude - ptsy[0])<0) && ((msg->Latitude - ptsy[1])> 0))))
|
|
|
- {
|
|
|
- LOG(INFO)<<Name()<<" ExcuteSelfDriving plan route does not alter.";
|
|
|
- }
|
|
|
- else
|
|
|
- {
|
|
|
+ int iPredictPoints = iPredictionPointsPerNumber;
|
|
|
+ double px = msg->Longitude;
|
|
|
+ double py = msg->Latitude;
|
|
|
+ bool bRetLon = (((ptsx[1] - ptsx[0])>= 0)&&((px - ptsx[1])> 0))||
|
|
|
+ (((ptsx[1] - ptsx[0])< 0)&&((px - ptsx[1])< 0));
|
|
|
+ bool bRetLat = (((ptsy[1] - ptsy[0])>= 0)&&((py - ptsy[1])> 0))||
|
|
|
+ (((ptsy[1] - ptsy[0])< 0)&&((py - ptsy[1])< 0));
|
|
|
+ if (bRetLon|| bRetLat)
|
|
|
+ {
|
|
|
std::vector<LOCATION_RULE_INFO> tempInfo;
|
|
|
- LOG(INFO)<<Name()<<" ExcuteSelfDriving QueryLocationRuleInfo start.";
|
|
|
tempInfo = DecisionDatabase::QueryLocationRuleInfo(iLocalNowNumber, iPredictPoints);
|
|
|
ptsx.clear();
|
|
|
ptsy.clear();
|
|
@@ -301,15 +305,18 @@ void Decision::ExcuteSelfDriving(const localization::localMsg::ConstPtr& msg)
|
|
|
ptsy.push_back( tempInfo[i].LocationLat);
|
|
|
}
|
|
|
mpc.ref_speed = tempInfo[1].LocationSpeed;
|
|
|
+ iLocalNowNumber ++;
|
|
|
+ UpdataCarRuleResults();
|
|
|
LOG(INFO)<<Name()<<" ExcuteSelfDriving plan route altered.";
|
|
|
}
|
|
|
- double px = msg->Longitude;
|
|
|
- double py = msg->Latitude;
|
|
|
+ else
|
|
|
+ {
|
|
|
+ LOG(INFO)<<Name()<<" ExcuteSelfDriving plan route does not alter.";
|
|
|
+ }
|
|
|
double psi = deg2rad(msg->AngleDeviated + 90);
|
|
|
double v = (double)fCarNowSpeed;
|
|
|
MPC_Controller(ptsx, ptsy, px, py, psi, v);
|
|
|
- UpdataCarRuleResults();
|
|
|
- iLocalNowNumber ++;
|
|
|
+
|
|
|
}
|
|
|
|
|
|
void Decision::TestMPC()
|
|
@@ -362,18 +369,16 @@ bool Decision::CarAutoDriveStop()
|
|
|
{
|
|
|
try{
|
|
|
|
|
|
- if (ilocalTotalNumbers == iLocalNowNumber){
|
|
|
- uiCarMode = CMD_AUTODRIE_END;
|
|
|
+ if (ilocalTotalNumbers == iLocalNowNumber + iPredictionPointsPerNumber){
|
|
|
+ iCommandCmd = CMD_AUTODRIE_END;
|
|
|
double breakCtr = -5; // 踩刹车到50%
|
|
|
- ros::Rate loop_rate(1);
|
|
|
- while(ros::ok()){
|
|
|
+ iLocalNowNumber ++;
|
|
|
+ while(true){
|
|
|
if (SpeedControl(breakCtr) == 1)
|
|
|
{
|
|
|
if (BreakCtrControl(SFT_PSNG_LRN_REQ_PARK) == 1) // P挡
|
|
|
break;
|
|
|
}
|
|
|
- ros::spinOnce();
|
|
|
- loop_rate.sleep();
|
|
|
}
|
|
|
decision::rulesMsg ruleMsg;
|
|
|
if (std::abs(fCarNowSpeed - mpc.ref_speed) < 1) //低速( <10km/h):± 0.25km/h 高速( >10km/h):± 1km/h
|
|
@@ -435,9 +440,9 @@ void Decision::MPC_Controller(std::vector<double> &ptsx, std::vector<double> &pt
|
|
|
Eigen::MatrixXd waypoints = transformGlobal2Vehicle(px, py, psi, ptsx, ptsy);
|
|
|
Eigen::VectorXd Ptsx = waypoints.row(0);
|
|
|
Eigen::VectorXd Ptsy = waypoints.row(1);
|
|
|
- for (unsigned i=0; i < ptsx.size(); ++i) {
|
|
|
- LOG(INFO)<<Name()<< " lon: "<< Ptsx(i)<< " lat: "<<Ptsy(i);
|
|
|
- }
|
|
|
+// for (unsigned i=0; i < ptsx.size(); ++i) {
|
|
|
+// LOG(INFO)<<Name()<< " lon: "<< Ptsx(i)<< " lat: "<<Ptsy(i);
|
|
|
+// }
|
|
|
// 获取状态值
|
|
|
auto coeffs = polyfit(Ptsx, Ptsy, 3);
|
|
|
double cte = evaluateCte(coeffs);
|
|
@@ -448,18 +453,18 @@ void Decision::MPC_Controller(std::vector<double> &ptsx, std::vector<double> &pt
|
|
|
// 这个地方有延迟,取第三个
|
|
|
vector<vector<double>> result = mpc.Solve(state, coeffs);
|
|
|
// 展示MPC预测的路径
|
|
|
- vector<double> mpc_x_vals=result[0];
|
|
|
- vector<double> mpc_y_vals=result[1];
|
|
|
- std::vector<double>::iterator iter_start = mpc_x_vals.begin();
|
|
|
- std::vector<double>::iterator iter_end = mpc_x_vals.end();
|
|
|
- std::vector<double>::iterator iter;
|
|
|
- int i = 0;
|
|
|
- for ( iter = iter_start; iter != iter_end; iter++)
|
|
|
- {
|
|
|
- LOG(INFO)<<Name()<<" prediction LocationLon = "<<std::setprecision(15)<<mpc_x_vals[i]
|
|
|
- <<"prediction LocationLat = "<< std::setprecision(14)<<mpc_y_vals[i];
|
|
|
- i++;
|
|
|
- }
|
|
|
+// vector<double> mpc_x_vals=result[0];
|
|
|
+// vector<double> mpc_y_vals=result[1];
|
|
|
+// std::vector<double>::iterator iter_start = mpc_x_vals.begin();
|
|
|
+// std::vector<double>::iterator iter_end = mpc_x_vals.end();
|
|
|
+// std::vector<double>::iterator iter;
|
|
|
+// int i = 0;
|
|
|
+// for ( iter = iter_start; iter != iter_end; iter++)
|
|
|
+// {
|
|
|
+// LOG(INFO)<<Name()<<" prediction LocationLon = "<<std::setprecision(15)<<mpc_x_vals[i]
|
|
|
+// <<"prediction LocationLat = "<< std::setprecision(14)<<mpc_y_vals[i];
|
|
|
+// i++;
|
|
|
+// }
|
|
|
double steer_value = result[2][mpc.latency];
|
|
|
double throttle_value = result[3][mpc.latency];
|
|
|
mpc.delta_prev = steer_value;
|