|
@@ -31,34 +31,6 @@ int Decision::Init(){
|
|
uiCarMode =6; // undefied
|
|
uiCarMode =6; // undefied
|
|
|
|
|
|
PlanRouteName = "siasun";
|
|
PlanRouteName = "siasun";
|
|
- ilocalTotalNumbers = 100;
|
|
|
|
- fCarNowSpeed = 0;
|
|
|
|
- vPx.push_back(127.162345678);
|
|
|
|
- vPy.push_back(41.843340742);
|
|
|
|
- vPx.push_back(127.162345678);
|
|
|
|
- vPy.push_back(41.843340742);
|
|
|
|
- vPx.push_back(127.162345678);
|
|
|
|
- vPy.push_back(41.843340742);
|
|
|
|
- vPx.push_back(127.162345678);
|
|
|
|
- vPy.push_back(41.843340742);
|
|
|
|
- vPx.push_back(127.162345678);
|
|
|
|
- vPy.push_back(41.843340742);
|
|
|
|
- vPx.push_back(127.162345678);
|
|
|
|
- vPy.push_back(41.843340742);
|
|
|
|
- vPx.push_back(127.160345678);
|
|
|
|
- vPy.push_back(41.843340742);
|
|
|
|
- vPx.push_back(127.154345678);
|
|
|
|
- vPy.push_back(41.851940742);
|
|
|
|
- vPx.push_back(127.154345678);
|
|
|
|
- vPy.push_back(41.851940742);
|
|
|
|
- vPx.push_back(127.154345678);
|
|
|
|
- vPy.push_back(41.851940742);
|
|
|
|
- vPx.push_back(127.154345678);
|
|
|
|
- vPy.push_back(41.851940742);
|
|
|
|
- for (int i =0; i< 11; i++)
|
|
|
|
- {
|
|
|
|
- TestMPC();
|
|
|
|
- }
|
|
|
|
|
|
|
|
LOG(INFO)<<Name()<<" init() end.";
|
|
LOG(INFO)<<Name()<<" init() end.";
|
|
return ROBOT_SUCCESS;
|
|
return ROBOT_SUCCESS;
|
|
@@ -101,7 +73,7 @@ int Decision::Start(){
|
|
|
|
|
|
void Decision::Stop(){
|
|
void Decision::Stop(){
|
|
LOG(INFO)<<Name()<<": stop start...";
|
|
LOG(INFO)<<Name()<<": stop start...";
|
|
- LOG(INFO)<<Name()<<": stop end.";
|
|
|
|
|
|
+ LOG(INFO)<<Name()<<": stop end.";
|
|
return ;
|
|
return ;
|
|
}
|
|
}
|
|
|
|
|
|
@@ -131,13 +103,13 @@ bool Decision::CommandRequestService(decision::CommandModeCmd::Request &req,
|
|
ros::spinOnce();
|
|
ros::spinOnce();
|
|
loop_rate.sleep();
|
|
loop_rate.sleep();
|
|
}
|
|
}
|
|
- fCarNowSpeed = iLocalNowNumber = iRulesNowNumber = 0;
|
|
|
|
}
|
|
}
|
|
else if (req.Cmd == CMD_PLAN_START)
|
|
else if (req.Cmd == CMD_PLAN_START)
|
|
{
|
|
{
|
|
DecisionDatabase::DeleteActionDirective(PlanRouteName.c_str());
|
|
DecisionDatabase::DeleteActionDirective(PlanRouteName.c_str());
|
|
- DecisionDatabase::DeleteLocationRuleInfo(PlanRouteName.c_str());
|
|
|
|
|
|
+ DecisionDatabase::DeleteLocationRuleInfo(PlanRouteName.c_str());
|
|
}
|
|
}
|
|
|
|
+ fCarNowSpeed = iLocalNowNumber = iRulesNowNumber = 0;
|
|
res.CmdResult = iCommandCmd = req.Cmd;
|
|
res.CmdResult = iCommandCmd = req.Cmd;
|
|
LOG(INFO)<<Name()<<"CommandRequestService end."<< res.CmdResult;
|
|
LOG(INFO)<<Name()<<"CommandRequestService end."<< res.CmdResult;
|
|
return true;
|
|
return true;
|
|
@@ -176,7 +148,6 @@ void Decision::LoadPlanRoute()
|
|
// 接收RTK数据分为自动驾驶模式开始和结束和路径规划模式开始和结束
|
|
// 接收RTK数据分为自动驾驶模式开始和结束和路径规划模式开始和结束
|
|
void Decision::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
|
|
void Decision::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
|
|
{
|
|
{
|
|
-// double secsStart =ros::Time::now().toSec();
|
|
|
|
switch (iCommandCmd) {
|
|
switch (iCommandCmd) {
|
|
case CMD_PLAN_START:
|
|
case CMD_PLAN_START:
|
|
ExcutePlanRoute(msg);
|
|
ExcutePlanRoute(msg);
|
|
@@ -193,8 +164,6 @@ void Decision::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
|
|
default:
|
|
default:
|
|
break;
|
|
break;
|
|
}
|
|
}
|
|
-// double secsEnd =ros::Time::now().toSec();
|
|
|
|
-// ros::Duration(2 - secsEnd + secsStart).sleep();
|
|
|
|
}
|
|
}
|
|
|
|
|
|
|
|
|
|
@@ -216,40 +185,44 @@ void Decision::ExcutePlanRoute(const localization::localMsg::ConstPtr& msg)
|
|
double secsStart =ros::Time::now().toSec();
|
|
double secsStart =ros::Time::now().toSec();
|
|
try {
|
|
try {
|
|
iLocalNowNumber ++;
|
|
iLocalNowNumber ++;
|
|
- LOG(INFO)<<Name()<<"ExcutePlanRoute iLocalNowNumber = "<< iLocalNowNumber;
|
|
|
|
- if ((iLocalNowNumber % iPredictionPointsPerNumber) == 0) {
|
|
|
|
- iRulesNowNumber++;
|
|
|
|
- std::stringstream strRuleContent;
|
|
|
|
- if (iRulesNowNumber == 1)
|
|
|
|
- strRuleContent<<"动作"<<iRulesNowNumber<<":起步请加速到"<< fCarNowSpeed<<"km/h";
|
|
|
|
- else{
|
|
|
|
- if (fCarNowSpeed > fCarLastSpeed)
|
|
|
|
- {
|
|
|
|
- strRuleContent<<"动作"<<iRulesNowNumber<<":加速到"<< fCarNowSpeed<<"km/h";
|
|
|
|
- }
|
|
|
|
- else if (fCarNowSpeed < fCarLastSpeed)
|
|
|
|
- {
|
|
|
|
- strRuleContent<<"动作"<<iRulesNowNumber<<":制动至"<< fCarNowSpeed<<"km/h";
|
|
|
|
- }
|
|
|
|
- else if (fCarNowSpeed == fCarLastSpeed)
|
|
|
|
- {
|
|
|
|
- strRuleContent<<"动作"<<iRulesNowNumber<<":保持"<< fCarNowSpeed<<"km/h";
|
|
|
|
- }
|
|
|
|
- }
|
|
|
|
-
|
|
|
|
- int iRet = DecisionDatabase::InsertLocationRuleInfoAndActionDirective(PlanRouteName.c_str(), iRulesNowNumber,
|
|
|
|
- strRuleContent.str().c_str(), RULE_EXCUTE_RESULT_INIT, iLocalNowNumber, msg->Longitude,
|
|
|
|
- msg->Latitude, msg->Height, fCarNowSpeed);
|
|
|
|
- if (iRet < 0) {
|
|
|
|
- iLocalNowNumber--;
|
|
|
|
- iRulesNowNumber--;
|
|
|
|
- }
|
|
|
|
- fCarLastSpeed = fCarNowSpeed;
|
|
|
|
- }
|
|
|
|
|
|
+ LOG(INFO)<<Name()<<"ExcutePlanRoute iLocalNowNumber = "<< iLocalNowNumber;
|
|
|
|
+ iRulesNowNumber++;
|
|
|
|
+ std::stringstream strRuleContent;
|
|
|
|
+ if (iRulesNowNumber == 1)
|
|
|
|
+ strRuleContent<<"动作"<<iRulesNowNumber<<":起步请加速到"<< fCarNowSpeed<<"km/h";
|
|
else{
|
|
else{
|
|
- DecisionDatabase::InsertLocationRuleInfo(PlanRouteName.c_str(),iLocalNowNumber, msg->Longitude,
|
|
|
|
- msg->Latitude, msg->Height, fCarNowSpeed);
|
|
|
|
- }
|
|
|
|
|
|
+ if (fCarNowSpeed > fCarLastSpeed)
|
|
|
|
+ {
|
|
|
|
+ strRuleContent<<"动作"<<iRulesNowNumber<<":加速到"<< fCarNowSpeed<<"km/h";
|
|
|
|
+ }
|
|
|
|
+ else if (fCarNowSpeed < fCarLastSpeed)
|
|
|
|
+ {
|
|
|
|
+ strRuleContent<<"动作"<<iRulesNowNumber<<":制动至"<< fCarNowSpeed<<"km/h";
|
|
|
|
+ }
|
|
|
|
+ else if (fCarNowSpeed == fCarLastSpeed)
|
|
|
|
+ {
|
|
|
|
+ strRuleContent<<"动作"<<iRulesNowNumber<<":保持"<< fCarNowSpeed<<"km/h";
|
|
|
|
+ }
|
|
|
|
+ }
|
|
|
|
+ int iRet = DecisionDatabase::InsertLocationRuleInfoAndActionDirective(PlanRouteName.c_str(), iRulesNowNumber,
|
|
|
|
+ strRuleContent.str().c_str(), RULE_EXCUTE_RESULT_INIT, iLocalNowNumber, msg->Longitude,
|
|
|
|
+ msg->Latitude, msg->Height, fCarNowSpeed);
|
|
|
|
+ if (iRet < 0) {
|
|
|
|
+ iLocalNowNumber--;
|
|
|
|
+ iRulesNowNumber--;
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+ decision::localizationMsg localMsg;
|
|
|
|
+ localMsg.Locationid = iLocalNowNumber;
|
|
|
|
+ localMsg.Longitude = msg->Longitude;
|
|
|
|
+ localMsg.Latitude = msg->Latitude;
|
|
|
|
+ localMsg.Height = msg->Height;
|
|
|
|
+ localMsg.LocationSpeed = fCarNowSpeed;
|
|
|
|
+ localMsg.AngleDeviated = msg->AngleDeviated;
|
|
|
|
+ Geometry_pub.publish(localMsg);
|
|
|
|
+ }
|
|
|
|
+ fCarLastSpeed = fCarNowSpeed;
|
|
}
|
|
}
|
|
catch (ros::Exception ex)
|
|
catch (ros::Exception ex)
|
|
{
|
|
{
|
|
@@ -264,36 +237,29 @@ void Decision::StopPlanRoute(const localization::localMsg::ConstPtr& msg)
|
|
{
|
|
{
|
|
LOG(INFO)<<Name()<<"StopPlanRoute last record iLocalNowNumber = "<< iLocalNowNumber;
|
|
LOG(INFO)<<Name()<<"StopPlanRoute last record iLocalNowNumber = "<< iLocalNowNumber;
|
|
iCommandCmd = CMD_UNDEFIED;// set undefied mode
|
|
iCommandCmd = CMD_UNDEFIED;// set undefied mode
|
|
- if ((iLocalNowNumber % iPredictionPointsPerNumber) == 0){
|
|
|
|
- LOG(INFO)<<Name()<<"StopPlanRoute last record iLocalNowNumber = "<< iLocalNowNumber
|
|
|
|
- << " iLocalNowNumber % iPredictionPointsPerNumber = 0";
|
|
|
|
- return;
|
|
|
|
|
|
+ iLocalNowNumber++;
|
|
|
|
+ iRulesNowNumber++;
|
|
|
|
+ std::stringstream strRuleContent;
|
|
|
|
+ if (fCarNowSpeed > fCarLastSpeed)
|
|
|
|
+ {
|
|
|
|
+ strRuleContent<<"动作"<<iRulesNowNumber<<":加速到"<< fCarNowSpeed<<"km/h";
|
|
}
|
|
}
|
|
- else{
|
|
|
|
- iLocalNowNumber++;
|
|
|
|
- iRulesNowNumber++;
|
|
|
|
- std::stringstream strRuleContent;
|
|
|
|
- if (fCarNowSpeed > fCarLastSpeed)
|
|
|
|
- {
|
|
|
|
- strRuleContent<<"动作"<<iRulesNowNumber<<":加速到"<< fCarNowSpeed<<"km/h";
|
|
|
|
- }
|
|
|
|
- else if (fCarNowSpeed < fCarLastSpeed)
|
|
|
|
- {
|
|
|
|
- strRuleContent<<"动作"<<iRulesNowNumber<<":制动至"<< fCarNowSpeed<<"km/h";
|
|
|
|
- }
|
|
|
|
- else if (fCarNowSpeed == fCarLastSpeed)
|
|
|
|
- {
|
|
|
|
- strRuleContent<<"动作"<<iRulesNowNumber<<":保持"<< fCarNowSpeed<<"km/h";
|
|
|
|
- }
|
|
|
|
- int iRet = DecisionDatabase::InsertLocationRuleInfoAndActionDirective(PlanRouteName.c_str(), iRulesNowNumber,
|
|
|
|
- strRuleContent.str().c_str(), 0, iLocalNowNumber, msg->Longitude, msg->Latitude,
|
|
|
|
- msg->Height, fCarNowSpeed);
|
|
|
|
- if (iRet < 0) {
|
|
|
|
- LOG(ERROR)<<Name()<<"StopPlanRoute insert iLocalNowNumber = "<< iLocalNowNumber<< " last record error";
|
|
|
|
- iLocalNowNumber--;
|
|
|
|
- iRulesNowNumber--;
|
|
|
|
- }
|
|
|
|
|
|
+ else if (fCarNowSpeed < fCarLastSpeed)
|
|
|
|
+ {
|
|
|
|
+ strRuleContent<<"动作"<<iRulesNowNumber<<":制动至"<< fCarNowSpeed<<"km/h";
|
|
}
|
|
}
|
|
|
|
+ else if (fCarNowSpeed == fCarLastSpeed)
|
|
|
|
+ {
|
|
|
|
+ strRuleContent<<"动作"<<iRulesNowNumber<<":保持"<< fCarNowSpeed<<"km/h";
|
|
|
|
+ }
|
|
|
|
+ int iRet = DecisionDatabase::InsertLocationRuleInfoAndActionDirective(PlanRouteName.c_str(), iRulesNowNumber,
|
|
|
|
+ strRuleContent.str().c_str(), 0, iLocalNowNumber, msg->Longitude, msg->Latitude,
|
|
|
|
+ msg->Height, fCarNowSpeed);
|
|
|
|
+ if (iRet < 0) {
|
|
|
|
+ LOG(ERROR)<<Name()<<"StopPlanRoute insert iLocalNowNumber = "<< iLocalNowNumber<< " last record error";
|
|
|
|
+ iLocalNowNumber--;
|
|
|
|
+ iRulesNowNumber--;
|
|
|
|
+ }
|
|
}
|
|
}
|
|
|
|
|
|
// 执行自动驾驶MPC算法
|
|
// 执行自动驾驶MPC算法
|
|
@@ -305,7 +271,7 @@ void Decision::ExcuteSelfDriving(const localization::localMsg::ConstPtr& msg)
|
|
// LOG(INFO)<<Name()<<" ExcuteSelfDriving uiCarMode is not auto drive.";
|
|
// LOG(INFO)<<Name()<<" ExcuteSelfDriving uiCarMode is not auto drive.";
|
|
// return;
|
|
// return;
|
|
// }
|
|
// }
|
|
- LOG(INFO)<<Name()<<" ilocalTotalNumbers::."<<ilocalTotalNumbers << "iLocalNowNumber:"<<iLocalNowNumber;
|
|
|
|
|
|
+ LOG(INFO)<<Name()<<" ilocalTotalNumbers:"<<ilocalTotalNumbers << "iLocalNowNumber:"<<iLocalNowNumber;
|
|
if (CarAutoDriveStop())
|
|
if (CarAutoDriveStop())
|
|
return;
|
|
return;
|
|
int iPredictPoints;
|
|
int iPredictPoints;
|
|
@@ -438,27 +404,25 @@ bool Decision::CarAutoDriveStop()
|
|
bool Decision::UpdataCarRuleResults()
|
|
bool Decision::UpdataCarRuleResults()
|
|
{
|
|
{
|
|
try{
|
|
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
|
|
|
|
- {
|
|
|
|
- ruleMsg.DirectiveResult = RULE_EXCUTE_RESULT_SUCCESS;
|
|
|
|
- }
|
|
|
|
- else
|
|
|
|
- {
|
|
|
|
- ruleMsg.DirectiveResult = RULE_EXCUTE_RESULT_FIAL;
|
|
|
|
- }
|
|
|
|
- int iResult = ruleMsg.DirectiveResult;
|
|
|
|
- DecisionDatabase::UpdateActionDirective(PlanRouteName.c_str(), iRulesNowNumber, iResult);
|
|
|
|
- ruleMsg.DirectiveID = iRulesNowNumber;
|
|
|
|
- Rules_pub.publish(ruleMsg);
|
|
|
|
- iRulesNowNumber ++;
|
|
|
|
|
|
+ iRulesNowNumber ++;
|
|
|
|
+ decision::rulesMsg ruleMsg;
|
|
|
|
+ if (std::abs(fCarNowSpeed - mpc.ref_speed) < 1) //低速( <10km/h):± 0.25km/h 高速( >10km/h):± 1km/h
|
|
|
|
+ {
|
|
|
|
+ ruleMsg.DirectiveResult = RULE_EXCUTE_RESULT_SUCCESS;
|
|
}
|
|
}
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+ ruleMsg.DirectiveResult = RULE_EXCUTE_RESULT_FIAL;
|
|
|
|
+ }
|
|
|
|
+ int iResult = ruleMsg.DirectiveResult;
|
|
|
|
+ DecisionDatabase::UpdateActionDirective(PlanRouteName.c_str(), iRulesNowNumber, iResult);
|
|
|
|
+ ruleMsg.DirectiveID = iRulesNowNumber;
|
|
|
|
+ Rules_pub.publish(ruleMsg);
|
|
return true;
|
|
return true;
|
|
}
|
|
}
|
|
catch (ros::Exception ex)
|
|
catch (ros::Exception ex)
|
|
{
|
|
{
|
|
- LOG(ERROR)<<Name()<<":UpdataCarRuleResults error."<< ex.what();
|
|
|
|
|
|
+ LOG(ERROR)<<Name()<<" UpdataCarRuleResults error."<< ex.what();
|
|
return false;
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
@@ -492,8 +456,8 @@ void Decision::MPC_Controller(std::vector<double> &ptsx, std::vector<double> &pt
|
|
int i = 0;
|
|
int i = 0;
|
|
for ( iter = iter_start; iter != iter_end; iter++)
|
|
for ( iter = iter_start; iter != iter_end; iter++)
|
|
{
|
|
{
|
|
- LOG(INFO)<<Name()<<" MPC_Controller prediction LocationLon = "<<std::setprecision(16)<<mpc_x_vals[i]
|
|
|
|
- <<" LocationLat = "<< std::setprecision(16)<<mpc_y_vals[i];
|
|
|
|
|
|
+ LOG(INFO)<<Name()<<" prediction LocationLon = "<<std::setprecision(15)<<mpc_x_vals[i]
|
|
|
|
+ <<"prediction LocationLat = "<< std::setprecision(14)<<mpc_y_vals[i];
|
|
i++;
|
|
i++;
|
|
}
|
|
}
|
|
double steer_value = result[2][mpc.latency];
|
|
double steer_value = result[2][mpc.latency];
|
|
@@ -501,20 +465,19 @@ void Decision::MPC_Controller(std::vector<double> &ptsx, std::vector<double> &pt
|
|
mpc.delta_prev = steer_value;
|
|
mpc.delta_prev = steer_value;
|
|
mpc.a_prev = throttle_value;
|
|
mpc.a_prev = throttle_value;
|
|
LOG(INFO)<<Name()<< " stree alter value:"<<steer_value<<" throttle alter value:"<<throttle_value;
|
|
LOG(INFO)<<Name()<< " stree alter value:"<<steer_value<<" throttle alter value:"<<throttle_value;
|
|
-// SpeedControl(steer_value);
|
|
|
|
-// StreeControl(throttle_value);
|
|
|
|
-// decision::controlMsg control;
|
|
|
|
-// control.nowSpeed = fCarNowSpeed;
|
|
|
|
-// control.refSpeed = mpc.ref_speed;
|
|
|
|
-// control.steer = mpc.delta_prev;
|
|
|
|
-// control.throttle = mpc.a_prev;
|
|
|
|
-// control.cte = cte;
|
|
|
|
-// Control_pub.publish(control);
|
|
|
|
|
|
+ SpeedControl(steer_value);
|
|
|
|
+ StreeControl(throttle_value);
|
|
|
|
+ decision::controlMsg control;
|
|
|
|
+ control.nowSpeed = fCarNowSpeed;
|
|
|
|
+ control.refSpeed = mpc.ref_speed;
|
|
|
|
+ control.steer = mpc.delta_prev;
|
|
|
|
+ control.throttle = mpc.a_prev;
|
|
|
|
+ control.cte = cte;
|
|
|
|
+ Control_pub.publish(control);
|
|
}
|
|
}
|
|
|
|
|
|
unsigned int Decision::SpeedControl(double &speed)
|
|
unsigned int Decision::SpeedControl(double &speed)
|
|
{
|
|
{
|
|
-
|
|
|
|
canbus::PedalActCtrReq srv;
|
|
canbus::PedalActCtrReq srv;
|
|
if (mpc.a_prev >0){
|
|
if (mpc.a_prev >0){
|
|
srv.request.uiAccPsngCtrReq =(unsigned int)(speed *10);
|
|
srv.request.uiAccPsngCtrReq =(unsigned int)(speed *10);
|