|
@@ -1,3 +1,8 @@
|
|
|
|
+/******************************************************************************
|
|
|
|
+ * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
|
|
|
|
+ * Author: madesheng
|
|
|
|
+ * Licensed under the Apache License, Version 1.0 (the "License");
|
|
|
|
+ *****************************************************************************/
|
|
#include "decision.h"
|
|
#include "decision.h"
|
|
#include "../common/message.h"
|
|
#include "../common/message.h"
|
|
#include "decisiondatatype.h"
|
|
#include "decisiondatatype.h"
|
|
@@ -28,7 +33,7 @@ int Decision::Init(){
|
|
iCommandCmd = CMD_UNDEFIED;// set undefied mode
|
|
iCommandCmd = CMD_UNDEFIED;// set undefied mode
|
|
iAbandon = iRulesTotalNumbers = ilocalTotalNumbers =fCarNowSpeed = iLocalNowNumber = iRulesNowNumber = 0;
|
|
iAbandon = iRulesTotalNumbers = ilocalTotalNumbers =fCarNowSpeed = iLocalNowNumber = iRulesNowNumber = 0;
|
|
iPredictionPointsPerNumber = 5;
|
|
iPredictionPointsPerNumber = 5;
|
|
- uiCarMode =6; // undefied
|
|
|
|
|
|
+ uiCarMode = CTR_MD_REQ_ROBOT_UNDIFED; // undefied
|
|
fCarNowSpeed = iLocalNowNumber = iRulesNowNumber = 0;
|
|
fCarNowSpeed = iLocalNowNumber = iRulesNowNumber = 0;
|
|
PlanRouteName = "siasun";
|
|
PlanRouteName = "siasun";
|
|
// for (int i =0; i< 10; i++)
|
|
// for (int i =0; i< 10; i++)
|
|
@@ -92,6 +97,10 @@ bool Decision::CommandRequestService(decision::CommandModeCmd::Request &req,
|
|
{
|
|
{
|
|
if (SpeedControl(throttle_Last) == 1)
|
|
if (SpeedControl(throttle_Last) == 1)
|
|
{
|
|
{
|
|
|
|
+<<<<<<< HEAD
|
|
|
|
+=======
|
|
|
|
+ ros::Duration(2).sleep(); // sleep for two second
|
|
|
|
+>>>>>>> 51b16792f0443ff81a2e13d8a533e12f91083390
|
|
LOG(INFO)<<Name()<<"CommandRequestService break control 70% excute success.";
|
|
LOG(INFO)<<Name()<<"CommandRequestService break control 70% excute success.";
|
|
if (BreakCtrControl(SFT_PSNG_LRN_REQ_FORWARD_1) == 1) // 前进挡
|
|
if (BreakCtrControl(SFT_PSNG_LRN_REQ_FORWARD_1) == 1) // 前进挡
|
|
break;
|
|
break;
|
|
@@ -201,7 +210,7 @@ void Decision::ExcutePlanRoute(const localization::localMsg::ConstPtr& msg)
|
|
|
|
|
|
try {
|
|
try {
|
|
iLocalNowNumber ++;
|
|
iLocalNowNumber ++;
|
|
- LOG(INFO)<<Name()<<"ExcutePlanRoute iLocalNowNumber = "<< iLocalNowNumber;
|
|
|
|
|
|
+ LOG(INFO)<<Name()<<"ExcutePlanRoute iLocalNowNumber = "<< iLocalNowNumber;
|
|
iRulesNowNumber++;
|
|
iRulesNowNumber++;
|
|
std::stringstream strRuleContent;
|
|
std::stringstream strRuleContent;
|
|
if (iRulesNowNumber == 1)
|
|
if (iRulesNowNumber == 1)
|
|
@@ -288,7 +297,7 @@ void Decision::ExcuteSelfDriving(const localization::localMsg::ConstPtr& msg)
|
|
(((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))||
|
|
bool bRetLat = (((ptsy[1] - ptsy[0])>= 0)&&((py - ptsy[1])> 0))||
|
|
(((ptsy[1] - ptsy[0])< 0)&&((py - ptsy[1])< 0));
|
|
(((ptsy[1] - ptsy[0])< 0)&&((py - ptsy[1])< 0));
|
|
- if (bRetLon|| bRetLat){
|
|
|
|
|
|
+ if (bRetLon|| bRetLat){
|
|
std::vector<LOCATION_RULE_INFO> tempInfo;
|
|
std::vector<LOCATION_RULE_INFO> tempInfo;
|
|
tempInfo = DecisionDatabase::QueryLocationRuleInfo(iLocalNowNumber, iPredictPoints);
|
|
tempInfo = DecisionDatabase::QueryLocationRuleInfo(iLocalNowNumber, iPredictPoints);
|
|
ptsx.clear();
|
|
ptsx.clear();
|
|
@@ -300,13 +309,13 @@ void Decision::ExcuteSelfDriving(const localization::localMsg::ConstPtr& msg)
|
|
}
|
|
}
|
|
mpc.ref_speed = tempInfo[1].LocationSpeed;
|
|
mpc.ref_speed = tempInfo[1].LocationSpeed;
|
|
iLocalNowNumber ++;
|
|
iLocalNowNumber ++;
|
|
- UpdataCarRuleResults();
|
|
|
|
|
|
+ UpdataCarRuleResults();
|
|
LOG(INFO)<<Name()<<" ExcuteSelfDriving plan route altered.";
|
|
LOG(INFO)<<Name()<<" ExcuteSelfDriving plan route altered.";
|
|
}
|
|
}
|
|
else{
|
|
else{
|
|
LOG(INFO)<<Name()<<" ExcuteSelfDriving plan route does not alter.";
|
|
LOG(INFO)<<Name()<<" ExcuteSelfDriving plan route does not alter.";
|
|
}
|
|
}
|
|
- double psi = deg2rad(msg->AngleDeviated + 90);
|
|
|
|
|
|
+ double psi = msg->AngleDeviated + 90;
|
|
double v = (double)fCarNowSpeed;
|
|
double v = (double)fCarNowSpeed;
|
|
MPC_Controller(ptsx, ptsy, px, py, psi, v);
|
|
MPC_Controller(ptsx, ptsy, px, py, psi, v);
|
|
|
|
|
|
@@ -337,16 +346,23 @@ bool Decision::CarAutoDriveStop()
|
|
if (ilocalTotalNumbers == iLocalNowNumber + iPredictionPointsPerNumber){
|
|
if (ilocalTotalNumbers == iLocalNowNumber + iPredictionPointsPerNumber){
|
|
iCommandCmd = CMD_AUTODRIE_END;
|
|
iCommandCmd = CMD_AUTODRIE_END;
|
|
int breakCtr = -70; // 踩刹车到70%
|
|
int breakCtr = -70; // 踩刹车到70%
|
|
|
|
+ //int breakCtr = (iRulesTotalNumbers -iRulesNowNumber) *10 -70; // 踩刹车到70%
|
|
|
|
+
|
|
//iLocalNowNumber ++;
|
|
//iLocalNowNumber ++;
|
|
while(true){
|
|
while(true){
|
|
- if (SpeedControl(breakCtr) == 1)
|
|
|
|
- {
|
|
|
|
|
|
+ if (SpeedControl(breakCtr) == 1){
|
|
|
|
+ ros::Duration(2).sleep(); // sleep for two second
|
|
if (BreakCtrControl(SFT_PSNG_LRN_REQ_PARK) == 1) // P挡
|
|
if (BreakCtrControl(SFT_PSNG_LRN_REQ_PARK) == 1) // P挡
|
|
break;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
if (iRulesNowNumber == iRulesTotalNumbers)
|
|
if (iRulesNowNumber == iRulesTotalNumbers)
|
|
return true;
|
|
return true;
|
|
|
|
+ //if (iRulesNowNumber == iRulesTotalNumbers){
|
|
|
|
+ //iCommandCmd = CMD_UNDEFIED;
|
|
|
|
+ //return true;
|
|
|
|
+ // }
|
|
|
|
+
|
|
iRulesNowNumber ++;
|
|
iRulesNowNumber ++;
|
|
decision::rulesMsg ruleMsg;
|
|
decision::rulesMsg ruleMsg;
|
|
if (std::abs(fCarNowSpeed - mpc.ref_speed) < 1) //低速( <10km/h):± 0.25km/h 高速( >10km/h):± 1km/h
|
|
if (std::abs(fCarNowSpeed - mpc.ref_speed) < 1) //低速( <10km/h):± 0.25km/h 高速( >10km/h):± 1km/h
|
|
@@ -455,8 +471,8 @@ unsigned int Decision::SpeedControl(int &speed)
|
|
srv.request.uiBrkPsngCtrReq = -speed *10;
|
|
srv.request.uiBrkPsngCtrReq = -speed *10;
|
|
}
|
|
}
|
|
srv.request.iAccSpdCtrReq = 20;
|
|
srv.request.iAccSpdCtrReq = 20;
|
|
- srv.request.iBrkSpdCtrReq = 50;
|
|
|
|
srv.request.uiBrkCtrMdReq = 1;
|
|
srv.request.uiBrkCtrMdReq = 1;
|
|
|
|
+ srv.request.iBrkSpdCtrReq = 90;
|
|
srv.request.uiBrkFocCtrReq = 350;
|
|
srv.request.uiBrkFocCtrReq = 350;
|
|
if (clientPedalActCtrReq.call(srv))
|
|
if (clientPedalActCtrReq.call(srv))
|
|
{
|
|
{
|
|
@@ -476,7 +492,7 @@ unsigned int Decision::StreeControl(int &streer)
|
|
canbus::StrActCtrReq srv;
|
|
canbus::StrActCtrReq srv;
|
|
srv.request.iStrAngCtrReq = streer *100;
|
|
srv.request.iStrAngCtrReq = streer *100;
|
|
srv.request.uiStrCtrMdReq =1;
|
|
srv.request.uiStrCtrMdReq =1;
|
|
- srv.request.iStrSpdCtrReq = 200;
|
|
|
|
|
|
+ srv.request.iStrSpdCtrReq = 290;
|
|
srv.request.iStrTrqCtrReq = 50;
|
|
srv.request.iStrTrqCtrReq = 50;
|
|
if (clientStrActCtrReq.call(srv))
|
|
if (clientStrActCtrReq.call(srv))
|
|
{
|
|
{
|
|
@@ -491,7 +507,7 @@ unsigned int Decision::StreeControl(int &streer)
|
|
|
|
|
|
|
|
|
|
unsigned int Decision::BreakCtrControl(unsigned int breakmode)
|
|
unsigned int Decision::BreakCtrControl(unsigned int breakmode)
|
|
-{
|
|
|
|
|
|
+{
|
|
canbus::SftActCtrReq srv;
|
|
canbus::SftActCtrReq srv;
|
|
srv.request.uiSftPsngCtrReq = breakmode;
|
|
srv.request.uiSftPsngCtrReq = breakmode;
|
|
srv.request.uiSftMdReq = SFT_MD_REQ_2_SECOND;
|
|
srv.request.uiSftMdReq = SFT_MD_REQ_2_SECOND;
|