/******************************************************************************
 * Copyright 2019 The siasun Transport Authors. All Rights Reserved.
 * Author: limengqi
 * Licensed under the Apache License, Version 1.0 (the "License");
 *****************************************************************************/

#include "canbus.h"
#include"candatatype.h"
#include"messagecoder.h"
#include"../common/message.h"
#include"../common/canmsgobdmsgid.h"
#include "std_msgs/String.h"
#include <canbus/canMsg.h>
#include <iostream>
#include <fstream>


namespace  robot{
namespace canbusSpace {



std::string Canbus::Name() const { return "canbus"; }

int Canbus::Init() {
  LOG(INFO)<< Name()<<" Init start...";
  node_handle_.reset(new ros::NodeHandle());

  //load libpcan.so using dlopen function,return handle for further use
  libm_handle = dlopen("libpcan.so", RTLD_LAZY );
  if (!libm_handle){
      LOG(ERROR)<< Name()<<" Init Open Error:" << dlerror();//if file can't be loaded,return null,get reason using dlerror function
      return ROBOT_FAILTURE;
  }
  char *errorInfo;//error information pointer
  //one-to-one mapping using dlsym function,if return null,mapping would be failed
  funCAN_Init         =(funCAN_Init_TYPE)         dlsym(libm_handle,"CAN_Init");
  funLINUX_CAN_Open   =(funLINUX_CAN_Open_TYPE)   dlsym(libm_handle,"LINUX_CAN_Open");
  funCAN_Close        =(funCAN_Close_TYPE)        dlsym(libm_handle,"CAN_Close");
  funCAN_VersionInfo  =(funCAN_VersionInfo_TYPE)  dlsym(libm_handle,"CAN_VersionInfo");
  funLINUX_CAN_Read   =(funLINUX_CAN_Read_TYPE)   dlsym(libm_handle,"LINUX_CAN_Read");
  funCAN_Status       =(funCAN_Status_TYPE)       dlsym(libm_handle,"CAN_Status");
  funnGetLastError    =(funnGetLastError_TYPE)    dlsym(libm_handle,"nGetLastError");
  funCAN_Write        =(funCAN_Write_TYPE)       dlsym(libm_handle, "CAN_Write");

  errorInfo = dlerror();//get error using dlerror function,and clear the error list in memory
  if (errorInfo != NULL){
      LOG(ERROR)<<Name()<<" Init Dlsym Error:"<<errorInfo;
      return ROBOT_FAILTURE;
  }

  //initialize the VehHBrkStat as Hold
  status1.uiVehHBrkStat = 1;


  InitServiceAndMsg();

  LOG(INFO)<<Name()<<" init end.";
  return ROBOT_SUCCESS;
}



//打开Can总线
int Canbus::Start() {
  char chVersion[VERSIONSTRING_LEN];            //store information of can version
  const char  *szDevNode = DEFAULT_NODE;  //define const pointer point to device name
  pcan_handle = funLINUX_CAN_Open(szDevNode, O_RDWR );//use mapping function

  //judge whether the call is success.if pcan_handle=null,the call would be failed
  if(pcan_handle){
      LOG(INFO)<<Name()<<": "<<szDevNode<<" opened";
      errno = funCAN_VersionInfo(pcan_handle, chVersion);
      if (!errno)
          LOG(INFO)<<Name()<<": driver version ="<<chVersion;
      else {
          LOG(ERROR)<<Name()<<": CAN_VersionInfo() error";
      }
      if (unBaudrate) {
              errno = funCAN_Init(pcan_handle, unBaudrate, nExtended);
              if (errno) {
                  LOG(ERROR)<<Name()<<": CAN_Init() error.";
              }
              else
                  LOG(INFO)<<" Device Info: " << szDevNode <<"Baudrate:"<<unBaudrate;
          }
  }
  else
  {
      LOG(INFO)<<Name()<<": can't open ."<<szDevNode;
  }

  ros::Rate loop_rate(1/rate);
  // 5. set timer to triger publish info periodly
  timer_ = CreateTimer(ros::Duration(duration), &Canbus::OnTimer, this);

  // this location is necessary

  canRCUData_pub = node_handle_->advertise<canbus::canMsg>(robot::common::CanDataAnalysis, 1);
  canObdData_pub = node_handle_->advertise<canbus::obdMsg>(robot::common::ObdData, 1);

  ros::ServiceServer ActuatorEnableService = node_handle_->advertiseService(robot::common::ActuatorEnableService,
                                                                &Canbus::ActuatorEnableRequest, this);
  ros::ServiceServer LearnModeService = node_handle_->advertiseService(robot::common::LearnModeService,
                                                            &Canbus::ExcuteLearnModeRequest, this);
  ros::ServiceServer ShiftActCtrService = node_handle_->advertiseService(robot::common::ShiftActCtrService,
                                                              &Canbus::SftActCtrRequest, this);
  ros::ServiceServer SteerActCtrService = node_handle_->advertiseService(robot::common::SteerActCtrService,
                                                              &Canbus::StrActCtrRequest, this);
  ros::ServiceServer AccPedalActCtrService = node_handle_->advertiseService(robot::common::AccPedalActCtrService,
                                                              &Canbus::AccPedalActCtrRequest, this);
  ros::ServiceServer BrkPedalActCtrService = node_handle_->advertiseService(robot::common::BrkPedalActCtrService,
                                                              &Canbus::BrkPedalActCtrRequest, 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);
  ros::ServiceServer AccLoopService = node_handle_->advertiseService(robot::common::AccLoopService,
                                                                     &Canbus::AccLoopReq,this);
  ros::ServiceServer BrkLoopService = node_handle_->advertiseService(robot::common::BrkLoopService,
                                                                     &Canbus::BrkLoopReq,this);
  ros::ServiceServer StrTxtService = node_handle_->advertiseService(robot::common::StrTxtService,
                                                                    &Canbus::StrTxtReq,this);
  ros::ServiceServer BrkTxtService = node_handle_->advertiseService(robot::common::BrkTxtService,
                                                                    &Canbus::BrkTxtReq,this);

  ros::ServiceServer AccTxtService = node_handle_->advertiseService(robot::common::AccTxtService,
                                                                    &Canbus::AccTxtReq,this);

  ros::ServiceServer StrTxtLineService = node_handle_->advertiseService(robot::common::StrTxtLineService,
                                                                        &Canbus::StrTxtLineReq,this);

  ros::ServiceServer VehicleStatSetService = node_handle_->advertiseService(robot::common::VehicleStatSetService,
                                                                            &Canbus::VehicleStatSetReq,this);

  ros::ServiceServer RbtDriverCtrlService = node_handle_->advertiseService(robot::common::RbtDriverCtrlService,
                                                                           &Canbus::RbtDriverControl,this);

  ros::ServiceServer StrTangLineCtrService  = node_handle_->advertiseService(robot::common::StrTangLineCtrService,
                                                                             &Canbus::StrTangLineCtrReq,this);

  ros::ServiceServer StrPosTimCtrService   = node_handle_->advertiseService(robot::common::StrPosTimCtrService,
                                                                            &Canbus::StrPosTimCtrReq,this);

  ros::ServiceServer StrSinCtrService   = node_handle_->advertiseService(robot::common::StrSinCtrService ,
                                                                         &Canbus::StrSinCtrReq,this);

  ros::ServiceServer EstpCtrlService   = node_handle_->advertiseService(robot::common::EstpCtrlService ,
                                                                        &Canbus::EstpCtrl,this);

  ros::ServiceServer RobotWorkMdService   = node_handle_->advertiseService(robot::common::RobotWorkMdService ,
                                                                           &Canbus::RobotWorkMd,this);

  ros::ServiceServer SysCtrlModeService   = node_handle_->advertiseService(robot::common::SysCtrlModeService ,
                                                                           &Canbus::SysCtrlMode,this);

  ros::ServiceServer BrkTimCtrlStpService   = node_handle_->advertiseService(robot::common::BrkTimCtrlStpService ,
                                                                             &Canbus::BrkTimCtrlStp,this);

  ros::ServiceServer AccPosCtrlStpService   = node_handle_->advertiseService(robot::common::AccPosCtrlStpService ,
                                                                             &Canbus::AccPosCtrlStp,this);

  ros::ServiceServer StrAccCtrlReqService   = node_handle_->advertiseService(robot::common::StrAccCtrlReqService ,
                                                                             &Canbus::StrAccCtrlReq,this);

  ros::ServiceServer RbtSlfChckCtrlReqService = node_handle_->advertiseService(robot::common::RbtSlfChckCtrlReqService ,
                                                                               &Canbus::RbtSlfChckCtrlReq,this);

  ros::ServiceServer IOModeService = node_handle_->advertiseService(robot::common::IOModeService ,
                                                                    &Canbus::IOMode,this);

  ros::ServiceServer SftTxtService = node_handle_->advertiseService(robot::common::SftTxtService,
                                                                    &Canbus::SftTxtReq,this);

  string str = "/home/madesheng/angle.txt";
  int flag = Canbus::read_txt(str,1);
  if(flag != ROBOT_SUCCESS)
  {
    return ROBOT_FAILTURE;
  }
   str = "/home/madesheng/psng.txt";
   flag = Canbus::read_txt(str,2);
   if(flag != ROBOT_SUCCESS)
   {
     return ROBOT_FAILTURE;
   }

   str = "/home/madesheng/acc.txt";
   flag = Canbus::read_txt(str,3);
   if(flag != ROBOT_SUCCESS)
   {
     return ROBOT_FAILTURE;
   }

   str = "/home/madesheng/angleLine.txt";
   flag = Canbus::read_txt(str,4);
   if(flag != ROBOT_SUCCESS)
   {
     return ROBOT_FAILTURE;
   }

   str = "/home/madesheng/sft.txt";
   flag = Canbus::read_txt(str,5);
   if(flag != ROBOT_SUCCESS)
   {
     return ROBOT_FAILTURE;
   }

  //data receive
  while(ros::ok())
  {
      read_loop();
      ros::spinOnce();
      loop_rate.sleep();
   }

   return ROBOT_SUCCESS;
}




void Canbus::OnTimer(const ros::TimerEvent &) {
  //LOG(INFO)<< Name()<< " timer start ..";
  if(heatbeat > 15)
  {
      heatbeat = 0;
  }
  icuMonitor.uiICUAlive = heatbeat++;

  try
  {
      canObdData_pub.publish(obd);
      TPCANMsg canIcuMonitor; //ICUMonitor Can Message
      MessageCoder::CanMsgPackage((void*)&icuMonitor, robot::common::ICUMonitor, &canIcuMonitor);
      write_data(&canIcuMonitor, false);

      if(workModeCnfg.uiOnlineOffline == VEH_DATA_TYPE_VIHICLE_DEPEND)
      {
        TPCANMsg canStatus1; //VehicleStaus1 Can Message
        MessageCoder::CanMsgPackage((void*)&status1, robot::common::VehicleStatus1, &canStatus1);
        write_data(&canStatus1, false);
        TPCANMsg canStatus2;  //VehicleStatus2 Can Message
        MessageCoder::CanMsgPackage((void*)&status2, robot::common::VehicleStatus2, &canStatus2);
        write_data(&canStatus2, false);
        TPCANMsg canStatus3;  //VehicleStatus3 Can Message
        MessageCoder::CanMsgPackage((void*)&status3, robot::common::VehicleStatus3, &canStatus3);
        write_data(&canStatus3, false);
      }
      else if(workModeCnfg.uiOnlineOffline == VEH_DATA_TYPE_STAND_ALONE)
      {
        status2.iStrAng = 0;
        status2.iStrAngGrd = 0;
        status2.uiEngSpd = 0;
        TPCANMsg canStatus2;  //VehicleStatus2 Can Message
        MessageCoder::CanMsgPackage((void*)&status2, robot::common::VehicleStatus2, &canStatus2);
        write_data(&canStatus2, false);
      }
  }
  catch (ros::Exception ex)
  {
       LOG(ERROR)<<Name()<< " OnTimer error. "<< ex.what();
  }
}

void Canbus::Stop() {
timer_.stop();
sleep(0.5);
  TPCANMsg canIcu; //ICUMonitor Can Message
  icuMonitor.uiICUClsReq = 1;
  MessageCoder::CanMsgPackage((void*)&icuMonitor, robot::common::ICUMonitor, &canIcu);
  write_data(&canIcu, false);
  LOG(INFO)<<Name()<<"send IcuMonitor CloseReq";

  //Must close h handle firstly,then close file using dlclose
  if (pcan_handle) {
      funCAN_Close(pcan_handle);
  }
  LOG(INFO)<<Name()<<": closed. ";
  ros::shutdown();
  node_handle_.release();
  LOG(INFO)<<Name()<<": node shutdown. ";
}


void Canbus::InitServiceAndMsg()
{
     obd.AccPsng = 0;
     obd.BrkPsng = 0;
     obd.SftPsng = 0;
     obd.StrAng = 0;
     obd.VehHBrkStat = 0;
     obd.VehSpd = 10;
     obd.VehSwStat = 0;
     status2.uiVehSpd = 10;

     icuMonitor.uiICUClsReq = 0;

     workModeCnfg.uiOnlineOffline = 2;
     workModeCnfg.uiRbtAplySenro = 2;
}

//read from CAN forever - until manual break
int Canbus::read_loop()
{
    TPCANRdMsg msg;
    __u32 status;

    if (funLINUX_CAN_Read(pcan_handle, &msg)) {
        LOG(ERROR)<<Name()<<" receive: LINUX_CAN_Read() error.";
        return errno;
    }
    read_data(&(msg.Msg));

     // deal receive data
    // check if a CAN status is pending
    if (msg.Msg.MSGTYPE & MSGTYPE_STATUS) {
        status = funCAN_Status(pcan_handle);
        if ((int)status < 0) {
            errno = funnGetLastError();
            LOG(ERROR)<<Name()<<" receive: CAN_Status() error.";
            return errno;
        }
    }

    return ROBOT_SUCCESS;
}

int Canbus::read_data(TPCANMsg *pMsgBuff)
{
//    char strId[4] = {0};
//    sprintf(strId,"%3xh",pMsgBuff->ID);
//    char strData[16]={0};
//    MessageCoder::ConvertUnCharToStr(strData,pMsgBuff->DATA,8);

  //  LOG(INFO)<<Name()<<" Receive CanData : ID: "<<std::string(strId)<<",Data: "<<std::string(strData);

  //通过读取车辆OBD的数据，赋值给OBD的变量  原始代码 开始
//    if(pMsgBuff->ID == robot::common::ObdAccPsng)
//    {
//       T_OBD_ACC_PSNG tOBDAccPsng = *((T_OBD_ACC_PSNG*)MessageCoder::CanMsgAnalyse(robot::common::ObdAccPsng,pMsgBuff->DATA));
//       status1.uiAccPsng = tOBDAccPsng.obdAccPsng;
//       obd.AccPsng = status1.uiAccPsng * 0.1;
//    }
//    else if(pMsgBuff->ID == robot::common::ObdBrkPsng)
//    {
//       T_OBD_BRK_PSNG tOBDBrkPsng = *((T_OBD_BRK_PSNG*)MessageCoder::CanMsgAnalyse(robot::common::ObdBrkPsng,pMsgBuff->DATA));
//       status1.uiBrkPsng = tOBDBrkPsng.obdBrkPsng;
//       obd.BrkPsng =  status1.uiBrkPsng * 0.1;
//    }
//    else if(pMsgBuff->ID == robot::common::ObdSftPsng)
//    {
//       T_OBD_SFT_PSNG tOBDSftPsng = *((T_OBD_SFT_PSNG*)MessageCoder::CanMsgAnalyse(robot::common::ObdSftPsng,pMsgBuff->DATA));
//       status1.uiSftPsng = tOBDSftPsng.obdSftPsng;
//       obd.SftPsng = status1.uiSftPsng;
//    }
//    else if(pMsgBuff->ID == robot::common::ObdVehHBrkStat)
//    {
//       T_OBD_VEH_BRK_STAT tOBDVehBrkStat = *((T_OBD_VEH_BRK_STAT*)MessageCoder::CanMsgAnalyse(robot::common::ObdVehHBrkStat,pMsgBuff->DATA));
//       obd.VehHBrkStat = status1.uiVehHBrkStat = tOBDVehBrkStat.obdVehHBrkStat;
//    }
//    else if(pMsgBuff->ID == robot::common::ObdVehSwStat)
//    {
//        T_OBD_VEH_SW_STAT tOBDVehSwStat = *((T_OBD_VEH_SW_STAT*)MessageCoder::CanMsgAnalyse(robot::common::ObdVehSwStat,pMsgBuff->DATA));
//        obd.VehSwStat = status1.uiVehSwStat = tOBDVehSwStat.obdVehSwStat;
//    }
//    else if(pMsgBuff->ID == robot::common::ObdStrAng)
//    {
//       T_OBD_STR_ANG tOBDStrAng = *((T_OBD_STR_ANG*)MessageCoder::CanMsgAnalyse(robot::common::ObdStrAng,pMsgBuff->DATA));
//       status2.iStrAng = tOBDStrAng.obdStrAng;
//       obd.StrAng = status2.iStrAng * 0.01;
//    }
//    else if(pMsgBuff->ID == robot::common::ObdEngSpd)
//    {
//        T_OBD_ENG_SPD tOBDEngSpd = *((T_OBD_ENG_SPD*)MessageCoder::CanMsgAnalyse(robot::common::ObdEngSpd,pMsgBuff->DATA));
//        status2.uiEngSpd = tOBDEngSpd.obdEngSpd;
//        obd.EngSpd = status2.uiEngSpd;
//    }
//    else if(pMsgBuff->ID == robot::common::ObdSpeed)
//    {
//        T_OBD_VEH_SPD tOBDVehSpd = *((T_OBD_VEH_SPD*)MessageCoder::CanMsgAnalyse(robot::common::ObdSpeed,pMsgBuff->DATA));
//        status2.uiVehSpd = tOBDVehSpd.obdVehSpd;
//        obd.VehSpd = tOBDVehSpd.obdVehSpd;
//    }
//    else if((pMsgBuff->ID >= robot::common::PowerOnReq && pMsgBuff->ID <= robot::common::BrakeForceCtrStp) ||
//            (pMsgBuff->ID >= robot::common::SysMonitorBack && pMsgBuff->ID <= robot::common::SteerPosTimeReq) ||
//            (pMsgBuff->ID >= robot::common::RobotAcuatorPos && pMsgBuff->ID <= robot::common::ComputerStaus) ||
//            (pMsgBuff->ID >= robot::common::SteerTranLineCtr && pMsgBuff->ID <= robot::common::VehicleStart) ||
//            pMsgBuff->ID ==robot::common::EstopInfo || pMsgBuff->ID ==robot::common::EstopErr)
//    {
//        publish_data(pMsgBuff);
//    }
    //通过读取车辆OBD的数据，赋值给OBD的变量  原始代码 结束

    //通过设置相关变量，赋值给OBD的变量   测试版代码 开始
  if(pMsgBuff->ID == robot::common::ObdAccPsng)
  {
   T_OBD_ACC_PSNG tOBDAccPsng = *((T_OBD_ACC_PSNG*)MessageCoder::CanMsgAnalyse(robot::common::ObdAccPsng,pMsgBuff->DATA));
     status1.uiAccPsng = tOBDAccPsng.obdAccPsng;
     status1.uiBrkPsng = 0;
     obd.AccPsng = tOBDAccPsng.obdAccPsng;
     obd.BrkPsng = 0;
     status2.iStrAng = 0;
     obd.StrAng = 0;
  }
  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::SysMonitorBack && pMsgBuff->ID <= robot::common::SteerPosTimeReq) ||
          pMsgBuff->ID ==robot::common::EstopInfo || pMsgBuff->ID ==robot::common::EstopErr ||
          pMsgBuff->ID == robot::common::EstpCtrl)
  {
      publish_data(pMsgBuff);
  }

