|
|
@@ -25,12 +25,41 @@ int Decision::Init(){
|
|
|
LOG(INFO)<<Name()<<": init() start...";
|
|
|
node_handle_.reset(new ros::NodeHandle());
|
|
|
DecisionDatabase::InitDB();
|
|
|
- iCommandCmd = 0;// set undefied mode
|
|
|
+ iCommandCmd = CMD_UNDEFIED;// set undefied mode
|
|
|
iRulesTotalNumbers = ilocalTotalNumbers =fCarNowSpeed = iLocalNowNumber = iRulesNowNumber = 0;
|
|
|
- iPredictionPointsPerNumber = 20;
|
|
|
+ iPredictionPointsPerNumber = 5;
|
|
|
uiCarMode =6; // undefied
|
|
|
|
|
|
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.";
|
|
|
return ROBOT_SUCCESS;
|
|
|
|
|
|
@@ -57,13 +86,13 @@ int Decision::Start(){
|
|
|
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);
|
|
|
+ clientSftActCtrReq = node_handle_->serviceClient<canbus::SftActCtrReq>(robot::common::ShiftActCtrService);
|
|
|
|
|
|
clientPedalActCtrReq = node_handle_->serviceClient<canbus::PedalActCtrReq>(robot::common::PedalActCtrService);
|
|
|
|
|
|
- clientStrActCtrReq = node_handle_->serviceClient<canbus::PedalActCtrReq>(robot::common::SteerActCtrService);
|
|
|
+ clientStrActCtrReq = node_handle_->serviceClient<canbus::StrActCtrReq>(robot::common::SteerActCtrService);
|
|
|
|
|
|
- ros::AsyncSpinner spinner(6); // Use 6 threads
|
|
|
+ ros::AsyncSpinner spinner(2); // thread_count The number of threads to use. A value of 0 means to use the number of processor cores
|
|
|
spinner.start();
|
|
|
ros::waitForShutdown();
|
|
|
LOG(INFO)<<Name()<<" start() end.";
|
|
|
@@ -89,13 +118,14 @@ bool Decision::CommandRequestService(decision::CommandModeCmd::Request &req,
|
|
|
while(ros::ok())
|
|
|
{
|
|
|
if (SpeedControl(breakCtr) == 1)
|
|
|
- {
|
|
|
+ {
|
|
|
+ LOG(INFO)<<Name()<<"CommandRequestService break control 50% excute success.";
|
|
|
if (BreakCtrControl(SFT_PSNG_LRN_REQ_FORWARD_1) == 1) // 前进挡
|
|
|
break;
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
- break;
|
|
|
+ LOG(ERROR)<<Name()<<"CommandRequestService break control 50% excute error.";
|
|
|
return false;
|
|
|
}
|
|
|
ros::spinOnce();
|
|
|
@@ -146,6 +176,7 @@ void Decision::LoadPlanRoute()
|
|
|
// 接收ＲＴＫ数据分为自动驾驶模式开始和结束和路径规划模式开始和结束
|
|
|
void Decision::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
|
|
|
{
|
|
|
+// double secsStart =ros::Time::now().toSec();
|
|
|
switch (iCommandCmd) {
|
|
|
case CMD_PLAN_START:
|
|
|
ExcutePlanRoute(msg);
|
|
|
@@ -157,33 +188,35 @@ void Decision::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
|
|
|
StopPlanRoute(msg);
|
|
|
break;
|
|
|
case CMD_AUTODRIE_END:
|
|
|
+ CarAutoDriveStop();
|
|
|
break;
|
|
|
default:
|
|
|
break;
|
|
|
}
|
|
|
+// double secsEnd =ros::Time::now().toSec();
|
|
|
+// ros::Duration(2 - secsEnd + secsStart).sleep();
|
|
|
}
|
|
|
|
|
|
|
|
|
void Decision::OBDCallback(const canbus::obdMsg::ConstPtr& msg)
|
|
|
{
|
|
|
fCarNowSpeed = msg->VehSpd;
|
|
|
- LOG(INFO)<<Name()<< " car real speed:"<<fCarNowSpeed;
|
|
|
+ //LOG(INFO)<<Name()<< " car real speed:"<<fCarNowSpeed;
|
|
|
}
|
|
|
|
|
|
void Decision::RCUModeCallback(const perception::si2Msg::ConstPtr& msg)
|
|
|
{
|
|
|
uiCarMode = msg->CtrMdStat;
|
|
|
- LOG(INFO)<<Name()<< " car excute mode :"<<uiCarMode;
|
|
|
+ //LOG(INFO)<<Name()<< " car excute mode :"<<uiCarMode;
|
|
|
}
|
|
|
|
|
|
//自动生成测试用例
|
|
|
void Decision::ExcutePlanRoute(const localization::localMsg::ConstPtr& msg)
|
|
|
{
|
|
|
+ double secsStart =ros::Time::now().toSec();
|
|
|
try {
|
|
|
iLocalNowNumber ++;
|
|
|
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;
|
|
|
@@ -222,12 +255,18 @@ void Decision::ExcutePlanRoute(const localization::localMsg::ConstPtr& msg)
|
|
|
{
|
|
|
LOG(ERROR)<<Name()<<":ExcutePlanRoute error."<< ex.what();
|
|
|
}
|
|
|
+ double secsEnd =ros::Time::now().toSec();
|
|
|
+ ros::Duration(2 - secsEnd + secsStart).sleep();
|
|
|
}
|
|
|
|
|
|
// 自动生成测试用例停止
|
|
|
void Decision::StopPlanRoute(const localization::localMsg::ConstPtr& msg)
|
|
|
{
|
|
|
+ LOG(INFO)<<Name()<<"StopPlanRoute last record iLocalNowNumber = "<< iLocalNowNumber;
|
|
|
+ iCommandCmd = CMD_UNDEFIED;// set undefied mode
|
|
|
if ((iLocalNowNumber % iPredictionPointsPerNumber) == 0){
|
|
|
+ LOG(INFO)<<Name()<<"StopPlanRoute last record iLocalNowNumber = "<< iLocalNowNumber
|
|
|
+ << " iLocalNowNumber % iPredictionPointsPerNumber = 0";
|
|
|
return;
|
|
|
}
|
|
|
else{
|
|
|
@@ -250,6 +289,7 @@ void Decision::StopPlanRoute(const localization::localMsg::ConstPtr& msg)
|
|
|
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--;
|
|
|
}
|
|
|
@@ -260,8 +300,12 @@ void Decision::StopPlanRoute(const localization::localMsg::ConstPtr& msg)
|
|
|
void Decision::ExcuteSelfDriving(const localization::localMsg::ConstPtr& msg)
|
|
|
{
|
|
|
// 判断当前RCU是否为自动驾驶模式
|
|
|
- if (uiCarMode != CTR_MD_REQ_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;
|
|
|
@@ -271,16 +315,28 @@ void Decision::ExcuteSelfDriving(const localization::localMsg::ConstPtr& msg)
|
|
|
else{
|
|
|
iPredictPoints = iPredictionPointsPerNumber;
|
|
|
}
|
|
|
- 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++)
|
|
|
+ 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))))
|
|
|
{
|
|
|
- ptsx.push_back( tempInfo[i].LocationLon);
|
|
|
- ptsy.push_back( tempInfo[i].LocationLat);
|
|
|
+ LOG(INFO)<<Name()<<" ExcuteSelfDriving plan route does not alter.";
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ std::vector<LOCATION_RULE_INFO> tempInfo;
|
|
|
+ LOG(INFO)<<Name()<<" ExcuteSelfDriving QueryLocationRuleInfo start.";
|
|
|
+ tempInfo = DecisionDatabase::QueryLocationRuleInfo(iLocalNowNumber, iPredictPoints);
|
|
|
+ ptsx.clear();
|
|
|
+ ptsy.clear();
|
|
|
+ for (int i= 0; i< iPredictPoints; i++)
|
|
|
+ {
|
|
|
+ ptsx.push_back( tempInfo[i].LocationLon);
|
|
|
+ ptsy.push_back( tempInfo[i].LocationLat);
|
|
|
+ }
|
|
|
+ mpc.ref_speed = tempInfo[1].LocationSpeed;
|
|
|
+ LOG(INFO)<<Name()<<" ExcuteSelfDriving plan route altered.";
|
|
|
}
|
|
|
- mpc.ref_speed = tempInfo[iPredictPoints-1].LocationSpeed;
|
|
|
double px = msg->Longitude;
|
|
|
double py = msg->Latitude;
|
|
|
double psi = deg2rad(msg->AngleDeviated + 90);
|
|
|
@@ -290,11 +346,57 @@ void Decision::ExcuteSelfDriving(const localization::localMsg::ConstPtr& msg)
|
|
|
iLocalNowNumber ++;
|
|
|
}
|
|
|
|
|
|
+void Decision::TestMPC()
|
|
|
+{
|
|
|
+ if (CarAutoDriveStop())
|
|
|
+ return;
|
|
|
+ int iPredictPoints;
|
|
|
+ if (ilocalTotalNumbers < (iLocalNowNumber + iPredictionPointsPerNumber)){
|
|
|
+ iPredictPoints = ilocalTotalNumbers - iLocalNowNumber;
|
|
|
+ }
|
|
|
+ else{
|
|
|
+ iPredictPoints = iPredictionPointsPerNumber;
|
|
|
+ }
|
|
|
+ if ((iLocalNowNumber > 0)&&((((vPx[iLocalNowNumber] - ptsx[0])>0) && ((vPx[iLocalNowNumber] - ptsx[1])< 0) ||
|
|
|
+ ((vPx[iLocalNowNumber] - ptsx[0])<0) && ((vPx[iLocalNowNumber] - ptsx[1])> 0))&&
|
|
|
+ (((vPy[iLocalNowNumber] - ptsy[0])>0) && ((vPy[iLocalNowNumber] - ptsy[1])< 0) ||
|
|
|
+ ((vPy[iLocalNowNumber] - ptsy[0])<0) && ((vPy[iLocalNowNumber]- ptsy[1])> 0))))
|
|
|
+ {
|
|
|
+ LOG(INFO)<<Name()<<" ExcuteSelfDriving plan route does not alter.";
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ std::vector<LOCATION_RULE_INFO> tempInfo;
|
|
|
+ LOG(INFO)<<Name()<<" ExcuteSelfDriving QueryLocationRuleInfo start.";
|
|
|
+ tempInfo = DecisionDatabase::QueryLocationRuleInfo(iLocalNowNumber, iPredictPoints);
|
|
|
+ ptsx.clear();
|
|
|
+ ptsy.clear();
|
|
|
+ for (int i= 0; i< iPredictPoints; i++)
|
|
|
+ {
|
|
|
+ ptsx.push_back( tempInfo[i].LocationLon);
|
|
|
+ ptsy.push_back( tempInfo[i].LocationLat);
|
|
|
+ }
|
|
|
+ mpc.ref_speed = tempInfo[1].LocationSpeed;
|
|
|
+ LOG(INFO)<<Name()<<" ExcuteSelfDriving plan route altered.";
|
|
|
+ }
|
|
|
+
|
|
|
+ double px = vPx[iLocalNowNumber];
|
|
|
+ double py = vPy[iLocalNowNumber];
|
|
|
+ double psi = deg2rad(40.45 + 90);
|
|
|
+ double v = (double)fCarNowSpeed;
|
|
|
+ LOG(INFO)<<Name()<< " MPC start";
|
|
|
+ MPC_Controller(ptsx, ptsy, px, py, psi, v);
|
|
|
+ LOG(INFO)<<Name()<< " MPC end";
|
|
|
+ //UpdataCarRuleResults();
|
|
|
+ iLocalNowNumber ++;
|
|
|
+}
|
|
|
+
|
|
|
// 自动驾驶停止
|
|
|
bool Decision::CarAutoDriveStop()
|
|
|
{
|
|
|
try{
|
|
|
- if (ilocalTotalNumbers = iLocalNowNumber){
|
|
|
+
|
|
|
+ if (ilocalTotalNumbers == iLocalNowNumber){
|
|
|
uiCarMode = CMD_AUTODRIE_END;
|
|
|
double breakCtr = -5; // 踩刹车到50%
|
|
|
ros::Rate loop_rate(1);
|
|
|
@@ -336,7 +438,7 @@ bool Decision::CarAutoDriveStop()
|
|
|
bool Decision::UpdataCarRuleResults()
|
|
|
{
|
|
|
try{
|
|
|
- if (iLocalNowNumber >0 && (iLocalNowNumber % iPredictionPointsPerNumber == 0)){
|
|
|
+ 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
|
|
|
{
|
|
|
@@ -369,6 +471,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);
|
|
|
+ }
|
|
|
// 获取状态值
|
|
|
auto coeffs = polyfit(Ptsx, Ptsy, 3);
|
|
|
double cte = evaluateCte(coeffs);
|
|
|
@@ -378,20 +483,33 @@ void Decision::MPC_Controller(std::vector<double> &ptsx, std::vector<double> &pt
|
|
|
// 计算方向和油门，范围都在[-1, 1]之间
|
|
|
// 这个地方有延迟，取第三个
|
|
|
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()<<" MPC_Controller prediction LocationLon = "<<std::setprecision(16)<<mpc_x_vals[i]
|
|
|
+ <<" LocationLat = "<< std::setprecision(16)<<mpc_y_vals[i];
|
|
|
+ i++;
|
|
|
+ }
|
|
|
double steer_value = result[2][mpc.latency];
|
|
|
double throttle_value = result[3][mpc.latency];
|
|
|
mpc.delta_prev = steer_value;
|
|
|
mpc.a_prev = 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);
|
|
|
+ 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);
|
|
|
}
|
|
|
|
|
|
unsigned int Decision::SpeedControl(double &speed)
|
|
|
@@ -412,11 +530,11 @@ unsigned int Decision::SpeedControl(double &speed)
|
|
|
srv.request.uiBrkFocCtrReq = 350;
|
|
|
if (clientPedalActCtrReq.call(srv))
|
|
|
{
|
|
|
- LOG(INFO)<<Name()<<"Speed Control service return success, value = "<<srv.response.uiReturn;
|
|
|
+ LOG(INFO)<<Name()<<" Speed Control service return success, value = "<<srv.response.uiReturn;
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
- LOG(ERROR)<<Name()<<"Speed Control service return falil";
|
|
|
+ LOG(ERROR)<<Name()<<" Speed Control service return falil";
|
|
|
}
|
|
|
return srv.response.uiReturn;
|
|
|
}
|
|
|
@@ -432,11 +550,11 @@ unsigned int Decision::StreeControl(double &streer)
|
|
|
srv.request.iStrTrqCtrReq = 50;
|
|
|
if (clientStrActCtrReq.call(srv))
|
|
|
{
|
|
|
- LOG(INFO)<<Name()<<"stree Control service return success, value = "<<srv.response.uiReturn;
|
|
|
+ LOG(INFO)<<Name()<<" stree Control service return success, value = "<<srv.response.uiReturn;
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
- LOG(ERROR)<<Name()<<"stree Control service return falil";
|
|
|
+ LOG(ERROR)<<Name()<<" stree Control service return falil";
|
|
|
}
|
|
|
return srv.response.uiReturn;
|
|
|
}
|
|
|
@@ -451,11 +569,11 @@ unsigned int Decision::BreakCtrControl(unsigned int breakmode)
|
|
|
srv.request.iClchSpdCtrReq = 0;
|
|
|
if (clientSftActCtrReq.call(srv))
|
|
|
{
|
|
|
- LOG(INFO)<<Name()<<"break Control service return success, value = "<<srv.response.uiReturn;
|
|
|
+ LOG(INFO)<<Name()<<" break Control service return success, value = "<<srv.response.uiReturn;
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
- LOG(ERROR)<<Name()<<"break Control service return falil";
|
|
|
+ LOG(ERROR)<<Name()<<" break Control service return falil";
|
|
|
}
|
|
|
return srv.response.uiReturn;
|
|
|
}
|
|
|
@@ -488,7 +606,7 @@ unsigned int Decision::BreakCtrControl(unsigned int breakmode)
|
|
|
return -atan(coeffs[1]);
|
|
|
}
|
|
|
|
|
|
- // 把全局坐标转换为汽车坐标，以当前汽车的位置作为远点
|
|
|
+ // 把全局坐标转换为汽车坐标，以当前汽车的位置作为原点
|
|
|
Eigen::MatrixXd Decision::transformGlobal2Vehicle(double x, double y, double psi,
|
|
|
const vector<double> & ptsx, const vector<double> & ptsy) {
|
|
|
assert(ptsx.size() == ptsy.size());
|
