src/canbus/CMakeLists.txt

@@ -59,6 +59,11 @@ add_message_files(FILES
    SftActCtrReq.srv
    StrActCtrReq.srv
    RBTManager.srv
+   ActuatorCtrLimit.srv
+   PowerOnRequest.srv
+   SlopeMode.srv
+   VehicleStart.srv
+   StrLoopReq.srv
  )
 
 ## Generate actions in the 'action' folder

src/canbus/canbus.cpp

@@ -109,6 +109,16 @@ int Canbus::Start() {
                                                               &Canbus::PedalActCtrRequest, this);
   ros::ServiceServer RBTManagerService = node_handle_->advertiseService(robot::common::RBTManagerService,
                                                               &Canbus::RBTManager, this);
+  ros::ServiceServer PowerOnService = node_handle_->advertiseService(robot::common::PowerOnService,
+                                                                     &Canbus::PowerOnRequest, this);
+  ros::ServiceServer ActuatorCtrLimitService = node_handle_->advertiseService(robot::common::ActuatorCtrLimitService,
+                                                                              &Canbus::ActuatorCtrLimit,this);
+  ros::ServiceServer VehicleStartService = node_handle_->advertiseService(robot::common::VehicleStartService,
+                                                                           &Canbus::VehicleStart,this);
+  ros::ServiceServer SlopeModeService = node_handle_->advertiseService(robot::common::SlopeModeService,
+                                                                       &Canbus::SlopeMode,this);
+  ros::ServiceServer StrLoopService = node_handle_->advertiseService(robot::common::StrLoopService,
+                                                                       &Canbus::StrLoopReq,this);
   //data receive
   while(ros::ok())
   {
@@ -521,6 +531,213 @@ bool Canbus::RBTManager(canbus::RBTManager::Request &req,
     return true;
 }
 
+bool Canbus::PowerOnRequest(canbus::PowerOnRequest::Request &req,
+                      canbus::PowerOnRequest::Response &res)
+{
+    LOG(INFO)<< "controlservice start PowerOnRequest";
+    // PowerON request
+    TPCANMsg canPowerOnReqMsg;
+    try{
+        T_POWER_ON_REQUEST request;
+        request.uiAccCtrPowReq = req.uiAccCtrPowReq;
+        request.uiBrkCtrPowReq = req.uiBrkCtrPowReq;
+        request.uiClchCtrPowReq = req.uiClchCtrPowReq;
+        request.uiRCUPowReq = req.uiRCUPowReq;
+        request.uiStrCtrPowReq= req.uiStrCtrPowReq;
+        request.uiXSftArmCtrPowReq = req.uiXSftArmCtrPowReq;
+        request.uiYSftArmCtrPowReq = req.uiYSftArmCtrPowReq;
+
+        MessageCoder::CanMsgPackage((void*)&request, robot::common::PowerOnReq, &canPowerOnReqMsg);
+
+        if (write_data(&canPowerOnReqMsg) != ROBOT_SUCCESS)
+        {
+            res.uiReturn = 0; // =0
+            return true;
+        }
+        res.uiReturn = 1;
+    }
+    catch (ros::Exception ex)
+    {
+        res.uiReturn = 0;
+        LOG(ERROR)<< "controlservice PowerOnRequest failed "<< ex.what();
+        return true;
+    }
+    return true;
+}
+
+bool Canbus::ActuatorCtrLimit(canbus::ActuatorCtrLimit::Request &req,
+                              canbus::ActuatorCtrLimit::Response &res)
+{
+    LOG(INFO)<< "controlservice start ActuatorCtrLimit";
+    // ActuatorCtrLimit request
+    TPCANMsg canActCtrLimitMsg;
+    try{
+        T_ACT_CTR_LIMT request;
+        request.iStrMaxLimt = req.iStrMaxLimit;
+        request.uiAccMaxLimt = req.uiAccMaxLimit;
+        request.uiBrkMaxLimt = req.uiBrkMaxLimit;
+        request.uiRbtAplySenro = req.uiRbtAplySenro;
+        request.uiRbtCfgMd= req.uiRbtCfgMd;
+        request.uiSftType = req.uiSftType;
+
+        MessageCoder::CanMsgPackage((void*)&request, robot::common::ActuatorCtrLimit, &canActCtrLimitMsg);
+
+        if (write_data(&canActCtrLimitMsg) != ROBOT_SUCCESS)
+        {
+            res.uiReturn = 0; // =0
+            return true;
+        }
+        res.uiReturn = 1;
+    }
+    catch (ros::Exception ex)
+    {
+        res.uiReturn = 0;
+        LOG(ERROR)<< "controlservice ActuatorCtrLimit failed "<< ex.what();
+        return true;
+    }
+    return true;
+}
+
+bool Canbus::VehicleStart(canbus::VehicleStart::Request &req,
+                           canbus::VehicleStart::Response &res)
+{
+    LOG(INFO)<< "controlservice start VehicleStart";
+    // VehicleStart request
+    TPCANMsg canVechStrtMsg;
+    try{
+        T_VEHICLE_START request;
+        request.uiVehSwReq = req.uiVehSwReq;
+
+        MessageCoder::CanMsgPackage((void*)&request, robot::common::VehicleStart, &canVechStrtMsg);
+
+        if (write_data(&canVechStrtMsg) != ROBOT_SUCCESS)
+        {
+            res.uiReturn = 0; // =0
+            return true;
+        }
+        res.uiReturn = 1;
+    }
+    catch (ros::Exception ex)
+    {
+        res.uiReturn = 0;
+        LOG(ERROR)<< "controlservice VehicleStart failed "<< ex.what();
+        return true;
+    }
+    return true;
+}
+
+bool Canbus::SlopeMode(canbus::SlopeMode::Request &req,
+                       canbus::SlopeMode::Response &res)
+{
+    LOG(INFO)<< "controlservice start SlopeMode";
+    // SlopeMode request
+    TPCANMsg canSlpMdMsg;
+    try{
+        T_Slope_MODE request;
+        request.iSlopeReq = req.iSlopeRequest;
+
+        MessageCoder::CanMsgPackage((void*)&request, robot::common::SlopeMODE, &canSlpMdMsg);
+
+        if (write_data(&canSlpMdMsg) != ROBOT_SUCCESS)
+        {
+            res.uiReturn = 0; // =0
+            return true;
+        }
+        res.uiReturn = 1;
+    }
+    catch (ros::Exception ex)
+    {
+        res.uiReturn = 0;
+        LOG(ERROR)<< "controlservice SlopeMode failed "<< ex.what();
+        return true;
+    }
+    return true;
+}
+
+bool Canbus::StrLoopReq(canbus::StrLoopReq::Request &req,
+                       canbus::StrLoopReq::Response &res)
+{
+    LOG(INFO)<< "controlservice start StrLoopReq";
+    // StrLoopReq request
+    TPCANMsg canStrLpReqMsg;
+    try{
+      if(!req.bStart)
+      {
+        if(strTimerState)
+        {
+          str_timer_.stop();
+          strTimerState = false;
+          strLoopNum = 0;
+          tStrActCtrReq = {0};
+        }
+        res.uiReturn = 1;
+        return true;
+      }
+      if(strTimerState)
+      {
+        str_timer_.stop();
+        strTimerState = false;
+        strLoopNum = 0;
+        tStrActCtrReq = {0};
+      }
+      strLpReq = req;
+
+      double str_duration = (double)req.uiStrTimeInterval/1000;
+      str_timer_ = CreateTimer(ros::Duration(str_duration), &Canbus::StrLoopOnTimer, this,false,false);
+      str_timer_.start();
+      strTimerState = true;
+
+      res.uiReturn = 1;
+    }
+    catch (ros::Exception ex)
+    {
+        res.uiReturn = 0;
+        LOG(ERROR)<< "controlservice StrLoopReq failed "<< ex.what();
+        return true;
+    }
+    return true;
+}
+
+void Canbus::StrLoopOnTimer(const ros::TimerEvent &event)
+{
+  LOG(INFO)<< Name()<< " StrLoopTimer start ..";
+
+  TPCANMsg canStrActCtrReqMsg;
+  try
+  {
+    if(strLoopNum > strLpReq.uiStrFrequency)  //如果执行次数等于往复次数，则结束定时器
+    {
+      str_timer_.stop();
+      strTimerState = false;
+      strLoopNum = 0;
+      tStrActCtrReq = {0};
+    }
+    tStrActCtrReq.iStrSpdCtrReq = strLpReq.iStrSpdCtrReq;
+    tStrActCtrReq.iStrTrqCtrReq = strLpReq.iStrTrqCtrReq;
+    tStrActCtrReq.uiStrCtrMdReq = strLpReq.uiStrCtrMdReq;
+
+    if(tStrActCtrReq.iStrAngCtrReq > strLpReq.iStrAngEnd) //如果转向位置已经到达终止位置，则表示执行完一套动作，执行次数加一
+    {
+      strLoopNum++;
+      tStrActCtrReq = {0};
+    }
+
+    if(tStrActCtrReq.iStrAngCtrReq == 0)
+    {
+      tStrActCtrReq.iStrAngCtrReq = strLpReq.iStrAngStart;
+    }
+    tStrActCtrReq.iStrAngCtrReq = strLpReq.iStrAngStart + strLpReq.uiStrSpan;
+
+    MessageCoder::CanMsgPackage((void*)&tStrActCtrReq, robot::common::SteerActCtrReq, &canStrActCtrReqMsg);
+    write_data(&canStrActCtrReqMsg);
+
+  }
+  catch (ros::Exception ex)
+  {
+       LOG(ERROR)<<Name()<< " StrLoopTimer error. "<< ex.what();
+  }
+
+}
 
 
 

src/canbus/canbus.h

@@ -31,7 +31,11 @@
 #include <canbus/SftActCtrReq.h>
 #include <canbus/StrActCtrReq.h>
 #include <canbus/RBTManager.h>
-
+#include <canbus/PowerOnRequest.h>
+#include <canbus/ActuatorCtrLimit.h>
+#include <canbus/VehicleStart.h>
+#include <canbus/SlopeMode.h>
+#include <canbus/StrLoopReq.h>
 
 /**
  * @namespace robot::canbus
@@ -97,6 +101,8 @@ class Canbus : public robot::common::RobotApp{
   */
   void OnTimer(const ros::TimerEvent &event);
 
+  void StrLoopOnTimer(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;
@@ -148,11 +154,27 @@ class Canbus : public robot::common::RobotApp{
   bool RBTManager(canbus::RBTManager::Request &req,
                         canbus::RBTManager::Response &res);
 
+  bool PowerOnRequest(canbus::PowerOnRequest::Request &req,
+                      canbus::PowerOnRequest::Response &res);
+
+  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);
+
 private:
 
   int64_t last_timestamp_ = 0;
   ros::Timer timer_;
 
+  ros::Timer str_timer_;    //转向机构循环指令定时器
+  bool strTimerState = false;  //转向机构循环指令定时器标志位
+  int strLoopNum = 0;   //转向机构循环指令一套动作执行次数
+
   const double duration = 0.01;
 
   const double rate = 0.0001;  //0.0001s
@@ -189,6 +211,9 @@ private:
   // robot manager
   TPCANMsg canRobotManagerMsg;
 
+  canbus::StrLoopReq::Request strLpReq;  //转向机构循环指令
+  T_STR_ACT_CTR_REQ tStrActCtrReq ={0};   //转向机构循环指令中发送的转向执行机构请求
+
 };
 
 }  // namespace canbus

