Merge branch 'master' of http://118.24.47.114/madesheng/self-driving-

robot
合并冲突

src/canbus/canbus.cpp

@@ -1,6 +1,6 @@
 /******************************************************************************
  * Copyright 2019 The siasun Transport Authors. All Rights Reserved.
- *
+ * Author: limengqi
  * Licensed under the Apache License, Version 1.0 (the "License");
  *****************************************************************************/
 

src/canbus/canbus.h

@@ -1,6 +1,6 @@
 /******************************************************************************
  * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
- *
+ * Author: limengqi
  * Licensed under the Apache License, Version 1.0 (the "License");
  *****************************************************************************/
 

src/canbus/canbus_app.cpp

@@ -1,6 +1,6 @@
 /******************************************************************************
  * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
- *
+ * Author: limengqi
  * Licensed under the Apache License, Version 1.0 (the "License");
  *****************************************************************************/
 #include "../common/robotapp.h"

src/common/commonmain.cpp

@@ -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 <ros/ros.h>
 #include <iostream>
 #include "gloghelper.h"

src/common/file.cpp

@@ -1,7 +1,6 @@
 /******************************************************************************
  * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
- *。
- *
+ * Author: madesheng
  * Licensed under the Apache License, Version 1.0 (the "License");
  *****************************************************************************/
 

src/common/file.h

@@ -1,10 +1,8 @@
 /******************************************************************************
  * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
- * 。
- *
+ * Author: madesheng
  * Licensed under the Apache License, Version 1.0 (the "License");
  *****************************************************************************/
-
 /**
  * @file
  */

src/common/gloghelper.cpp

@@ -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 <stdlib.h>
 #include "gloghelper.h"
 
@@ -39,16 +44,14 @@ GLogHelper::GLogHelper(char* program, std::string appName)
     FLAGS_colorlogtostderr=true;
 
     //设置 google::ERROR 级别的日志存储路径和文件名称前缀
-    //google::SetLogDestination(google::ERROR,"log/error_");
+    google::SetLogDestination(google::ERROR,"log/error_");
 
     //设置 google::INFO 级别的日志存储路径和文件名称前缀
     google::SetLogDestination(google::INFO,LOGDIR"/log_");
 
     //设置 google::WARNING 级别的日志存储路径和文件名称前缀
-    // google::SetLogDestination(google::WARNING,LOGDIR"/WARNING_");
+    google::SetLogDestination(google::WARNING,LOGDIR"/WARNING_");
 
-    //设置 google::ERROR 级别的日志存储路径和文件名称前缀
-    // google::SetLogDestination(google::ERROR,LOGDIR"/ERROR_");
     //缓冲日志输出，默觉得30秒。此处改为马上输出
     FLAGS_logbufsecs =0;
     //最大日志大小为 100MB

src/common/mpc.cpp

@@ -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 "mpc.h"
 #include <cppad/cppad.hpp>
 #include <cppad/ipopt/solve.hpp>
