提交测试出现的调用服务ｂｕｇ

src/canbus/canbus.cpp

@@ -134,6 +134,7 @@ void Canbus::OnTimer(const ros::TimerEvent &) {
 
   try
   {
+
       TPCANMsg canIcuMonitor; //ICUMonitor Can Message
       MessageCoder::CanMsgPackage((void*)&icuMonitor, robot::common::ICUMonitor, &canIcuMonitor);
       write_data(&canIcuMonitor);
@@ -347,7 +348,7 @@ bool Canbus::ActuatorEnableRequest(canbus::ActuatorEnableRequest::Request &req,
     {
         res.uiReturn = 0;
         LOG(ERROR)<< "controlservice ActuatorEnableRequest failed "<< ex.what();
-        return false;
+        return true;
     }
     return true;
 }
@@ -373,14 +374,18 @@ bool Canbus::ExcuteLearnModeRequest(canbus::LearnModeRequest::Request & req,
         request.uiSysLrnReq = req.uiSysLrnReq;
 
         MessageCoder::CanMsgPackage((void*)&request, robot::common::LearnModeReq, &canLearnModeReqMsg);
-        write_data(&canLearnModeReqMsg);
+        if (write_data(&canLearnModeReqMsg)!=ROBOT_SUCCESS)
+        {
+            res.uiReturn = 0;
+            return true;
+        }
         res.uiReturn = 1;
     }
     catch (ros::Exception ex)
     {
         res.uiReturn = 0;
         LOG(ERROR)<< "controlservice ExcuteLearnModeRequest failed "<< ex.what();
-        return false;
+        return true;
     }
     return true;
 }
@@ -403,14 +408,18 @@ bool Canbus::PedalActCtrRequest(canbus::PedalActCtrReq::Request &req,
         request.uiBrkPsngCtrReq = req.uiBrkPsngCtrReq;
 
         MessageCoder::CanMsgPackage((void*)&request, robot::common::PedalActCtrReq, &canPedalActCtrReqMsg);
-        write_data(&canPedalActCtrReqMsg);
+        if (write_data(&canPedalActCtrReqMsg) != ROBOT_SUCCESS)
+        {
+            res.uiReturn = 0;
+            return true;
+        }
         res.uiReturn = 1;
     }
     catch (ros::Exception ex)
     {
         res.uiReturn = 0;
         LOG(ERROR)<< "controlservice PedalActCtrRequest failed "<< ex.what();
-        return false;
+        return true;
     }
     return true;
 }
@@ -429,14 +438,18 @@ bool Canbus::SftActCtrRequest(canbus::SftActCtrReq::Request &req,
         request.uiSftPsngCtrReq = req.uiSftPsngCtrReq;
 
         MessageCoder::CanMsgPackage((void*)&request, robot::common::ShiftActCtrReq, &canSftActCtrReqMsg);
-        write_data(&canSftActCtrReqMsg);
+        if (write_data(&canSftActCtrReqMsg) != ROBOT_SUCCESS)
+        {
+            res.uiReturn = 0;
+            return true;
+        }
         res.uiReturn = 1;
     }
     catch (ros::Exception ex)
     {
         res.uiReturn = 0;
         LOG(ERROR)<< "controlservice SftActCtrRequest failed "<< ex.what();
-        return false;
+        return true;
     }
     return true;
 }
@@ -455,14 +468,18 @@ bool Canbus::StrActCtrRequest(canbus::StrActCtrReq::Request &req,
         request.uiStrCtrMdReq = req.uiStrCtrMdReq;
 
         MessageCoder::CanMsgPackage((void*)&request, robot::common::SteerActCtrReq, &canStrActCtrReqMsg);
-        write_data(&canStrActCtrReqMsg);
+        if (write_data(&canStrActCtrReqMsg) !=ROBOT_SUCCESS)
+        {
+            res.uiReturn = 0;
+            return true;
+        }
         res.uiReturn = 1;
     }
     catch (ros::Exception ex)
     {
         res.uiReturn = 0;
         LOG(ERROR)<< "controlservice StrActCtrRequest failed "<< ex.what();
-        return false;
+        return true;
     }
     return true;
 }
