添加台架任务服务请求，模糊PID算法，RTK数据接收与解析

src/canbus/CMakeLists.txt

@@ -45,9 +45,11 @@ find_package(catkin REQUIRED COMPONENTS
 ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 
 ## Generate messages in the 'msg' folder
-add_message_files(FILES
+add_message_files(
+    FILES
   canMsg.msg
   obdMsg.msg
+  rackTestReplyMsg.msg
 )
 
 ## Generate services in the 'srv' folder
@@ -85,6 +87,7 @@ add_message_files(FILES
    RbtSlfChckCtrlReq.srv
    IOMode.srv
    SftTxtReq.srv
+   RackTestReq.srv
  )
 
 ## Generate actions in the 'action' folder
@@ -246,6 +249,8 @@ add_executable(canbus
     candatatype.h
     messagecoder.h
     messagecoder.cpp
+    pid.h
+    pid.cpp
  )
 add_dependencies(canbus ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 target_link_libraries(canbus

src/canbus/canbus.cpp

@@ -7,19 +7,22 @@
 #include "canbus.h"
 #include"candatatype.h"
 #include"messagecoder.h"
+#include "pid.h"
 #include"../common/message.h"
 #include"../common/canmsgobdmsgid.h"
 #include "std_msgs/String.h"
 #include <canbus/canMsg.h>
+#include <canbus/rackTestReplyMsg.h>
 #include <iostream>
 #include <fstream>
+#include <chrono>
+#include <sys/time.h>
 
 
 namespace  robot{
 namespace canbusSpace {
 
 
-
 std::string Canbus::Name() const { return "canbus"; }
 
 int Canbus::Init() {
@@ -98,6 +101,7 @@ int Canbus::Start() {
 
   canRCUData_pub = node_handle_->advertise<canbus::canMsg>(robot::common::CanDataAnalysis, 1);
   canObdData_pub = node_handle_->advertise<canbus::obdMsg>(robot::common::ObdData, 1);
+  canRackData_pub = node_handle_->advertise<canbus::rackTestReplyMsg>(robot::common::RackReplyData,1);
 
   ros::ServiceServer ActuatorEnableService = node_handle_->advertiseService(robot::common::ActuatorEnableService,
                                                                 &Canbus::ActuatorEnableRequest, this);
@@ -179,6 +183,12 @@ int Canbus::Start() {
 
   ros::ServiceServer SftTxtService = node_handle_->advertiseService(robot::common::SftTxtService,
                                                                     &Canbus::SftTxtReq,this);
+  ros::ServiceServer RackTestService = node_handle_->advertiseService(robot::common::RackTestService,
+                                                                     &Canbus::RackTestReq,this);
+
+  ros::Subscriber GeometrySub = node_handle_->subscribe(robot::common::Geometry, 1,
+                                                        &Canbus::RTKLocationCallback, this);
+
 
   string str = "/home/madesheng/angle.txt";
   int flag = Canbus::read_txt(str,1);
@@ -214,6 +224,13 @@ int Canbus::Start() {
      return ROBOT_FAILTURE;
    }
 
+   str = "/home/madesheng/NEDC Speed.txt";
+   flag = Canbus::read_txt(str,6);
+   if(flag != ROBOT_SUCCESS)
+   {
+     return ROBOT_FAILTURE;
+   }
+
   //data receive
   while(ros::ok())
   {
@@ -230,6 +247,7 @@ int Canbus::Start() {
 
 void Canbus::OnTimer(const ros::TimerEvent &) {
   //LOG(INFO)<< Name()<< " timer start ..";
+  //TODO dakai
   if(heatbeat > 15)
   {
       heatbeat = 0;
@@ -269,6 +287,7 @@ void Canbus::OnTimer(const ros::TimerEvent &) {
   {
        LOG(ERROR)<<Name()<< " OnTimer error. "<< ex.what();
   }
+  //TODO
 }
 
 void Canbus::Stop() {
@@ -313,11 +332,12 @@ int Canbus::read_loop()
 {
     TPCANRdMsg msg;
     __u32 status;
-
+    //TODO dakai
     if (funLINUX_CAN_Read(pcan_handle, &msg)) {
         LOG(ERROR)<<Name()<<" receive: LINUX_CAN_Read() error.";
         return errno;
     }
+
     read_data(&(msg.Msg));
 
      // deal receive data
@@ -330,7 +350,7 @@ int Canbus::read_loop()
             return errno;
         }
     }
-
+//TODO
     return ROBOT_SUCCESS;
 }
 
@@ -1571,6 +1591,34 @@ int Canbus::read_txt(string &path,int flag)
     }
     cout << "sft NUMBER:" <<sft_psng_number<<endl;
   }
+  else if(flag == 6)
+  {
+    for (int i = 0; i < 1190; i++)
+    {
+        if(myfile.eof())
+        {
+            nedc_speed_number  = i;
+            break;
+        }
+        myfile >> nedc_speeds[i];
+
+    }
+    cout << "nedc speed NUMBER:" <<nedc_speed_number<<endl;
+  }
+  else if(flag == 7)
+  {
+    for (int i = 0; i < 3; i++)
+    {
+        if(myfile.eof())
+        {
+            fuzzy_pid_number  = i;
+            break;
+        }
+        myfile >> fuzzy_pid[i];
+
+    }
+    cout << "Fuzzy PID NUMBER:" <<fuzzy_pid_number<<endl;
+  }
 
 
    myfile.close();
@@ -1984,7 +2032,410 @@ bool Canbus::IOMode(canbus::IOMode::Request &req,
 
 }
 
+bool Canbus::RackTestReq(canbus::RackTestReq::Request &req,
+                         canbus::RackTestReq::Response &res)
+{
+  LOG(INFO)<< "controlservice start RackTestReq";
+
+  // RackTestReq request
+  try{
+    if(req.uiStart == 0)
+    {
+      if(rackTestState)
+      {
+         //TODO zhushidiao
+        //rack_test_timer_.stop();
+         //TODO
+        rackTestState = false;
+      }
+      if(rackTestSendTimerState)
+      {
+        rack_reply_timer_.stop();
+        rackTestSendTimerState = false;
+        rackTestReplyState = 0;
+      }
+      publish_rack_speed();
+      res.uiReturn = 1;
+      return true;
+    }
+    if(rackTestState || rackTestSendTimerState)
+    {
+      res.uiReturn = 2;
+      return true;
+    }
+    nedc_speed_count = 0;
+    rack_test_send_count = 0;
+//TODO zhushidiao
+   // rack_test_timer_ = CreateTimer(ros::Duration(duration_rack), &Canbus::RackTestOnTimer, this,false,false);
+  //  rack_test_timer_.start();
+    //TODO
+    rackTestState = true;
+
+    rack_reply_timer_ =
+        CreateTimer(ros::Duration(duration_rack_reply), &Canbus::RackTestReplyOnTimer, this,false,false);
+    rack_reply_timer_.start();
+    rackTestSendTimerState = true;
+    res.uiReturn = 1;
+  }
+  catch (ros::Exception ex)
+  {
+      res.uiReturn = 0;
+      LOG(ERROR)<< "controlservice RackTestReq failed "<< ex.what();
+      return true;
+  }
+  return true;
+
+}
+
+void Canbus::RackTestOnTimer(const ros::TimerEvent &event)
+{
+  PID PidControll;
+  try
+  {
+    if(nedc_speed_count == nedc_speed_number)
+    {
+      rack_test_timer_.stop();
+      rackTestState = false;
+      rack_reply_timer_.stop();
+      rackTestSendTimerState = false;
+      rackTestReplyState = 0;
+      publish_rack_speed();
+      return;
+    }
+
+    gettimeofday(&now_time, NULL);
+    float delta_time = 0.0;
+    if(sizeof(last_time) != 0)
+    {
+      long elapsed = (now_time.tv_sec-last_time.tv_sec) * 1000000 + (now_time.tv_usec - last_time.tv_usec);
+      delta_time = elapsed / 1000000;
+    }
+    cout << "time sub" << delta_time <<endl;
+
+    last_time = now_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_speed = set_speed1 + (set_speed2 - set_speed1) * delta_time;
+    float actual_speed = status2.uiVehSpd;
+
+    string str = "/home/madesheng/FuzzyPidFactor.txt";
+    int flag = Canbus::read_txt(str,7);
+    if(flag != ROBOT_SUCCESS)
+    {
+      LOG(INFO)<< "Get FuzzyPIDFactor.txt error";
+      return;
+    }
+
+    float pi_out = 0.01;
+    pi_out = PidControll.FuzzyPIDcontroller(set_speed,actual_speed,delta_time,
+                                            fuzzy_pid[0],fuzzy_pid[1],fuzzy_pid[2]);
+
+    LOG(INFO)<< "P:"<<std::setprecision(18)<<fuzzy_pid[0];
+    LOG(INFO)<< "I:"<<std::setprecision(18)<<fuzzy_pid[1];
+    LOG(INFO)<< "D:"<<std::setprecision(18)<<fuzzy_pid[2];
+    LOG(INFO)<< "Set Speed:"<<std::setprecision(18)<<set_speed;
+    LOG(INFO)<< "Actual Speed:"<<std::setprecision(18)<<actual_speed;
+    LOG(INFO)<< "pi_out:"<<std::setprecision(18)<<pi_out;
+
+    TPCANMsg canBrkPedalActCtrReqMsg;
+    T_BRK_PEDAL_ACT_CTR_REQ requestBrk;
+    TPCANMsg canAccPedalActCtrReqMsg;
+    T_ACC_PEDAL_ACT_CTR_REQ requestAcc;
+
+    if(set_speed == 0)
+    {
 
+          requestBrk.uiBrkSpdCtrReq = 0;
+          requestBrk.uiBrkAccCtrReq = 0;
+          requestBrk.uiBrkCtrMdReq = 0;
+          requestBrk.uiBrkDecCtrReq = 0;
+          requestBrk.uiBrkFocCtrReq = 0;
+          requestBrk.uiBrkPsngCtrReq = (50/30) * 5;
+
+          LOG(INFO)<< "Send Brk Psng:"<<requestBrk.uiBrkPsngCtrReq;
+
+          MessageCoder::CanMsgPackage((void*)&requestBrk, robot::common::BrkPedalActCtrReq, &canBrkPedalActCtrReqMsg);
+          write_data(&canBrkPedalActCtrReqMsg);
+    }
+    else
+    {
+      if(pi_out >= 0)
+      {
+         requestAcc.uiAccSpdCtrReq = 0;
+         requestAcc.uiAccAccCtrReq = 0;
+         requestAcc.uiAccPsngCtrReq = pi_out *100;
+         requestAcc.uiAccDecCtrReq = 0;
+
+         LOG(INFO)<< "Send Acc Psng:"<<requestAcc.uiAccPsngCtrReq;
+
+         MessageCoder::CanMsgPackage((void*)&requestAcc, robot::common::AccPedalActCtrReq, &canAccPedalActCtrReqMsg);
+         write_data(&canAccPedalActCtrReqMsg);
+
+      }
+      else
+      {
+        requestBrk.uiBrkSpdCtrReq = 0;
+        requestBrk.uiBrkAccCtrReq = 0;
+        requestBrk.uiBrkCtrMdReq = 0;
+        requestBrk.uiBrkDecCtrReq = 0;
+        requestBrk.uiBrkFocCtrReq = 0;
+        requestBrk.uiBrkPsngCtrReq = (50/30) * (-pi_out * 30);
+
+        LOG(INFO)<< "Send Brk Psng:"<<requestBrk.uiBrkPsngCtrReq;
+
+        MessageCoder::CanMsgPackage((void*)&requestBrk, robot::common::BrkPedalActCtrReq, &canBrkPedalActCtrReqMsg);
+        write_data(&canBrkPedalActCtrReqMsg);
+      }
+    }
+    nedc_speed_count = rack_test_send_count;
+
+  }
+  catch (ros::Exception ex)
+  {
+       LOG(ERROR)<<Name()<< " RackTestOnTimer error. "<< ex.what();
+  }
+}
+
+void Canbus::RackTestReplyOnTimer(const ros::TimerEvent &event)
+{
+  try
+  {
+    if(rackTestSendTimerState)
+    {
+       rackTestReplyState = 1;
+    }
+    else
+    {
+      rackTestReplyState = 0;
+    }
+
+    // 发布消息，rackTestReplyState,actual_speed 1s回复前端一次消息
+    publish_rack_speed();
+    rack_test_send_count ++;
+  }
+  catch (ros::Exception ex)
+  {
+    LOG(ERROR)<<Name()<< " RackTestReplyOnTimer error. "<< ex.what();
+  }
+
+}
+
+//获取当前时间和毫秒
+int Canbus::get_datetime_mill(tm *datetime)
+{
+  try
+  {
+    auto time_now = std::chrono::system_clock::now();
+
+    //通过不同精度获取相差的毫秒数
+    uint64_t difference_millseconds =
+        std::chrono::duration_cast<std::chrono::milliseconds>(time_now.time_since_epoch()).count()
+        - std::chrono::duration_cast<std::chrono::seconds>(time_now.time_since_epoch()).count() * 1000;
+    time_t t_time = std::chrono::system_clock::to_time_t(time_now);
+    tm *time_date = localtime(&t_time);
+    char strTime[25] = { 0 };
+    sprintf(strTime, "[%d-%02d-%02d %02d:%02d:%02d.%03d]", time_date->tm_year + 1900,
+            time_date->tm_mon + 1, time_date->tm_mday, time_date->tm_hour,
+            time_date->tm_min, time_date->tm_sec, (int)difference_millseconds);
+    /* 打印时间 */
+  //  std::cout << strTime << std::endl;
+
+    datetime->tm_year = time_date->tm_year + 1900;
+    datetime->tm_mon = time_date->tm_mon + 1;
+    datetime->tm_mday = time_date->tm_mday;
+    datetime->tm_hour = time_date->tm_hour;
+    datetime->tm_min = time_date->tm_min;
+    datetime->tm_sec = time_date->tm_sec;
+    return (int)difference_millseconds;
+
+  }
+  catch (ros::Exception ex)
+  {
+    LOG(ERROR)<<Name()<< " Get Now DateTime And Millseconds error. "<< ex.what();
+  }
+
+}
+
+//发布台架测试任务实际车速
+void Canbus::publish_rack_speed()
+{
+  canbus::rackTestReplyMsg rackMsg;
+  //rackMsg.ActualSpeed = nedc_speeds[nedc_speed_count];
+  rackMsg.ActualSpeed = msg_RTK.Speed;
+  rackMsg.RackTestReplyState = rackTestReplyState;
+  canRackData_pub.publish(rackMsg);
+
+}
+
+// 接收ＲＴＫ数据
+void Canbus::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
+{
+  msg_RTK.AngleDeviated = msg->AngleDeviated;
+  msg_RTK.Height = msg->Height;
+  msg_RTK.Latitude = msg->Latitude;
+  msg_RTK.Longitude = msg->Longitude;
+  msg_RTK.Speed = msg->Speed;
+  try
+  {
+    if(nedc_speed_count == nedc_speed_number)
+    {
+      rackTestState = false;
+      rack_reply_timer_.stop();
+      rackTestSendTimerState = false;
+      rackTestReplyState = 0;
+      publish_rack_speed();
+      return;
+    }
+    if(!rackTestState)
+    {
+      return;
+    }
+    LOG(INFO)<< "RTKLocationCallback Start FUZZY PID";
+    PID PidControll;
+
+
+    gettimeofday(&now_time, NULL);
+    float delta_time = 0.0;
+    if(sizeof(last_time) != 0)
+    {
+      long elapsed = (now_time.tv_sec-last_time.tv_sec) * 1000000 + (now_time.tv_usec - last_time.tv_usec);
+      delta_time = (float)elapsed / 1000000;
+    }
+
+    LOG(INFO)<< "delta time:"<<std::setprecision(18)<<delta_time<<" s";
+   // cout << "time sub " << delta_time <<" s"<<endl;
+
+    last_time = now_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_speed = set_speed1 + (set_speed2 - set_speed1) * delta_time;
+    float actual_speed = msg_RTK.Speed;
+
+    string str = "/home/madesheng/FuzzyPidFactor.txt";
+    int flag = Canbus::read_txt(str,7);
+    if(flag != ROBOT_SUCCESS)
+    {
+      LOG(INFO)<< "Get FuzzyPIDFactor.txt error";
+      return;
+    }
+
+    float pi_out = 0.01;
+    pi_out = PidControll.FuzzyPIDcontroller(set_speed,actual_speed,delta_time,
+                                            fuzzy_pid[0],fuzzy_pid[1],fuzzy_pid[2]);
+
+    LOG(INFO)<< "P:"<<std::setprecision(18)<<fuzzy_pid[0];
+    LOG(INFO)<< "I:"<<std::setprecision(18)<<fuzzy_pid[1];
+    LOG(INFO)<< "D:"<<std::setprecision(18)<<fuzzy_pid[2];
+    LOG(INFO)<< "Set Speed:"<<std::setprecision(18)<<set_speed;
+    LOG(INFO)<< "Actual Speed:"<<std::setprecision(18)<<actual_speed;
+    LOG(INFO)<< "pi_out:"<<std::setprecision(18)<<pi_out;
+
+    TPCANMsg canBrkPedalActCtrReqMsg;
+    T_BRK_PEDAL_ACT_CTR_REQ requestBrk;
+    TPCANMsg canAccPedalActCtrReqMsg;
+    T_ACC_PEDAL_ACT_CTR_REQ requestAcc;
+
+    if(set_speed == 0)
+    {
+      requestAcc.uiAccSpdCtrReq = 0;
+      requestAcc.uiAccAccCtrReq = 0;
+      requestAcc.uiAccPsngCtrReq = 0;
+      requestAcc.uiAccDecCtrReq = 0;
+
+      MessageCoder::CanMsgPackage((void*)&requestAcc, robot::common::AccPedalActCtrReq, &canAccPedalActCtrReqMsg);
+      write_data(&canAccPedalActCtrReqMsg,true);
+
+          requestBrk.uiBrkSpdCtrReq = 0;
+          requestBrk.uiBrkAccCtrReq = 0;
+          requestBrk.uiBrkCtrMdReq = 0;
+          requestBrk.uiBrkDecCtrReq = 0;
+          requestBrk.uiBrkFocCtrReq = 0;
+          requestBrk.uiBrkPsngCtrReq = 35 * 10;
+
+          LOG(INFO)<< "Send Brk Psng:"<<requestBrk.uiBrkPsngCtrReq;
+
+          MessageCoder::CanMsgPackage((void*)&requestBrk, robot::common::BrkPedalActCtrReq, &canBrkPedalActCtrReqMsg);
+          write_data(&canBrkPedalActCtrReqMsg,true);
+    }
+    else
+    {
+      if(pi_out >= 0)
+      {
+
+        requestBrk.uiBrkSpdCtrReq = 0;
+        requestBrk.uiBrkAccCtrReq = 0;
+        requestBrk.uiBrkCtrMdReq = 0;
+        requestBrk.uiBrkDecCtrReq = 0;
+        requestBrk.uiBrkFocCtrReq = 0;
+        requestBrk.uiBrkPsngCtrReq = 0;
+
+        MessageCoder::CanMsgPackage((void*)&requestBrk, robot::common::BrkPedalActCtrReq, &canBrkPedalActCtrReqMsg);
+        write_data(&canBrkPedalActCtrReqMsg,true);
+
+         requestAcc.uiAccSpdCtrReq = 0;
+         requestAcc.uiAccAccCtrReq = 0;
+         requestAcc.uiAccPsngCtrReq = pi_out *100 * 10;
+         requestAcc.uiAccDecCtrReq = 0;
+
+         LOG(INFO)<< "Send Acc Psng:"<<requestAcc.uiAccPsngCtrReq;
+
+         MessageCoder::CanMsgPackage((void*)&requestAcc, robot::common::AccPedalActCtrReq, &canAccPedalActCtrReqMsg);
+         write_data(&canAccPedalActCtrReqMsg,true);
+
+      }
+      else
+      {
+
+        requestAcc.uiAccSpdCtrReq = 0;
+        requestAcc.uiAccAccCtrReq = 0;
+        requestAcc.uiAccPsngCtrReq = 0;
+        requestAcc.uiAccDecCtrReq = 0;
+
+        MessageCoder::CanMsgPackage((void*)&requestAcc, robot::common::AccPedalActCtrReq, &canAccPedalActCtrReqMsg);
+        write_data(&canAccPedalActCtrReqMsg,true);
+
+        requestBrk.uiBrkSpdCtrReq = 0;
+        requestBrk.uiBrkAccCtrReq = 0;
+        requestBrk.uiBrkCtrMdReq = 0;
+        requestBrk.uiBrkDecCtrReq = 0;
+        requestBrk.uiBrkFocCtrReq = 0;
+        requestBrk.uiBrkPsngCtrReq = (80 * (-pi_out * 30) / 30) * 10;
+
+        LOG(INFO)<< "Send Brk Psng:"<<requestBrk.uiBrkPsngCtrReq;
+
+        MessageCoder::CanMsgPackage((void*)&requestBrk, robot::common::BrkPedalActCtrReq, &canBrkPedalActCtrReqMsg);
+        write_data(&canBrkPedalActCtrReqMsg,true);
+      }
+    }
+    nedc_speed_count = rack_test_send_count;
+
+  }
+  catch (ros::Exception ex)
+  {
+       LOG(ERROR)<<Name()<< " RTKLocationCallback error. "<< ex.what();
+  }
+
+}
 
 
 

src/canbus/canbus.h

@@ -58,6 +58,10 @@
 #include <canbus/IOMode.h>
 #include "std_msgs/String.h"
 #include <canbus/SftTxtReq.h>
+#include <canbus/RackTestReq.h>
+#include <canbus/rackTestReplyMsg.h>
+#include <localization/localMsg.h>
+#include <ctime>
 
 using namespace std;
 
@@ -116,7 +120,6 @@ class Canbus : public robot::common::RobotApp{
   */
   void Stop() override;
 
-
  private:
 
   void InitServiceAndMsg();
@@ -141,6 +144,10 @@ class Canbus : public robot::common::RobotApp{
 
   void SftTxtOnTimer(const ros::TimerEvent &event); //挡位文件定时器回调函数
 
+  void RackTestOnTimer(const ros::TimerEvent &event);  //台架测试任务定时器回调函数
+
+  void RackTestReplyOnTimer(const ros::TimerEvent &event); //台架测试任务回复消息定时器回调函数
+
   //define function pointer,there is a one-to-one mapping between target function and your defined function
   funCAN_Init_TYPE        funCAN_Init;
   funLINUX_CAN_Open_TYPE  funLINUX_CAN_Open;
@@ -173,6 +180,10 @@ class Canbus : public robot::common::RobotApp{
 
   int read_txt(string &path,int flag);  //读取TXT文件
 
+  int get_datetime_mill(tm *datetime);  //获取当前时间和毫秒
+
+  void publish_rack_speed(); //发布台架测试任务实际车速
+
   /**
   * @brief create services
   */
@@ -265,6 +276,14 @@ class Canbus : public robot::common::RobotApp{
   bool SftTxtReq(canbus::SftTxtReq::Request &req,
                  canbus::SftTxtReq::Response &res);
 
+  bool RackTestReq(canbus::RackTestReq::Request &req,
+                   canbus::RackTestReq::Response &res);
+
+  /**
+  * RTK定位回调函数
+  */
+  void RTKLocationCallback(const localization::localMsg::ConstPtr& msg);
+
 private:
 
   int64_t last_timestamp_ = 0;
@@ -337,14 +356,34 @@ private:
    int sft_psng_number = 0; //TXT文件中的位置值的数量
    T_SFT_ACT_CTR_REQ tSftCtrReqTxt = {0};
 
+   const double duration_rack = 0.01;//10ms//台架测试任务执行时间
+   const double duration_rack_reply = 1; //1s//台架测试任务回复消息时间
+   ros::Timer rack_test_timer_;    //台架测试任务定时器
+   ros::Timer rack_reply_timer_;   //台架测试任务回复消息定时器
+   bool rackTestState = false;  //台架测试任务标志位
+   bool rackTestSendTimerState = false;  //台架测试任务发送数据定时器标志位
+   int rackTestReplyState = 0;   //台架测试任务回复消息标志位  0表示前端界面不显示回复，1表示前端界面显示回复
+  // double nedc_speeds[1182] = {0};  //TXT文件中的速度值
+   double nedc_speeds[1182] = {0};  //TXT文件中的速度值
+   int nedc_speed_count = 0;   //TXT文件中的速度值的编号
+   int nedc_speed_number = 0; //TXT文件中的速度值的数量
+   int rack_test_send_count = 0; //台架测试任务发送数据计数器，1s一个速度数据
+   struct timeval now_time;//当前时间
+   struct timeval last_time;//上一次时间
+   localization::localMsg msg_RTK;
+
+   int fuzzy_pid_number = 0;
+   double fuzzy_pid[3] = {0};
 
-  const double duration = 0.01;
+
+
+  const double duration = 0.01; //10ms
 
   const double rate = 0.0001;  //0.0001s
   //GLOBALS
   void *libm_handle = NULL;//define pointer used for file acess of libpcan.so
   HANDLE pcan_handle =NULL;//void *pcan_handle
-  unsigned short unBaudrate = CAN_BAUD_1M; //set the communicate baud rate of can bus
+  unsigned short unBaudrate = CAN_BAUD_500K; //set the communicate baud rate of can bus
   int nExtended = CAN_INIT_TYPE_ST;       //set can message int standard model   CAN_INIT_TYPE_EX/CAN_INIT_TYPE_ST
 
   T_VEHICLE_STATUS_1 status1 = {0};    //VehicleStaus1
@@ -368,6 +407,8 @@ private:
 
   ros::Publisher canObdData_pub; // 发布OBD数据包
 
+  ros::Publisher canRackData_pub;  //发布台架测试任务回复数据包
+
   canbus::obdMsg obd; // obd data
 
 };

src/canbus/msg/rackTestReplyMsg.msg

@@ -0,0 +1,2 @@
+uint32 RackTestReplyState
+float64 ActualSpeed

src/canbus/pid.cpp

@@ -0,0 +1,429 @@
+/*
+ * PID.cpp
+ *
+ *  Created on: Dec 7, 2023
+ *      Author: siasun
+ */
+#include "pid.h"
+namespace  robot{
+namespace canbusSpace {
+PID::PID()  //构造函数
+{
+
+
+    num_area = 8;
+    qdetail_kp = 0;               //增量kp对应论域中的值
+    qdetail_ki = 0;               //增量ki对应论域中的值
+    qdetail_kd = 0;               //增量kd对应论域中的值
+
+    /****************隶属度*******************/
+    //输入e的隶属值
+    e_membership_values[0] = -6;
+    e_membership_values[1] = -4;
+    e_membership_values[2] = -2;
+    e_membership_values[3] = 0;
+    e_membership_values[4] = 2;
+    e_membership_values[5] = 4;
+    e_membership_values[6] = 6;
+
+    //输入de/dt的隶属值
+    ec_membership_values[0] = -6;
+    ec_membership_values[1] = -4;
+    ec_membership_values[2] = -2;
+    ec_membership_values[3] = 0;
+    ec_membership_values[4] = 2;
+    ec_membership_values[5] = 4;
+    ec_membership_values[6] = 6;
+
+    //输出增量kp的隶属值
+    kp_menbership_values[0] = -1;//输出增量kp的隶属值
+    kp_menbership_values[1] = -0.6667;
+    kp_menbership_values[2] = -0.3333;
+    kp_menbership_values[3] = 0;
+    kp_menbership_values[4] = 0.3333;
+    kp_menbership_values[5] = 0.6667;
+    kp_menbership_values[6] = 1;
+
+    //输出增量ki的隶属值
+    ki_menbership_values[0] = -1; //输出增量ki的隶属值
+    ki_menbership_values[1] = -0.6667;
+    ki_menbership_values[2] = -0.3333;
+    ki_menbership_values[3] = 0;
+    ki_menbership_values[4] = 0.3333;
+    ki_menbership_values[5] = 0.6667;
+    ki_menbership_values[6] = 1;
+
+    //输出增量kd的隶属值
+    kd_menbership_values[0] = -1;  //输出增量kd的隶属值
+    kd_menbership_values[1] = -0.6667;
+    kd_menbership_values[2] = -0.3333;
+    kd_menbership_values[3] = 0;
+    kd_menbership_values[4] = 0.3333;
+    kd_menbership_values[5] = 0.6667;
+    kd_menbership_values[6] = 1;
+}
+
+PID::~PID()//析构函数
+{
+}
+
+
+/**********************************位置PID****************************************/
+
+float PID::Pid(int TargetSpeed  ,int ActualSpeed)
+{
+  float error = 0; //误差
+  static float error_1 = 0; //上一次的误差
+  static float error_i = 0;
+  float out = 0;  // 输出
+  float out_i =0;  // 输出
+
+  float kp= 0.1;
+  float ki= 0.001;
+  float kd= 0.1;
+
+
+  error = TargetSpeed  - ActualSpeed;
+  error_i += error;
+  out_i =  ki * error_i;
+
+
+  if(out_i > 1000.0)
+    out_i = 1000.0;
+  if(out_i < -1000.0)
+    out_i = -1000.0;
+
+  out = kp * error  + out_i + kd*(error -error_1);
+  error_1 = error;
+
+
+    return out;
+
+}
+
+
+/**********************************增量PID******************************************/
+
+
+float PID::ADD_PID(int Target ,int Actual)
+{
+  float error; //误差
+  static float error_1; //上一次的误差
+  static float error_2; //上上次的误差
+  float out;  // 输出
+  static float out_last; //上次输出
+
+  float kp= 0.1;
+  float ki= 0.1;
+  float kd= 0.1;
+
+
+  error = Target - Actual;
+  float  increamentspeed = kp*(error -error_1) + ki*error_1 + kd*(error + error_2 -2*error_1);
+  out = out_last  + increamentspeed;
+  out_last  =  out;
+  error_1 = error;
+  error_2 = error_1;
+
+    return out;
+
+}
+
+
+
+
+/****************************** 模糊PID**************************************/
+
+//输入e与de/dt隶属度计算函数
+void PID::Get_grad_membership(float erro,float erro_c)
+{
+    if (erro > e_membership_values[0] && erro < e_membership_values[6])
+    {
+        for (int i = 0; i < num_area - 2; i++)
+        {
+            if (erro >= e_membership_values[i] && erro <= e_membership_values[i + 1])
+            {
+                e_gradmembership[0] = -(erro - e_membership_values[i + 1]) / (e_membership_values[i + 1] - e_membership_values[i]);
+                e_gradmembership[1] = 1+(erro - e_membership_values[i + 1]) / (e_membership_values[i + 1] - e_membership_values[i]);
+                e_grad_index[0] = i;
+                e_grad_index[1] = i + 1;
+                break;
+            }
+        }
+    }
+    else
+    {
+        if (erro <= e_membership_values[0])
+        {
+            e_gradmembership[0] = 1;
+            e_gradmembership[1] = 0;
+            e_grad_index[0] = 0;
+            e_grad_index[1] = -1;
+        }
+        else if (erro >= e_membership_values[6])
+        {
+            e_gradmembership[0] = 1;
+            e_gradmembership[1] = 0;
+            e_grad_index[0] = 6;
+            e_grad_index[1] = -1;
+        }
+    }
+
+    if (erro_c > ec_membership_values[0] && erro_c < ec_membership_values[6])
+    {
+        for (int i = 0; i < num_area - 2; i++)
+        {
+            if (erro_c >= ec_membership_values[i] && erro_c <= ec_membership_values[i + 1])
+            {
+                ec_gradmembership[0] = -(erro_c - ec_membership_values[i + 1]) / (ec_membership_values[i + 1] - ec_membership_values[i]);
+                ec_gradmembership[1] = 1 + (erro_c - ec_membership_values[i + 1]) / (ec_membership_values[i + 1] - ec_membership_values[i]);
+                ec_grad_index[0] = i;
+                ec_grad_index[1] = i + 1;
+                break;
+            }
+        }
+    }
+    else
+    {
+        if (erro_c <= ec_membership_values[0])
+        {
+            ec_gradmembership[0] = 1;
+            ec_gradmembership[1] = 0;
+            ec_grad_index[0] = 0;
+            ec_grad_index[1] = -1;
+        }
+        else if (erro_c >= ec_membership_values[6])
+        {
+            ec_gradmembership[0] = 1;
+            ec_gradmembership[1] = 0;
+            ec_grad_index[0] = 6;
+            ec_grad_index[1] = -1;
+        }
+    }
+
+}
+
+//获取输出增量kp,ki,kd的总隶属度
+void PID::GetSumGrad()
+{
+
+  int NB = -3, NM = -2, NS = -1, ZO = 0, PS = 1, PM = 2, PB = 3; //论域隶属值
+
+    int  Kp_rule_list[7][7] = { {PB,PB,PM,PM,PS,ZO,ZO},     //kp规则表
+                                {PB,PB,PM,PS,PS,ZO,NS},
+                                {PM,PM,PM,PS,ZO,NS,NS},
+                                {PM,PM,PS,ZO,NS,NM,NM},
+                                {PS,PS,ZO,NS,NS,NM,NM},
+                                {PS,ZO,NS,NM,NM,NM,NB},
+                                {ZO,ZO,NM,NM,NM,NB,NB} };
+
+    int  Ki_rule_list[7][7] = { {NB,NB,NM,NM,NS,ZO,ZO},     //ki规则表
+                                {NB,NB,NM,NS,NS,ZO,ZO},
+                                {NB,NM,NS,NS,ZO,PS,PS},
+                                {NM,NM,NS,ZO,PS,PM,PM},
+                                {NM,NS,ZO,PS,PS,PM,PB},
+                                {ZO,ZO,PS,PS,PM,PB,PB},
+                                {ZO,ZO,PS,PM,PM,PB,PB} };
+
+    int  Kd_rule_list[7][7] = { {PS,NS,NB,NB,NB,NM,PS},    //kd规则表
+                                {PS,NS,NB,NM,NM,NS,ZO},
+                                {ZO,NS,NM,NM,NS,NS,ZO},
+                                {ZO,NS,NS,NS,NS,NS,ZO},
+                                {ZO,ZO,ZO,ZO,ZO,ZO,ZO},
+                                {PB,NS,PS,PS,PS,PS,PB},
+                                {PB,PM,PM,PM,PS,PS,PB} };
+
+
+
+    for (int i = 0; i < num_area - 1; i++)
+    {
+        KpgradSums[i] = 0;
+        KigradSums[i] = 0;
+        KdgradSums[i] = 0;
+
+    }
+   for (int i=0;i<2;i++)
+   {
+      if (e_grad_index[i] == -1)
+      {
+       continue;
+      }
+      for (int j = 0; j < 2; j++)
+      {
+          if (ec_grad_index[j] != -1 && e_grad_index[i] != -1)
+          {
+              int indexKp = Kp_rule_list[e_grad_index[i]][ec_grad_index[j]] + 3;//求出下标
+              int indexKi = Ki_rule_list[e_grad_index[i]][ec_grad_index[j]] + 3;
+              int indexKd = Kd_rule_list[e_grad_index[i]][ec_grad_index[j]] + 3;
+
+              KpgradSums[indexKp]= KpgradSums[indexKp] + (e_gradmembership[i] * ec_gradmembership[j]);
+              KigradSums[indexKi] = KigradSums[indexKi] + (e_gradmembership[i] * ec_gradmembership[j]);
+              KdgradSums[indexKd] = KdgradSums[indexKd] + (e_gradmembership[i] * ec_gradmembership[j]);
+
+
+          }
+          else
+          {
+            continue;
+          }
+
+      }
+  }
+
+}
+
+//计算输出增量kp,kd,ki对应论域值
+void PID::GetOUT()
+{
+
+    for (int i = 0; i < num_area - 1; i++)
+    {
+        qdetail_kp += kp_menbership_values[i] * KpgradSums[i];
+        qdetail_ki += ki_menbership_values[i] * KigradSums[i];
+        qdetail_kd += kd_menbership_values[i] * KdgradSums[i];
+    }
+}
+
+//模糊PID控制实现函数
+float PID::FuzzyPIDcontroller(float TargetSpeed,float ActualSpeed,float Calculation_Time,
+                              float KP_Factor,float KI_Factor,float KD_Factor)
+{
+    float kp = KP_BaseValue;
+    float ki = KI_BaseValue;
+    float kd = KD_BaseValue;
+
+
+    float detail_kp = 0;               //输出增量kp
+    float detail_ki = 0;               //输出增量ki
+    float detail_kd = 0;               //输出增量kd
+    float qerro = 0;                    //输入e对应论域中的值
+    float qerro_c = 0;                  //输入de/dt对应论域中的值
+
+  float erro = 0;//误差
+  float erro_c = 0;//误差变化
+  static float erro_pre = 0;//上次误差
+  static float errp_ppre = 0;//上上次误差
+  float output = 0;//输出
+  static float output_last = 0;//上次输出
+  static float error_i;//累计误差
+  float out_i = 0;//i输出
+
+
+  erro = TargetSpeed - ActualSpeed;//本次误差e（目标位置-实际位置）
+  if(Calculation_Time <= 0)
+  {
+    erro_c = 0;
+  }
+  else
+  {
+      erro_c = (erro - erro_pre)/Calculation_Time;//误差变化量 需要除以时间 de/dt
+  }
+
+  erro_pre = erro;
+
+    qerro = Quantization( erro, ERROR );//将误差映射在-6～6之间
+    qerro_c = Quantization( erro_c, ERROR_C );//将误差的差值映射在-6～6之间
+    Get_grad_membership(qerro, qerro_c); //找出误差 e和de/dt的隶属度
+    GetSumGrad();//获取输出增量kp,ki,kd
+    GetOUT();//计算输出增量kp,kd,ki对应论域值
+
+    //增量kp,kd,ki乘以系数
+    detail_kp = Inverse_quantization(KP_Factor ,qdetail_kp);
+    detail_ki = Inverse_quantization(KI_Factor ,qdetail_ki);
+    detail_kd = Inverse_quantization(KD_Factor ,qdetail_kd);
+
+
+//    printf(" kp= %f   ki= %f  kd= %f  detail_kp= %f  detail_ki= %f d etail_kd= %f\n",
+//    		kp  ,ki, kd ,detail_kp, detail_ki ,detail_kd);
+
+
+
+#  if 0
+    //增量pid
+
+    kp = kp + detail_kp;
+    ki = ki + detail_ki;
+    kd = kd + detail_kd;
+
+    float  increamentspeed  = kp*(erro - erro_pre) + ki * erro + kd * (erro - 2 * erro_pre + errp_ppre);
+
+    output =  output_last + increamentspeed ;
+    output_last = output;
+
+    erro_pre = erro;
+    errp_ppre =  erro_pre;
+
+#endif
+
+
+#if 1
+    //位置pid
+
+  error_i += erro;//累计误差的和
+
+  if( TargetSpeed == 0 ) //目标速度为0时清空积分的值 或 if( ActualSpeed == 0 )
+  {
+    error_i = 0;
+  }
+
+  //积分限值
+  if(ki != 0 && (error_i > (MaxValue  / ki)))
+  {
+    error_i = MaxValue /ki;
+  }
+  if(ki != 0 &&( error_i < ( MinValue / ki)))
+  {
+    error_i =  MinValue /ki;
+  }
+    kp = kp + detail_kp;
+    ki = ki + detail_ki;
+    kd = kd + detail_kd;
+  out_i = ki * error_i;//积分项输出
+
+  output = kp * erro  + out_i + kd*(erro - erro_pre);//输出
+
+
+
+    //输出限值
+  if(output > MaxValue )
+  {
+    output = MaxValue ;
+  }
+  if(output <  MinValue)
+  {
+    output =  MinValue;
+  }
+
+    qdetail_kd = 0;
+    qdetail_ki = 0;
+    qdetail_kp = 0;
+
+#endif
+
+
+
+    return output;
+}
+
+///区间映射函数 将e ec映射在[-6～6]之间
+float PID::Quantization(float x,float y)
+{
+    float qvalues =  6 * x / y;
+    return qvalues;
+}
+
+
+//反区间映射函数  增量kp,kd,ki乘以系数
+float PID::Inverse_quantization( float k,float qvalues)
+{
+    float x = k * qvalues ;
+    return x;
+}
+
+int PID::test()
+{
+  return 2023;
+}
+}// namespace canbusSpace
+}// namespace robot
+

src/canbus/pid.h

@@ -0,0 +1,90 @@
+/*
+ * PID.h
+ *
+ *  Created on: Dec 7, 2023
+ *      Author: siasun
+ */
+
+#ifndef PID_H_
+#define PID_H_
+
+//#define Calculation_Time  0.01//10MS
+
+#define KP_BaseValue  0.06
+#define KI_BaseValue  0.000006
+#define KD_BaseValue  0.01
+
+//#define KP_Factor   6
+//#define KI_Factor   0.008
+//#define KD_Factor   0.001
+
+#define ERROR     120
+#define ERROR_C   240
+
+#define MaxValue  1.0
+#define MinValue -1.0
+
+namespace robot {
+namespace canbusSpace {
+class PID
+{
+
+
+public:
+
+    PID();
+    ~PID();
+private:
+    /********************************位置PID*************************************/
+
+    float Pid(int TargetSpeed  ,int ActualSpeed);
+
+    /*********************************增量PID************************************/
+
+    float ADD_PID(int TargetSpeed ,int ActualSpeed);
+
+    /*********************************模糊PID*************************************/
+     void Get_grad_membership(float erro, float erro_c);////输入e与de/dt隶属度计算函数
+      float Quantization(float x,float y);//设定e与de/dt范围
+      float Inverse_quantization( float k,float qvalues);//增量kp,ki,kd 乘以系数
+      void GetSumGrad();//获取输出增量kp,ki,kd的总隶属度
+      void GetOUT();//计算输出增量kp,kd,ki对应论域值
+
+
+   int  num_area ; //划分区域个数
+   float qdetail_kp  ;               //增量kp对应论域中的值
+   float qdetail_ki  ;               //增量ki对应论域中的值
+   float qdetail_kd  ;               //增量kd对应论域中的值
+
+   float e_membership_values[7]  ; //输入e的隶属值
+   float ec_membership_values[7]  ;//输入de/dt的隶属值
+   float kp_menbership_values[7]  ;//输出增量kp的隶属值
+  float ki_menbership_values[7]  ; //输出增量ki的隶属值
+   float kd_menbership_values[7] ;  //输出增量kd的隶属值
+
+
+   float e_gradmembership[2] ;      //输入e的隶属度
+   float ec_gradmembership[2];     //输入de/dt的隶属度
+   int e_grad_index[2] ;            //输入e隶属度在规则表的索引
+   int ec_grad_index[2] ;           //输入de/dt隶属度在规则表的索引
+   float gradSums[7] ;
+   float KpgradSums[7] ;   //输出增量kp总的隶属度
+   float KigradSums[7] ;   //输出增量ki总的隶属度
+   float KdgradSums[7] ;   //输出增量kd总的隶属度
+
+public:
+  float FuzzyPIDcontroller(float TargetSpeed,float ActualSpeed,float Calculation_Time,
+                           float KP_Factor,float KI_Factor,float KD_Factor);//模糊PID控制实现函数
+  int test();
+
+/****************************************************************************************/
+
+
+};
+extern PID PidControll;
+}// namespace canbusSpace
+}// namespace robot
+
+
+
+#endif /* PID_H_ */

src/canbus/srv/RackTestReq.srv

@@ -0,0 +1,3 @@
+uint32 uiStart                   #启动标志位
+---
+uint32 uiReturn

src/common/message.h

@@ -67,6 +67,8 @@ const std::string Si1Data = "si1Data";
 const std::string SmData = "smData";
 
 const std::string ZSftData = "zsftData";
+
+const std::string RackReplyData = "rackreplydata";
 // -----------------topics ----------------------------
 
 // -----------------services ----------------------------
@@ -135,6 +137,8 @@ const std::string RobotWorkMdService = "robotworkmdservice";
 const std::string EstpCtrlService = "estpctrlservice";
 
 const std::string SftTxtService = "sfttxtservice";
+
+const std::string RackTestService = "racktestservice";
 // -----------------services ----------------------------
 
 

src/localization/localization.cpp

@@ -10,7 +10,7 @@
 #include <localization/localMsg.h>
 #include "../common/message.h"
 #include <iostream>
-
+#include <cmath>
 
 
 
@@ -23,8 +23,10 @@ int Localization::Init() {
     {
         LOG(INFO)<<Name()<<" init start ...";
         serial_port = "/dev/ttyUSB0";
-        baudrate = 115200;
-        timeout = 1000;
+//        baudrate = 115200;
+//        serial_port = "/dev/tty0";
+        baudrate = 460800;
+        timeout = 1000;//ms
         node_handle_.reset(new ros::NodeHandle());
         LOG(INFO)<<Name()<<" init end.";
     }
@@ -53,16 +55,30 @@ int Localization::Start() {
          ros::Rate loop_rate(rate);
          while(ros::ok())
          {
-
-           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());
-               LOG(INFO)<<Name()<<"  Read: " << strSerialResult;
-               NMEA0183 nmea;
-               if (Analyse(strSerialResult, nmea) == ROBOT_SUCCESS)
-                   PublishData(nmea);
+           while(serial_gnss.available() >0)
+           {
+             std::string strSerialResult;
+             strSerialResult = serial_gnss.readline();
+           //  LOG(INFO)<<Name()<<"  Read: " << strSerialResult;
+             NMEA0183 nmea;
+             if(Analyse_ReadLine(strSerialResult,nmea) == ROBOT_SUCCESS)
+             {
+               PublishData(nmea);
+             }
            }
+           //TODO
+//           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());
+//               strSerialResult = serial_gnss.readline(serial_gnss.available());
+//               LOG(INFO)<<Name()<<"  Read: " << strSerialResult;
+//               NMEA0183 nmea;
+//               if (Analyse(strSerialResult, nmea) == ROBOT_SUCCESS)
+//                   PublishData(nmea);
+//           }
+           //TODO
+
            ros::spinOnce();
            loop_rate.sleep();
          }
@@ -107,8 +123,11 @@ int Localization::Analyse(const std::string &strSerialResult,  NMEA0183 &nmea){
     {
         std::string delim("\r\n");
         std::vector<std::string> v_nmeaData = Split(strSerialResult, delim);
-        if (v_nmeaData.size() !=3)
-            return ROBOT_FAILTURE;
+        int number = v_nmeaData.size();
+              //TODO
+//        if (v_nmeaData.size() !=3)
+//            return ROBOT_FAILTURE;
+              //TODO
         std::vector<std::string>::iterator iter_start = v_nmeaData.begin();
         std::vector<std::string>::iterator iter_end = v_nmeaData.end();
         std::vector<std::string>::iterator iter;
@@ -135,6 +154,25 @@ int Localization::Analyse(const std::string &strSerialResult,  NMEA0183 &nmea){
                 //nmea.gpgga.print();
                 //LOG(INFO)<<Name()<<" para GPGGA end.";
             }
+            else if (type.compare("IFDYN") == 0)
+            {
+              IFDYN tempIFDYN;
+              if(ParaIFDYN(*iter, tempIFDYN) != ROBOT_SUCCESS)
+                return ROBOT_FAILTURE;
+              nmea.ifdyn = tempIFDYN;
+              nmea.ifdyn.print();
+              LOG(INFO)<<Name()<<" para IFDYN end.";
+
+            }
+            else if(type.compare("GPFPD") == 0)
+            {
+              GPFPD tempGPFPD;
+              if(ParaGPFPD(*iter, tempGPFPD) != ROBOT_SUCCESS)
+                return ROBOT_FAILTURE;
+              nmea.gpfpd = tempGPFPD;
+              nmea.gpfpd.print();
+              LOG(INFO)<<Name()<<" para GPFPD end.";
+            }
         }
 
     }
@@ -143,7 +181,7 @@ int Localization::Analyse(const std::string &strSerialResult,  NMEA0183 &nmea){
         LOG(ERROR)<<Name()<< " Analyse  error" << ex.what();
         return ROBOT_FAILTURE;
     }
-    //LOG(INFO)<<Name()<<" Analyse end.";
+    LOG(INFO)<<Name()<<" Analyse end.";
     return ROBOT_SUCCESS;
 
 }
@@ -220,6 +258,12 @@ int Localization::PublishData(NMEA0183 &nmea)
         localization.Latitude = to_string_with_precision(nmea.gprmc.Latitude, 11);
         localization.Height = to_string_with_precision(nmea.gpgga.Height, 4);
         localization.AngleDeviated = to_string_with_precision(nmea.gprmc.Deviated, 4);
+       // localization.Speed = to_string_with_precision(nmea.gprmc.Deviated, 4) * SPEED_FACTOR;
+       // localization.Speed = to_string_with_precision(nmea.ifdyn.VelHori,4) * SPEED_FACTOR;
+        double eastSpeed = to_string_with_precision(nmea.gpfpd.VelEast, 4) * SPEED_FACTOR;
+        double northSpeed = to_string_with_precision(nmea.gpfpd.VelNorth, 4) * SPEED_FACTOR;
+        double speeds = pow(eastSpeed, 2) + pow(northSpeed,2);
+        localization.Speed = sqrt(speeds);
         geometry_pub.publish(localization);
     }
     catch (ros::Exception ex)
@@ -242,6 +286,106 @@ std::vector<std::string> Localization::Split(const std::string& in, const std::s
 }
 
 
+int Localization::ParaIFDYN(std::string &strIFDYN, IFDYN &ifdyn)
+{
+  try
+  {
+      std::vector<std::string> nmea_ifdyn = Split(strIFDYN, ",");
+      ifdyn.YawAngle = atof(nmea_ifdyn[1].c_str());
+      ifdyn.RollAngle = atof(nmea_ifdyn[2].c_str());
+      ifdyn.PitchAngle = atof(nmea_ifdyn[3].c_str());
+      ifdyn.VelHori = atof(nmea_ifdyn[4].c_str());
+      ifdyn.VelEast = atof(nmea_ifdyn[5].c_str());
+      ifdyn.VelNorth = atof(nmea_ifdyn[6].c_str());
+      ifdyn.VelD = atof(nmea_ifdyn[7].c_str());
+      ifdyn.AccelX = atof(nmea_ifdyn[8].c_str());
+      ifdyn.AccelY = atof(nmea_ifdyn[9].c_str());
+      ifdyn.AccelZ = atof(nmea_ifdyn[10].c_str());
+      ifdyn.AngleRateX = atof(nmea_ifdyn[11].c_str());
+      ifdyn.AngleRateY = atof(nmea_ifdyn[12].c_str());
+      ifdyn.AngleRateZ = atof(nmea_ifdyn[13].c_str());
+  }
+  catch(ros::Exception ex)
+  {
+      LOG(ERROR)<<Name()<< " ParaIFDYN  error" << ex.what();
+      return ROBOT_FAILTURE;
+  }
+  return ROBOT_SUCCESS;
+}
+
+int Localization::ParaGPFPD(std::string &strGPFPD, GPFPD &gpfpd)
+{
+  try
+  {
+
+      std::vector<std::string> nmea_gpfpd = Split(strGPFPD, ",");
+      //TODO
+      if(nmea_gpfpd.size() < 16)
+      {
+        return ROBOT_FAILTURE;
+      }
+      gpfpd.GPSWeek = atoi(nmea_gpfpd[1].c_str());
+      memset(&gpfpd.GPSWeekSecond, 0x00, sizeof(gpfpd.GPSWeekSecond));
+      memcpy(&gpfpd.GPSWeekSecond, nmea_gpfpd[2].c_str(), 16);
+      gpfpd.Heading = atof(nmea_gpfpd[3].c_str());
+      gpfpd.Pitch = atof(nmea_gpfpd[4].c_str());
+      gpfpd.Roll = atof(nmea_gpfpd[5].c_str());
+      gpfpd.Latitude = atof(nmea_gpfpd[6].c_str());
+      gpfpd.Longitude = atof(nmea_gpfpd[7].c_str());
+      gpfpd.Alt = atof(nmea_gpfpd[8].c_str());
+      gpfpd.VelEast = atof(nmea_gpfpd[9].c_str());
+      gpfpd.VelNorth = atof(nmea_gpfpd[10].c_str());
+      gpfpd.VelU = atof(nmea_gpfpd[11].c_str());
+      gpfpd.BaseLine = atof(nmea_gpfpd[12].c_str());
+      gpfpd.Sat1Num = atoi(nmea_gpfpd[13].c_str());
+      gpfpd.Sat2Num = atoi(nmea_gpfpd[14].c_str());
+      memset(&gpfpd.Status, 0x00, sizeof(gpfpd.Status));
+      memcpy(&gpfpd.Status, nmea_gpfpd[15].c_str(), 4);
+  }
+  catch(ros::Exception ex)
+  {
+      LOG(ERROR)<<Name()<< " ParaGPFPD  error" << ex.what();
+      return ROBOT_FAILTURE;
+  }
+  return ROBOT_SUCCESS;
+}
+
+int Localization::Analyse_ReadLine(std::string &strSerialResult,  NMEA0183 &nmea)
+{
+  //LOG(INFO)<<Name()<<" Analyse_ReadLine start.";
+  try
+  {
+    std::string type = strSerialResult.substr(1,5);
+    std::string *str = &strSerialResult;
+    LOG(INFO)<<Name()<<" type:"<<type;
+    if(type.compare("IFDYN") == 0)
+    {
+      IFDYN tempIFDYN;
+      if(ParaIFDYN(*str,tempIFDYN) != ROBOT_SUCCESS)
+        return ROBOT_FAILTURE;
+      nmea.ifdyn = tempIFDYN;
+      nmea.ifdyn.print();
+      LOG(INFO)<<Name()<<" para IFDYN end.";
+    }
+    else if(type.compare("GPFPD") == 0)
+    {
+      GPFPD tempGPFPD;
+      if(ParaGPFPD(*str, tempGPFPD) != ROBOT_SUCCESS)
+        return ROBOT_FAILTURE;
+      nmea.gpfpd = tempGPFPD;
+      nmea.gpfpd.print();
+      LOG(INFO)<<Name()<<" para GPFPD end.";
+    }
+  }
+  catch (ros::Exception ex)
+  {
+      LOG(ERROR)<<Name()<< " Analyse_ReadLine  error" << ex.what();
+      return ROBOT_FAILTURE;
+  }
+  LOG(INFO)<<Name()<<" Analyse_ReadLine end.";
+  return ROBOT_SUCCESS;
+
+}
 
 }  // namespace localizationspace
 }  // namespace robot

src/localization/localization.h

@@ -70,11 +70,17 @@ class Localization : public robot::common::RobotApp{
 
   int Analyse(const std::string &strSerialResult, NMEA0183 &nmea);
 
+  int Analyse_ReadLine(std::string &strSerialResult, NMEA0183 &nmea);
+
   std::vector<std::string> Split(const std::string& in, const std::string& delim);
 
   int ParaGPRMC(std::string &nmea_gprmc, GPRMC &gprmc);
 
   int ParaGPGGA(std::string &nmea_gpgga, GPGGA &gpgga);
+
+  int ParaIFDYN(std::string &nmea_ifdyn, IFDYN &ifdyn);
+
+  int ParaGPFPD(std::string &nmea_gpfpd, GPFPD &gpfpd);
   /**
    * @brief string localization ddmm convert to double dd type
    * @param strResult
@@ -114,7 +120,7 @@ private:
 
   ros::Publisher geometry_pub;
 
-  const double rate = 20;  //20HZ
+  const double rate = 100;  //100HZ
 
 };
 

src/localization/localizationdata.h

@@ -46,7 +46,10 @@ const double UTM_E4 = (UTM_E2*UTM_E2);		// e^4
 const double UTM_E6 = (UTM_E4*UTM_E2);		// e^6
 const double UTM_EP2 = (UTM_E2/(1-UTM_E2));	// e'^2
 
-
+/**GPRMC  <7> 地面速率 单位为节(knots)，1节=1.852km/h**/
+//#define SPEED_FACTOR   1.852
+/**IFDYN  <4> 水平合成速度 单位为 m/s 1m/s=3.6km/h**/
+#define SPEED_FACTOR   3.6
 /**
 * NMEA0183协议-GPRMC
 */
@@ -64,7 +67,7 @@ struct GPRMC
     double Longitude;
     // <6> 经度半球E（东经）或W（西经）
     char LonType[2];
-    // <7> 地面速率（000.0~999.9节，前面的0也将被传输）
+    // <7> 地面速率（000.0~999.9节，前面的0也将被传输）单位为节(knots)，1节=1.852km/h
     double Speed;
     // <8> 地面航向（000.0~359.9度，以真北为参考基准，前面的0也将被传输）
     double Deviated;
@@ -133,6 +136,126 @@ struct GPGGA
     }
 };
 
+/**
+* IFDYN
+*/
+struct IFDYN
+{
+  // 字段1：车辆航向角 -180~180 度 0.001
+  double YawAngle;
+  // 字段2：车辆横滚角，-180~180 度 0.001
+  double RollAngle;
+  // 字段3：车辆俯仰角 -90~90 度 0.001
+  double PitchAngle;
+  // 字段4：水平合成速度-地球坐标系中的水平速度 0~200 m/s 0.001
+  double VelHori;
+  // 字段5：东向水平速度-地球坐标系中的东向水平速度分量 -190~190 m/s 0.001
+  double VelEast;
+  // 字段6：北向水平速度-地球坐标系中的北向水平速度分量 -190~190 m/s 0.001
+  double VelNorth;
+  // 字段7：地球坐标系中的地向速度 -190~190 m/s 0.001
+  double VelD;
+  // 字段8：纵向加速度-车辆坐标系x轴加速度  -58.5~58.8 m/s2  0.001
+  double AccelX;
+  // 字段9：横向加速度-车辆坐标系y轴加速度 -58.5~58.8 m/s2   0.001
+  double AccelY;
+  // 字段10：地向加速度-车辆坐标系z轴加速度 -58.5~58.8 m/s2  0.001
+  double  AccelZ;
+  // 字段11：横滚角速度-车辆坐标系x轴角速度 -300~300 。/s    0.001
+  double  AngleRateX;
+  // 字段12：俯仰角速度-车辆坐标系y轴角速度 -300~300 。/s    0.001
+  double  AngleRateY;
+  // 字段13：航向加速度-车辆坐标系z轴角速度 -300~300 。/s    0.001
+  double  AngleRateZ;
+  void print()
+  {
+      LOG(INFO)<< "YawAngle:"<<std::setprecision(5)<<YawAngle;
+      LOG(INFO)<< "RollAngle:"<<std::setprecision(4)<<RollAngle;
+      LOG(INFO)<< "PitchAngle:"<<std::setprecision(4)<<PitchAngle;
+      LOG(INFO)<< "VelHori:"<<std::setprecision(4)<<VelHori;
+      LOG(INFO)<< "VelEast:"<<std::setprecision(4)<<VelEast;
+      LOG(INFO)<< "VelNorth:"<<std::setprecision(4)<<VelNorth;
+      LOG(INFO)<< "VelD:"<<std::setprecision(4)<<VelD;
+      LOG(INFO)<< "AccelX:"<<std::setprecision(4)<<AccelX;
+      LOG(INFO)<< "AccelY:"<<std::setprecision(4)<<AccelY;
+      LOG(INFO)<< "AccelZ:"<<std::setprecision(4)<<AccelZ;
+      LOG(INFO)<< "AngleRateX:"<<std::setprecision(4)<<AngleRateX;
+      LOG(INFO)<< "AngleRateY:"<<std::setprecision(4)<<AngleRateY;
+      LOG(INFO)<< "AngleRateZ:"<<std::setprecision(4)<<AngleRateZ;
+  }
+
+};
+
+/**
+* GPFPD
+*/
+struct GPFPD
+{
+  // <1> GPS周-自1980-1-6至当前的星期数（格林尼治时间）
+  unsigned int GPSWeek;
+  // <2> GPS周秒-自本周日0:00:00至当前的秒数（格林尼治时间）
+  char GPSWeekSecond[16];
+  // <3> 航向角 0~360度
+  double Heading;
+  // <4> 俯仰角 -90~90度
+  double Pitch;
+  // <5> 车辆横滚角 -180~180度
+  double Roll;
+  // <6> 纬度 -90~90度
+  double Latitude;
+  // <7> 经度  -180~180度
+  double Longitude;
+  // <8> 高度 单位米
+  double Alt;
+  // <9> 东向水平速度-地球坐标系中的东向水平速度分量 -190~190 m/s 0.001
+  double VelEast;
+  // <10> 北向水平速度-地球坐标系中的北向水平速度分量 -190~190 m/s 0.001
+  double VelNorth;
+  // <11> 天向速度-地球坐标系中的天向速度 -190~190 m/s 0.001
+  double VelU;
+  // <12> 基线长度 单位米
+  double BaseLine;
+  // <13> 天线1卫星数
+  int Sat1Num;
+  // <14> 天线2卫星数
+  int Sat2Num;
+  // <15> 状态  定位状态 00：定位无效   03：GNSS单点定位   04：组合惯导单点定位  05：GNSS差分定位
+  //06：组合惯导轮速辅助  08：纯惯导推算  4B：组合惯导RTK固定  5B：组合惯导RTK浮点
+  char Status[4];
+  void print()
+  {
+      LOG(INFO)<< "GPSWeek:"<<GPSWeek;
+      LOG(INFO)<< "GPSWeekSecond:"<<std::string(GPSWeekSecond);
+      LOG(INFO)<< "Heading:"<<std::setprecision(5)<<Heading;
+      LOG(INFO)<< "Pitch:"<<std::setprecision(3)<<Pitch;
+      LOG(INFO)<< "Roll:"<<std::setprecision(3)<<Roll;
+      LOG(INFO)<< "Latitude:"<<std::setprecision(9)<<Latitude;
+      LOG(INFO)<< "Longitude:"<<std::setprecision(10)<<Longitude;
+      LOG(INFO)<< "Alt:"<<std::setprecision(3)<<Alt;
+      LOG(INFO)<< "VelEast:"<<std::setprecision(4)<<VelEast;
+      LOG(INFO)<< "VelNorth:"<<std::setprecision(4)<<VelNorth;
+      LOG(INFO)<< "VelU:"<<std::setprecision(4)<<VelU;
+      LOG(INFO)<< "BaseLine:"<<std::setprecision(4)<<BaseLine;
+      LOG(INFO)<< "Sat1Num:"<<Sat1Num;
+      LOG(INFO)<< "Sat2Num:"<<Sat2Num;
+      LOG(INFO)<< "Status:"<<std::string(Status);
+
+//      printf("*************GPFDP_data*****************\n");
+//        printf("GPSWeek:%d\n",gpfdp_data->GPSWeek);
+//          printf("GPSTime:%s\n",gpfdp_data->GPSTime);
+//          printf("Heading:%f\n",gpfdp_data->Heading);
+//          printf("Pitch:%f\n",gpfdp_data->Pitch);
+//          printf("Roll:%f\n",gpfdp_data->Roll);
+//          printf("Latitude:%12.9lf\n",gpfdp_data->Latitude);
+//          printf("Longitude:%12.9lf\n",gpfdp_data->Longitude);
+//          printf("Altitude:%12.9lf\n",gpfdp_data->Altitude);
+//          printf("[Ve Vn Vu]=[%f %f %f]\n",gpfdp_data->Ve,gpfdp_data->Vn,gpfdp_data->Vu);
+//        printf("Baseline =%f\n",gpfdp_data->Baseline);
+//          printf("Status:%s\n",gpfdp_data->Status);
+  }
+
+};
+
 /**
 * NMEA0183协议
 */
@@ -140,6 +263,8 @@ struct NMEA0183
 {
     struct GPRMC gprmc;
     struct GPGGA gpgga;
+    struct IFDYN ifdyn;
+    struct GPFPD gpfpd;
 };
 
 

src/localization/msg/localMsg.msg

@@ -2,3 +2,4 @@ float64 Longitude
 float64 Latitude
 float64 Height
 float64 AngleDeviated
+float64 Speed