添加转向反馈6F7

src/canbus/canbus.cpp

@@ -382,7 +382,7 @@ int Canbus::read_data(TPCANMsg *pMsgBuff)
   }
   else if((pMsgBuff->ID >= robot::common::RobotWorkMd && pMsgBuff->ID <= robot::common::ICUMonitor) ||
           (pMsgBuff->ID >= robot::common::BrkPedalActCtrReq && pMsgBuff->ID <= robot::common::IOMode) ||
-          (pMsgBuff->ID >= robot::common::RobotAcuatorPos && pMsgBuff->ID <= robot::common::ComputerStaus) ||
+          (pMsgBuff->ID >= robot::common::RobotAcuatorPos && pMsgBuff->ID <= robot::common::StrStaus) ||
           (pMsgBuff->ID >= robot::common::SysMonitorBack && pMsgBuff->ID <= robot::common::SteerPosTimeReq) ||
           pMsgBuff->ID ==robot::common::EstopInfo || pMsgBuff->ID ==robot::common::EstopErr ||
           pMsgBuff->ID == robot::common::EstpCtrl)
@@ -1111,7 +1111,7 @@ bool Canbus::StrTxtReq(canbus::StrTxtReq::Request &req,canbus::StrTxtReq::Respon
         res.uiReturn = 1;
         return true;
       }
