1、添加暂停和定速巡航功能

2、taskdata，taskdataui数据库插入改为异步方式
3、localization添加定时器，模拟模拟量用于测试

src/canbus/canbus.cpp

@@ -88,7 +88,8 @@ int Canbus::Start() {
         return ROBOT_FAILTURE;
     }
     LOG(INFO)<< Name() <<": read param rcu_canbus_ip:port = " << canbus_ip << ":" << canbus_port;
-    canbus_socket = socket(AF_INET, SOCK_STREAM, 0);
+    canbus_socket = socket(AF_INET, SOCK_STREAM | SOCK_NONBLOCK, IPPROTO_TCP);
+//    canbus_socket = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
     struct sockaddr_in serv_addr;
     memset(&serv_addr, 0, sizeof(serv_addr));  //每个字节都用0填充
     serv_addr.sin_family = AF_INET;  //使用IPv4地址
@@ -336,7 +337,18 @@ int Canbus::read_loop()
     if (canbus_mode == "tcp") {
       int recv_len = read(canbus_socket, s_g_pBufGet, sizeof(s_g_pBufGet));
       if (recv_len != sizeof(s_g_pBufGet)) {
-         LOG(ERROR) << Name() <<" canbus socket read error: " << recv_len;
+        int len = 13 - recv_len;
+        size_t a = static_cast<size_t>(len);
+        unsigned char s_g_pBufGet2[len];
+        int recv_len2 = read(canbus_socket, s_g_pBufGet2, sizeof(s_g_pBufGet2));
+        int sizeArr1 = sizeof(s_g_pBufGet) / sizeof(s_g_pBufGet[0]);
+        int sizeArr2 = sizeof(s_g_pBufGet2) / sizeof(s_g_pBufGet2[0]);
+        std::copy(s_g_pBufGet2, s_g_pBufGet2 + sizeArr2, s_g_pBufGet + sizeArr1);
+        if(recv_len + recv_len2 != 13)
+        {
+       //   LOG(ERROR) << Name() <<" canbus socket read error: " << recv_len;
+        }
+//         LOG(ERROR) << Name() <<" canbus socket read error: " << recv_len;
       }
       else {
         msg.Msg.ID = static_cast<int>(s_g_pBufGet[3]) * 256 + static_cast<int>(s_g_pBufGet[4]);
@@ -514,7 +526,8 @@ int Canbus::write_data(TPCANMsg *pMsgBuff, bool print)
 //           cout << hex << setw(2) << setfill('0') << static_cast<int>(s_g_pBufSet[i]) << " ";
 //    }
 //    cout << endl;
-    int send_length = write(canbus_socket, s_g_pBufSet, sizeof(s_g_pBufSet));
+//    int send_length = write(canbus_socket, s_g_pBufSet, sizeof(s_g_pBufSet));
+    int send_length = send(canbus_socket, s_g_pBufSet, sizeof(s_g_pBufSet), 0);
     if (send_length != sizeof(s_g_pBufSet)){
        LOG(ERROR) << Name() <<" canbus socket write error: " << send_length;
        return errno;
@@ -2115,16 +2128,31 @@ bool Canbus::RackTestReq(canbus::RackTestReq::Request &req,
   try{
     if(req.uiStart == 0)
     {
-      rackTestState = false;
+      rackTestState = 0;
       res.uiReturn = 1;
       return true;
     }
-    if(rackTestState || rackTestSendTimerState)
+    if(req.uiStart == 2)
+    {
+      if(rack_set_speed == 0 && rack_actual_speed == 0)
+      {
+        rackTestState = 2;
+        res.uiReturn = 1;
+        return true;
+      }
+      else
+      {
+        res.uiReturn = 3;
+        return true;
+      }
+
+    }
+    if(rackTestState == 1)
     {
       res.uiReturn = 2;
       return true;
     }
-    rackTestState = true;
+
     nedc_speeds[1810] = {0};
     string str = "/home/madesheng/publish/wwwroot/Upload/File/CLTC Speed.txt";
     if(req.uiStandard == 1)
@@ -2142,12 +2170,29 @@ bool Canbus::RackTestReq(canbus::RackTestReq::Request &req,
       standard_name = "CLTC";
       str = "/home/madesheng/publish/wwwroot/Upload/File/CLTC Speed.txt";
     }
-    int flag = Canbus::read_txt(str,6);
-    if(flag != ROBOT_SUCCESS)
+    else if(req.uiStandard == 100)
     {
-      res.uiReturn = 0;
-      return false;
+      standard_name = "CruiseControl";
+      rack_constant_speed = req.uiConstantSpeed;
     }
+    if(req.uiStandard != 100)
+    {
+      int flag = Canbus::read_txt(str,6);
+      if(flag != ROBOT_SUCCESS)
+      {
+        res.uiReturn = 0;
+        return false;
+      }
+    }
+    if(rackTestState == 2)
+    {
+      rackTestState = 1;
+      res.uiReturn = 1;
+      return true;
+    }
+
+
+    rackTestState = 1;
     nedc_speed_count = 0;
     rack_test_send_count = 0;
     set_speed_last = 0;
@@ -2180,7 +2225,15 @@ bool Canbus::RackTestReq(canbus::RackTestReq::Request &req,
     std::string datetime_now;
     datetime_now = get_datetime_mill();
     //插入测试任务信息表
-    task_num = req.iTaskNum;
+    if(req.uiStandard == 100)
+    {
+      task_num = 1;
+    }
+    else
+    {
+      task_num = req.iTaskNum;
+    }
+
     long result = CanbusMySqlDataBase::InsertTaskInfo(req.uiCarId,standard_name,
                                                       datetime_now,over_threshold_value,task_num);
     if(result <= 0)
@@ -2344,7 +2397,6 @@ void Canbus::RackTestOnTimer(const ros::TimerEvent &event)
 //      LOG(INFO)<< "actual_speed too more";
 //      return;
 //    }
-    set_speed_reply = set_speed;
     LOG(INFO)<< "nedc_speed_count:"<<nedc_speed_count;
 //    string str = "/home/madesheng/FuzzyPid.txt";
 //    int flag = Canbus::read_txt(str,7);
@@ -2496,6 +2548,13 @@ void Canbus::RackTestReplyOnTimer(const ros::TimerEvent &event)
 {
   try
   {
+    if(rackTestState == 2)
+    {
+      rackTestReplyState = 2;
+      publish_rack_speed();
+      return;
+    }
+
     if(rack_set_speed - rack_actual_speed > over_threshold_value || rack_set_speed - rack_actual_speed < -over_threshold_value)
     {
       if(!over_speed_flag)
@@ -2530,6 +2589,10 @@ void Canbus::RackTestReplyOnTimer(const ros::TimerEvent &event)
 
 void Canbus::RackTestReplyDetailOnTimer(const ros::TimerEvent &event)
 {
+  if(rackTestState == 2)
+  {
+    return;
+  }
   try
   {
     gettimeofday(&now_time_ui, NULL);
@@ -2551,8 +2614,31 @@ void Canbus::RackTestReplyDetailOnTimer(const ros::TimerEvent &event)
        rackTestReplyState = 1;
        //插入测试任务数据采集表界面显示
        int data_task_already_num = task_already_num + 1;
-       long result = CanbusMySqlDataBase::InsertTaskDataUI(task_id,rack_set_speed,rack_actual_speed,
-                                                           task_time_stamp_ui,over_speed_num,data_task_already_num);
+//       long result = CanbusMySqlDataBase::InsertTaskDataUI(task_id,rack_set_speed,rack_actual_speed,
+//                                                           task_time_stamp_ui,over_speed_num,data_task_already_num);
+       auto ThreadInsertTaskDataUI = [&](long TaskId,
+           double SetSpeed,
+           double ActualSpeed,
+           double StandardSpeedTimeStamp,
+           int OverSpeedNum,
+           int StandardTaskCount) -> void {
+           LOG(INFO)<< "Start ThreadInsertTaskDataUI";
+           mtx_taskdataui.lock();
+
+           CanbusMySqlDataBase::InsertTaskDataUI(TaskId,SetSpeed,ActualSpeed,
+                                                           StandardSpeedTimeStamp,OverSpeedNum,
+                                                           StandardTaskCount);
+
+          mtx_taskdataui.unlock();
+          LOG(INFO)<< "END ThreadInsertTaskDataUI";
+          };
+
+
+       std::thread thread_taskdataui (ThreadInsertTaskDataUI,
+                                      task_id,rack_set_speed,rack_actual_speed,
+                                      task_time_stamp_ui,over_speed_num,
+                                      data_task_already_num);
+       thread_taskdataui.join();
     }
     else
     {
@@ -2606,7 +2692,15 @@ void Canbus::publish_rack_speed()
     rackMsg.RackTestReplyState = rackTestReplyState;
     rackMsg.SetSpeed = rack_set_speed;
     rackMsg.TaskCurrentTime = nedc_speed_count +(nedc_speed_number * task_already_num);
-    rackMsg.TaskTotalTime = nedc_speed_number * task_num;
+    if(standard_name == "CruiseControl")
+    {
+      rackMsg.TaskTotalTime = rackMsg.TaskCurrentTime;
+    }
+    else
+    {
+      rackMsg.TaskTotalTime = nedc_speed_number * task_num;
+    }
+
     rackMsg.OverSpeedNum =over_speed_num;
     rackMsg.TaskId = task_id;
     rackMsg.TaskAlreadyNum = task_already_num + 1;
@@ -2624,9 +2718,6 @@ void Canbus::publis_rack_detail_speed()
   try
   {
     canbus::rackTestReplyDetailMsg detailMsg;
-    detailMsg.ActualSpeed = set_speed_reply;
-    detailMsg.SetSpeed = set_speed_reply;
-    detailMsg.RackTestReplyState = rackTestReplyState;
     canRackDetailData_pub.publish(detailMsg);
   }
   catch (ros::Exception ex)
@@ -2661,7 +2752,11 @@ void Canbus::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
     float set_speed = 0;
     float actual_speed = msg_RTK.Speed;
     float set_speed_1s = 0;
-    if(!rackTestState)
+    if(rackTestState == 2)
+    {
+      return;
+    }
+    if(rackTestState == 0)
     {
       if(!rackTestSendTimerState)
       {
@@ -2685,93 +2780,104 @@ void Canbus::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
       set_speed = 0;
 
     }
-    else
+    else if(rackTestState == 1)
     {
-      if(nedc_speed_count == nedc_speed_number)
+      if(standard_name == "CruiseControl")  //定速巡航模式
       {
-        task_already_num ++;
-        if(task_already_num < task_num)
+        set_speed = rack_constant_speed;
+        set_speed_1s = rack_constant_speed;
+        task_already_num = 0;
+        nedc_speed_number = 0;
+      }
+      else  //规范文件模式
+      {
+        if(nedc_speed_count == nedc_speed_number)
         {
-          nedc_speed_count = 0;
-          rack_test_send_count = 0;
-          set_speed_last = 0;
-          task_time_stamp = 0;
-          task_time_stamp_ui = 0;
+          task_already_num ++;
+          if(task_already_num < task_num)
+          {
+            nedc_speed_count = 0;
+            rack_test_send_count = 0;
+            set_speed_last = 0;
+            task_time_stamp = 0;
+            task_time_stamp_ui = 0;
+          }
+          else
+          {
+            rackTestState = 0;
+          //  rack_test_timer_.stop();
+            rack_reply_timer_.stop();
+            rack_reply_detail_timer_.stop();
+            rackTestSendTimerState = false;
+            rackTestReplyState = 0;
+            task_id = 0;
+            task_num = 0;
+            task_time_stamp =0;
+            publish_rack_speed();
+            return;
+
+          }
+
         }
-        else
+        if(delta_time > 0 && delta_time < 1)
         {
-          rackTestState = false;
-          rack_test_timer_.stop();
-          rack_reply_timer_.stop();
-          rack_reply_detail_timer_.stop();
-          rackTestSendTimerState = false;
-          rackTestReplyState = 0;
-          task_id = 0;
-          task_num = 0;
-          task_time_stamp =0;
-          publish_rack_speed();
-          return;
-
+         task_time_stamp += delta_time;
         }
 
-      }
-      if(delta_time > 0 && delta_time < 1)
-      {
-       task_time_stamp += delta_time;
-      }
+        //根据时间差计算速度插值
+        float set_speed1 = nedc_speeds[nedc_speed_count];
+        float set_speed2 = 0;
+        if(nedc_speed_count == nedc_speed_number - 1)
+        {
+          set_speed2 = nedc_speeds[nedc_speed_count];
+        }
+        else
+        {
+          set_speed2 = nedc_speeds[nedc_speed_count + 1];
+        }
 
-      //根据时间差计算速度插值
-      float set_speed1 = nedc_speeds[nedc_speed_count];
-      float set_speed2 = 0;
-      if(nedc_speed_count == nedc_speed_number - 1)
-      {
-        set_speed2 = nedc_speeds[nedc_speed_count];
-      }
-      else
-      {
-        set_speed2 = nedc_speeds[nedc_speed_count + 1];
-      }
+        if(set_speed1 == 0 && set_speed2 ==0)
+        {
+          set_speed = 0;
+        }
+        else
+        {
+          set_speed = set_speed_last + (set_speed2 - set_speed1) * delta_time;
+        }
+        if(set_speed < 0)
+        {
+          set_speed = 0;
+        }
 
-      if(set_speed1 == 0 && set_speed2 ==0)
-      {
-        set_speed = 0;
-      }
-      else
-      {
-        set_speed = set_speed_last + (set_speed2 - set_speed1) * delta_time;
-      }
-      if(set_speed < 0)
-      {
-        set_speed = 0;
-      }
+        float set_speed1_1s = 0;
+        float set_speed2_1s = 0;
+        if(nedc_speed_count > 0)
+        {
+          set_speed1_1s = nedc_speeds[nedc_speed_count-1];
+        }
+        if(nedc_speed_count-1 == nedc_speed_number - 2)
+        {
+          set_speed2_1s = nedc_speeds[nedc_speed_count-1];
+        }
+        else
+        {
+          set_speed2_1s = nedc_speeds[nedc_speed_count];
+        }
 
-      float set_speed1_1s = 0;
-      float set_speed2_1s = 0;
-      if(nedc_speed_count > 0)
-      {
-        set_speed1_1s = nedc_speeds[nedc_speed_count-1];
-      }
-      if(nedc_speed_count-1 == nedc_speed_number - 2)
-      {
-        set_speed2_1s = nedc_speeds[nedc_speed_count-1];
-      }
-      else
-      {
-        set_speed2_1s = nedc_speeds[nedc_speed_count];
+        if(set_speed1_1s == 0 && set_speed2_1s ==0)
+        {
+          set_speed_1s = 0;
+        }
+        else
+        {
+          set_speed_1s = set_speed_last_1s + (set_speed2_1s - set_speed1_1s) * delta_time;
+        }
+        if(set_speed_1s < 0)
+        {
+          set_speed_1s = 0;
+        }
       }
 
-      if(set_speed1_1s == 0 && set_speed2_1s ==0)
-      {
-        set_speed_1s = 0;
-      }
-      else
-      {
-        set_speed_1s = set_speed_last_1s + (set_speed2_1s - set_speed1_1s) * delta_time;
-      }
-      if(set_speed_1s < 0)
-      {
-        set_speed_1s = 0;
-      }
 
     }
     LOG(INFO)<< "delta time:"<<std::setprecision(10)<<delta_time<<" s";
@@ -2781,21 +2887,9 @@ void Canbus::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
 //    {
 //      LOG(INFO)<< "actual_speed too more";
 //      return;
-//    }
-    set_speed_reply = set_speed;
-    LOG(INFO)<< "nedc_speed_count:"<<nedc_speed_count;
-//    string str = "/home/madesheng/FuzzyPid.txt";
-//    int flag = Canbus::read_txt(str,7);
-//    if(flag != ROBOT_SUCCESS)
-//    {
-//      LOG(INFO)<< "Get FuzzyPID.txt error";
-//      return;
 //    }
 
-//    int brkFactor = 0;
-//    int brkMax = 0;
-//    brkFactor = fuzzy_pid[6];
-//    brkMax = fuzzy_pid[7];
+    LOG(INFO)<< "nedc_speed_count:"<<nedc_speed_count;
 
     float pi_out = 0.01;
     pi_out = PidControll.FuzzyPIDcontroller(set_speed,actual_speed,delta_time,
@@ -2907,11 +3001,39 @@ void Canbus::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
 
     //插入测试任务数据采集表
     int data_task_already_num = task_already_num + 1;
-    long result = CanbusMySqlDataBase::InsertTaskData(task_id,set_speed,actual_speed,
-                                                      requestAcc.uiAccPsngCtrReq/10,
-                                                      requestBrk.uiBrkPsngCtrReq / 10,
-                                                      task_time_stamp,over_speed_num,
-                                                      data_task_already_num);
+//    CanbusMySqlDataBase::InsertTaskData(task_id,set_speed,actual_speed,
+//                                                      requestAcc.uiAccPsngCtrReq/10,
+//                                                      requestBrk.uiBrkPsngCtrReq / 10,
+//                                                      task_time_stamp,over_speed_num,
+//                                                      data_task_already_num);
+    auto ThreadInsertTaskData = [&](long TaskId,
+        double SetSpeed,
+        double ActualSpeed,
+        long AccOrderPsng,
+        long BrkOrderPsng,
+        double StandardSpeedTimeStamp,
+        int OverSpeedNum,
+        int StandardTaskCount) -> void {
+        LOG(INFO)<< "Start ThreadInsertTaskData";
+        mtx_taskdata.lock();
+
+        CanbusMySqlDataBase::InsertTaskData(TaskId,SetSpeed,ActualSpeed,
+                                                        AccOrderPsng,
+                                                        BrkOrderPsng,
+                                                        StandardSpeedTimeStamp,OverSpeedNum,
+                                                        StandardTaskCount);
+
+         mtx_taskdata.unlock();
+         LOG(INFO)<< "End ThreadInsertTaskData";
+      };
+
+    std::thread thread_taskdata(ThreadInsertTaskData,
+                                task_id,set_speed,actual_speed,
+                                requestAcc.uiAccPsngCtrReq/10,
+                                requestBrk.uiBrkPsngCtrReq / 10,
+                                task_time_stamp,over_speed_num,
+                                data_task_already_num);
+    thread_taskdata.join();
 
   }
   catch (ros::Exception ex)

src/canbus/canbus.h

@@ -63,6 +63,9 @@
 #include <canbus/rackTestReplyDetailMsg.h>
 #include <localization/localMsg.h>
 #include <ctime>
+#include <iostream>
+#include <thread>
+#include <mutex>
 
 using namespace std;
 /**
@@ -364,7 +367,7 @@ private:
    const double duration_rack_reply = 1; //1s//台架测试任务回复消息时间
    ros::Timer rack_test_timer_;    //台架测试任务定时器
    ros::Timer rack_reply_timer_;   //台架测试任务回复消息定时器
-   bool rackTestState = false;  //台架测试任务标志位
+   int rackTestState = 0;  //台架测试任务标志位  1:任务执行中 2:任务暂停中 0:任务结束
    bool rackTestSendTimerState = false;  //台架测试任务发送数据定时器标志位
    int rackTestReplyState = 0;   //台架测试任务回复消息标志位  0表示前端界面不显示回复，1表示前端界面显示回复
   // double nedc_speeds[1182] = {0};  //TXT文件中的速度值
@@ -383,7 +386,6 @@ private:
 
    const double duration_rack_reply_detail = 0.1;//100ms //台架测试任务细节图所用数据回复时间
    ros::Timer rack_reply_detail_timer_;  //台架测试任务细节图所用数据回复定时器
-   float set_speed_reply = 0;
 
    double P = 0;
    double I = 0;
@@ -410,6 +412,8 @@ private:
    bool over_speed_flag = false; //超差标志
    int over_speed_num = 0;  //超差次数
 
+   int rack_constant_speed = 0;  //定速巡航速度值
+
   const double duration = 0.01; //10ms
 
   const double rate = 0.0001;  //0.0001s
@@ -452,7 +456,9 @@ private:
   int canbus_socket;
 
   unsigned char s_g_pBufGet[13] , s_g_pBufSet[13];
-        unsigned char s_g_pBuf1[13] , s_g_pBuf2[13];
+
+   std::mutex mtx_taskdata;
+   std::mutex mtx_taskdataui;
 
 };
 

src/canbus/srv/RackTestReq.srv

@@ -2,5 +2,6 @@ uint32 uiStart                   #启动标志位
 uint32 uiCarId                   #车型ID
 int32 iTaskNum                   #规范文件执行次数
 uint32 uiStandard
+uint32 uiConstantSpeed
 ---
 uint32 uiReturn

src/localization/localization.cpp

@@ -24,7 +24,7 @@ int Localization::Init() {
   if (!ros::param::get("car_input_mode", car_input_mode))
     car_input_mode = "rtk";
   LOG(INFO)<< Name()<<": read param car_input_mode = " << car_input_mode;
-
+//TODO dakai
   if (car_input_mode == "modbus") {
     std::string car_input_ip;
     int car_input_port;
@@ -65,7 +65,9 @@ int Localization::Init() {
         return ROBOT_FAILTURE;
     }
   }
+    //TODO
   node_handle_.reset(new ros::NodeHandle());
+
   LOG(INFO)<<Name()<<" init end.";
   return ROBOT_SUCCESS;
 }
@@ -73,7 +75,11 @@ int Localization::Init() {
 int Localization::Start() {
   LOG(INFO)<<Name()<<" start start...";
   geometry_pub = node_handle_->advertise<localization::localMsg>(robot::common::Geometry, 1);
-  if (car_input_mode == "modbus") {
+  //TODO  zhushidiao
+//      rack_test_timer_ = CreateTimer(ros::Duration(duration_rack), &Localization::RackTestOnTimer, this,false,false);
+//      rack_test_timer_.start();
+  //TODO dakai
+if (car_input_mode == "modbus") {
     if (!ros::param::get("car_input_volspeed_max", car_input_volspeed_max))
         car_input_volspeed_max = 0;
 
@@ -99,6 +105,10 @@ int Localization::Start() {
               int voltage = (unsigned int)modbus_recv[sizeof(modbus_recv)-2] * 256 + (unsigned int)modbus_recv[sizeof(modbus_recv)-1];
               int max = car_input_volspeed_max;
               localization.Speed = voltage / 65535.0 * max;
+              if(localization.Speed < 0.4)
+              {
+              localization.Speed = 0;
+              }
               geometry_pub.publish(localization);
               LOG(INFO) << Name() << " publish speed:" << std::setprecision(4) << localization.Speed;
           }
@@ -159,10 +169,23 @@ int Localization::Start() {
       return ROBOT_FAILTURE;
     }
   }
+  //TODO
   LOG(INFO)<<Name()<<" start end.";
   return ROBOT_SUCCESS;
 }
 
+void Localization::RackTestOnTimer(const ros::TimerEvent &event)
+{
+  localization::localMsg localization;
+  localization.Longitude =  0;
+  localization.Latitude = 0;
+  localization.Height =0;
+  localization.AngleDeviated = 0;
+  localization.Speed = 0;
+  geometry_pub.publish(localization);
+
+}
+
 bool Localization::IsOpen()
 {
   //检测串口是否已经打开，并给出提示信息
@@ -397,7 +420,6 @@ int Localization::ParaGPFPD(std::string &strGPFPD, GPFPD &gpfpd)
   {
 
       std::vector<std::string> nmea_gpfpd = Split(strGPFPD, ",");
-      //TODO
       if(nmea_gpfpd.size() < 16)
       {
         return ROBOT_FAILTURE;

src/localization/localization.h

@@ -132,6 +132,10 @@ private:
 
   unsigned char modbus_recv[11];
   //----------
+
+  const double duration_rack = 0.005;//10ms//台架测试任务执行时间
+     ros::Timer rack_test_timer_;    //台架测试任务定时器
+      void RackTestOnTimer(const ros::TimerEvent &event);  //台架测试任务定时器回调函数
 };