增加转向、油门、制动循环指令

src/canbus/CMakeLists.txt

@@ -64,6 +64,8 @@ add_message_files(FILES
    SlopeMode.srv
    VehicleStart.srv
    StrLoopReq.srv
+   BrkLoopReq.srv
+   AccLoopReq.srv
  )
 
 ## Generate actions in the 'action' folder

src/canbus/canbus.cpp

@@ -119,6 +119,10 @@ int Canbus::Start() {
                                                                        &Canbus::SlopeMode,this);
   ros::ServiceServer StrLoopService = node_handle_->advertiseService(robot::common::StrLoopService,
                                                                        &Canbus::StrLoopReq,this);
+  ros::ServiceServer AccLoopService = node_handle_->advertiseService(robot::common::AccLoopService,
+                                                                     &Canbus::AccLoopReq,this);
+  ros::ServiceServer BrkLoopService = node_handle_->advertiseService(robot::common::BrkLoopService,
+                                                                     &Canbus::BrkLoopReq,this);
   //data receive
   while(ros::ok())
   {
@@ -133,7 +137,7 @@ int Canbus::Start() {
 
 
 void Canbus::OnTimer(const ros::TimerEvent &) {
-  LOG(INFO)<< Name()<< " timer start ..";
+  //LOG(INFO)<< Name()<< " timer start ..";
   if(heatbeat > 15)
   {
       heatbeat = 0;
@@ -145,20 +149,20 @@ void Canbus::OnTimer(const ros::TimerEvent &) {
       canObdData_pub.publish(obd);
       TPCANMsg canIcuMonitor; //ICUMonitor Can Message
       MessageCoder::CanMsgPackage((void*)&icuMonitor, robot::common::ICUMonitor, &canIcuMonitor);
-      write_data(&canIcuMonitor);
+      write_data(&canIcuMonitor, false);
       TPCANMsg canStatus1; //VehicleStaus1 Can Message
       MessageCoder::CanMsgPackage((void*)&status1, robot::common::VehicleStatus1, &canStatus1);
-      write_data(&canStatus1);
+      write_data(&canStatus1, false);
       TPCANMsg canStatus2;  //VehicleStatus2 Can Message
       MessageCoder::CanMsgPackage((void*)&status2, robot::common::VehicleStatus2, &canStatus2);
-      write_data(&canStatus2);
+      write_data(&canStatus2, false);
       TPCANMsg canStatus3;  //VehicleStatus3 Can Message
       MessageCoder::CanMsgPackage((void*)&status3, robot::common::VehicleStatus3, &canStatus3);
-      write_data(&canStatus3);
+      write_data(&canStatus3, false);
       // enable
-      write_data(&canActuaEnableReqMsg);
+      write_data(&canActuaEnableReqMsg, false);
       // robot manager
-      write_data(&canRobotManagerMsg);
+      write_data(&canRobotManagerMsg, false);
   }
   catch (ros::Exception ex)
   {
@@ -216,7 +220,6 @@ void Canbus::InitServiceAndMsg()
 //read from CAN forever - until manual break
 int Canbus::read_loop()
 {
-
     TPCANRdMsg msg;
     __u32 status;
 
@@ -312,8 +315,6 @@ void Canbus::publish_data(TPCANMsg *pMsgBuff)
     //std::stringstream ss;
     //ss << pMsgBuff->DATA;
     //rosMsg.data = ss.str();
-
-
     char strId[4] = {0};
     sprintf(strId,"%3xh",pMsgBuff->ID);
     char chData[16]={0};
@@ -321,16 +322,17 @@ void Canbus::publish_data(TPCANMsg *pMsgBuff)
     //std::string strData = chData;
     rosMsg.data = chData;
     canRCUData_pub.publish(rosMsg);
-    LOG(INFO)<<Name()<<" Publish CanData :" << "ID:" << strId << ",Data:"<<std::string(chData);
+    //LOG(INFO)<<Name()<<" Publish CanData :" << "ID:" << strId << ",Data:"<<std::string(chData);
 }
 
-int Canbus::write_data(TPCANMsg *pMsgBuff)
+int Canbus::write_data(TPCANMsg *pMsgBuff, bool print)
 {
   char strId[4] = {0};
   sprintf(strId,"%3xh",pMsgBuff->ID);
   char strData[16]={0};
   MessageCoder::ConvertUnCharToStr(strData,pMsgBuff->DATA,8);
-  LOG(INFO)<<Name()<<" send CanData : ID: "<<std::string(strId)<<",Data:"<<std::string(strData);
+  if (print)
+    LOG(INFO)<<Name()<<" send CanData : ID: "<<std::string(strId)<<",Data:"<<std::string(strData);
   if (funCAN_Write(pcan_handle, pMsgBuff)){
      LOG(ERROR)<<Name()<< " write: LINUX_CAN_write_timeout error. ";
      return errno;
@@ -338,7 +340,6 @@ int Canbus::write_data(TPCANMsg *pMsgBuff)
    return ROBOT_SUCCESS;
 }
 
-
 bool Canbus::ActuatorEnableRequest(canbus::ActuatorEnableRequest::Request &req,
                            canbus::ActuatorEnableRequest::Response &res)
 {
@@ -659,9 +660,8 @@ bool Canbus::StrLoopReq(canbus::StrLoopReq::Request &req,
 {
     LOG(INFO)<< "controlservice start StrLoopReq";
     // StrLoopReq request
-    TPCANMsg canStrLpReqMsg;
     try{
-      if(!req.bStart)
+      if(req.uiStart == 0)
       {
         if(strTimerState)
         {
@@ -683,10 +683,15 @@ bool Canbus::StrLoopReq(canbus::StrLoopReq::Request &req,
       strLpReq = req;
 
       double str_duration = (double)req.uiStrTimeInterval/1000;
+      strLoopNum = 0;
+      strIncrease = true;
+      strAngle = strLpReq.iStrAngStart;
+      tStrActCtrReq.iStrSpdCtrReq = strLpReq.iStrSpdCtrReq;
+      tStrActCtrReq.iStrTrqCtrReq = strLpReq.iStrTrqCtrReq;
+      tStrActCtrReq.uiStrCtrMdReq = strLpReq.uiStrCtrMdReq;
       str_timer_ = CreateTimer(ros::Duration(str_duration), &Canbus::StrLoopOnTimer, this,false,false);
       str_timer_.start();
       strTimerState = true;
-
       res.uiReturn = 1;
     }
     catch (ros::Exception ex)
@@ -700,46 +705,222 @@ bool Canbus::StrLoopReq(canbus::StrLoopReq::Request &req,
 
 void Canbus::StrLoopOnTimer(const ros::TimerEvent &event)
 {
-  LOG(INFO)<< Name()<< " StrLoopTimer start ..";
-
+  //LOG(INFO)<< Name()<< " StrLoopTimer start ..";
   TPCANMsg canStrActCtrReqMsg;
   try
   {
+    // 发送当前角度
+    tStrActCtrReq.iStrAngCtrReq = strAngle;
+    MessageCoder::CanMsgPackage((void*)&tStrActCtrReq, robot::common::SteerActCtrReq, &canStrActCtrReqMsg);
+    write_data(&canStrActCtrReqMsg);
+    //LOG(INFO)<<Name()<< " Angle "<< std::to_string(tStrActCtrReq.iStrAngCtrReq);
+    //LOG(INFO)<<Name()<< " Num "<< std::to_string(strLoopNum);
+    if (strAngle <= strLpReq.iStrAngStart)
+      strLoopNum++;
     if(strLoopNum > strLpReq.uiStrFrequency)  //如果执行次数等于往复次数，则结束定时器
     {
       str_timer_.stop();
       strTimerState = false;
-      strLoopNum = 0;
+      strLoopNum = 1;
       tStrActCtrReq = {0};
+      return;
     }
-    tStrActCtrReq.iStrSpdCtrReq = strLpReq.iStrSpdCtrReq;
-    tStrActCtrReq.iStrTrqCtrReq = strLpReq.iStrTrqCtrReq;
-    tStrActCtrReq.uiStrCtrMdReq = strLpReq.uiStrCtrMdReq;
-
-    if(tStrActCtrReq.iStrAngCtrReq > strLpReq.iStrAngEnd) //如果转向位置已经到达终止位置，则表示执行完一套动作，执行次数加一
+    if (strIncrease)
     {
-      strLoopNum++;
-      tStrActCtrReq = {0};
+      strAngle += strLpReq.uiStrSpan;
+      if (strAngle >= strLpReq.iStrAngEnd)
+        strIncrease = false;
     }
+    else
+    {
+      strAngle -= strLpReq.uiStrSpan;
+      if (strAngle <= strLpReq.iStrAngStart)
+        strIncrease = true;
+    }
+  }
+  catch (ros::Exception ex)
+  {
+       LOG(ERROR)<<Name()<< " StrLoopTimer error. "<< ex.what();
+  }
+}
 
-    if(tStrActCtrReq.iStrAngCtrReq == 0)
+
+bool Canbus::AccLoopReq(canbus::AccLoopReq::Request &req,
+                        canbus::AccLoopReq::Response &res)
+{
+  LOG(INFO)<< "controlservice start AccLoopReq";
+  // AccLoopReq request
+  try{
+    if(req.uiStart == 0)
     {
-      tStrActCtrReq.iStrAngCtrReq = strLpReq.iStrAngStart;
+      if(accTimerState)
+      {
+        acc_timer_.stop();
+        accTimerState = false;
+        accLoopNum = 0;
+        tAccCtrReq = {0};
+      }
+      res.uiReturn = 1;
+      return true;
     }
-    tStrActCtrReq.iStrAngCtrReq = strLpReq.iStrAngStart + strLpReq.uiStrSpan;
+    if(accTimerState)
+    {
+      acc_timer_.stop();
+      accTimerState = false;
+      accLoopNum = 0;
+      tAccCtrReq = {0};
+    }
+    accLpReq = req;
+
+    double acc_duration = (double)req.uiAccTimeInterval/1000;
+    accLoopNum = 0;
+    accIncrease = true;
+    accPsng = accLpReq.uiAccPsngStart;
+    tAccCtrReq.iAccSpdCtrReq = accLpReq.iAccSpdCtrReq;
+    acc_timer_ = CreateTimer(ros::Duration(acc_duration), &Canbus::AccLoopOnTimer, this,false,false);
+    acc_timer_.start();
+    accTimerState = true;
+    res.uiReturn = 1;
+  }
+  catch (ros::Exception ex)
+  {
+      res.uiReturn = 0;
+      LOG(ERROR)<< "controlservice AccLoopReq failed "<< ex.what();
+      return true;
+  }
+  return true;
+}
 
-    MessageCoder::CanMsgPackage((void*)&tStrActCtrReq, robot::common::SteerActCtrReq, &canStrActCtrReqMsg);
-    write_data(&canStrActCtrReqMsg);
+void Canbus::AccLoopOnTimer(const ros::TimerEvent &event)
+{
+  LOG(INFO)<< Name()<< " AccLoopTimer start ..";
 
+  TPCANMsg canAccCtrReqMsg;
+  try
+  {
+    tAccCtrReq.uiAccPsngCtrReq = accPsng;
+    MessageCoder::CanMsgPackage((void*)&tAccCtrReq, robot::common::PedalActCtrReq, &canAccCtrReqMsg);
+    write_data(&canAccCtrReqMsg);
+    if (accPsng <= accLpReq.uiAccPsngStart)
+      accLoopNum++;
+    if(accLoopNum > accLpReq.uiAccFrequency)  //如果执行次数等于往复次数，则结束定时器
+    {
+      acc_timer_.stop();
+      accTimerState = false;
+      accLoopNum = 1;
+      tAccCtrReq = {0};
+      return;
+    }
+    if (accIncrease)
+    {
+      accPsng += accLpReq.uiAccSpan;
+      if (accPsng >= accLpReq.uiAccPsngEnd)
+        accIncrease = false;
+    }
+    else
+    {
+      accPsng -= accLpReq.uiAccSpan;
+      if (accPsng <= accLpReq.uiAccPsngStart)
+        accIncrease = true;
+    }
   }
   catch (ros::Exception ex)
   {
-       LOG(ERROR)<<Name()<< " StrLoopTimer error. "<< ex.what();
+       LOG(ERROR)<<Name()<< " AccLoopTimer error. "<< ex.what();
   }
 
 }
 
 
+bool Canbus::BrkLoopReq(canbus::BrkLoopReq::Request &req,
+                        canbus::BrkLoopReq::Response &res)
+{
+  LOG(INFO)<< "controlservice start BrkLoopReq";
+  // BrkLoopReq request
+  try{
+    if(req.uiStart == 0)
+    {
+      if(brkTimerState)
+      {
+        brk_timer_.stop();
+        brkTimerState = false;
+        brkLoopNum = 0;
+        tBrkCtrReq = {0};
+      }
+      res.uiReturn = 1;
+      return true;
+    }
+    if(brkTimerState)
+    {
+      brk_timer_.stop();
+      brkTimerState = false;
+      brkLoopNum = 0;
+      tBrkCtrReq = {0};
+    }
+    brkLpReq = req;
+
+    double brk_duration = (double)req.uiBrkTimeInterval/1000;
+    brkLoopNum = 0;
+    brkIncrease = true;
+    brkPsng = brkLpReq.uiBrkPsngStart;
+    tBrkCtrReq.iBrkSpdCtrReq = brkLpReq.iBrkSpdCtrReq;
+    tBrkCtrReq.uiBrkCtrMdReq = brkLpReq.uiBrkCtrMdReq;
+    tBrkCtrReq.uiBrkFocCtrReq = brkLpReq.uiBrkFocCtrReq;
+    brk_timer_ = CreateTimer(ros::Duration(brk_duration), &Canbus::BrkLoopOnTimer, this,false,false);
+    brk_timer_.start();
+    brkTimerState = true;
+    res.uiReturn = 1;
+  }
+  catch (ros::Exception ex)
+  {
+      res.uiReturn = 0;
+      LOG(ERROR)<< "controlservice BrkLoopReq failed "<< ex.what();
+      return true;
+  }
+  return true;
+}
+
+void Canbus::BrkLoopOnTimer(const ros::TimerEvent &event)
+{
+  LOG(INFO)<< Name()<< " BrkLoopTimer start ..";
+
+  TPCANMsg canBrkCtrReqMsg;
+  try
+  {
+    tBrkCtrReq.uiBrkPsngCtrReq = brkPsng;
+    MessageCoder::CanMsgPackage((void*)&tBrkCtrReq, robot::common::PedalActCtrReq, &canBrkCtrReqMsg);
+    write_data(&canBrkCtrReqMsg);
+    if (brkPsng <= brkLpReq.uiBrkPsngStart)
+      brkLoopNum++;
+    if(brkLoopNum > brkLpReq.uiBrkFrequency)  //如果执行次数等于往复次数，则结束定时器
+    {
+      brk_timer_.stop();
+      brkTimerState = false;
+      brkLoopNum = 1;
+      tBrkCtrReq = {0};
+      return;
+    }
+    if (brkIncrease)
+    {
+      brkPsng += brkLpReq.uiBrkSpan;
+      if (brkPsng >= brkLpReq.uiBrkPsngEnd)
+        brkIncrease = false;
+    }
+    else
+    {
+      brkPsng -= brkLpReq.uiBrkSpan;
+      if (brkPsng <= brkLpReq.uiBrkPsngStart)
+        brkIncrease = true;
+    }
+
+  }
+  catch (ros::Exception ex)
+  {
+       LOG(ERROR)<<Name()<< " BrkLoopTimer error. "<< ex.what();
+  }
+
+}
+
 
 
 }  // namespace canbus

src/canbus/canbus.h

@@ -36,6 +36,8 @@
 #include <canbus/VehicleStart.h>
 #include <canbus/SlopeMode.h>
 #include <canbus/StrLoopReq.h>
+#include <canbus/AccLoopReq.h>
+#include <canbus/BrkLoopReq.h>
 
 /**
  * @namespace robot::canbus
@@ -103,6 +105,10 @@ class Canbus : public robot::common::RobotApp{
 
   void StrLoopOnTimer(const ros::TimerEvent &event);  //转向机构循环指令定时器回调函数
 
+  void AccLoopOnTimer(const ros::TimerEvent &event);  //油门踏板机构循环指令定时器回调函数
+
+  void BrkLoopOnTimer(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;
@@ -126,7 +132,7 @@ class Canbus : public robot::common::RobotApp{
   /**
   * @brief send data to can bus
   */
-  int write_data(TPCANMsg* pMsgBuff);
+  int write_data(TPCANMsg* pMsgBuff, bool print=true);
 
   /**
   * @brief publish data from can bus
@@ -159,13 +165,24 @@ class Canbus : public robot::common::RobotApp{
 
   bool ActuatorCtrLimit(canbus::ActuatorCtrLimit::Request &req,
                         canbus::ActuatorCtrLimit::Response &res);
+
   bool VehicleStart(canbus::VehicleStart::Request &req,
                      canbus::VehicleStart::Response &res);
+
   bool SlopeMode(canbus::SlopeMode::Request &req,
                  canbus::SlopeMode::Response &res);
+
   bool StrLoopReq(canbus::StrLoopReq::Request &req,
                  canbus::StrLoopReq::Response &res);
 
+  bool AccLoopReq(canbus::AccLoopReq::Request &req,
+                 canbus::AccLoopReq::Response &res);
+
+
+  bool BrkLoopReq(canbus::BrkLoopReq::Request &req,
+                 canbus::BrkLoopReq::Response &res);
+
+
 private:
 
   int64_t last_timestamp_ = 0;
@@ -174,6 +191,29 @@ private:
   ros::Timer str_timer_;    //转向机构循环指令定时器
   bool strTimerState = false;  //转向机构循环指令定时器标志位
   int strLoopNum = 0;   //转向机构循环指令一套动作执行次数
+  int strAngle ;         //方向盘转角
+  bool strIncrease;      //递增/递减
+  canbus::StrLoopReq::Request strLpReq;  //转向机构循环指令
+  T_STR_ACT_CTR_REQ tStrActCtrReq ={0};   //转向机构循环指令中发送的转向执行机构请求
+
+
+  ros::Timer acc_timer_;      //油门踏板机构循环指令定时器
+  bool accTimerState = false;  //油门踏板机构循环指令定时器标志位
+  int accLoopNum = 0;          //油门踏板机构循环指令一套动作执行次数
+  uint accPsng ;         //油门踏板位置
+  bool accIncrease;      //递增/递减
+  canbus::AccLoopReq::Request accLpReq;   //油门踏板机构循环指令
+  T_PEDAL_ACT_CTR_REQ tAccCtrReq = {0};  //油门踏板机构循环指令中发送的踏板执行机构控制命令
+
+
+  ros::Timer brk_timer_;    //制动踏板机构循环指令定时器
+  bool brkTimerState = false;  //制动踏板机构循环指令定时器标志位
+  int brkLoopNum = 0;          //制动踏板机构循环指令一套动作执行次数
+  uint brkPsng ;         //制动踏板位置
+  bool brkIncrease;      //递增/递减
+  canbus::BrkLoopReq::Request brkLpReq;  //制动踏板机构循环指令
+  T_PEDAL_ACT_CTR_REQ tBrkCtrReq = {0};  //制动踏板机构循环指令中发送的踏板执行机构控制命令
+
 
   const double duration = 0.01;
 
@@ -211,9 +251,6 @@ private:
   // robot manager
   TPCANMsg canRobotManagerMsg;
 
-  canbus::StrLoopReq::Request strLpReq;  //转向机构循环指令
-  T_STR_ACT_CTR_REQ tStrActCtrReq ={0};   //转向机构循环指令中发送的转向执行机构请求
-
 };
 
 }  // namespace canbus

src/canbus/srv/AccLoopReq.srv

@@ -0,0 +1,9 @@
+uint32 uiAccPsngStart		#油门踏板开始位置
+uint32 uiAccPsngEnd		#油门踏板终止位置
+int32  iAccSpdCtrReq		#油门踏板速度控制请求
+uint32 uiAccTimeInterval        #时间间隔
+uint32 uiAccSpan                #跨度间隔
+uint32 uiAccFrequency           #往复次数
+uint32 uiStart                   #启动标志位
+---
+uint32 uiReturn

src/canbus/srv/BrkLoopReq.srv

@@ -0,0 +1,11 @@
+uint32 uiBrkPsngStart		#制动踏板开始位置
+uint32 uiBrkPsngEnd		#制动踏板终止位置
+uint32 uiBrkCtrMdReq		#制动踏板控制模式请求
+int32  iBrkSpdCtrReq		#制动踏板速度控制请求
+uint32 uiBrkFocCtrReq		#制动踏板力控制请求
+uint32 uiBrkTimeInterval        #时间间隔
+uint32 uiBrkSpan                #跨度间隔
+uint32 uiBrkFrequency           #往复次数
+uint32 uiStart                   #启动标志位
+---
+uint32 uiReturn

src/canbus/srv/StrLoopReq.srv

@@ -6,6 +6,6 @@ int32 iStrTrqCtrReq		#方向盘转矩控制请求
 uint32 uiStrTimeInterval        #时间间隔
 uint32 uiStrSpan                #跨度间隔
 uint32 uiStrFrequency           #往复次数
-bool   bStart                   #启动标志位
+uint32 uiStart                   #启动标志位
 ---
 uint32 uiReturn

src/common/message.h

@@ -79,6 +79,10 @@ const std::string VehicleStartService = "vehiclestartservice";
 const std::string SlopeModeService = "slopemodeservice";
 
 const std::string StrLoopService = "strloopservice";
+
+const std::string AccLoopService = "accloopservice";
+
+const std::string BrkLoopService = "brkloopservice";
 // -----------------services ----------------------------