    //通过设置相关变量，赋值给OBD的变量   测试版代码 结束
    return ROBOT_SUCCESS;
}

void Canbus::publish_data(TPCANMsg *pMsgBuff)
{
try
{
    canbus::canMsg rosMsg;
    rosMsg.id = pMsgBuff->ID;
    //std::stringstream ss;
    //ss << pMsgBuff->DATA;
    //rosMsg.data = ss.str();
    char strId[4] = {0};
    sprintf(strId,"%3xh",pMsgBuff->ID);
    char chData[16]={0};
    MessageCoder::ConvertUnCharToStr(chData,pMsgBuff->DATA,8);
    //std::string strData = chData;
    rosMsg.data = chData;
    canRCUData_pub.publish(rosMsg);
if(pMsgBuff->ID == robot::common::SysLearnBack)
{
  //  LOG(INFO)<<Name()<<" Publish CanData :" << "ID:" << strId << ",Data:"<<std::string(chData);
}

}
    catch (ros::Exception ex)
    {
        LOG(ERROR)<< "publish failed "<< ex.what();
    }


}

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);
  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;
  }
   return ROBOT_SUCCESS;
}

bool Canbus::ActuatorEnableRequest(canbus::ActuatorEnableRequest::Request &req,
                           canbus::ActuatorEnableRequest::Response &res)
{
    LOG(INFO)<< "controlservice start ActuatorEnableRequest";
    TPCANMsg canActuaEnableReqMsg;
    try{
        T_ACTUATOR_ENABLE_REQUEST request;
        request.uiAccClchCtrReq = req.uiAccClchCtrReq;
        request.uiAccMotEnReq = req.uiAccMotEnReq;
        request.uiBrkMotEnReq = req.uiBrkMotEnReq;
        request.uiClchMotEnReq = req.uiClchMotEnReq;
        request.uiStrMotEnReq = req.uiStrMotEnReq;
        request.uiXSftMotEnReq = req.uiXSftMotEnReq;
        request.uiYSftMotEnReq = req.uiYSftMotEnReq;
        request.uiZSftMotEnReq = req.uiZSftMotEnReq;

        MessageCoder::CanMsgPackage((void*)&request, robot::common::ActuatorEnableReq, &canActuaEnableReqMsg);
        if (write_data(&canActuaEnableReqMsg)!=ROBOT_SUCCESS)
        {
            res.uiReturn = 0;  //= 0
            return true;
        }
        res.uiReturn = 1;

//       TPCANMsg pMsgTest;
//       T_STR_LRN_PSNG test;
//       test.iClbtStrAng0 = 0;
//        test.iClbtStrAngLftMax = -120000;
//       test.uiClbtStrAngRtMax = 120000;
//       MessageCoder::CanMsgPackage((void*)&test, robot::common::SteerLearnPosBack, &pMsgTest);
//        publish_data(& pMsgTest);
    }
    catch (ros::Exception ex)
    {
        res.uiReturn = 0;
        LOG(ERROR)<< "controlservice ActuatorEnableRequest failed "<< ex.what();
        return true;
    }
    return true;
}