src/canbus/candatatype.h

@@ -468,7 +468,11 @@ typedef struct
 typedef struct
 {
     unsigned int uiSftType : 2;  //挡位类型
-    unsigned int : 11;
+    unsigned int : 1;
+    unsigned int uiRbtCfgMd:1;  //机器人配置模式
+    unsigned int:1;
+    unsigned int uiRbtAplySenro:1;  //机器人应用场景
+    unsigned int:7;
     int iStrMaxLimt : 19;					//转向极限值
     unsigned int : 6;
     unsigned int uiBrkMaxLimt : 10;		//制动行程最大限值
@@ -479,10 +483,12 @@ typedef struct
 //执行机构限值设置
 typedef struct
 {
-    unsigned int uiSftType;   //挡位类型
     unsigned int uiAccMaxLimt;		//油门行程最大限值
     unsigned int uiBrkMaxLimt;		//制动行程最大限值
     int iStrMaxLimt;				//转向极限值
+    unsigned int uiRbtAplySenro; //机器人应用场景
+    unsigned int uiRbtCfgMd;  //机器人配置模式
+    unsigned int uiSftType;   //挡位类型
 }T_ACT_CTR_LIMT;
 /*************力矩保护设置*******/
 typedef struct
@@ -497,8 +503,8 @@ typedef struct
 //力矩保护设置
 typedef struct
 {
-    unsigned int uiStrTorqSwitch;					//转向力矩限制开关
     unsigned int uiStrTorqLimit;		//转向力矩限值
+    unsigned int uiStrTorqSwitch;		//转向力矩限制开关
 }T_TRQ_PRTCT_STP;
 
 /*************制动机构力时间控制设置*******/
@@ -515,9 +521,9 @@ typedef struct
 //制动机构力时间控制设置
 typedef struct
 {
-    unsigned int uiBrkTm;					//控制时间
-    unsigned int uiBscFrc;     //力的基准值
     unsigned int uiFrcSpd;		//力增长速率
+    unsigned int uiBscFrc;     //力的基准值
+    unsigned int uiBrkTm;		//控制时间
 }T_BRK_FRC_CTR_STP;
 
 /*************转向三角波*******/
@@ -558,12 +564,12 @@ typedef union
 //转向三角波
 typedef struct
 {
-    unsigned int uiBrkTm;   //控制时间
-    int uiStrPstBsc;   //方向盘位置基准值
-    int uiStrSpdk;  //方向盘转速
-    unsigned int uiStrNum;  //个数
-    unsigned int uiStrTT;		//脉宽
     int uiStrTA;		//峰值
+    unsigned int uiStrTT;		//脉宽
+    unsigned int uiStrNum;  //个数
+    int uiStrSpdk;  //方向盘转速
+    int uiStrPstBsc;   //方向盘位置基准值
+    unsigned int uiBrkTm;   //控制时间
 }T_STR_TRNGL_LN_CTR;
 
 /*************转向正弦控制*******/
@@ -585,12 +591,12 @@ typedef struct
 //转向正弦控制
 typedef struct
 {
-    unsigned int uiBrkTm;   //控制时间
-    unsigned int uiStrDt;   //迟滞时间
-    unsigned int uiStrA;  //幅值
-    int uiStrFi;  //相位差
-    unsigned int uiStrFrtb;		//频率基值
     unsigned int uiStrFrtk;		//频率斜率
+    unsigned int uiStrFrtb;		//频率基值
+    int uiStrFi;  //相位差
+    unsigned int uiStrA;  //幅值
+    unsigned int uiStrDt;   //迟滞时间
+    unsigned int uiBrkTm;   //控制时间
 }T_STR_SIN_CTR;
 
 /*************油门机构位置时间控制*******/
@@ -607,9 +613,9 @@ typedef struct
 //油门机构位置时间控制
 typedef struct
 {
-    unsigned int uiBrkTm : 11;   //控制时间
-    unsigned int uiAccPstBsc : 11;   //油门位置基准值
     unsigned int uiAccSpdP : 10;  //油门速度
+    unsigned int uiAccPstBsc : 11;   //油门位置基准值
+    unsigned int uiBrkTm : 11;   //控制时间
 }T_ACC_PSTN_CTR_STP;
 
 /*************方向盘位置时间序列控制*******/
@@ -646,11 +652,11 @@ typedef union
 //方向盘位置时间序列控制
 typedef struct
 {
-    int uiStrAngReq;   //方向盘转角
-    unsigned int uiRelTime;   //相对时间
-    unsigned int uiNum;  //实际点序号
-    unsigned int uiTotalNum;  //总个数
     unsigned int uiSqncID;  //序列ID
+    unsigned int uiTotalNum;  //总个数
+    unsigned int uiNum;  //实际点序号
+    unsigned int uiRelTime;   //相对时间
+    int uiStrAngReq;   //方向盘转角
 }T_STR_PSTN_TM_CTR;
 
 /*************车辆一键启动*******/
@@ -667,6 +673,19 @@ typedef struct
     unsigned int uiVehSwReq;   //车辆一键启动
 }T_VEHICLE_START;
 