@@ -11,7 +16,7 @@ namespace common {
 
 // 设置预测状态点的个数和两个相邻状态点之间的时间间隔
 size_t N = 10;
-double dt = 0.2; // 200ms
+double dt = 0.05; // 50ms
 
 // 设置汽车头到车辆重心之间的距离
 const double Lf = 2.67;

src/common/mpc.h

@@ -1,7 +1,6 @@
 /******************************************************************************
  * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
- * Author:mds。
- *
+ * Author: madesheng
  * Licensed under the Apache License, Version 1.0 (the "License");
  *****************************************************************************/
 #ifndef ROBOT_MPC_H

src/common/robotapp.cpp

@@ -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 "robotapp.h"
 
 namespace robot {

src/common/robotapp.h

@@ -1,6 +1,6 @@
 /******************************************************************************
  * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
- *
+ * Author: madesheng
  * Licensed under the Apache License, Version 1.0 (the "License");
  *****************************************************************************/
 

src/common/sqlite3helper.cpp

@@ -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 "sqlite3helper.h"
 #include <string.h>
 #include <stdio.h>

src/common/sqlite3helper.h

@@ -1,7 +1,6 @@
 /******************************************************************************
  * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
- * Author:mds。
- *
+ * Author: madesheng
  * Licensed under the Apache License, Version 1.0 (the "License");
  *****************************************************************************/
 

src/common/string_tokenizer.cpp

@@ -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 "string_tokenizer.h"
 #include "std_msgs/String.h"
 

src/common/string_tokenizer.h

@@ -1,7 +1,6 @@
 /******************************************************************************
  * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
- * Author:mds。
- *
+ * Author: madesheng
  * Licensed under the Apache License, Version 1.0 (the "License");
  *****************************************************************************/
 #ifndef COMMON_STRING_TOKENIZER_H

src/common/time.h

@@ -1,7 +1,6 @@
 /******************************************************************************
  * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
- * Author:mds。
- *
+ * Author: madesheng
  * Licensed under the Apache License, Version 1.0 (the "License");
  *****************************************************************************/
 

src/common/util.cpp

@@ -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 "util.h"
 
 namespace robot {

src/common/util.h

@@ -1,7 +1,6 @@
 /******************************************************************************
  * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
- * Author:mds。
- *
+ * Author: madesheng
  * Licensed under the Apache License, Version 1.0 (the "License");
  *****************************************************************************/
 

src/decision/decision.cpp

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

src/decision/decision.h

@@ -1,7 +1,6 @@
-
 /******************************************************************************
  * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
- *
+ * Author: madesheng
  * Licensed under the Apache License, Version 1.0 (the "License");
  *****************************************************************************/
 #ifndef ROBOT_ROBOT_DECISION_H

src/decision/decision_app.cpp

@@ -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 "../common/robotapp.h"
 #include "decision.h"
 

src/decision/decisiondatabase.cpp

@@ -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 "decisiondatabase.h"
 #include <string>
 namespace robot {

src/decision/decisiondatabase.h

@@ -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");
+ *****************************************************************************/
 #ifndef DECISIONDATABASE_H
 #define DECISIONDATABASE_H
 

src/decision/decisiondatatype.h

@@ -1,6 +1,6 @@
 /******************************************************************************
  * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
- *
+ * Author: madesheng
  * Licensed under the Apache License, Version 1.0 (the "License");
  *****************************************************************************/
 #ifndef ROBOT_DECISIONDATATYPE_H
@@ -31,6 +31,7 @@ namespace decisionSpace {
 #define	CTR_MD_REQ_HAND_DRIVE		3	//控制模式请求-人工驾驶模式
 #define	CTR_MD_REQ_ROBOT_CHECK		4	//控制模式请求-机器人自检模式
 #define	CTR_MD_REQ_ROBOT_DEBUG		5	//控制模式请求-机器人调试模式
+#define	CTR_MD_REQ_ROBOT_UNDIFED		6	//控制模式请求-机器人调试模式
 
 #define RULE_EXCUTE_RESULT_INIT           0
 #define RULE_EXCUTE_RESULT_FIAL           1

src/localization/coordsconvertion.cpp

@@ -1,6 +1,6 @@
 /******************************************************************************
- * Copyright 2019 The siasun Transport Authors. All Rights Reserved.
- *
+ * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
+ * Author: madesheng
  * Licensed under the Apache License, Version 1.0 (the "License");
  *****************************************************************************/
 #include "coordsconvertion.h"

src/localization/coordsconvertion.h

@@ -1,8 +1,8 @@
 /******************************************************************************
-* Copyright 2020 The siasun Transport Authors. All Rights Reserved.
-*
-* Licensed under the Apache License, Version 1.0 (the "License");
-*****************************************************************************/
+ * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
+ * Author: madesheng
+ * Licensed under the Apache License, Version 1.0 (the "License");
+ *****************************************************************************/
 
 /**
 * @file

src/localization/localization.cpp

@@ -1,6 +1,6 @@
 /******************************************************************************
- * Copyright 2019 The siasun Transport Authors. All Rights Reserved.
- *
+ * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
+ * Author: madesheng
  * Licensed under the Apache License, Version 1.0 (the "License");
  *****************************************************************************/
 #include <iterator>
@@ -54,7 +54,7 @@ int Localization::Start() {
          while(ros::ok())
          {
 
-           if(serial_gnss.available() >0){
+           if(serial_gnss.available() >200){
                LOG(INFO)<<Name()<<"  Reading from serial port. data size :"<<serial_gnss.available();
                std::string strSerialResult;
                strSerialResult= serial_gnss.read(serial_gnss.available());

src/localization/localization.h

@@ -1,6 +1,6 @@
 /******************************************************************************
  * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
- *
+ * Author: madesheng
  * Licensed under the Apache License, Version 1.0 (the "License");
  *****************************************************************************/
 

src/localization/localization_app.cpp

@@ -1,6 +1,6 @@
 /******************************************************************************
  * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
- *
+ * Author: madesheng
  * Licensed under the Apache License, Version 1.0 (the "License");
  *****************************************************************************/
 #include "../common/robotapp.h"

src/localization/localizationdata.h

@@ -1,10 +1,8 @@
 /******************************************************************************
  * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
- *
+ * Author: madesheng
  * Licensed under the Apache License, Version 1.0 (the "License");
  *****************************************************************************/
-
-
 /**
  * @file
  */

src/perception/perception.cpp

@@ -1,3 +1,8 @@
+/******************************************************************************
+ * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
+ * Author: zhangyu,Altered: madesheng
+ * Licensed under the Apache License, Version 1.0 (the "License");
+ *****************************************************************************/
 #include "perception.h"
 #include "../common/message.h"
 #include "../common/canmsgobdmsgid.h"
@@ -36,7 +41,6 @@ int Perception::Start()
     {
         ros::Subscriber sub = node_handle_->subscribe(robot::common::CanDataAnalysis, 1, &Perception::CanTopicCallback, this);
 
-        bus_pub_6E4 = node_handle_->advertise<::perception::powerMsg>(robot::common::PowerData, 1);
         bus_pub_6E5 = node_handle_->advertise<::perception::aesMsg>(robot::common::AesData, 1);
         bus_pub_6EE = node_handle_->advertise<::perception::slMsg>(robot::common::SlData, 1);
         bus_pub_6ED = node_handle_->advertise<::perception::si2Msg>(robot::common::Si2Data, 1);
@@ -54,6 +58,8 @@ int Perception::Start()
 
         clientActuatorEnableRequest = node_handle_->serviceClient<canbus::ActuatorEnableRequest>(robot::common::ActuatorEnableService);
 
+        bus_pub_6E4 = node_handle_->advertise<::perception::powerMsg>(robot::common::PowerData, 1);
+
         ros::spin();
     }
     catch(ros::Exception e)

src/perception/perception.h

@@ -1,3 +1,8 @@
+/******************************************************************************
+ * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
+ * Author: zhangyu,Altered: madesheng
+ * Licensed under the Apache License, Version 1.0 (the "License");
+ *****************************************************************************/
 #ifndef ROBOT_ROBOT_PERCEPTION_H
 #define ROBOT_ROBOT_PERCEPTION_H
 

src/perception/perception_app.cpp

@@ -1,3 +1,8 @@
+/******************************************************************************
+ * Copyright 2020 The siasun Transport Authors. All Rights Reserved.
+ * Author: zhangyu
+ * Licensed under the Apache License, Version 1.0 (the "License");
+ *****************************************************************************/
 #include "../common/robotapp.h"
 #include "perception.h"