bool Canbus::ExcuteLearnModeRequest(canbus::LearnModeRequest::Request & req,
                           canbus::LearnModeRequest::Response &res)
{
    LOG(INFO)<< "controlservice start ExcuteLearnModeRequest";
    // learn mode request
    TPCANMsg canLearnModeReqMsg;
    try{
        T_LEARN_MODE_REQUEST request;
        request.uiClrAccTbl = req.uiClrAccTbl;
        request.uiClrBrkTbl = req.uiClrBrkTbl;
        request.uiClrClchTbl = req.uiClrClchTbl;
        request.uiClrSftTbl = req.uiClrSftTbl;
        request.uiClrStrTbl= req.uiClrStrTbl;
        request.uiPsngLrnReq = req.uiPsngLrnReq;
        request.uiSftAutLrnFnsConf = req.uiSftAutLrnFnsConf;
        request.uiSftPsngLrnReq = req.uiSftPsngLrnReq;
        request.uiStpAutLrn = req.uiStpAutLrn;
        request.uiSysLrnReq = req.uiSysLrnReq;

        MessageCoder::CanMsgPackage((void*)&request, robot::common::LearnModeReq, &canLearnModeReqMsg);
        if (write_data(&canLearnModeReqMsg)!=ROBOT_SUCCESS)
        {
            res.uiReturn = 0;  //= 0
            return true;
        }
        res.uiReturn = 1;
    }
    catch (ros::Exception ex)
    {
        res.uiReturn = 0;
        LOG(ERROR)<< "controlservice ExcuteLearnModeRequest failed "<< ex.what();
        return true;
    }
    return true;
}