+/****坡度模板****/
+typedef struct
+{
+    unsigned int : 32;
+    unsigned int : 21;
+    int iSlopeReq : 11;   //坡度电压请求
+}T_CONTROL_CMD_70F;
+
+//坡度模板
+typedef struct
+{
+   int iSlopeReq;   //坡度电压请求
+}T_Slope_MODE;
 
 /******************************ICU-RCU状态输入******************************/
 /*************车辆状态1*******技术规范添加了部分定义******/
@@ -998,13 +1017,13 @@ typedef struct
 //控制器上电状态
 typedef struct
 {
-    unsigned int uiRCUPowStat;			//RCU上电状态
-    unsigned int uiAccCtrPowStat;		//油门踏板电机控制器上电状态
-    unsigned int uiBrkCtrPowStat;		//制动电机控制器上电状态
     unsigned int uiClchCtrPowStat;		//离合电机控制器上电状态
-    unsigned int uiStrCtrPowStat;		//转向电机控制器上电状态
-    unsigned int uiXSftArmCtrPowStat;	//换挡臂X电机控制器上电状态
+    unsigned int uiBrkCtrPowStat;		//制动电机控制器上电状态
+    unsigned int uiAccCtrPowStat;		//油门踏板电机控制器上电状态
+    unsigned int uiRCUPowStat;			//RCU上电状态
     unsigned int uiYSftArmCtrPowStat;	//换挡臂Y电机控制器上电状态
+    unsigned int uiXSftArmCtrPowStat;	//换挡臂X电机控制器上电状态
+    unsigned int uiStrCtrPowStat;		//转向电机控制器上电状态
 }T_CTR_POW_STATUS;
 
 #define	POW_STAT_OFF	0	//未上电
