|
@@ -1,10 +1,15 @@
|
|
|
#include "decision.h"
|
|
|
-
|
|
|
+#include "../common/message.h"
|
|
|
+#include <strstream>
|
|
|
#include <string.h>
|
|
|
+#include <canbus/PedalActCtrReq.h>
|
|
|
+#include <canbus/SftActCtrReq.h>
|
|
|
+#include <canbus/StrActCtrReq.h>
|
|
|
+#include <stdlib.h>
|
|
|
|
|
|
|
|
|
namespace robot {
|
|
|
-namespace decision {
|
|
|
+namespace decisionSpace {
|
|
|
|
|
|
|
|
|
|
|
@@ -13,28 +18,28 @@ std::string Decision::Name() const { return "decision"; }
|
|
|
int Decision::Init(){
|
|
|
|
|
|
LOG(INFO)<<Name()<<":Decision init...";
|
|
|
+ node_handle_.reset(new ros::NodeHandle());
|
|
|
Database::InitDB();
|
|
|
- Database::InsertActionDirective("1",1,"1",1);
|
|
|
- Database::InsertLocationMap(0.1,0.1,0.1,0.1,0.1,0.1,"1",0.1,0.1);
|
|
|
- Database::InsertLocationRuleInfo("1",1,0.1,0.1,0.1,0.1);
|
|
|
- Database::UpdateActionDirective("1",1,2);
|
|
|
- Database::UpdateLocationMap(0.2,0.2,0.2,0.2,0.2,0.2,0.2,0.2,"1");
|
|
|
-// Database::DeleteActionDirective("1");
|
|
|
- Database::DeleteActionDirective(1);
|
|
|
- Database::DeleteLocationMap("1");
|
|
|
-// Database::DeleteLocationRuleInfo("1");
|
|
|
- Database::DeleteLocationRuleInfo(1);
|
|
|
- return 0;
|
|
|
-
|
|
|
+ iCommandCmd = 0;// set undefied mode
|
|
|
+ realCarSpeed = localizationNumbers = RulesNumbers =0;
|
|
|
+ PredictionPoints = 20;
|
|
|
LOG(INFO)<<Name()<<"Decision init...";
|
|
|
return ROBOT_SUCCESS;
|
|
|
|
|
|
}
|
|
|
|
|
|
int Decision::Start(){
|
|
|
- LOG(INFO)<<"Decision start...";
|
|
|
-// ros::Subscriber sub = node_handle_->subscribe("canDataAnalysis", 20000, RTKLocationCallback);
|
|
|
+ LOG(INFO)<<Name()<<"Decision start...";
|
|
|
+ ros::ServiceServer CommandService = node_handle_->advertiseService(robot::common::CommandModeCmdService,
|
|
|
+ &Decision::CommandRequest, this);
|
|
|
+ ros::Subscriber GeometrySub = node_handle_->subscribe(robot::common::Geometry, 20000,
|
|
|
+ &Decision::RTKLocationCallback, this);
|
|
|
+
|
|
|
+ ros::Subscriber OBDSub = node_handle_->subscribe(robot::common::ObdData, 20000, &Decision::OBDCallback, this);
|
|
|
+
|
|
|
+// ros::Subscriber RCUModeSub = node_handle_->subscribe(robot::common::AesData, &Decision::RCUModeCallback, this);
|
|
|
ros::spin();
|
|
|
+ LOG(INFO)<<Name()<<"Decision end.";
|
|
|
return ROBOT_SUCCESS;
|
|
|
}
|
|
|
|
|
@@ -44,6 +49,231 @@ void Decision::Stop(){
|
|
|
}
|
|
|
|
|
|
|
|
|
+bool Decision::CommandRequest(decision::CommandModeCmd::Request &req,
|
|
|
+ decision::CommandModeCmd::Response &res)
|
|
|
+{
|
|
|
+
|
|
|
+ if (req.Cmd == CMD_AUTODRIE_START)
|
|
|
+ {
|
|
|
+ double breakCtr = -5;
|
|
|
+ ros::Rate loop_rate(1);
|
|
|
+ while(ros::ok())
|
|
|
+ {
|
|
|
+ if (SpeedControl(breakCtr) == 1)
|
|
|
+ {
|
|
|
+
|
|
|
+ if (BreakCtrControl() == 1)
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ ros::spinOnce();
|
|
|
+ loop_rate.sleep();
|
|
|
+ }
|
|
|
+
|
|
|
+ }
|
|
|
+ res.CmdResult = iCommandCmd = req.Cmd;
|
|
|
+ return true;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+ void Decision::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
|
|
|
+ {
|
|
|
+
|
|
|
+ switch (iCommandCmd) {
|
|
|
+ case CMD_PLAN_START:
|
|
|
+ ExcutePlanRoute(msg);
|
|
|
+ break;
|
|
|
+ case CMD_AUTODRIE_START:
|
|
|
+ ExcuteSelfDriving(msg);
|
|
|
+ break;
|
|
|
+ case CMD_PLAN_END:
|
|
|
+ break;
|
|
|
+ case CMD_AUTODRIE_END:
|
|
|
+ break;
|
|
|
+ default:
|
|
|
+ break;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+void Decision::OBDCallback(const canbus::obdMsg::ConstPtr& msg)
|
|
|
+{
|
|
|
+ realCarSpeed = msg->VehSpd;
|
|
|
+}
|
|
|
+
|
|
|
+//void Decision::RCUModeCallback(const perception::aesMsg::ConstPtr& msg)
|
|
|
+//{
|
|
|
+
|
|
|
+//}
|
|
|
+
|
|
|
+void Decision::ExcutePlanRoute(const localization::localMsg::ConstPtr& msg)
|
|
|
+{
|
|
|
+ try
|
|
|
+ {
|
|
|
+ localizationNumbers ++;
|
|
|
+ std::string PlanRouteName = "siasun";
|
|
|
+ if ((localizationNumbers % PredictionPoints) == 0)
|
|
|
+ {
|
|
|
+ lastCarSpeed = realCarSpeed;
|
|
|
+ RulesNumbers++;
|
|
|
+ std::stringstream strRuleContent;
|
|
|
+ if (RulesNumbers == 1)
|
|
|
+ strRuleContent<<"动作"<<RulesNumbers<<":起步请加速到"<< realCarSpeed<<"KPH";
|
|
|
+ else{
|
|
|
+ if (realCarSpeed > lastCarSpeed)
|
|
|
+ {
|
|
|
+ strRuleContent<<"动作"<<RulesNumbers<<":加速到"<< realCarSpeed<<"KPH";
|
|
|
+ }
|
|
|
+ else if (realCarSpeed < lastCarSpeed)
|
|
|
+ {
|
|
|
+ strRuleContent<<"动作"<<RulesNumbers<<":制动至"<< realCarSpeed<<"KPH";
|
|
|
+ }
|
|
|
+ else if (realCarSpeed == lastCarSpeed)
|
|
|
+ {
|
|
|
+ strRuleContent<<"动作"<<RulesNumbers<<":保持"<< realCarSpeed<<"KPH";
|
|
|
+ }
|
|
|
+ }
|
|
|
+ int iRet = Database::InsertLocationRuleInfoAndActionDirective(PlanRouteName.c_str(), RulesNumbers,
|
|
|
+ strRuleContent.str().c_str(), 0, localizationNumbers, msg->Longitude, msg->Latitude,
|
|
|
+ msg->Height, realCarSpeed);
|
|
|
+ if (iRet < 0) {
|
|
|
+ localizationNumbers--;
|
|
|
+ RulesNumbers--;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ Database::InsertLocationRuleInfo(PlanRouteName.c_str(),localizationNumbers, msg->Longitude,
|
|
|
+ msg->Latitude, msg->Height, realCarSpeed);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ catch (ros::Exception ex)
|
|
|
+ {
|
|
|
+ LOG(ERROR)<<Name()<<":ExcutePlanRoute error."<< ex.what();
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+void Decision::ExcuteSelfDriving(const localization::localMsg::ConstPtr& msg)
|
|
|
+{
|
|
|
+ // 判断当前RCU是否为自动驾驶模式
|
|
|
+}
|
|
|
+
|
|
|
+void Decision::MPC_Controller(std::vector<double> &ptsx, std::vector<double> &ptsy,
|
|
|
+ double &px, double &py, double &psi, double &v)
|
|
|
+{
|
|
|
+ // 转换坐标到当前车辆坐标
|
|
|
+ Eigen::MatrixXd waypoints = transformGlobal2Vehicle(px, py, psi, ptsx, ptsy);
|
|
|
+ Eigen::VectorXd Ptsx = waypoints.row(0);
|
|
|
+ Eigen::VectorXd Ptsy = waypoints.row(1);
|
|
|
+ // 获取状态值
|
|
|
+ auto coeffs = polyfit(Ptsx, Ptsy, 3);
|
|
|
+ double cte = evaluateCte(coeffs);
|
|
|
+ double epsi = evaluateEpsi(coeffs);
|
|
|
+ Eigen::VectorXd state(6);
|
|
|
+ state << 0, 0, 0, v, cte, epsi;
|
|
|
+ // 计算方向和油门,范围都在[-1, 1]之间
|
|
|
+ // 这个地方有延迟,取第三个
|
|
|
+ vector<vector<double>> result = mpc.Solve(state, coeffs);
|
|
|
+ double steer_value = result[2][mpc.latency];
|
|
|
+ double throttle_value = result[3][mpc.latency];
|
|
|
+ mpc.delta_prev = steer_value;
|
|
|
+ mpc.a_prev = throttle_value;
|
|
|
+ SpeedControl(steer_value);
|
|
|
+ StreeControl(throttle_value);
|
|
|
+}
|
|
|
+
|
|
|
+unsigned int Decision::SpeedControl(double &speed)
|
|
|
+{
|
|
|
+ ros::ServiceClient clientPedalActCtrReq = node_handle_->serviceClient<canbus::PedalActCtrReq>(robot::common::PedalActCtrService);
|
|
|
+ canbus::PedalActCtrReq srv;
|
|
|
+ if (mpc.a_prev >0){
|
|
|
+ srv.request.uiAccPsngCtrReq =(unsigned int)(speed *10);
|
|
|
+ srv.request.uiBrkPsngCtrReq =0;
|
|
|
+ }
|
|
|
+ else{
|
|
|
+ srv.request.uiAccPsngCtrReq = 0;
|
|
|
+ srv.request.uiBrkPsngCtrReq =(unsigned int)(speed *10);
|
|
|
+ }
|
|
|
+ srv.request.iAccSpdCtrReq = 20;
|
|
|
+ srv.request.iBrkSpdCtrReq = 50;
|
|
|
+ srv.request.uiBrkCtrMdReq = 1;
|
|
|
+ srv.request.uiBrkFocCtrReq = 350;
|
|
|
+ if (clientPedalActCtrReq.call(srv))
|
|
|
+ {
|
|
|
+ LOG(INFO)<<srv.response.uiReturn;
|
|
|
+ }
|
|
|
+ return srv.response.uiReturn;
|
|
|
+}
|
|
|
+unsigned int Decision::StreeControl(double &streer)
|
|
|
+{
|
|
|
+ ros::ServiceClient clientStrActCtrReq = node_handle_->serviceClient<canbus::PedalActCtrReq>(robot::common::SteerActCtrService);
|
|
|
+ canbus::StrActCtrReq srv;
|
|
|
+ srv.request.iStrAngCtrReq =(unsigned int)(streer *100);
|
|
|
+ srv.request.uiStrCtrMdReq =1;
|
|
|
+ srv.request.iStrSpdCtrReq = 200;
|
|
|
+ srv.request.iStrTrqCtrReq = 50;
|
|
|
+ if (clientStrActCtrReq.call(srv))
|
|
|
+ {
|
|
|
+ LOG(INFO)<<srv.response.uiReturn;
|
|
|
+ }
|
|
|
+ return srv.response.uiReturn;
|
|
|
+}
|
|
|
+
|
|
|
+unsigned int Decision::BreakCtrControl()
|
|
|
+{
|
|
|
+ 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.uiSftMdReq = SFT_MD_REQ_2_SECOND;
|
|
|
+ srv.request.uiClchPsngCtrReq = 1;
|
|
|
+ srv.request.iClchSpdCtrReq = 0;
|
|
|
+ if (clientSftActCtrReq.call(srv))
|
|
|
+ {
|
|
|
+ LOG(INFO)<<srv.response.uiReturn;
|
|
|
+ }
|
|
|
+ return srv.response.uiReturn;
|
|
|
+}
|
|
|
+
|
|
|
+ // 根据输入值x,y解方程
|
|
|
+ Eigen::VectorXd Decision::polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals, int order) {
|
|
|
+ assert(xvals.size() == yvals.size());
|
|
|
+ assert(order >= 1 && order <= xvals.size() - 1);
|
|
|
+ Eigen::MatrixXd A(xvals.size(), order + 1);
|
|
|
+ for (int i = 0; i < xvals.size(); i++) {
|
|
|
+ A(i, 0) = 1.0;
|
|
|
+ }
|
|
|
+ for (int j = 0; j < xvals.size(); j++) {
|
|
|
+ for (int i = 0; i < order; i++) {
|
|
|
+ A(j, i + 1) = A(j, i) * xvals(j);
|
|
|
+ }
|
|
|
+ }
|
|
|
+ auto Q = A.householderQr();
|
|
|
+ auto result = Q.solve(yvals);
|
|
|
+ return result;
|
|
|
+ }
|
|
|
+
|
|
|
+// 计算CTE误差
|
|
|
+ double Decision::evaluateCte(Eigen::VectorXd coeffs) {
|
|
|
+ return polyeval(coeffs, 0);
|
|
|
+ }
|
|
|
+
|
|
|
+ // 计算Epsi误差
|
|
|
+ double Decision::evaluateEpsi(Eigen::VectorXd coeffs) {
|
|
|
+ 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());
|
|
|
+ unsigned len = ptsx.size();
|
|
|
+ auto waypoints = Eigen::MatrixXd(2, len);
|
|
|
+ for (auto i = 0; i < len; ++i) {
|
|
|
+ waypoints(0, i) = cos(psi) * (ptsx[i] - x) + sin(psi) * (ptsy[i] - y);
|
|
|
+ waypoints(1, i) = -sin(psi) * (ptsx[i] - x) + cos(psi) * (ptsy[i] - y);
|
|
|
+ }
|
|
|
+ return waypoints;
|
|
|
+}
|
|
|
|
|
|
}
|
|
|
}
|