@@ -491,7 +508,7 @@ bool Canbus::RBTManager(canbus::RBTManager::Request &req,
     {
         res.uiReturn = 0;
         LOG(ERROR)<< "RBTManager ActuatorEnableRequest failed "<< ex.what();
-        return false;
+        return true;
     }
     return true;
 }

src/decision/decision.cpp

@@ -38,8 +38,10 @@ int Decision::Init(){
 
 int Decision::Start(){
     LOG(INFO)<<Name()<<" start() start...";
+    // under ros service server
     ros::ServiceServer CommandService = node_handle_->advertiseService(robot::common::CommandModeCmdService,
                                                                 &Decision::CommandRequestService, this);
+    // under ros subscriber
     ros::Subscriber GeometrySub = node_handle_->subscribe(robot::common::Geometry, 2000,
                                                           &Decision::RTKLocationCallback, this);
 
@@ -47,12 +49,20 @@ int Decision::Start(){
 
     ros::Subscriber RCUModeSub = node_handle_->subscribe(robot::common::Si2Data, 2000, &Decision::RCUModeCallback, this);
 
+    // under ros publish
     Rules_pub = node_handle_->advertise<decision::rulesMsg>(robot::common::RuleInfo, 200);
 
     Geometry_pub = node_handle_->advertise<decision::localizationMsg>(robot::common::GeometryPlan, 200);
 
     Control_pub = node_handle_->advertise<decision::controlMsg>(robot::common::ControlInfo, 200);
 
+    // under ros service of client
+    clientSftActCtrReq = node_handle_->serviceClient<canbus::PedalActCtrReq>(robot::common::ShiftActCtrService);
+
+    clientPedalActCtrReq = node_handle_->serviceClient<canbus::PedalActCtrReq>(robot::common::PedalActCtrService);
+
+    clientStrActCtrReq = node_handle_->serviceClient<canbus::PedalActCtrReq>(robot::common::SteerActCtrService);
+
     ros::AsyncSpinner spinner(6); // Use 6 threads
     spinner.start();
     ros::waitForShutdown();
@@ -66,23 +76,28 @@ void Decision::Stop(){
     return ;
 }
 
-
+//  执行自动驾驶模式和路径规划模式，自动驾驶模式踩刹车然后挂前进当；路径规划模式删除数据库记录
 bool Decision::CommandRequestService(decision::CommandModeCmd::Request &req,
                       decision::CommandModeCmd::Response &res)
 {
-    LOG(INFO)<<Name()<<"CommandRequest get command ID:."<< req.Cmd;
+    LOG(INFO)<<Name()<<"CommandRequestService get command ID:."<< req.Cmd;
     if (req.Cmd == CMD_AUTODRIE_START)
     {
+        LoadPlanRoute();
         double breakCtr = -5; // 踩刹车到50%
         ros::Rate loop_rate(1);
         while(ros::ok())
         {
             if (SpeedControl(breakCtr) == 1)
-            {
-                LoadPlanRoute();
+            {              
                 if (BreakCtrControl(SFT_PSNG_LRN_REQ_FORWARD_1) == 1) // 前进挡
                     break;
             }
+            else
+            {
+                break;
+                return false;
+            }
             ros::spinOnce();
             loop_rate.sleep();
         }
@@ -94,17 +109,20 @@ bool Decision::CommandRequestService(decision::CommandModeCmd::Request &req,
         DecisionDatabase::DeleteLocationRuleInfo(PlanRouteName.c_str());
     }
     res.CmdResult = iCommandCmd = req.Cmd;
+    LOG(INFO)<<Name()<<"CommandRequestService end."<< res.CmdResult;
     return true;
 }
 
+
+// 加载规划路径，同时把规划的路径轨迹和测试的用例发送给显示界面
 void Decision::LoadPlanRoute()
 {
     ilocalTotalNumbers = DecisionDatabase::countOfLocationRuleInfo();
     iRulesTotalNumbers = DecisionDatabase::countOfActionDirective();
-
+    LOG(INFO)<<Name()<<" LoadPlanRoute select ilocalTotalNumbers: "<<ilocalTotalNumbers<<"  iRulesTotalNumbers: "<<iRulesTotalNumbers;
+    std::vector<LOCATION_RULE_INFO> tempInfo = DecisionDatabase::QueryLocationRuleInfo(0, ilocalTotalNumbers);
     for (int i = 0; i< ilocalTotalNumbers; i++)
     {
-        std::vector<LOCATION_RULE_INFO> tempInfo = DecisionDatabase::QueryLocationRuleInfo(i, 1);
         decision::localizationMsg localMsg;
         localMsg.Locationid = tempInfo[i].LocationID;
         localMsg.Longitude = tempInfo[i].LocationLon;
@@ -113,20 +131,19 @@ void Decision::LoadPlanRoute()
         localMsg.LocationSpeed = tempInfo[i].LocationSpeed;
         Geometry_pub.publish(localMsg);
     }
-
+    std::vector<ACTION_DIRECTIVE> tempAction = DecisionDatabase::QueryActionDirective(0, iRulesTotalNumbers);
     for (int i = 0; i< iRulesTotalNumbers; i++)
     {
-        std::vector<ACTION_DIRECTIVE> tempInfo = DecisionDatabase::QueryActionDirective(i, 1);
         decision::rulesMsg ruleMsg;
-        ruleMsg.DirectiveID = tempInfo[i].DirectiveID;
-        ruleMsg.DirectiveInfo = tempInfo[i].DirectiveInfo;
-        ruleMsg.DirectiveResult = tempInfo[i].DirectiveResult;
+        ruleMsg.DirectiveID = tempAction[i].DirectiveID;
+        ruleMsg.DirectiveInfo = tempAction[i].DirectiveInfo;
+        ruleMsg.DirectiveResult = tempAction[i].DirectiveResult;
         Rules_pub.publish(ruleMsg);
     }
 
 }
 
-
+// 接收ＲＴＫ数据分为自动驾驶模式开始和结束和路径规划模式开始和结束
 void Decision::RTKLocationCallback(const localization::localMsg::ConstPtr& msg)
 {
      switch (iCommandCmd) {
@@ -159,13 +176,15 @@ void Decision::RCUModeCallback(const perception::si2Msg::ConstPtr& msg)
    LOG(INFO)<<Name()<< " car excute mode :"<<uiCarMode;
 }
 
+//自动生成测试用例
 void Decision::ExcutePlanRoute(const localization::localMsg::ConstPtr& msg)
 {
-    try
-    {
+    try {
         iLocalNowNumber ++;
-        if ((iLocalNowNumber % iPredictionPointsPerNumber)  == 0)
-        {          
+        LOG(INFO)<<Name()<<"ExcutePlanRoute iLocalNowNumber = "<< iLocalNowNumber;
+        LOG(INFO)<<Name()<<"Longitude:"<<std::setprecision(18)<< msg->Longitude<<"Latitude:"<<
+                std::setprecision(18)<<msg->Latitude;
+        if ((iLocalNowNumber % iPredictionPointsPerNumber)  == 0) {          
             iRulesNowNumber++;
             std::stringstream strRuleContent;
             if (iRulesNowNumber == 1)
@@ -186,16 +205,15 @@ void Decision::ExcutePlanRoute(const localization::localMsg::ConstPtr& msg)
            }
 
            int iRet = DecisionDatabase::InsertLocationRuleInfoAndActionDirective(PlanRouteName.c_str(), iRulesNowNumber,
-                            strRuleContent.str().c_str(), RULE_EXCUTE_RESULT_INIT, iLocalNowNumber, msg->Longitude, msg->Latitude,
-                            msg->Height, fCarNowSpeed);
+                            strRuleContent.str().c_str(), RULE_EXCUTE_RESULT_INIT, iLocalNowNumber, msg->Longitude,
+                            msg->Latitude, msg->Height, fCarNowSpeed);
            if (iRet < 0) {
                 iLocalNowNumber--;
                 iRulesNowNumber--;
            }
            fCarLastSpeed = fCarNowSpeed;
         }
-        else
-        {
+        else{
            DecisionDatabase::InsertLocationRuleInfo(PlanRouteName.c_str(),iLocalNowNumber, msg->Longitude,
                                              msg->Latitude, msg->Height, fCarNowSpeed);
         }
@@ -206,15 +224,13 @@ void Decision::ExcutePlanRoute(const localization::localMsg::ConstPtr& msg)
     }
 }
 
-
+// 自动生成测试用例停止
 void Decision::StopPlanRoute(const localization::localMsg::ConstPtr& msg)
 {
-     if ((iLocalNowNumber % iPredictionPointsPerNumber)  == 0)
-     {
+     if ((iLocalNowNumber % iPredictionPointsPerNumber)  == 0){
          return;
      }
-     else
-     {
+     else{
          iLocalNowNumber++;
          iRulesNowNumber++;
          std::stringstream strRuleContent;
@@ -240,7 +256,7 @@ void Decision::StopPlanRoute(const localization::localMsg::ConstPtr& msg)
      }
 }
 
-
+// 执行自动驾驶ＭＰＣ算法
 void Decision::ExcuteSelfDriving(const localization::localMsg::ConstPtr& msg)
 {
    // 判断当前RCU是否为自动驾驶模式
@@ -249,15 +265,14 @@ void Decision::ExcuteSelfDriving(const localization::localMsg::ConstPtr& msg)
     if (CarAutoDriveStop())
         return;
     int iPredictPoints;
-    if (ilocalTotalNumbers < (iLocalNowNumber + iPredictionPointsPerNumber))
-    {
+    if (ilocalTotalNumbers < (iLocalNowNumber + iPredictionPointsPerNumber)){
         iPredictPoints = ilocalTotalNumbers - iLocalNowNumber;
     }
-    else
-    {
+    else{
         iPredictPoints = iPredictionPointsPerNumber;
     }
-    std::vector<LOCATION_RULE_INFO> tempInfo = DecisionDatabase::QueryLocationRuleInfo(iLocalNowNumber, iPredictPoints);
+    std::vector<LOCATION_RULE_INFO> tempInfo;
+    tempInfo = DecisionDatabase::QueryLocationRuleInfo(iLocalNowNumber, iPredictPoints);
     std::vector<double> ptsy(iPredictPoints);
     std::vector<double> ptsx(iPredictPoints);
     for (int i= 0; i< iPredictPoints; i++)
@@ -275,17 +290,15 @@ void Decision::ExcuteSelfDriving(const localization::localMsg::ConstPtr& msg)
     iLocalNowNumber ++;
 }
 
+// 自动驾驶停止
 bool  Decision::CarAutoDriveStop()
 {
-    try
-    {
-        if (ilocalTotalNumbers = iLocalNowNumber)
-        {
+    try{
+        if (ilocalTotalNumbers = iLocalNowNumber){
             uiCarMode = CMD_AUTODRIE_END;
             double breakCtr = -5; // 踩刹车到50%
             ros::Rate loop_rate(1);
-            while(ros::ok())
-            {
+            while(ros::ok()){
                 if (SpeedControl(breakCtr) == 1)
                 {
                     if (BreakCtrControl(SFT_PSNG_LRN_REQ_PARK) == 1) // P挡
@@ -322,10 +335,8 @@ bool  Decision::CarAutoDriveStop()
 
 bool Decision::UpdataCarRuleResults()
 {
-    try
-    {
-        if (iLocalNowNumber >0 && (iLocalNowNumber % iPredictionPointsPerNumber == 0))
-        {
+    try{
+        if (iLocalNowNumber >0 && (iLocalNowNumber % iPredictionPointsPerNumber == 0)){
             decision::rulesMsg ruleMsg;
             if (std::abs(fCarNowSpeed - mpc.ref_speed) < 1) //低速（ <10km/h）：± 0.25km/h  高速（ >10km/h）：± 1km/h
             {
@@ -350,6 +361,7 @@ bool Decision::UpdataCarRuleResults()
     }
 }
 
+
 void Decision::MPC_Controller(std::vector<double> &ptsx, std::vector<double> &ptsy,
                               double &px, double &py,  double &psi, double &v)
 {
@@ -384,7 +396,7 @@ void Decision::MPC_Controller(std::vector<double> &ptsx, std::vector<double> &pt
 
 unsigned int Decision::SpeedControl(double &speed)
 {
-     ros::ServiceClient clientPedalActCtrReq = node_handle_->serviceClient<canbus::PedalActCtrReq>(robot::common::PedalActCtrService);
+
      canbus::PedalActCtrReq srv;
      if (mpc.a_prev >0){
          srv.request.uiAccPsngCtrReq =(unsigned int)(speed *10);
@@ -408,9 +420,11 @@ unsigned int Decision::SpeedControl(double &speed)
      }
     return srv.response.uiReturn;
 }
+
+
 unsigned int Decision::StreeControl(double &streer)
 {
-     ros::ServiceClient clientStrActCtrReq = node_handle_->serviceClient<canbus::PedalActCtrReq>(robot::common::SteerActCtrService);
+
      canbus::StrActCtrReq srv;
      srv.request.iStrAngCtrReq =(unsigned int)(streer *100);
      srv.request.uiStrCtrMdReq =1;
@@ -427,9 +441,9 @@ unsigned int Decision::StreeControl(double &streer)
      return srv.response.uiReturn;
 }
 
+
 unsigned int Decision::BreakCtrControl(unsigned int breakmode)
-{
-     ros::ServiceClient clientSftActCtrReq = node_handle_->serviceClient<canbus::PedalActCtrReq>(robot::common::ShiftActCtrService);
+{     
      canbus::SftActCtrReq srv;
      srv.request.uiSftPsngCtrReq = breakmode;
      srv.request.uiSftMdReq = SFT_MD_REQ_2_SECOND;

src/decision/decision.h

@@ -190,6 +190,12 @@ private:
 
     ros::Publisher Control_pub;
 
+    ros::ServiceClient clientPedalActCtrReq;
+
+    ros::ServiceClient clientStrActCtrReq;
+
+    ros::ServiceClient clientSftActCtrReq;
+
 };
 
 

src/decision/decisiondatabase.cpp

@@ -6,6 +6,12 @@ namespace decisionSpace {
 
 std::string DecisionDatabase::Name() { return "Database"; }
 
+std::string to_string_with_precision(double a_value,int n){
+    std::ostringstream out;
+    out<<std::setprecision(n)<<a_value;
+    return out.str();
+}
+
 int DecisionDatabase::InitDB(){
     LOG(INFO)<<Name()<<":Decision database init...";
 
@@ -212,10 +218,10 @@ int DecisionDatabase::InsertLocationRuleInfo(const char *ruleName,
     /********************向表中插入一条记录 start *****************************/
     std::string sRuleName=ruleName;
     std::string sLocationID=std::to_string(iLocationID);
-    std::string sLocationLon=std::to_string(dLocationLon);
-    std::string sLocationLat=std::to_string(dLocationLat);
-    std::string sLocationHeight=std::to_string(dLocationHeight);
-    std::string sLocationSpeed=std::to_string(fLocationSpeed);
+    std::string sLocationLon=to_string_with_precision(dLocationLon,15);
+    std::string sLocationLat=to_string_with_precision(dLocationLat,14);
+    std::string sLocationHeight=to_string_with_precision(dLocationHeight,5);
+    std::string sLocationSpeed=to_string_with_precision(fLocationSpeed, 5);
     std::string s_sql="INSERT INTO location_rule_info ( \
             rule_name,                                  \
             location_id,                                \
@@ -232,7 +238,7 @@ int DecisionDatabase::InsertLocationRuleInfo(const char *ruleName,
             "+sLocationHeight+",                        \
             "+sLocationSpeed+"                          \
         );";
-    LOG(INFO)<<s_sql;
+    //LOG(INFO)<<s_sql;
     const char* sql=s_sql.c_str();
     robot::common::Sqlite3helper::Instance().update(sqlite,sql);
     /********************向表中插入一条记录 end *******************************/
@@ -267,10 +273,10 @@ int DecisionDatabase::InsertLocationRuleInfoAndActionDirective(const char* ruleN
         /********************向表中插入一条记录 start *****************************/
         std::string sRuleName=ruleName;
         std::string sLocationID=std::to_string(iLocationID);
-        std::string sLocationLon=std::to_string(dLocationLon);
-        std::string sLocationLat=std::to_string(dLocationLat);
-        std::string sLocationHeight=std::to_string(dLocationHeight);
-        std::string sLocationSpeed=std::to_string(fLocationSpeed);
+        std::string sLocationLon=to_string_with_precision(dLocationLon,15);
+        std::string sLocationLat=to_string_with_precision(dLocationLat,14);
+        std::string sLocationHeight=to_string_with_precision(dLocationHeight,5);
+        std::string sLocationSpeed=to_string_with_precision(fLocationSpeed, 5);
         std::string s_sqlsqlLocation="INSERT INTO location_rule_info ( \
                 rule_name,                                  \
                 location_id,                                \
@@ -287,7 +293,7 @@ int DecisionDatabase::InsertLocationRuleInfoAndActionDirective(const char* ruleN
                 "+sLocationHeight+",                        \
                 "+sLocationSpeed+"                          \
             );";
-        LOG(INFO)<<s_sqlsqlLocation;
+        //LOG(INFO)<<s_sqlsqlLocation;
         const char* sqlLocation=s_sqlsqlLocation.c_str();
         robot::common::Sqlite3helper::Instance().update(sqlite, sqlLocation);
 
@@ -305,7 +311,7 @@ int DecisionDatabase::InsertLocationRuleInfoAndActionDirective(const char* ruleN
                       "+sDirectiveID+",                     \
                     \'"+sDirectiveInfo+"\',                 \
                       "+sDirectiveResult+");";
-        LOG(INFO)<<s_sql;
+        //LOG(INFO)<<s_sql;
         const char* sql=s_sql.c_str();
         robot::common::Sqlite3helper::Instance().update(sqlite,sql);
         /********************向表中插入一条记录 end *******************************/
@@ -332,7 +338,7 @@ int DecisionDatabase::DeleteActionDirective(const char *ruleName){
     /***************************打开数据库 end ******************************/
 
     std::string sRuleName= ruleName;
-    std::string s_sql="DELETE FROM action_directive WHERE rule_name=" + sRuleName;
+    std::string s_sql="DELETE FROM action_directive WHERE rule_name=\'" + sRuleName+"\'";
     LOG(INFO)<<s_sql;
     const char* sql=s_sql.c_str();
     int deleteResult = robot::common::Sqlite3helper::Instance().update(sqlite,sql);
@@ -357,7 +363,7 @@ int DecisionDatabase::DeleteActionDirective(int iDirectiveID){
 
     std::string sDirectiveID=std::to_string(iDirectiveID);
     std::string s_sql="DELETE FROM action_directive WHERE directive_id=" + sDirectiveID+";";
-    LOG(INFO)<<s_sql;
+    //LOG(INFO)<<s_sql;
     const char* sql=s_sql.c_str();
     int deleteResult = robot::common::Sqlite3helper::Instance().update(sqlite,sql);
     robot::common::Sqlite3helper::Instance().close(sqlite);
@@ -381,7 +387,7 @@ int DecisionDatabase::DeleteLocationMap(const char *mapID){
 
     std::string sMapID=mapID;
     std::string s_sql="DELETE FROM location_map WHERE map_id=" + sMapID;
-    LOG(INFO)<<s_sql;
+    //LOG(INFO)<<s_sql;
     const char* sql=s_sql.c_str();
     int deleteResult = robot::common::Sqlite3helper::Instance().update(sqlite,sql);
     robot::common::Sqlite3helper::Instance().close(sqlite);
@@ -404,8 +410,8 @@ int DecisionDatabase::DeleteLocationRuleInfo(const char *ruleName){
     /***************************打开数据库 end ******************************/
 
     std::string sRuleName=ruleName;
-    std::string s_sql="DELETE FROM location_rule_info WHERE rule_name=" + sRuleName;
-    LOG(INFO)<<s_sql;
+    std::string s_sql="DELETE FROM location_rule_info WHERE rule_name=\'" + sRuleName+"\'";
+    //LOG(INFO)<<s_sql;
     const char* sql=s_sql.c_str();
     int deleteResult = robot::common::Sqlite3helper::Instance().update(sqlite,sql);
     robot::common::Sqlite3helper::Instance().close(sqlite);
@@ -429,7 +435,7 @@ int DecisionDatabase::DeleteLocationRuleInfo(int iLocationID){
 
     std::string sLocationID=std::to_string(iLocationID);
     std::string s_sql="DELETE FROM location_rule_info WHERE location_id=" + sLocationID;
-    LOG(INFO)<<s_sql;
+    //LOG(INFO)<<s_sql;
     const char* sql=s_sql.c_str();
     int deleteResult = robot::common::Sqlite3helper::Instance().update(sqlite,sql);
     robot::common::Sqlite3helper::Instance().close(sqlite);
@@ -458,7 +464,7 @@ int DecisionDatabase::UpdateActionDirective(const char* ruleName,int iDirectiveI
                         directive_result=\'"+sNewDirectiveResult+"\'    \
                         WHERE rule_name=\'"+sRuleName+"\' AND           \
                         directive_id="+sDirectiveID+";";
-    LOG(INFO)<<s_sql;
+    //LOG(INFO)<<s_sql;
     const char* sql=s_sql.c_str();
     int updateResult = robot::common::Sqlite3helper::Instance().update(sqlite,sql);
     if(updateResult == 0){
@@ -505,7 +511,7 @@ int DecisionDatabase::UpdateLocationMap(float fNewLocationLon1,
             location_X_2="+sLocationX2+",       \
             location_Y_2="+sLocationY2+"        \
             WHERE map_id=\'"+sMapId+"\';";
-    LOG(INFO)<<s_sql;
+    //LOG(INFO)<<s_sql;
     const char* sql=s_sql.c_str();
     int updateResult = robot::common::Sqlite3helper::Instance().update(sqlite,sql);
     robot::common::Sqlite3helper::Instance().close(sqlite);
@@ -518,7 +524,7 @@ int DecisionDatabase::UpdateLocationMap(float fNewLocationLon1,
 }
 
 std::vector<ACTION_DIRECTIVE> DecisionDatabase::QueryActionDirective(int iStart, int iLength){
-    std::vector<ACTION_DIRECTIVE> actionDirectives(iLength);
+    std::vector<ACTION_DIRECTIVE> actionDirectives;
     /***************************打开数据库 start ****************************/
     sqlite3 * sqlite = robot::common::Sqlite3helper::Instance().open("robot.db");
     if(sqlite==nullptr){
@@ -529,7 +535,7 @@ std::vector<ACTION_DIRECTIVE> DecisionDatabase::QueryActionDirective(int iStart,
     std::string sStart=std::to_string(iStart);
     std::string sLength=std::to_string(iLength);
     std::string s_sql="SELECT * FROM action_directive limit "+sStart+","+sLength;
-    LOG(INFO)<<s_sql;
+    //LOG(INFO)<<s_sql;
     const char* sql=s_sql.c_str();
     robot::common::query_result_t* result=robot::common::Sqlite3helper::Instance().query(sqlite,sql);
     while(result->next()){
@@ -547,7 +553,7 @@ std::vector<ACTION_DIRECTIVE> DecisionDatabase::QueryActionDirective(int iStart,
 
 
 std::vector<LOCATION_RULE_INFO> DecisionDatabase::QueryLocationRuleInfo(int iStart, int iLength){
-    std::vector<LOCATION_RULE_INFO> locationRuleInfos(iLength);
+    std::vector<LOCATION_RULE_INFO> locationRuleInfos;
     /***************************打开数据库 start ****************************/
     sqlite3 * sqlite = robot::common::Sqlite3helper::Instance().open("robot.db");
     if(sqlite==nullptr){
@@ -585,7 +591,7 @@ int DecisionDatabase::countOfActionDirective(){
     }
     /***************************打开数据库 end ******************************/
     std::string s_sql="SELECT count(*) FROM action_directive";
-    LOG(INFO)<<s_sql;
+    //LOG(INFO)<<s_sql;
     const char* sql=s_sql.c_str();
     robot::common::query_result_t* result=robot::common::Sqlite3helper::Instance().query(sqlite,sql);
     while(result->next()){
@@ -602,7 +608,7 @@ int DecisionDatabase::countOfLocationMap(){
     }
     /***************************打开数据库 end ******************************/
     std::string s_sql="SELECT count(*) FROM location_map";
-    LOG(INFO)<<s_sql;
+    //LOG(INFO)<<s_sql;
     const char* sql=s_sql.c_str();
     robot::common::query_result_t* result=robot::common::Sqlite3helper::Instance().query(sqlite,sql);
     while(result->next()){
@@ -619,7 +625,7 @@ int DecisionDatabase::countOfLocationRuleInfo(){
     }
     /***************************打开数据库 end ******************************/
     std::string s_sql="SELECT count(*) FROM location_rule_info";
-    LOG(INFO)<<s_sql;
+    //LOG(INFO)<<s_sql;
     const char* sql=s_sql.c_str();
     robot::common::query_result_t* result=robot::common::Sqlite3helper::Instance().query(sqlite,sql);
     while(result->next()){

src/launch/robot.launch

@@ -7,8 +7,12 @@ respawn="true"当roslaunch启动完所有该启动的节点之后，会监测每
 required="true"当被此属性标记的节点终止时，roslaunch会将其他的节点一并终止。注意此属性不可以与respawn="true"一起描述同一个节点
 launch-prefix = "command-prefix"我的理解是，相当于在执行启动命令时加上一段命令前缀
 ns = "NAME_SPACE" 这个属性可以让你在自定义的命名空间里运行节点-->
+
 <node pkg="decision" name="decision" type="decision" output="screen" required="true"/>
 <node pkg="perception" name="perception" type="perception" output="screen" respawn="true"/>
 <node pkg="localization" name="localization" type="localization" output="screen" respawn="true"/>
 <node pkg="canbus" name="canbus" type="canbus" output="screen" respawn="true"/>
+<include file="$(find rosbridge_server)/launch/rosbridge_websocket.launch" >
+    <arg name="port" value="9090"/>
+ </include>
 </launch>

src/maintest/maintest.cpp

@@ -15,23 +15,32 @@ int Maintest::Init(){
 
 int Maintest::Start(){
     LOG(INFO)<<" mainTest start...";
+    ros::Rate loop_rate(10);
     ros::Subscriber sub = node_handle_->subscribe("canDataAnalysis", 20000, chatterCallback);
     ros::ServiceClient client = node_handle_->serviceClient<canbus::LearnModeRequest>(robot::common::LearnModeService);
-    canbus::LearnModeRequest srv;
-    srv.request.uiClrAccTbl = 1;
-    srv.request.uiClrBrkTbl = 1;
-    srv.request.uiClrClchTbl =1;
-    srv.request.uiClrSftTbl = 1;
-    srv.request.uiClrStrTbl = 1;
-    srv.request.uiPsngLrnReq =1;
-    srv.request.uiSftAutLrnFnsConf = 1;
-    srv.request.uiSftPsngLrnReq = 1;
-    srv.request.uiStpAutLrn =1;
-     srv.request.uiSysLrnReq =1;
-     if (client.call(srv))
-     {
-         LOG(INFO)<<srv.response.uiReturn;
+    //data receive
+    while(ros::ok())
+    {
+        canbus::LearnModeRequest srv;
+        srv.request.uiClrAccTbl = 1;
+        srv.request.uiClrBrkTbl = 1;
+        srv.request.uiClrClchTbl =1;
+        srv.request.uiClrSftTbl = 1;
+        srv.request.uiClrStrTbl = 1;
+        srv.request.uiPsngLrnReq =1;
+        srv.request.uiSftAutLrnFnsConf = 1;
+        srv.request.uiSftPsngLrnReq = 1;
+        srv.request.uiStpAutLrn =1;
+         srv.request.uiSysLrnReq =1;
+         if (client.call(srv))
+         {
+             LOG(INFO)<<srv.response.uiReturn;
+         }
+        ros::spinOnce();
+        loop_rate.sleep();
      }
+
+
     //ros::spin();
     return 0;
 }