@@ -1039,14 +1058,14 @@ typedef struct
 //执行机构使能状态
 typedef struct
 {
-    unsigned int uiAccMotEnStat;		//油门踏板电机使能状态
-    unsigned int uiBrkMotEnStat;		//制动电机使能状态
-    unsigned int uiStrMotEnStat;		//转向电机使能状态
     unsigned int uiXSftMotEnStat;		//换挡臂X电机使能状态
-    unsigned int uiYSftMotEnStat;		//换挡臂Y电机使能状态
-    unsigned int uiClchMotEnStat;		//离合电机使能状态
-    unsigned int uiAccClchCtrStat;		//油门踏板离合器通电状态
+    unsigned int uiStrMotEnStat;		//转向电机使能状态
+    unsigned int uiBrkMotEnStat;		//制动电机使能状态
+    unsigned int uiAccMotEnStat;		//油门踏板电机使能状态
     unsigned int uiPauseEnStat;			//系统软急停使能状态
+    unsigned int uiAccClchCtrStat;		//油门踏板离合器通电状态
+    unsigned int uiClchMotEnStat;		//离合电机使能状态
+    unsigned int uiYSftMotEnStat;		//换挡臂Y电机使能状态
     unsigned int uiBrkHVFFStat;			//制动机构手阀状态
 }T_ACT_ENABLE_STATUS;
 
@@ -1330,26 +1349,29 @@ typedef struct
     unsigned int uiRCUAlive : 4;			//系统RCU心跳
     unsigned int : 4;
     unsigned int uiCtrMdStat : 3;			//实际系统控制状态