bool Canbus::AccPedalActCtrRequest(canbus::AccPedalActCtrReq::Request &req,
                        canbus::AccPedalActCtrReq::Response &res)
{
    LOG(INFO)<< "controlservice start AccPedalActCtrRequest";
    // AccPedalActCtr request
    TPCANMsg canAccPedalActCtrReqMsg;
    try{
        T_ACC_PEDAL_ACT_CTR_REQ request;
        request.uiAccSpdCtrReq = req.uiAccSpdCtrReq;
        request.uiAccAccCtrReq = req.uiAccAccCtrReq;
        request.uiAccPsngCtrReq = req.uiAccPsngCtrReq;
        request.uiAccDecCtrReq = req.uiAccDecCtrReq;

        MessageCoder::CanMsgPackage((void*)&request, robot::common::AccPedalActCtrReq, &canAccPedalActCtrReqMsg);
        if (write_data(&canAccPedalActCtrReqMsg)!=ROBOT_SUCCESS)
        {
            res.uiReturn = 0;  //= 0
            return true;
        }
        res.uiReturn = 1;
    }
    catch (ros::Exception ex)
    {
        res.uiReturn = 0;
        LOG(ERROR)<< "controlservice AccPedalActCtrRequest failed "<< ex.what();
        return true;
    }
    return true;
}

bool Canbus::BrkPedalActCtrRequest(canbus::BrkPedalActCtrReq::Request &req,
                        canbus::BrkPedalActCtrReq::Response &res)
{
    LOG(INFO)<< "controlservice start BrkPedalActCtrRequest";
    // BrkPedalActCtr request
    TPCANMsg canBrkPedalActCtrReqMsg;
    try{
        T_BRK_PEDAL_ACT_CTR_REQ request;
        request.uiBrkSpdCtrReq = req.uiBrkSpdCtrReq;
        request.uiBrkAccCtrReq = req.uiBrkAccCtrReq;
        request.uiBrkCtrMdReq = req.uiBrkCtrMdReq;
        request.uiBrkDecCtrReq = req.uiBrkDecCtrReq;
        request.uiBrkFocCtrReq = req.uiBrkFocCtrReq;
        request.uiBrkPsngCtrReq = req.uiBrkPsngCtrReq;

        MessageCoder::CanMsgPackage((void*)&request, robot::common::BrkPedalActCtrReq, &canBrkPedalActCtrReqMsg);
        if (write_data(&canBrkPedalActCtrReqMsg)!=ROBOT_SUCCESS)
        {
            res.uiReturn = 0;  //= 0
            return true;
        }
        res.uiReturn = 1;
    }
    catch (ros::Exception ex)
    {
        res.uiReturn = 0;
        LOG(ERROR)<< "controlservice BrkPedalActCtrRequest failed "<< ex.what();
        return true;
    }
    return true;
}

bool Canbus::SftActCtrRequest(canbus::SftActCtrReq::Request &req,
                      canbus::SftActCtrReq::Response &res)
{
    LOG(INFO)<< "controlservice start SftActCtrRequest";
    // SftActCtr request
    TPCANMsg canSftActCtrReqMsg;
    try{
        T_SFT_ACT_CTR_REQ request;
        request.iClchSpdCtrReq = req.iClchSpdCtrReq;
        request.uiClchPsngCtrReq = req.uiClchPsngCtrReq;
        request.uiSftMdReq = req.uiSftMdReq;
        request.uiSftPsngCtrReq = req.uiSftPsngCtrReq;

        MessageCoder::CanMsgPackage((void*)&request, robot::common::ShiftActCtrReq, &canSftActCtrReqMsg);
        if (write_data(&canSftActCtrReqMsg) != ROBOT_SUCCESS)
        {
            res.uiReturn = 0; // =0
            return true;
        }
        res.uiReturn = 1;
    }
    catch (ros::Exception ex)
    {
        res.uiReturn = 0;
        LOG(ERROR)<< "controlservice SftActCtrRequest failed "<< ex.what();
        return true;
    }
    return true;
}

bool Canbus::StrActCtrRequest(canbus::StrActCtrReq::Request &req,
                      canbus::StrActCtrReq::Response &res)
{
    LOG(INFO)<< "controlservice start StrActCtrRequest";
    // StrActCtr request
    TPCANMsg canStrActCtrReqMsg;
    try{
        T_STR_ACT_CTR_REQ request;
        request.iStrAngCtrReq = req.iStrAngCtrReq;
        request.iStrSpdCtrReq = req.iStrSpdCtrReq;
        request.iStrTrqCtrReq = req.iStrTrqCtrReq;
        request.uiStrCtrMdReq = req.uiStrCtrMdReq;

        MessageCoder::CanMsgPackage((void*)&request, robot::common::SteerActCtrReq, &canStrActCtrReqMsg);
        if (write_data(&canStrActCtrReqMsg) !=ROBOT_SUCCESS)
        {
            res.uiReturn = 0; // =0
            return true;
        }
        res.uiReturn = 1;
    }
    catch (ros::Exception ex)
    {
        res.uiReturn = 0;
        LOG(ERROR)<< "controlservice StrActCtrRequest failed "<< ex.what();
        return true;
    }
    return true;
}


