|
@@ -6,7 +6,11 @@
|
|
|
#include <canbus/PedalActCtrReq.h>
|
|
|
#include <canbus/SftActCtrReq.h>
|
|
|
#include <canbus/StrActCtrReq.h>
|
|
|
+#include <decision/localizationMsg.h>
|
|
|
+#include <decision/rulesMsg.h>
|
|
|
+#include <decision/controlMsg.h>
|
|
|
#include <stdlib.h>
|
|
|
+#include <math.h>
|
|
|
|
|
|
|
|
|
namespace robot {
|
|
@@ -22,8 +26,8 @@ int Decision::Init(){
|
|
|
node_handle_.reset(new ros::NodeHandle());
|
|
|
DecisionDatabase::InitDB();
|
|
|
iCommandCmd = 0;// set undefied mode
|
|
|
- fRealCarSpeed = iLocalizationNumbers = iRulesNumbers = 0;
|
|
|
- iPredictionPoints = 20;
|
|
|
+ iRulesTotalNumbers = ilocalTotalNumbers =fCarNowSpeed = iLocalNowNumber = iRulesNowNumber = 0;
|
|
|
+ iPredictionPointsPerNumber = 20;
|
|
|
uiCarMode =6; // undefied
|
|
|
|
|
|
PlanRouteName = "siasun";
|
|
@@ -43,7 +47,13 @@ int Decision::Start(){
|
|
|
|
|
|
ros::Subscriber RCUModeSub = node_handle_->subscribe(robot::common::Si2Data, 2000, &Decision::RCUModeCallback, this);
|
|
|
|
|
|
- ros::AsyncSpinner spinner(4); // Use 4 threads
|
|
|
+ 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);
|
|
|
+
|
|
|
+ ros::AsyncSpinner spinner(6); // Use 6 threads
|
|
|
spinner.start();
|
|
|
spinner.stop();
|
|
|
ros::waitForShutdown();
|
|
@@ -63,20 +73,20 @@ bool Decision::CommandRequestService(decision::CommandModeCmd::Request &req,
|
|
|
LOG(INFO)<<Name()<<"CommandRequest get command ID:."<< req.Cmd;
|
|
|
if (req.Cmd == CMD_AUTODRIE_START)
|
|
|
{
|
|
|
- double breakCtr = -5;
|
|
|
+ double breakCtr = -5; // 踩刹车到50%
|
|
|
ros::Rate loop_rate(1);
|
|
|
while(ros::ok())
|
|
|
{
|
|
|
if (SpeedControl(breakCtr) == 1)
|
|
|
{
|
|
|
- // TODO send location and rule
|
|
|
- if (BreakCtrControl() == 1)
|
|
|
+ LoadPlanRoute();
|
|
|
+ if (BreakCtrControl(SFT_PSNG_LRN_REQ_FORWARD_1) == 1) // 前进挡
|
|
|
break;
|
|
|
}
|
|
|
ros::spinOnce();
|
|
|
loop_rate.sleep();
|
|
|
}
|
|
|
- fRealCarSpeed = iLocalizationNumbers = iRulesNumbers = 0;
|
|
|
+ fCarNowSpeed = iLocalNowNumber = iRulesNowNumber = 0;
|
|
|
}
|
|
|
else if (req.Cmd == CMD_PLAN_START)
|
|
|
{
|
|
@@ -89,11 +99,29 @@ bool Decision::CommandRequestService(decision::CommandModeCmd::Request &req,
|
|
|
|
|
|
void Decision::LoadPlanRoute()
|
|
|
{
|
|
|
- iLocalizationNumbers = DecisionDatabase::countOfLocationRuleInfo();
|
|
|
- iRulesNumbers = DecisionDatabase::countOfActionDirective();
|
|
|
- for (int i = 0; i< iLocalizationNumbers; i++)
|
|
|
+ ilocalTotalNumbers = DecisionDatabase::countOfLocationRuleInfo();
|
|
|
+ iRulesTotalNumbers = DecisionDatabase::countOfActionDirective();
|
|
|
+
|
|
|
+ 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;
|
|
|
+ localMsg.Latitude = tempInfo[i].LocationLat;
|
|
|
+ localMsg.Height = tempInfo[i].LocationHeight;
|
|
|
+ localMsg.LocationSpeed = tempInfo[i].LocationSpeed;
|
|
|
+ Geometry_pub.publish(localMsg);
|
|
|
+ }
|
|
|
+
|
|
|
+ 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;
|
|
|
+ Rules_pub.publish(ruleMsg);
|
|
|
}
|
|
|
|
|
|
}
|
|
@@ -121,8 +149,8 @@ void Decision::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
|
|
|
|
|
|
void Decision::OBDCallback(const canbus::obdMsg::ConstPtr& msg)
|
|
|
{
|
|
|
- fRealCarSpeed = msg->VehSpd;
|
|
|
- LOG(INFO)<<Name()<< "car real speed:"<<fRealCarSpeed;
|
|
|
+ fCarNowSpeed = msg->VehSpd;
|
|
|
+ LOG(INFO)<<Name()<< "car real speed:"<<fCarNowSpeed;
|
|
|
}
|
|
|
|
|
|
void Decision::RCUModeCallback(const perception::si2Msg::ConstPtr& msg)
|
|
@@ -135,41 +163,41 @@ void Decision::ExcutePlanRoute(const localization::localMsg::ConstPtr& msg)
|
|
|
{
|
|
|
try
|
|
|
{
|
|
|
- iLocalizationNumbers ++;
|
|
|
- if ((iLocalizationNumbers % iPredictionPoints) == 0)
|
|
|
+ iLocalNowNumber ++;
|
|
|
+ if ((iLocalNowNumber % iPredictionPointsPerNumber) == 0)
|
|
|
{
|
|
|
- iRulesNumbers++;
|
|
|
+ iRulesNowNumber++;
|
|
|
std::stringstream strRuleContent;
|
|
|
- if (iRulesNumbers == 1)
|
|
|
- strRuleContent<<"动作"<<iRulesNumbers<<":起步请加速到"<< fRealCarSpeed<<"km/h";
|
|
|
+ if (iRulesNowNumber == 1)
|
|
|
+ strRuleContent<<"动作"<<iRulesNowNumber<<":起步请加速到"<< fCarNowSpeed<<"km/h";
|
|
|
else{
|
|
|
- if (fRealCarSpeed > fLastCarSpeed)
|
|
|
+ if (fCarNowSpeed > fCarLastSpeed)
|
|
|
{
|
|
|
- strRuleContent<<"动作"<<iRulesNumbers<<":加速到"<< fRealCarSpeed<<"km/h";
|
|
|
+ strRuleContent<<"动作"<<iRulesNowNumber<<":加速到"<< fCarNowSpeed<<"km/h";
|
|
|
}
|
|
|
- else if (fRealCarSpeed < fLastCarSpeed)
|
|
|
+ else if (fCarNowSpeed < fCarLastSpeed)
|
|
|
{
|
|
|
- strRuleContent<<"动作"<<iRulesNumbers<<":制动至"<< fRealCarSpeed<<"km/h";
|
|
|
+ strRuleContent<<"动作"<<iRulesNowNumber<<":制动至"<< fCarNowSpeed<<"km/h";
|
|
|
}
|
|
|
- else if (fRealCarSpeed == fLastCarSpeed)
|
|
|
+ else if (fCarNowSpeed == fCarLastSpeed)
|
|
|
{
|
|
|
- strRuleContent<<"动作"<<iRulesNumbers<<":保持"<< fRealCarSpeed<<"km/h";
|
|
|
+ strRuleContent<<"动作"<<iRulesNowNumber<<":保持"<< fCarNowSpeed<<"km/h";
|
|
|
}
|
|
|
}
|
|
|
|
|
|
- int iRet = DecisionDatabase::InsertLocationRuleInfoAndActionDirective(PlanRouteName.c_str(), iRulesNumbers,
|
|
|
- strRuleContent.str().c_str(), 0, iLocalizationNumbers, msg->Longitude, msg->Latitude,
|
|
|
- msg->Height, fRealCarSpeed);
|
|
|
+ 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) {
|
|
|
- iLocalizationNumbers--;
|
|
|
- iRulesNumbers--;
|
|
|
+ iLocalNowNumber--;
|
|
|
+ iRulesNowNumber--;
|
|
|
}
|
|
|
- fLastCarSpeed = fRealCarSpeed;
|
|
|
+ fCarLastSpeed = fCarNowSpeed;
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
- DecisionDatabase::InsertLocationRuleInfo(PlanRouteName.c_str(),iLocalizationNumbers, msg->Longitude,
|
|
|
- msg->Latitude, msg->Height, fRealCarSpeed);
|
|
|
+ DecisionDatabase::InsertLocationRuleInfo(PlanRouteName.c_str(),iLocalNowNumber, msg->Longitude,
|
|
|
+ msg->Latitude, msg->Height, fCarNowSpeed);
|
|
|
}
|
|
|
}
|
|
|
catch (ros::Exception ex)
|
|
@@ -181,33 +209,33 @@ void Decision::ExcutePlanRoute(const localization::localMsg::ConstPtr& msg)
|
|
|
|
|
|
void Decision::StopPlanRoute(const localization::localMsg::ConstPtr& msg)
|
|
|
{
|
|
|
- if ((iLocalizationNumbers % iPredictionPoints) == 0)
|
|
|
+ if ((iLocalNowNumber % iPredictionPointsPerNumber) == 0)
|
|
|
{
|
|
|
return;
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
- iLocalizationNumbers++;
|
|
|
- iRulesNumbers++;
|
|
|
+ iLocalNowNumber++;
|
|
|
+ iRulesNowNumber++;
|
|
|
std::stringstream strRuleContent;
|
|
|
- if (fRealCarSpeed > fLastCarSpeed)
|
|
|
+ if (fCarNowSpeed > fCarLastSpeed)
|
|
|
{
|
|
|
- strRuleContent<<"动作"<<iRulesNumbers<<":加速到"<< fRealCarSpeed<<"km/h";
|
|
|
+ strRuleContent<<"动作"<<iRulesNowNumber<<":加速到"<< fCarNowSpeed<<"km/h";
|
|
|
}
|
|
|
- else if (fRealCarSpeed < fLastCarSpeed)
|
|
|
+ else if (fCarNowSpeed < fCarLastSpeed)
|
|
|
{
|
|
|
- strRuleContent<<"动作"<<iRulesNumbers<<":制动至"<< fRealCarSpeed<<"km/h";
|
|
|
+ strRuleContent<<"动作"<<iRulesNowNumber<<":制动至"<< fCarNowSpeed<<"km/h";
|
|
|
}
|
|
|
- else if (fRealCarSpeed == fLastCarSpeed)
|
|
|
+ else if (fCarNowSpeed == fCarLastSpeed)
|
|
|
{
|
|
|
- strRuleContent<<"动作"<<iRulesNumbers<<":保持"<< fRealCarSpeed<<"km/h";
|
|
|
+ strRuleContent<<"动作"<<iRulesNowNumber<<":保持"<< fCarNowSpeed<<"km/h";
|
|
|
}
|
|
|
- int iRet = DecisionDatabase::InsertLocationRuleInfoAndActionDirective(PlanRouteName.c_str(), iRulesNumbers,
|
|
|
- strRuleContent.str().c_str(), 0, iLocalizationNumbers, msg->Longitude, msg->Latitude,
|
|
|
- msg->Height, fRealCarSpeed);
|
|
|
+ int iRet = DecisionDatabase::InsertLocationRuleInfoAndActionDirective(PlanRouteName.c_str(), iRulesNowNumber,
|
|
|
+ strRuleContent.str().c_str(), 0, iLocalNowNumber, msg->Longitude, msg->Latitude,
|
|
|
+ msg->Height, fCarNowSpeed);
|
|
|
if (iRet < 0) {
|
|
|
- iLocalizationNumbers--;
|
|
|
- iRulesNumbers--;
|
|
|
+ iLocalNowNumber--;
|
|
|
+ iRulesNowNumber--;
|
|
|
}
|
|
|
}
|
|
|
}
|
|
@@ -217,7 +245,109 @@ void Decision::ExcuteSelfDriving(const localization::localMsg::ConstPtr& msg)
|
|
|
{
|
|
|
// 判断当前RCU是否为自动驾驶模式
|
|
|
if (uiCarMode != CTR_MD_REQ_AUTO_DRIVE)
|
|
|
- return ;
|
|
|
+ return;
|
|
|
+ if (CarAutoDriveStop())
|
|
|
+ return;
|
|
|
+ int iPredictPoints;
|
|
|
+ if (ilocalTotalNumbers < (iLocalNowNumber + iPredictionPointsPerNumber))
|
|
|
+ {
|
|
|
+ iPredictPoints = ilocalTotalNumbers - iLocalNowNumber;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ iPredictPoints = iPredictionPointsPerNumber;
|
|
|
+ }
|
|
|
+ std::vector<LOCATION_RULE_INFO> tempInfo = DecisionDatabase::QueryLocationRuleInfo(iLocalNowNumber, iPredictPoints);
|
|
|
+ std::vector<double> ptsy(iPredictPoints);
|
|
|
+ std::vector<double> ptsx(iPredictPoints);
|
|
|
+ for (int i= 0; i< iPredictPoints; i++)
|
|
|
+ {
|
|
|
+ ptsx.push_back( tempInfo[i].LocationLon);
|
|
|
+ ptsy.push_back( tempInfo[i].LocationLat);
|
|
|
+ }
|
|
|
+ mpc.ref_speed = tempInfo[iPredictPoints-1].LocationSpeed;
|
|
|
+ double px = msg->Longitude;
|
|
|
+ double py = msg->Latitude;
|
|
|
+ double psi = deg2rad(msg->AngleDeviated + 90);
|
|
|
+ double v = (double)fCarNowSpeed;
|
|
|
+ MPC_Controller(ptsx, ptsy, px, py, psi, v);
|
|
|
+ UpdataCarRuleResults();
|
|
|
+ iLocalNowNumber ++;
|
|
|
+}
|
|
|
+
|
|
|
+bool Decision::CarAutoDriveStop()
|
|
|
+{
|
|
|
+ try
|
|
|
+ {
|
|
|
+ if (ilocalTotalNumbers = iLocalNowNumber)
|
|
|
+ {
|
|
|
+ uiCarMode = CMD_AUTODRIE_END;
|
|
|
+ double breakCtr = -5; // 踩刹车到50%
|
|
|
+ ros::Rate loop_rate(1);
|
|
|
+ while(ros::ok())
|
|
|
+ {
|
|
|
+ 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
|
|
|
+ {
|
|
|
+ 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;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ catch (ros::Exception ex)
|
|
|
+ {
|
|
|
+ LOG(ERROR)<<Name()<<":CarAutoDriveStop error."<< ex.what();
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+bool Decision::UpdataCarRuleResults()
|
|
|
+{
|
|
|
+ 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 ++;
|
|
|
+ }
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+ catch (ros::Exception ex)
|
|
|
+ {
|
|
|
+ LOG(ERROR)<<Name()<<":UpdataCarRuleResults error."<< ex.what();
|
|
|
+ return false;
|
|
|
+ }
|
|
|
}
|
|
|
|
|
|
void Decision::MPC_Controller(std::vector<double> &ptsx, std::vector<double> &ptsy,
|
|
@@ -243,6 +373,12 @@ void Decision::MPC_Controller(std::vector<double> &ptsx, std::vector<double> &pt
|
|
|
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_pub.publish(control);
|
|
|
}
|
|
|
|
|
|
unsigned int Decision::SpeedControl(double &speed)
|
|
@@ -290,11 +426,11 @@ unsigned int Decision::StreeControl(double &streer)
|
|
|
return srv.response.uiReturn;
|
|
|
}
|
|
|
|
|
|
-unsigned int Decision::BreakCtrControl()
|
|
|
+unsigned int Decision::BreakCtrControl(unsigned int breakmode)
|
|
|
{
|
|
|
ros::ServiceClient clientSftActCtrReq = node_handle_->serviceClient<canbus::PedalActCtrReq>(robot::common::ShiftActCtrService);
|
|
|
canbus::SftActCtrReq srv;
|
|
|
- srv.request.uiSftPsngCtrReq = SFT_PSNG_LRN_REQ_FORWARD_1;
|
|
|
+ srv.request.uiSftPsngCtrReq = breakmode;
|
|
|
srv.request.uiSftMdReq = SFT_MD_REQ_2_SECOND;
|
|
|
srv.request.uiClchPsngCtrReq = 1;
|
|
|
srv.request.iClchSpdCtrReq = 0;
|