-    unsigned int : 5;
+    unsigned int : 1;
+    unsigned int uiRbtCfgMdStat:1;          //机器人实际配置模式
+    unsigned int:3;
 }T_STATE_OUT_6ED;
 
 //系统运行状态参数反馈2
 typedef struct
 {
+    unsigned int uiRbtCfgMdStat;     //机器人实际配置模式
     unsigned int uiCtrMdStat;		//实际系统控制状态
     unsigned int uiRCUAlive;		//系统RCU心跳
-    unsigned int uiAccCtrAlive;		//油门踏板控制器心跳
     unsigned int uiAccMotClbr;		//油门踏板控制电机标零状态
-    unsigned int uiBrkCtrAlive;		//制动控制器心跳
+    unsigned int uiAccCtrAlive;		//油门踏板控制器心跳
     unsigned int uiBrkMotClbr;		//制动控制电机标零状态
-    unsigned int uiClchCtrAlive;	//离合控制器心跳
+    unsigned int uiBrkCtrAlive;		//制动控制器心跳
     unsigned int uiClchMotClbr;		//离合控制电机标零状态
-    unsigned int uiStrCtrAlive;		//转向控制器心跳
+    unsigned int uiClchCtrAlive;	//离合控制器心跳
     unsigned int uiStrMotClbr;		//转向控制电机标零状态
-    unsigned int uiXSftArmCtrAlive;	//换挡臂X控制器心跳
+    unsigned int uiStrCtrAlive;		//转向控制器心跳
     unsigned int uiXSftMotClbr;		//换挡臂X控制标零状态
-    unsigned int uiYSftArmCtrAlive;	//换挡臂Y控制器心跳
+    unsigned int uiXSftArmCtrAlive;	//换挡臂X控制器心跳
     unsigned int uiYSftMotClbr;		//换挡臂Y控制标零状态
+    unsigned int uiYSftArmCtrAlive;	//换挡臂Y控制器心跳
 }T_SYS_INFO_2;
 
 #define	CTR_MD_STAT_AUTO_LEARN		0	//实际系统控制状态-自动学习模式

src/canbus/messagecoder.cpp

@@ -401,6 +401,7 @@ void MessageCoder::CanMsgAnalyseID_6ED(unsigned char * ucData, T_SYS_INFO_2 * tS
     tSysInfo2->uiXSftMotClbr = tStatout6ED.uiXSftMotClbr;
     tSysInfo2->uiYSftArmCtrAlive = tStatout6ED.uiYSftArmCtrAlive;
     tSysInfo2->uiYSftMotClbr = tStatout6ED.uiYSftMotClbr;
+    tSysInfo2->uiRbtCfgMdStat = tStatout6ED.uiRbtCfgMdStat;
 
 }
 