bool Canbus::RBTManager(canbus::RBTManager::Request &req,
                      canbus::RBTManager::Response &res)
{
    LOG(INFO)<< "RBTManager start ActuatorEnableRequest";
    TPCANMsg canRobotManagerMsg;
    try{
        T_RBT_MANAGER request;
        request.uiAccMotInt = req.uiAccMotInt;
        request.uiBrkMotInt = req.uiBrkMotInt;
        request.uiClchMotInt = req.uiClchMotInt;
        request.uiFltClr = req.uiFltClr;
        request.uiStrMotInt= req.uiStrMotInt;
        request.uiXSftMotInt = req.uiXSftMotInt;
        request.uiYSftMotInt = req.uiYSftMotInt;
        request.uiZSftMotInt =req.uiZSftMotInt;

        MessageCoder::CanMsgPackage((void*)&request, robot::common::RobotManager, &canRobotManagerMsg);
        if (write_data(&canRobotManagerMsg) != ROBOT_SUCCESS)
        {
            res.uiReturn = 0; // =0
            return true;
        }
        res.uiReturn = 1;
    }
    catch (ros::Exception ex)
    {
        res.uiReturn = 0;
        LOG(ERROR)<< "RBTManager ActuatorEnableRequest failed "<< ex.what();
        return true;
    }
    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.uiXSftCtrPowReq = req.uiXSftArmCtrPowReq;
        request.uiYSftCtrPowReq = req.uiYSftArmCtrPowReq;
        request.uiDriveFanReq = req.uiDriveFanReq;
        request.uiZSftArmCtrPowReq = req.uiZSftArmCtrPowReq;

        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.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.iSlopeRequest = 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
    try{
      if(req.uiStart == 0)
      {
        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;
      strLoopNum = 0;
      strIncrease = true;
      strAngle = strLpReq.iStrAngStart;
      tStrActCtrReq.iStrSpdCtrReq = strLpReq.uiStrSpdCtrReq;
      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)
    {
        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
  {
    // 发送当前角度
    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 = 1;
      tStrActCtrReq = {0};
      return;
    }
    if (strIncrease)
    {
      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();
  }
}


bool Canbus::AccLoopReq(canbus::AccLoopReq::Request &req,
                        canbus::AccLoopReq::Response &res)
{
  LOG(INFO)<< "controlservice start AccLoopReq";
  // AccLoopReq request
  try{
    if(req.uiStart == 0)
    {
      if(accTimerState)
      {
        acc_timer_.stop();
        accTimerState = false;
        accLoopNum = 0;
        tAccCtrReq = {0};
      }
      res.uiReturn = 1;
      return true;
    }
    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.uiAccSpdCtrReq = accLpReq.uiAccSpdCtrReq;
    tAccCtrReq.uiAccAccCtrReq = accLpReq.uiAccAccCtrReq;
    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;
}

void Canbus::AccLoopOnTimer(const ros::TimerEvent &event)
{
  LOG(INFO)<< Name()<< " AccLoopTimer start ..";

  TPCANMsg canAccCtrReqMsg;
  try
  {
    tAccCtrReq.uiAccPsngCtrReq = accPsng;
    MessageCoder::CanMsgPackage((void*)&tAccCtrReq, robot::common::AccPedalActCtrReq, &canAccCtrReqMsg);
    write_data(&canAccCtrReqMsg);
    if(accLpReq.uiAccPsngStart < accLpReq.uiAccPsngEnd)
    {
      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;
      }
    }
    else
    {
      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()<< " 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.uiBrkSpdCtrReq = brkLpReq.uiBrkSpdCtrReq;
    tBrkCtrReq.uiBrkCtrMdReq = brkLpReq.uiBrkCtrMdReq;
    tBrkCtrReq.uiBrkFocCtrReq = brkLpReq.uiBrkFocCtrReq;
    tBrkCtrReq.uiBrkAccCtrReq = brkLpReq.uiBrkAccCtrReq;
    tBrkCtrReq.uiBrkDecCtrReq = brkLpReq.uiBrkDecCtrReq;
    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::BrkPedalActCtrReq, &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();
  }

}

bool Canbus::StrTxtReq(canbus::StrTxtReq::Request &req,canbus::StrTxtReq::Response &res)
{
    LOG(INFO)<< "controlservice start StrTxtReq";
    // StrTxtReq request
    try{
      if(req.uiStrTxtFlag == 0)
      {
        if(strTxtTimerState)
        {
          str_txt_timer_.stop();
          strTxtTimerState = false;
          angles_count = 0;
        }
        res.uiReturn = 1;
        return true;
      }
      if(strTxtTimerState)
      {
        str_txt_timer_.stop();
        strTxtTimerState = false;
        angles_count = 0;
      }
      angles_count = 0;
      tStrActCtrReqTxt.iStrSpdCtrReq = req.uiStrSpdCtrReq;
      tStrActCtrReqTxt.iStrTrqCtrReq = req.iStrTrqCtrReq;
      tStrActCtrReqTxt.uiStrCtrMdReq = req.uiStrCtrMdReq;

      str_txt_timer_ = CreateTimer(ros::Duration(duration_txt), &Canbus::StrTxtOnTimer, this,false,false);
      str_txt_timer_.start();
      strTxtTimerState = true;
      res.uiReturn = 1;
    }
    catch (ros::Exception ex)
    {
        res.uiReturn = 0;
        LOG(ERROR)<< "controlservice StrTxtReq failed "<< ex.what();
        return true;
    }
    return true;
}

void Canbus::StrTxtOnTimer(const ros::TimerEvent &event)
{
  try
  {
    if(angles_count == angles_number)
    {
      str_txt_timer_.stop();
      strTxtTimerState = false;;
      angles_count = 0;
LOG(INFO)<<Name()<< " StrTxt END. ";
      return;
    }

    TPCANMsg canStrActCtrReqMsg;
    int angle = angles[angles_count] * 100;
    tStrActCtrReqTxt.iStrAngCtrReq = angle;
    //20220805zw 增加速度配套转速文件
    if (hasSpeedFile)
    {
      int angle_speed = angle_speeds[angles_count] * 100;
      tStrActCtrReqTxt.iStrSpdCtrReq = angle_speed;
      LOG(INFO)<<Name()<< " angle:. " <<angle<<" angle_speed:"<<angle_speed;
    }
    else
      LOG(INFO)<<Name()<< " angle:. " <<angle;
    MessageCoder::CanMsgPackage((void*)&tStrActCtrReqTxt, robot::common::SteerActCtrReq, &canStrActCtrReqMsg);
    write_data(&canStrActCtrReqMsg,true);
    angles_count ++;
  }
  catch (ros::Exception ex)
  {
       LOG(ERROR)<<Name()<< " OnTimer error. "<< ex.what();
  }
}

bool Canbus::BrkTxtReq(canbus::BrkTxtReq::Request &req,canbus::BrkTxtReq::Response &res)
{
  LOG(INFO)<< "controlservice start BrkTxtReq";
  // BrkTxtReq request
  try{
    if(req.uiBrkTxtFlag == 0)
    {
      if(brkTxtTimerState)
      {
        brk_txt_timer_.stop();
        brkTxtTimerState = false;
        psng_count = 0;
      }
      res.uiReturn = 1;
      return true;
    }
    if(brkTxtTimerState)
    {
      brk_txt_timer_.stop();
      brkTxtTimerState = false;
      psng_count = 0;
    }
    psng_count = 0;
    tBrkCtrReqTxt.uiBrkAccCtrReq = req.uiBrkAccCtrReq;
    tBrkCtrReqTxt.uiBrkSpdCtrReq = req.uiBrkSpdCtrReq;
    tBrkCtrReqTxt.uiBrkDecCtrReq = req.uiBrkDecCtrReq;
    tBrkCtrReqTxt.uiBrkCtrMdReq = req.uiBrkCtrMdReq;
    tBrkCtrReqTxt.uiBrkFocCtrReq = req.uiBrkFocCtrReq;

    brk_txt_timer_ = CreateTimer(ros::Duration(duration_txt), &Canbus::BrkTxtOnTimer, this,false,false);
    brk_txt_timer_.start();
    brkTxtTimerState = true;
    res.uiReturn = 1;
  }
  catch (ros::Exception ex)
  {
      res.uiReturn = 0;
      LOG(ERROR)<< "controlservice StrTxtReq failed "<< ex.what();
      return true;
  }
  return true;
}

void Canbus::BrkTxtOnTimer(const ros::TimerEvent &event)
{
  try
  {
    if(psng_count == psng_number)
    {
      brk_txt_timer_.stop();
      brkTxtTimerState = false;;
      psng_count = 0;
LOG(INFO)<<Name()<< " BrkTxt END. ";
      return;
    }

    TPCANMsg canBrkCtrReqMsg;
    int psng = psngs[psng_count] * 10;

    tBrkCtrReqTxt.uiBrkPsngCtrReq = psng;
    LOG(INFO)<<Name()<< " psng:. " <<psng;
    MessageCoder::CanMsgPackage((void*)&tBrkCtrReqTxt, robot::common::BrkPedalActCtrReq, &canBrkCtrReqMsg);
    write_data(&canBrkCtrReqMsg,true);
    psng_count ++;
  }
  catch (ros::Exception ex)
  {
       LOG(ERROR)<<Name()<< " OnTimer error. "<< ex.what();
  }
}

bool Canbus::AccTxtReq(canbus::AccTxtReq::Request &req,canbus::AccTxtReq::Response &res)
{
  LOG(INFO)<< "controlservice start AccTxtReq";
  // AccTxtReq request
  try{
    if(req.uiAccTxtFlag == 0)
    {
      if(accTxtTimerState)
      {
        acc_txt_timer_.stop();
        accTxtTimerState = false;
        acc_psng_count = 0;
      }
      res.uiReturn = 1;
      return true;
    }
    if(accTxtTimerState)
    {
      acc_txt_timer_.stop();
      accTxtTimerState = false;
      acc_psng_count = 0;
    }
    acc_psng_count = 0;
    tAccCtrReqTxt.uiAccSpdCtrReq = req.uiAccSpdCtrReq;
    tAccCtrReqTxt.uiAccAccCtrReq = req.uiAccAccCtrReq;
    tAccCtrReqTxt.uiAccDecCtrReq = req.uiAccDecCtrReq;

    acc_txt_timer_ = CreateTimer(ros::Duration(duration_txt_acc), &Canbus::AccTxtOnTimer, this,false,false);
    acc_txt_timer_.start();
    accTxtTimerState = true;
    res.uiReturn = 1;
  }
  catch (ros::Exception ex)
  {
      res.uiReturn = 0;
      LOG(ERROR)<< "controlservice AccTxtReq failed "<< ex.what();
      return true;
  }
  return true;
}

void Canbus::AccTxtOnTimer(const ros::TimerEvent &event)
{
  try
  {
    if(acc_psng_count == acc_psng_number)
    {
      acc_txt_timer_.stop();
      accTxtTimerState = false;;
      acc_psng_count = 0;
      return;
    }

    TPCANMsg canAccCtrReqMsg;
    int psng = acc_psngs[acc_psng_count] * 10;

    tAccCtrReqTxt.uiAccPsngCtrReq = psng;

    LOG(INFO)<<Name()<< "acc psng:. " <<psng;
    MessageCoder::CanMsgPackage((void*)&tAccCtrReqTxt, robot::common::AccPedalActCtrReq, &canAccCtrReqMsg);
    write_data(&canAccCtrReqMsg,true);
    acc_psng_count ++;
  }
  catch (ros::Exception ex)
  {
       LOG(ERROR)<<Name()<< " OnTimer error. "<< ex.what();
  }
}

bool Canbus::StrTxtLineReq(canbus::StrTxtLineReq::Request &req,canbus::StrTxtLineReq::Response &res)
{
    LOG(INFO)<< "controlservice start StrTxtLineReq";
    // StrTxtLineReq request
    try{
      if(req.uiStrTxtLineFlag == 0)
      {
        if(strTxtLineTimerState)
        {
          str_txt_line_timer_.stop();
          strTxtLineTimerState = false;
          angles_line_count = 0;
        }
        res.uiReturn = 1;
        return true;
      }
      if(strTxtLineTimerState)
      {
        str_txt_line_timer_.stop();
        strTxtLineTimerState = false;
        angles_line_count = 0;
      }
      angles_line_count = 0;
      tStrActCtrReqTxtLine.iStrSpdCtrReq = req.uiStrSpdCtrReq;
      tStrActCtrReqTxtLine.iStrTrqCtrReq = req.iStrTrqCtrReq;
      tStrActCtrReqTxtLine.uiStrCtrMdReq = req.uiStrCtrMdReq;

      str_txt_line_timer_ = CreateTimer(ros::Duration(duration_txt), &Canbus::StrTxtLineOnTimer, this,false,false);
      str_txt_line_timer_.start();
      strTxtLineTimerState = true;
      res.uiReturn = 1;
    }
    catch (ros::Exception ex)
    {
        res.uiReturn = 0;
        LOG(ERROR)<< "controlservice StrTxtLineReq failed "<< ex.what();
        return true;
    }
    return true;
}

void Canbus::StrTxtLineOnTimer(const ros::TimerEvent &event)
{
  try
  {
    if(angles_line_count == angles_line_number)
    {
      str_txt_line_timer_.stop();
      strTxtLineTimerState = false;;
      angles_line_count = 0;
      return;
    }

    TPCANMsg canStrActCtrReqMsg;
    int angle = angles_line[angles_line_count] * 100;

    tStrActCtrReqTxtLine.iStrAngCtrReq = angle;
    LOG(INFO)<<Name()<< " angle:. " <<angle;
    MessageCoder::CanMsgPackage((void*)&tStrActCtrReqTxtLine, robot::common::SteerActCtrReq, &canStrActCtrReqMsg);
    write_data(&canStrActCtrReqMsg,true);
    angles_line_count ++;
  }
  catch (ros::Exception ex)
  {
       LOG(ERROR)<<Name()<< " OnTimer error. "<< ex.what();
  }
}

bool Canbus::SftTxtReq(canbus::SftTxtReq::Request &req,canbus::SftTxtReq::Response &res)
{
  LOG(INFO)<< "controlservice start SftTxtReq";
  // SftTxtReq request
  try{
    if(req.uiSftTxtFlag == 0)
    {
      if(sftTxtTimerState)
      {
        sft_txt_timer_.stop();
        sftTxtTimerState = false;
        sft_psng_count = 0;
      }
      res.uiReturn = 1;
      return true;
    }
    if(sftTxtTimerState)
    {
      sft_txt_timer_.stop();
      sftTxtTimerState = false;
      sft_psng_count = 0;
    }
    sft_psng_count = 0;
    tSftCtrReqTxt.iClchSpdCtrReq = req.iClchSpdCtrReq;
    tSftCtrReqTxt.uiSftMdReq = req.uiSftMdReq;
    tSftCtrReqTxt.uiClchPsngCtrReq = req.uiClchPsngCtrReq;

    sft_txt_timer_ = CreateTimer(ros::Duration(duration_txt), &Canbus::SftTxtOnTimer, this,false,false);
    sft_txt_timer_.start();
    sftTxtTimerState = true;
    res.uiReturn = 1;
  }
  catch (ros::Exception ex)
  {
      res.uiReturn = 0;
      LOG(ERROR)<< "controlservice SftTxtReq failed "<< ex.what();
      return true;
  }
  return true;
}

void Canbus::SftTxtOnTimer(const ros::TimerEvent &event)
{
  try
  {
    if(sft_psng_count == sft_psng_number)
    {
      sft_txt_timer_.stop();
      sftTxtTimerState = false;;
      sft_psng_count = 0;
      return;
    }

    TPCANMsg canSftCtrReqMsg;
    int psng = sft_psngs[sft_psng_count];

    tSftCtrReqTxt.uiSftPsngCtrReq = psng;

    LOG(INFO)<<Name()<< "sft psng:. " <<psng;
    MessageCoder::CanMsgPackage((void*)&tSftCtrReqTxt, robot::common::ShiftActCtrReq, &canSftCtrReqMsg);
    write_data(&canSftCtrReqMsg,true);
    sft_psng_count ++;
  }
  catch (ros::Exception ex)
  {
       LOG(ERROR)<<Name()<< " OnTimer error. "<< ex.what();
  }
}

int Canbus::read_txt(string &path,int flag)
{
  //ifstream myfile("/home/madesheng/data.txt");
  ifstream myfile(path);
  if (!myfile.is_open())
  {
      LOG(ERROR)<<Name()<< "open src File  Error opening file" << endl;
      return ROBOT_FAILTURE;
  }
  //20220805zw 增加读取角度文件
  ifstream speedfile("/home/siasun/speed.txt");
  hasSpeedFile = speedfile.is_open();
  //-------------------
  if (flag == 1)
  {
    for (int i = 0; i < 70000; i++)
    {
        if(myfile.eof())
        {
            angles_number = i;
            break;
        }
        myfile >> angles[i];
        speedfile >> angle_speeds[i];
        //myfile >> temp;
        //cout <<"char:"<< temp<< endl;
       //cout <<"angle:"<< angles[i] <<endl;
    }
    cout << "str NUMBER:" <<angles_number<<endl;
  }
  else if(flag ==2)
  {
    for (int i = 0; i < 50000; i++)
    {
        if(myfile.eof())
        {
            psng_number = i;
            break;
        }
        myfile >> psngs[i];

        //myfile >> temp;
        //cout <<"char:"<< temp<< endl;
      //  cout <<"psngs:"<< psngs[i] <<endl;
    }
    cout << "brk NUMBER:" <<psng_number<<endl;
  }
  else if(flag ==3)
  {
    for (int i = 0; i < 80000; i++)
    {
        if(myfile.eof())
        {
            acc_psng_number = i;
            break;
        }
        myfile >> acc_psngs[i];

      //    cout <<"psngs:"<< acc_psngs[i] <<endl;
    }
    cout << "acc NUMBER:" <<acc_psng_number<<endl;
  }
  else if(flag ==4)
  {
    for (int i = 0; i < 5000; i++)
    {
        if(myfile.eof())
        {
            angles_line_number  = i;
            break;
        }
        myfile >> angles_line[i];

      //    cout <<"psngs:"<< acc_psngs[i] <<endl;
    }
    cout << "angle line NUMBER:" <<angles_line_number<<endl;
  }
  else if(flag == 5)
  {
    for (int i = 0; i < 70000; i++)
    {
        if(myfile.eof())
        {
            sft_psng_number  = i;
            break;
        }
        myfile >> sft_psngs[i];

      //    cout <<"psngs:"<< acc_psngs[i] <<endl;
    }
    cout << "sft NUMBER:" <<sft_psng_number<<endl;
  }


   myfile.close();
   return ROBOT_SUCCESS;
}


bool Canbus::VehicleStatSetReq(canbus::VehicleStatSetReq::Request &req,
                           canbus::VehicleStatSetReq::Response &res)
{
    LOG(INFO)<< "controlservice start VehicleStatSetReq";

    try{
        obd.SftPsng = status1.uiSftPsng = req.uiSftPsng;
        obd.VehHBrkStat = status1.uiVehHBrkStat = req.uiVehHBrkStat;
        obd.VehSwStat = status1.uiVehSwStat = req.uiVehSwStat;
        obd.VehSpd = status2.uiVehSpd = req.uiVehSpd;

        res.uiReturn = 1;
    }
    catch (ros::Exception ex)
    {
        res.uiReturn = 0;
        LOG(ERROR)<< "controlservice VehicleStatSetReq failed "<< ex.what();
        return true;
    }
    return true;
}

bool Canbus::RbtDriverControl(canbus::RbtDriverCtrl::Request &req,
                              canbus::RbtDriverCtrl::Response &res)
{
  LOG(INFO)<< "controlservice start RbtDriverControl";

  TPCANMsg canRbtDriverCtrl;
  try{
      T_RBT_DRIVER_CTR request;
      request.uiAccMotCtrl = req.uiAccMotCtrl;
      request.uiBrkMotCtrl = req.uiBrkMotCtrl;
      request.uiClchMotCtrl = req.uiClchMotCtrl;
      request.uiMotMove = req.uiMotMove;
      request.uiStrMotCtrl= req.uiStrMotCtrl;
      request.uiXSftMotCtrl = req.uiXSftMotCtrl;
      request.uiYSftMotCtrl = req.uiYSftMotCtrl;

      MessageCoder::CanMsgPackage((void*)&request, robot::common::RobotDriverCtr, &canRbtDriverCtrl);

      if (write_data(&canRbtDriverCtrl) != ROBOT_SUCCESS)
      {
          res.uiReturn = 0; // =0
          return true;
      }
      res.uiReturn = 1;
  }
  catch (ros::Exception ex)
  {
      res.uiReturn = 0;
      LOG(ERROR)<< "controlservice RbtDriverControl failed "<< ex.what();
      return true;
  }
  return true;

}


bool Canbus::StrTangLineCtrReq(canbus::StrTangLineCtrReq::Request &req,
                              canbus::StrTangLineCtrReq::Response &res)
{
  LOG(INFO)<< "controlservice start StrTangLineCtrReq";

  TPCANMsg canStrTangLineCtrReq;
  try{
      T_STR_TRIANGLE_LINE_CTR request;
      request.iStrPstBsc = req.iStrPstBsc;
      request.iStrSpdk = req.iStrSpdk;
      request.iStrTA = req.iStrTA;
      request.uiStrLT = req.uiStrLT;
      request.uiStrNum= req.uiStrNum;
      request.uiStrTT = req.uiStrTT;

      MessageCoder::CanMsgPackage((void*)&request, robot::common::SteerTranLineCtr, &canStrTangLineCtrReq);

      if (write_data(&canStrTangLineCtrReq) != ROBOT_SUCCESS)
      {
          res.uiReturn = 0; // =0
          return true;
      }
      res.uiReturn = 1;
  }
  catch (ros::Exception ex)
  {
      res.uiReturn = 0;
      LOG(ERROR)<< "controlservice StrTangLineCtrReq failed "<< ex.what();
      return true;
  }
  return true;

}

bool Canbus::StrSinCtrReq(canbus::StrSinCtrReq::Request &req,
                          canbus::StrSinCtrReq::Response &res)
{
  LOG(INFO)<< "controlservice start StrSinCtrReq";

  TPCANMsg canStrSinCtrReq;
  try{
      T_STR_SIN_CTR request;
      request.iStrFi = req.iStrFi;
      request.uiStrA = req.uiStrA;
      request.uiStrDt = req.uiStrDt;
      request.uiStrFrtb = req.uiStrFrtb;
      request.uiStrFrtk= req.uiStrFrtk;
      request.uiStrT = req.uiStrT;

      MessageCoder::CanMsgPackage((void*)&request, robot::common::SteerSinCtr, &canStrSinCtrReq);

      if (write_data(&canStrSinCtrReq) != ROBOT_SUCCESS)
      {
          res.uiReturn = 0; // =0
          return true;
      }
      res.uiReturn = 1;
  }
  catch (ros::Exception ex)
  {
      res.uiReturn = 0;
      LOG(ERROR)<< "controlservice StrSinCtrReq failed "<< ex.what();
      return true;
  }
  return true;

}


bool Canbus::StrPosTimCtrReq(canbus::StrPosTimCtrReq::Request &req,
                             canbus::StrPosTimCtrReq::Response &res)
{
  LOG(INFO)<< "controlservice start StrPosTimCtrReq";

  TPCANMsg canStrPosTimCtrReq;
  try{
      T_STR_POSITION_TIME_CTR request;
      request.iStrAngReq = req.iStrAngReq;
      request.uiNum = req.uiNum;
      request.uiRelTime = req.uiRelTime;
      request.uiSequenceID = req.uiSequenceID;
      request.uiTotalNum= req.uiTotalNum;

      MessageCoder::CanMsgPackage((void*)&request, robot::common::SteerPositionTimeCtr, &canStrPosTimCtrReq);

      if (write_data(&canStrPosTimCtrReq) != ROBOT_SUCCESS)
      {
          res.uiReturn = 0; // =0
          return true;
      }
      res.uiReturn = 1;
  }
  catch (ros::Exception ex)
  {
      res.uiReturn = 0;
      LOG(ERROR)<< "controlservice StrPosTimCtrReq failed "<< ex.what();
      return true;
  }
  return true;

}


bool Canbus::EstpCtrl(canbus::EstpCtrl::Request &req,
                      canbus::EstpCtrl::Response &res)
{
  LOG(INFO)<< "controlservice start EstpCtrl";

  TPCANMsg canEstpCtrl;
  try{
      T_ESTP_CTRL request;
      request.uiEstpEnReq = req.uiEstpEnReq;
      request.uiPauseEnReq = req.uiPauseEnReq;
      request.uiPmpEnReq = req.uiPmpEnReq;

      MessageCoder::CanMsgPackage((void*)&request, robot::common::EstpCtrl, &canEstpCtrl);

      if (write_data(&canEstpCtrl) != ROBOT_SUCCESS)
      {
          res.uiReturn = 0; // =0
          return true;
      }
      res.uiReturn = 1;
  }
  catch (ros::Exception ex)
  {
      res.uiReturn = 0;
      LOG(ERROR)<< "controlservice EstpCtrl failed "<< ex.what();
      return true;
  }
  return true;

}

bool Canbus::RobotWorkMd(canbus::RobotWorkMd::Request &req,
                         canbus::RobotWorkMd::Response &res)
{
  LOG(INFO)<< "controlservice start RobotWorkMd";

  TPCANMsg canRobotWorkMd;
  try{
      T_WORKING_MODE_CONFING request;
      request.uiOnlineOffline = req.uiOnlineOffline;
      request.uiRbtAplySenro = req.uiRbtAplySenro;

      MessageCoder::CanMsgPackage((void*)&request, robot::common::RobotWorkMd, &canRobotWorkMd);

      if (write_data(&canRobotWorkMd) != ROBOT_SUCCESS)
      {
          res.uiReturn = 0; // =0
          return true;
      }

      workModeCnfg.uiOnlineOffline = req.uiOnlineOffline;
      res.uiReturn = 1;
  }
  catch (ros::Exception ex)
  {
      res.uiReturn = 0;
      LOG(ERROR)<< "controlservice EstpCtrl failed "<< ex.what();
      return true;
  }
  return true;

}

bool Canbus::SysCtrlMode(canbus::SysCtrlMode::Request &req,
                         canbus::SysCtrlMode::Response &res)
{
  LOG(INFO)<< "controlservice start SysCtrlMode";

  TPCANMsg canSysCtrlMode;
  try{
      T_CONTROL_MODE request;
      request.uiCtrMdReq = req.uiCtrMdReq;

      MessageCoder::CanMsgPackage((void*)&request, robot::common::SysCtrlMode, &canSysCtrlMode);

      if (write_data(&canSysCtrlMode) != ROBOT_SUCCESS)
      {
          res.uiReturn = 0; // =0
          return true;
      }
      res.uiReturn = 1;
  }
  catch (ros::Exception ex)
  {
      res.uiReturn = 0;
      LOG(ERROR)<< "controlservice SysCtrlMode failed "<< ex.what();
      return true;
  }
  return true;

}

bool Canbus::BrkTimCtrlStp(canbus::BrkTimCtrlStp::Request &req,
                         canbus::BrkTimCtrlStp::Response &res)
{
  LOG(INFO)<< "controlservice start BrkTimCtrlStp";

  TPCANMsg canBrkTimCtrlStp;
  try{
      T_BRK_FORCE_CTR_SETUP request;
      request.iCtrSpd = req.iCtrSpd;
      request.uiBrkCtrWay = req.uiBrkCtrWay;
      request.uiBrkTm = req.uiBrkTm;
      request.uiBscCtr = req.uiBscCtr;

      MessageCoder::CanMsgPackage((void*)&request, robot::common::BrkTimCtrlStp, &canBrkTimCtrlStp);

      if (write_data(&canBrkTimCtrlStp) != ROBOT_SUCCESS)
      {
          res.uiReturn = 0; // =0
          return true;
      }
      res.uiReturn = 1;
  }
  catch (ros::Exception ex)
  {
      res.uiReturn = 0;
      LOG(ERROR)<< "controlservice BrkTimCtrlStp failed "<< ex.what();
      return true;
  }
  return true;

}

bool Canbus::AccPosCtrlStp(canbus::AccPosCtrlStp::Request &req,
                         canbus::AccPosCtrlStp::Response &res)
{
  LOG(INFO)<< "controlservice start AccPosCtrlStp";

  TPCANMsg canAccPosCtrlStp;
  try{
      T_ACC_POSITION_CTR_SETUP request;
      request.iAccSpdP = req.iAccSpdP;
      request.uiAccPstBsc = req.uiAccPstBsc;
      request.uiAccPT = req.uiAccPT;

      MessageCoder::CanMsgPackage((void*)&request, robot::common::AccPositionCtrStp, &canAccPosCtrlStp);

      if (write_data(&canAccPosCtrlStp) != ROBOT_SUCCESS)
      {
          res.uiReturn = 0; // =0
          return true;
      }
      res.uiReturn = 1;
  }
  catch (ros::Exception ex)
  {
      res.uiReturn = 0;
      LOG(ERROR)<< "controlservice AccPosCtrlStp failed "<< ex.what();
      return true;
  }
  return true;

}


bool Canbus::StrAccCtrlReq(canbus::StrAccCtrlReq::Request &req,
                         canbus::StrAccCtrlReq::Response &res)
{
  LOG(INFO)<< "controlservice start StrAccCtrlReq";

  TPCANMsg canStrAccCtrlReq;
  try{
      T_STR_ACC_CTR_REQ request;
      request.uiStrAccCtrReq = req.uiStrAccCtrReq;
      request.uiStrDecCtrReq = req.uiStrDecCtrReq;

      MessageCoder::CanMsgPackage((void*)&request, robot::common::ActuatorAccelerateCtrReq, &canStrAccCtrlReq);

      if (write_data(&canStrAccCtrlReq) != ROBOT_SUCCESS)
      {
          res.uiReturn = 0; // =0
          return true;
      }
      res.uiReturn = 1;
  }
  catch (ros::Exception ex)
  {
      res.uiReturn = 0;
      LOG(ERROR)<< "controlservice StrAccCtrlReq failed "<< ex.what();
      return true;
  }
  return true;

}

bool Canbus::RbtSlfChckCtrlReq(canbus::RbtSlfChckCtrlReq::Request &req,
                         canbus::RbtSlfChckCtrlReq::Response &res)
{
  LOG(INFO)<< "controlservice start RbtSlfChckCtrlReq";

  TPCANMsg canRbtSlfChckCtrlReq;
  try{
      T_RBT_SLF_CHK_CTR_REQ request;
      request.uiRbtSlfChkReq = req.uiRbtSlfChkReq;

      MessageCoder::CanMsgPackage((void*)&request, robot::common::RobotSelfCheckCtrReq, &canRbtSlfChckCtrlReq);

      if (write_data(&canRbtSlfChckCtrlReq) != ROBOT_SUCCESS)
      {
          res.uiReturn = 0; // =0
          return true;
      }
      res.uiReturn = 1;
  }
  catch (ros::Exception ex)
  {
      res.uiReturn = 0;
      LOG(ERROR)<< "controlservice RbtSlfChckCtrlReq failed "<< ex.what();
      return true;
  }
  return true;

}

bool Canbus::IOMode(canbus::IOMode::Request &req,
                         canbus::IOMode::Response &res)
{
  LOG(INFO)<< "controlservice start IOMode";

  TPCANMsg canIOMode;
  try{
      T_IO_REQ request;
      request.IO_0Req = req.uiIO_0Req;
      request.IO_1Req = req.uiIO_1Req;

      MessageCoder::CanMsgPackage((void*)&request, robot::common::IOMode, &canIOMode);

      if (write_data(&canIOMode) != ROBOT_SUCCESS)
      {
          res.uiReturn = 0; // =0
          return true;
      }
      res.uiReturn = 1;
  }
  catch (ros::Exception ex)
  {
      res.uiReturn = 0;
      LOG(ERROR)<< "controlservice IOMode failed "<< ex.what();
      return true;
  }
  return true;

}





}  // namespace canbus
}  // namespace robot