-      string str = "/home/madesheng/angle.txt";
+      string str = "/home/siasun/angle.txt";
             int flag = Canbus::read_txt(str,1);
             if(flag != ROBOT_SUCCESS)
             {
@@ -1194,7 +1194,7 @@ bool Canbus::BrkTxtReq(canbus::BrkTxtReq::Request &req,canbus::BrkTxtReq::Respon
       res.uiReturn = 1;
       return true;
     }
-    string str = "/home/madesheng/psng.txt";
+    string str = "/home/siasun/psng.txt";
     int flag = Canbus::read_txt(str,2);
     if(flag != ROBOT_SUCCESS)
     {
@@ -1214,7 +1214,7 @@ bool Canbus::BrkTxtReq(canbus::BrkTxtReq::Request &req,canbus::BrkTxtReq::Respon
     tBrkCtrReqTxt.uiBrkCtrMdReq = req.uiBrkCtrMdReq;
     tBrkCtrReqTxt.uiBrkFocCtrReq = req.uiBrkFocCtrReq;
 
-    brk_txt_timer_ = CreateTimer(ros::Duration(duration_txt), &Canbus::BrkTxtOnTimer, this,false,false);
+    brk_txt_timer_ = CreateTimer(ros::Duration(duration_txt_brk), &Canbus::BrkTxtOnTimer, this,false,false);
     brk_txt_timer_.start();
     brkTxtTimerState = true;
     res.uiReturn = 1;
@@ -1272,7 +1272,7 @@ bool Canbus::AccTxtReq(canbus::AccTxtReq::Request &req,canbus::AccTxtReq::Respon
       res.uiReturn = 1;
       return true;
     }
-    string str = "/home/madesheng/acc.txt";
+    string str = "/home/siasun/acc.txt";
     int flag = Canbus::read_txt(str,3);
     if(flag != ROBOT_SUCCESS)
     {
@@ -1348,7 +1348,7 @@ bool Canbus::StrTxtLineReq(canbus::StrTxtLineReq::Request &req,canbus::StrTxtLin
         res.uiReturn = 1;
         return true;
       }
-      string str = "/home/madesheng/angleLine.txt";
+      string str = "/home/siasun/angleLine.txt";
       int flag = Canbus::read_txt(str,4);
       if(flag != ROBOT_SUCCESS)
       {
@@ -1423,7 +1423,7 @@ bool Canbus::SftTxtReq(canbus::SftTxtReq::Request &req,canbus::SftTxtReq::Respon
       res.uiReturn = 1;
       return true;
     }
-    string str = "/home/madesheng/sft.txt";
+    string str = "/home/siasun/sft.txt";
     int flag = Canbus::read_txt(str,5);
     if(flag != ROBOT_SUCCESS)
     {

src/canbus/canbus.h

@@ -296,7 +296,9 @@ private:
   canbus::BrkLoopReq::Request brkLpReq;  //制动踏板机构循环指令
   T_BRK_PEDAL_ACT_CTR_REQ tBrkCtrReq = {0};  //制动踏板机构循环指令中发送的踏板执行机构控制命令
 
-   const double duration_txt = 0.01;//10ms//转向制动发送时间
+   const double duration_txt = 5;//10ms//转向制动发送时间
+
+const double duration_txt_brk = 0.01;//10ms//转向制动发送时间
 
    const double duration_txt_acc = 0.01; //10ms//油门发送时间
 
@@ -344,7 +346,7 @@ private:
   //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

src/canbus/candatatype.h

@@ -2192,6 +2192,26 @@ typedef struct
   unsigned int uiZSftArmCtrAlive;   //换挡臂 Z 控制器心跳
 }T_ZSFT_STATUS;
 
+/*************转向状态反馈*********/
+typedef struct
+{
+        unsigned int :2;
+        int iStrMotTrq:14;                        //计算转向电机转矩
+        unsigned int :4;
+        int iStrMotSpd:12;                        //实际转向电机转速
+        unsigned int :13;
+        int iStrActAng:19;                                //方向盘机构转角
+}T_STATE_OUT_6F7;
+
+
+typedef struct
+{
+        int iStrMotTrq;                        //计算转向电机转矩
+        int iStrMotSpd;                        //实际转向电机转速
+        int iStrActAng;                        //方向盘机构转角
+
+}T_STR_BRK;
+
 
 } // namespace canbus
 

src/canbus/messagecoder.cpp

@@ -149,6 +149,21 @@ void MessageCoder::CanMsgAnalyseID_6F5(unsigned char * ucData, T_ZSFT_STATUS * t
     tZSftStat->uiZSftMotCur = tStatOut6F5.uiZSftMotCur;
 }
 
+//转向状态反馈--数据解析
+void MessageCoder::CanMsgAnalyseID_6F7(unsigned char *ucData, T_STR_BRK *tStrBrk)
+{
+  T_STATE_OUT_6F7 tStatOut6F7 = { 0 };
+//    unsigned char ucCanMsgSrc[8] = { 0 };
+
+//    memcpy(ucCanMsgSrc, ucData, CAN_MSG_LEN);
+  CharReverse(ucData,8);
+  memcpy(&tStatOut6F7, ucData, CAN_MSG_LEN);
+
+  tStrBrk->iStrActAng = tStatOut6F7.iStrActAng;
+  tStrBrk->iStrMotSpd = tStatOut6F7.iStrMotSpd;
+  tStrBrk->iStrMotTrq = tStatOut6F7.iStrMotTrq;
+}
+
 //机器人执行机构实际位置--数据解析
 void MessageCoder::CanMsgAnalyseID_6E0(unsigned char * ucData, T_RBT_ACT_PSNG * tRbtActPsng)
 {
@@ -1267,9 +1282,15 @@ void * MessageCoder::CanMsgAnalyse(int id,unsigned char * ucData)
         p = &tComputerStat;
         break;
     case robot::common::ZSftStaus: //档位 Z 轴状态反馈
-      T_ZSFT_STATUS tZSftStatus;
-      CanMsgAnalyseID_6F5(ucData,&tZSftStatus);
-      p = &tZSftStatus;
+        T_ZSFT_STATUS tZSftStatus;
+        CanMsgAnalyseID_6F5(ucData,&tZSftStatus);
+        p = &tZSftStatus;
+        break;
+    case robot::common::StrStaus: //转向状态反馈
+        T_STR_BRK tStrBrk;
+        CanMsgAnalyseID_6F7(ucData,&tStrBrk);
+        p = &tStrBrk;
+        break;
     case robot::common::RobotAcuatorPos://机器人执行机构实际位置
         T_RBT_ACT_PSNG tRbtActPsng ;
         CanMsgAnalyseID_6E0(ucData, &tRbtActPsng);

src/canbus/messagecoder.h

@@ -39,6 +39,9 @@ private:
     //档位 Z 轴状态反馈--数据解析
     static void CanMsgAnalyseID_6F5(unsigned char *ucData, T_ZSFT_STATUS *tZSftStat);
 
+    //转向状态反馈--数据解析
+    static void CanMsgAnalyseID_6F7(unsigned char *ucData, T_STR_BRK *tStrBrk);
+
     //机器人执行机构实际位置--数据解析
     static void CanMsgAnalyseID_6E0(unsigned char *ucData, T_RBT_ACT_PSNG *tRbtActPsng);
 

src/common/canmsgobdmsgid.h

@@ -65,6 +65,8 @@ const int VehicleStatus3 = 0x6F3;
 const int ComputerStaus = 0x6F4;
 //档位 Z 轴状态反馈
 const int ZSftStaus = 0x6F5;
+//转向状态反馈
+const int StrStaus = 0x6F7;
 //机器人执行机构实际位置
 const int RobotAcuatorPos = 0x6E0;
 //(油门和制动)踏板学习位置反馈

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 StrData = "strData";
 // -----------------topics ----------------------------
 
 // -----------------services ----------------------------

src/perception/CMakeLists.txt

@@ -62,6 +62,7 @@ add_message_files(
   si1Msg.msg
   smMsg.msg
   zsftStatsMsg.msg
+  strStatsMsg.msg
   )
 
 ## Generate services in the 'srv' folder

src/perception/msg/strStatsMsg.msg

@@ -0,0 +1,3 @@
+float32 StrMotTrq                      #计算转向电机转矩
+int32 StrMotSpd                     #实际转向电机转速
+float32 StrActAng                     #方向盘机构转角

src/perception/perception.cpp

@@ -63,6 +63,7 @@ int Perception::Start()
         bus_pub_6D0 = node_handle_->advertise<::perception::smMsg>(robot::common::SmData,1);
 
         bus_pub_6F5 = node_handle_->advertise<::perception::zsftStatsMsg>(robot::common::ZSftData,1);
+        bus_pub_6F7 = node_handle_->advertise<::perception::strStatsMsg>(robot::common::StrData,1);
 
         ros::spin();
     }
@@ -87,9 +88,9 @@ void Perception::CanTopicCallback(const canbus::canMsg::ConstPtr& msg)
     unsigned char ucCanMsgSrc[8] = { 0 };
     HexStrToByte(msg->data.c_str(), ucCanMsgSrc, 16);
     CharReverse(ucCanMsgSrc,8);
-if(msg->id == robot::common::SysLearnBack)
+if(msg->id == robot::common::StrStaus)
 {
-LOG(INFO)<<Name()<<" Receive 6EE";
+//LOG(INFO)<<Name()<<" Receive 6F7";
 }
     switch (msg->id) {
     case robot::common::ActuatorEnableStatus:
@@ -155,6 +156,9 @@ LOG(INFO)<<Name()<<" Receive 6EE";
     case robot::common::ZSftStaus:
       PublishState_6F5(ucCanMsgSrc);
       break;
+    case robot::common::StrStaus:
+      PublishState_6F7(ucCanMsgSrc);
+      break;
     default:
         break;
     }
@@ -1023,6 +1027,40 @@ bool Perception::PublishState_6F5(unsigned char* msg)
     }
 }
 
+bool Perception::PublishState_6F7(unsigned char* msg)
+{
+//    int copySize;
+//    if(msg.size() < sizeof(s_state_6F7))
+//        copySize = msg->data.size();
+//    else
+//        copySize = sizeof(s_state_6F7);
+
+    robot::canbusSpace::T_STATE_OUT_6F7 state_6F7;
+    memset(&state_6F7, 0 ,sizeof(state_6F7));
+    memcpy(&state_6F7, msg, CAN_MSG_LEN);
+
+    if(state_6F7.iStrActAng != s_state_6F7.iStrActAng || state_6F7.iStrMotSpd != s_state_6F7.iStrMotSpd ||
+            state_6F7.iStrMotTrq != s_state_6F7.iStrMotTrq)
+    {
+
+        memcpy(&s_state_6F7, msg, CAN_MSG_LEN);
+
+       // LOG(INFO) << Name() <<"StrActAng="<<state_6F7.iStrActAng;
+        str_msg.StrActAng = state_6F7.iStrActAng * 0.01;
+        str_msg.StrMotSpd =  state_6F7.iStrMotSpd;
+        str_msg.StrMotTrq = state_6F7.iStrMotTrq * 0.01;
+
+      //  LOG(INFO) << Name() <<"StrActAng * 0.01="<<str_msg.StrActAng;
+
+        return true;
+    }
+    else
+    {
+ //       LOG(INFO) << Name() << " PublishState_6F7 data is repeat";
+        return false;
+    }
+}
+
 void Perception::OnTimer(const ros::TimerEvent &)
 {
   try
@@ -1199,6 +1237,17 @@ void Perception::OnTimer(const ros::TimerEvent &)
         LOG(ERROR) << Name() << " PublishState_6F5 publisher is unvalid";
     }
 
+    if(bus_pub_6F7)
+    {
+       bus_pub_6F7.publish(str_msg);
+       LOG(INFO) << Name() << " PublishState_6F7 " << str_msg;
+    }
+    else
+    {
+        LOG(ERROR) << Name() << " PublishState_6F7 publisher is unvalid";
+    }
+
+
   }
   catch (ros::Exception ex)
   {

src/perception/perception.h

@@ -29,6 +29,7 @@
 #include "canbus/EstpCtrl.h"
 #include "canbus/SysCtrlMode.h"
 #include "perception/zsftStatsMsg.h"
+#include "perception/strStatsMsg.h"
 
 namespace robot {
 namespace perception {
@@ -150,6 +151,8 @@ private:
 
     bool PublishState_6F5(unsigned char* msg);
 
+    bool PublishState_6F7(unsigned char* msg);
+
     bool SoftStopControl();
 
     void OnTimer(const ros::TimerEvent &event);
@@ -250,6 +253,9 @@ ros::Subscriber sub;
      robot::canbusSpace::T_STATE_OUT_6F5 s_state_6F5;
      ::perception::zsftStatsMsg zsft_msg;
 
+     robot::canbusSpace::T_STATE_OUT_6F7 s_state_6F7;
+     ::perception::strStatsMsg str_msg;
+
 
      ros::Publisher bus_pub_6E5;
      ros::Publisher bus_pub_6E4;
@@ -275,6 +281,7 @@ ros::Subscriber sub;
 
      ros::Publisher bus_pub_6D0;
      ros::Publisher bus_pub_6F5;
+     ros::Publisher bus_pub_6F7;
 
      ros::ServiceClient clientActuatorEnableRequest;