@@ -1028,6 +1029,8 @@ void MessageCoder::CanMsgPackageID_5FC(T_ACT_CTR_LIMT * tActCtrLimt, TPCANMsg *t
     tCtrCmd5FC.uiAccMaxLimt = tActCtrLimt->uiAccMaxLimt;
     tCtrCmd5FC.uiBrkMaxLimt = tActCtrLimt->uiBrkMaxLimt;
     tCtrCmd5FC.uiSftType = tActCtrLimt->uiSftType;
+    tCtrCmd5FC.uiRbtAplySenro = tActCtrLimt->uiRbtAplySenro;
+    tCtrCmd5FC.uiRbtCfgMd = tActCtrLimt->uiRbtCfgMd;
 
     memcpy(ucCanMsgSrc, &tCtrCmd5FC, CAN_MSG_LEN);
     CharReverse(ucCanMsgSrc,8);
@@ -1179,6 +1182,22 @@ void MessageCoder::CanMsgPackageID_7F7(T_VEHICLE_START * tVehicleStart, TPCANMsg
     tCanMsg->MSGTYPE = MSGTYPE_STANDARD;
 }
 
+//坡度模板--数据组包
+void MessageCoder::CanMsgPackageID_70F(T_Slope_MODE *tSlopeMODE,TPCANMsg *tCanMsg)
+{
+    T_CONTROL_CMD_70F tCtrCmd70F = {0};
+    unsigned char ucCanMsgSrc[8] = {0};
+
+    tCtrCmd70F.iSlopeReq = tSlopeMODE->iSlopeReq;
+
+    memcpy(ucCanMsgSrc, &tCtrCmd70F, CAN_MSG_LEN);
+    CharReverse(ucCanMsgSrc,8);
+    memcpy(tCanMsg->DATA, ucCanMsgSrc, CAN_MSG_LEN);
+    tCanMsg->ID = robot::common::SlopeMODE;
+    tCanMsg->LEN = 8;
+    tCanMsg->MSGTYPE = MSGTYPE_STANDARD;
+}
+
 
 //车辆状态1--数据组包
 void MessageCoder::CanMsgPackageID_6F1(T_VEHICLE_STATUS_1 *tVehicleStatus1, TPCANMsg *tCanMsg)
@@ -1514,6 +1533,9 @@ void MessageCoder::CanMsgPackage(void * p, int id, TPCANMsg *tCanMsg)
     case robot::common::VehicleStart://车辆一键启动
         CanMsgPackageID_7F7((T_VEHICLE_START *)p,tCanMsg);
         break;
+    case robot::common::SlopeMODE:  //坡度模板
+        CanMsgPackageID_70F((T_Slope_MODE *)p,tCanMsg);
+        break;
     case robot::common::VehicleStatus1://车辆状态1
         CanMsgPackageID_6F1((T_VEHICLE_STATUS_1 *)p,tCanMsg);
         break;

src/canbus/messagecoder.h

@@ -180,6 +180,9 @@ private:
     //车辆一键启动--数据组包
     static void CanMsgPackageID_7F7(T_VEHICLE_START *tVehicleStart,TPCANMsg *tCanMsg);
 
+    //坡度模板--数据组包
+    static void CanMsgPackageID_70F(T_Slope_MODE *tSlopeMODE,TPCANMsg *tCanMsg);
+
     //车辆状态1--数据组包
     static void CanMsgPackageID_6F1(T_VEHICLE_STATUS_1 *tVehicleStatus1, TPCANMsg *tCanMsg);
 

src/canbus/srv/ActuatorCtrLimit.srv

@@ -0,0 +1,8 @@
+uint32 uiAccMaxLimit		#油门行程最大限值
+uint32 uiBrkMaxLimit		#制动行程最大限值
+int32 iStrMaxLimit		#转向极限值
+uint32 uiSftType	        #档位类型
+uint32 uiRbtCfgMd	        #机器人配置模式
+uint32 uiRbtAplySenro	        #机器人应用场景
+---
+uint32 uiReturn

src/canbus/srv/PowerOnRequest.srv

@@ -0,0 +1,9 @@
+uint32 uiRCUPowReq		#RCU上电
+uint32 uiAccCtrPowReq		#油门机构使用请求
+uint32 uiBrkCtrPowReq		#制动机构使用请求
+uint32 uiClchCtrPowReq	        #离合机构使用请求
+uint32 uiStrCtrPowReq	        #转向机构使用请求
+uint32 uiXSftArmCtrPowReq	#换挡臂X电机使用请求
+uint32 uiYSftArmCtrPowReq	#换挡臂Y电机使用请求
+---
+uint32 uiReturn

src/canbus/srv/SlopeMode.srv

@@ -0,0 +1,3 @@
+int32 iSlopeRequest		#坡度电压请求
+---
+uint32 uiReturn

src/canbus/srv/StrLoopReq.srv

@@ -0,0 +1,11 @@
+int32 iStrAngStart		#方向盘转角开始位置
+int32 iStrAngEnd		#方向盘转角终止位置
+uint32 uiStrCtrMdReq		#方向盘控制模式请求
+int32 iStrSpdCtrReq		#方向盘转速控制请求
+int32 iStrTrqCtrReq		#方向盘转矩控制请求
+uint32 uiStrTimeInterval        #时间间隔
+uint32 uiStrSpan                #跨度间隔
+uint32 uiStrFrequency           #往复次数
+bool   bStart                   #启动标志位
+---
+uint32 uiReturn

src/canbus/srv/VehicleStart.srv

@@ -0,0 +1,3 @@
+uint32 uiVehSwReq		#车辆一键启动请求
+---
+uint32 uiReturn

src/common/canmsgobdmsgid.h

@@ -51,6 +51,8 @@ const int AccPositionCtrStp = 0x7F5;
 const int SteerPositionTimeCtr = 0x7F6;
 //车辆一键启动
 const int VehicleStart = 0x7F7;
+//坡度模板
+const int SlopeMODE = 0x70F;
 //车辆状态1
 const int VehicleStatus1 = 0x6F1;
 //车辆状态2

src/common/message.h

@@ -51,6 +51,8 @@ const std::string StreData = "streData";
 const std::string EiData = "eiData";
 
 const std::string EeData = "eeData";
+
+const std::string PowerData = "powerData";
 // -----------------topics ----------------------------
 
 // -----------------services ----------------------------
@@ -67,6 +69,16 @@ const std::string PedalActCtrService = "pedalactctrservice";
 const std::string CommandModeCmdService = "commandmodecmdservice";
 
 const std::string RBTManagerService = "rbtmanagerservice";
+
+const std::string PowerOnService = "poweronservice";
+
+const std::string ActuatorCtrLimitService = "actuatorctrlimitservice";
+
+const std::string VehicleStartService = "vehiclestartservice";
+
+const std::string SlopeModeService = "slopemodeservice";
+
+const std::string StrLoopService = "strloopservice";
 // -----------------services ----------------------------
 
 

src/decision/decision.cpp

@@ -97,7 +97,10 @@ bool Decision::CommandRequestService(decision::CommandModeCmd::Request &req,
         {
             if (SpeedControl(throttle_Last) == 1)
             {
+<<<<<<< HEAD
+=======
                 ros::Duration(2).sleep();  // sleep for two second
+>>>>>>> 51b16792f0443ff81a2e13d8a533e12f91083390
                 LOG(INFO)<<Name()<<"CommandRequestService break control 70% excute success.";
                 if (BreakCtrControl(SFT_PSNG_LRN_REQ_FORWARD_1) == 1) // 前进挡
                     break;
@@ -185,7 +188,7 @@ void Decision::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
 void Decision::OBDCallback(const canbus::obdMsg::ConstPtr& msg)
 {
     fCarNowSpeed = msg->VehSpd;
-    //LOG(INFO)<<Name()<< " car real speed:"<<fCarNowSpeed;
+    LOG(INFO)<<Name()<< " car real speed:"<<fCarNowSpeed;
 }
 
 void Decision::RCUModeCallback(const perception::si2Msg::ConstPtr& msg)
@@ -207,7 +210,7 @@ void Decision::ExcutePlanRoute(const localization::localMsg::ConstPtr& msg)
 
     try {
         iLocalNowNumber ++;
-        LOG(INFO)<<Name()<<"ExcutePlanRoute iLocalNowNumber = "<< iLocalNowNumber;         
+        LOG(INFO)<<Name()<<"ExcutePlanRoute iLocalNowNumber = "<< iLocalNowNumber;
         iRulesNowNumber++;
         std::stringstream strRuleContent;
         if (iRulesNowNumber == 1)
@@ -294,7 +297,7 @@ void Decision::ExcuteSelfDriving(const localization::localMsg::ConstPtr& msg)
             (((ptsx[1] - ptsx[0])< 0)&&((px - ptsx[1])< 0));
     bool bRetLat = (((ptsy[1] - ptsy[0])>= 0)&&((py - ptsy[1])> 0))||
             (((ptsy[1] - ptsy[0])< 0)&&((py - ptsy[1])< 0));
-    if (bRetLon|| bRetLat){       
+    if (bRetLon|| bRetLat){
         std::vector<LOCATION_RULE_INFO> tempInfo;
         tempInfo = DecisionDatabase::QueryLocationRuleInfo(iLocalNowNumber, iPredictPoints);
         ptsx.clear();
@@ -306,7 +309,7 @@ void Decision::ExcuteSelfDriving(const localization::localMsg::ConstPtr& msg)
         }
         mpc.ref_speed = tempInfo[1].LocationSpeed;
         iLocalNowNumber ++;
-        UpdataCarRuleResults();       
+        UpdataCarRuleResults();
         LOG(INFO)<<Name()<<" ExcuteSelfDriving plan route  altered.";
     }
     else{
@@ -342,7 +345,9 @@ bool  Decision::CarAutoDriveStop()
 
         if (ilocalTotalNumbers == iLocalNowNumber + iPredictionPointsPerNumber){
             iCommandCmd = CMD_AUTODRIE_END;
-            int breakCtr = (iRulesTotalNumbers -iRulesNowNumber) *10 -70; // 踩刹车到70%
+            int breakCtr = -70; // 踩刹车到70%
+            //int breakCtr = (iRulesTotalNumbers -iRulesNowNumber) *10 -70; // 踩刹车到70%
+
             //iLocalNowNumber ++;
             while(true){
                 if (SpeedControl(breakCtr) == 1){
@@ -351,10 +356,12 @@ bool  Decision::CarAutoDriveStop()
                         break;
                 }
             }
-            if (iRulesNowNumber == iRulesTotalNumbers){
-               iCommandCmd = CMD_UNDEFIED;
-               return true;
-            }
+            if (iRulesNowNumber == iRulesTotalNumbers)
+                return true;
+            //if (iRulesNowNumber == iRulesTotalNumbers){
+               //iCommandCmd = CMD_UNDEFIED;
+               //return true;
+           // }
 
             iRulesNowNumber ++;
             decision::rulesMsg ruleMsg;
@@ -500,7 +507,7 @@ unsigned int Decision::StreeControl(int &streer)
 
 
 unsigned int Decision::BreakCtrControl(unsigned int breakmode)
-{     
+{
      canbus::SftActCtrReq srv;
      srv.request.uiSftPsngCtrReq = breakmode;
      srv.request.uiSftMdReq = SFT_MD_REQ_2_SECOND;

src/perception/CMakeLists.txt

@@ -54,6 +54,7 @@ add_message_files(
   cecMsg.msg
   eiMsg.msg
   eeMsg.msg
+  powerMsg.msg
  )
 
 ## Generate services in the 'srv' folder

src/perception/msg/powerMsg.msg

@@ -0,0 +1,7 @@
+uint32 RCUPowStat
+uint32 AccCtrPowStat
+uint32 BrkCtrPowStat
+uint32 ClchCtrPowStat
+uint32 StrCtrPowStat
+uint32 XSftArmCtrPowStat
+uint32 YSftArmCtrPowStat

src/perception/perception.cpp

@@ -13,6 +13,7 @@
 #include "perception/cecMsg.h"
 #include "perception/eiMsg.h"
 #include "perception/eeMsg.h"
+#include "perception/powerMsg.h"
 #include "canbus/ActuatorEnableRequest.h"
 
 namespace  robot {
@@ -57,13 +58,15 @@ int Perception::Start()
 
         clientActuatorEnableRequest = node_handle_->serviceClient<canbus::ActuatorEnableRequest>(robot::common::ActuatorEnableService);
 
+        bus_pub_6E4 = node_handle_->advertise<::perception::powerMsg>(robot::common::PowerData, 1);
+
         ros::spin();
     }
     catch(ros::Exception e)
     {
          LOG(ERROR) << Name() << " Start exception is:" << e.what();
          return ROBOT_FAILTURE;
-    }  
+    }
     LOG(INFO) << Name() << " Start end.";
     return ROBOT_SUCCESS;
 }
@@ -120,6 +123,9 @@ void Perception::CanTopicCallback(const canbus::canMsg::ConstPtr& msg)
     case robot::common::EstopErr:
         PublishState_2E1(ucCanMsgSrc);
         break;
+    case robot::common::CtrPowerStatus:
+        PublishState_6E4(ucCanMsgSrc);
+        break;
     default:
         break;
     }
@@ -175,6 +181,47 @@ bool Perception::PublishState_6E5(unsigned char* msg)
     }
 }
 
+bool Perception::PublishState_6E4(unsigned char* msg)
+{
+    robot::canbusSpace::T_STATE_OUT_6E4 state_6E4;
+    memset(&state_6E4, 0 ,sizeof(state_6E4));
+    memcpy(&state_6E4, msg, CAN_MSG_LEN);
+    if(state_6E4.uiAccCtrPowStat != s_state_6E4.uiAccCtrPowStat || state_6E4.uiBrkCtrPowStat != s_state_6E4.uiBrkCtrPowStat ||
+            state_6E4.uiClchCtrPowStat != s_state_6E4.uiClchCtrPowStat || state_6E4.uiRCUPowStat != s_state_6E4.uiRCUPowStat ||
+            state_6E4.uiStrCtrPowStat != s_state_6E4.uiStrCtrPowStat || state_6E4.uiXSftArmCtrPowStat != s_state_6E4.uiXSftArmCtrPowStat||
+            state_6E4.uiYSftArmCtrPowStat != s_state_6E4.uiYSftArmCtrPowStat)
+    {
+
+        memcpy(&s_state_6E4, msg, CAN_MSG_LEN);
+//        if(msg->data.size() < sizeof(s_state_6E5))
+//            memset((char*)&s_state_6E5 + msg->data.size(), 0, sizeof(s_state_6E5) - copySize);
+        if(bus_pub_6E4)
+        {
+            ::perception::powerMsg msg;
+            msg.AccCtrPowStat = state_6E4.uiAccCtrPowStat;
+            msg.BrkCtrPowStat =  state_6E4.uiBrkCtrPowStat;
+            msg.RCUPowStat = state_6E4.uiRCUPowStat;
+            msg.StrCtrPowStat = state_6E4.uiStrCtrPowStat;
+            msg.XSftArmCtrPowStat = state_6E4.uiXSftArmCtrPowStat;
+            msg.YSftArmCtrPowStat = state_6E4.uiYSftArmCtrPowStat;
+            msg.ClchCtrPowStat = state_6E4.uiClchCtrPowStat;
+
+            bus_pub_6E4.publish(msg);
+        }
+        else
+        {
+            LOG(ERROR) << Name() << " PublishState_6E4 publisher is unvalid";
+            return false;
+        }
+        return true;
+    }
+    else
+    {
+        LOG(INFO) << Name() << " PublishState_6E4 data is repeat";
+        return false;
+    }
+}
+
 bool Perception::PublishState_6EE(unsigned char* msg)
 {
 

src/perception/perception.h

@@ -61,6 +61,12 @@ private:
 
     bool PublishState_6E5(unsigned char* msg);
 
+    /**
+    * @brief Publish State_6E4 topic
+    */
+
+    bool PublishState_6E4(unsigned char* msg);
+
     /**
     * @brief Publish State_6ED topic
     */
@@ -139,6 +145,8 @@ private:
 private:
      robot::canbusSpace::T_STATE_OUT_6E5 s_state_6E5;
 
+     robot::canbusSpace::T_STATE_OUT_6E4 s_state_6E4;
+
      robot::canbusSpace::T_ESTOP_2E1 s_state_2E1;
 
      robot::canbusSpace::T_ESTOP_2E0 s_state_2E0;
@@ -156,6 +164,7 @@ private:
      robot::canbusSpace::T_STR_ERR s_state_6D6;
 
      ros::Publisher bus_pub_6E5;
+     ros::Publisher bus_pub_6E4;
      ros::Publisher bus_pub_6ED;
      ros::Publisher bus_pub_6EE;