Merge branch 'master' of http://118.24.47.114/madesheng/self-driving-robot

src/perception/msg/aeMsg.msg

@@ -0,0 +1,35 @@
+uint32 AccErrag
+uint32 AccErrah
+uint32 AccErrai
+uint32 AccErry
+uint32 AccErrz
+uint32 AccErraa
+uint32 AccErrab
+uint32 AccErrac
+uint32 AccErrad
+uint32 AccErrae
+uint32 AccErraf
+uint32 AccErrq
+uint32 AccErrr
+uint32 AccErrs
+uint32 AccErrt
+uint32 AccErru
+uint32 AccErrv
+uint32 AccErrw
+uint32 AccErrx
+uint32 AccErri
+uint32 AccErrj
+uint32 AccErrk
+uint32 AccErrl
+uint32 AccErrm
+uint32 AccErrn
+uint32 AccErro
+uint32 AccErrp
+uint32 AccErra
+uint32 AccErrb
+uint32 AccErrc
+uint32 AccErrd
+uint32 AccErre
+uint32 AccErrf
+uint32 AccErrg
+uint32 AccErrh

src/perception/perception.cpp

@@ -29,6 +29,13 @@ int Perception::Start()
     try
     {
         ros::Subscriber sub = node_handle_->subscribe(robot::common::CanDataAnalysis, 1000, &Perception::CanTopicCallback, this);
+        //ros::Subscriber sub1 = node_handle_->subscribe(robot::common::AesData, 1000, &Perception::CanTopicCallbackTest, this);
+        bus_pub_6E5 = node_handle_->advertise<::perception::aesMsg>(robot::common::AesData, 2000);
+        bus_pub_6EE = node_handle_->advertise<::perception::slMsg>(robot::common::SlData, 2000);
+        bus_pub_6ED = node_handle_->advertise<::perception::si2Msg>(robot::common::Si2Data, 2000);
+
+        bus_pub_6D1 = node_handle_->advertise<::perception::seMsg>(robot::common::SeData, 2000);
+
         ros::spin();
     }
     catch(ros::Exception e)
@@ -36,14 +43,22 @@ int Perception::Start()
          LOG(INFO) << Name() << " Start exception is:" << e.what();
          return ROBOT_FAILTURE;
     }  
-     LOG(INFO) << Name() << " Start end.";
+
+
+
+    LOG(INFO) << Name() << " Start end.";
     return ROBOT_SUCCESS;
 }
 
+void Perception::CanTopicCallbackTest(const ::perception::aesMsg::ConstPtr& msg)
+{
+     LOG(INFO) << " Msg Test :" << "1:" << msg->AccMotEnStat << ",2:" << msg->AccClchCtrStat;
+}
+
 void Perception::Stop()
 {
    LOG(INFO) << Name() << " Stop start...";
-    LOG(INFO) << Name() << " Stop end.";
+   LOG(INFO) << Name() << " Stop end.";
 }
 
 
@@ -75,6 +90,31 @@ void Perception::CanTopicCallback(const canbus::canMsg::ConstPtr& msg)
         PublishState_6D1(msg);
     }
         break;
+    case robot::common::AccErr:
+    {
+        PublishState_6D2(msg);
+    }
+        break;
+    case robot::common::BrkErr:
+    {
+        PublishState_6D3(msg);
+    }
+        break;
+    case robot::common::XsftErr:
+    {
+        PublishState_6D4(msg);
+    }
+        break;
+    case robot::common::YsftErr:
+    {
+        PublishState_6D5(msg);
+    }
+        break;
+    case robot::common::StrErr:
+    {
+        PublishState_6D6(msg);
+    }
+        break;
     default:
         break;
     }
@@ -83,10 +123,16 @@ void Perception::CanTopicCallback(const canbus::canMsg::ConstPtr& msg)
 bool Perception::PublishState_6E5(const canbus::canMsg::ConstPtr &msg)
 {
     static robot::canbusSpace::T_STATE_OUT_6E5 s_state_6E5;
-    memset(&s_state_6E5, 0 ,sizeof(s_state_6E5));
+
+    int copySize;
+    if(msg->data.size() < sizeof(s_state_6E5))
+        copySize = msg->data.size();
+    else
+        copySize = sizeof(s_state_6E5);
 
     robot::canbusSpace::T_STATE_OUT_6E5 state_6E5;
-    memcpy(&state_6E5, msg->data.c_str(), sizeof(state_6E5));
+    memset(&state_6E5, 0 ,sizeof(state_6E5));
+    memcpy(&state_6E5, msg->data.c_str(), copySize);
 
     if(state_6E5.uiAccClchCtrStat != s_state_6E5.uiAccClchCtrStat || state_6E5.uiAccMotEnStat != s_state_6E5.uiAccMotEnStat ||
             state_6E5.uiBrkHVFFStat != s_state_6E5.uiBrkHVFFStat || state_6E5.uiBrkMotEnStat != s_state_6E5.uiBrkMotEnStat ||
@@ -94,32 +140,26 @@ bool Perception::PublishState_6E5(const canbus::canMsg::ConstPtr &msg)
             state_6E5.uiStrMotEnStat != s_state_6E5.uiStrMotEnStat || state_6E5.uiXSftMotEnStat != s_state_6E5.uiXSftMotEnStat ||
             state_6E5.uiYSftMotEnStat != s_state_6E5.uiYSftMotEnStat)
     {
-        try
+
+        memcpy(&s_state_6E5, msg->data.c_str(), copySize);
+
+        if(bus_pub_6E5)
         {
-            ros::Publisher publisher = node_handle_->advertise<::perception::aesMsg>(robot::common::AesData, 2000);
-            if(publisher)
-            {
-                ::perception::aesMsg msg;
-                msg.AccClchCtrStat = state_6E5.uiAccClchCtrStat;
-                msg.AccMotEnStat =  state_6E5.uiAccMotEnStat;
-                msg.BrkHVFFStat = state_6E5.uiBrkHVFFStat;
-                msg.BrkMotEnStat = state_6E5.uiBrkMotEnStat;
-                msg.ClchMotEnStat = state_6E5.uiClchMotEnStat;
-                msg.PauseEnStat = state_6E5.uiPauseEnStat;
-                msg.StrMotEnStat = state_6E5.uiStrMotEnStat;
-                msg.XSftMotEnStat = state_6E5.uiXSftMotEnStat;
-                msg.YSftMotEnStat = state_6E5.uiYSftMotEnStat;
-                publisher.publish(msg);
-            }
-            else
-            {
-                LOG(INFO) << Name() << " PublishState_6E5 publisher is unvalid";
-                return false;
-            }
+            ::perception::aesMsg msg;
+            msg.AccClchCtrStat = state_6E5.uiAccClchCtrStat;
+            msg.AccMotEnStat =  state_6E5.uiAccMotEnStat;
+            msg.BrkHVFFStat = state_6E5.uiBrkHVFFStat;
+            msg.BrkMotEnStat = state_6E5.uiBrkMotEnStat;
+            msg.ClchMotEnStat = state_6E5.uiClchMotEnStat;
+            msg.PauseEnStat = state_6E5.uiPauseEnStat;
+            msg.StrMotEnStat = state_6E5.uiStrMotEnStat;
+            msg.XSftMotEnStat = state_6E5.uiXSftMotEnStat;
+            msg.YSftMotEnStat = state_6E5.uiYSftMotEnStat;
+            bus_pub_6E5.publish(msg);
         }
-        catch(ros::Exception e)
+        else
         {
-            LOG(INFO) << Name() << " PublishState_6E5 exception is:" << e.what() ;
+            LOG(INFO) << Name() << " PublishState_6E5 publisher is unvalid";
             return false;
         }
         return true;
@@ -134,10 +174,16 @@ bool Perception::PublishState_6E5(const canbus::canMsg::ConstPtr &msg)
 bool Perception::PublishState_6EE(const canbus::canMsg::ConstPtr &msg)
 {
     static robot::canbusSpace::T_STATE_OUT_6EE s_state_6EE;
-    memset(&s_state_6EE, 0 ,sizeof(s_state_6EE));
+
+    int copySize;
+    if(msg->data.size() < sizeof(s_state_6EE))
+        copySize = msg->data.size();
+    else
+        copySize = sizeof(s_state_6EE);
 
     robot::canbusSpace::T_STATE_OUT_6EE state_6EE;
-    memcpy(&state_6EE, msg->data.c_str(), sizeof(state_6EE));
+    memset(&state_6EE, 0 ,sizeof(state_6EE));
+    memcpy(&state_6EE, msg->data.c_str(), copySize);
 
     if(state_6EE.uiAcc0LrnStat != s_state_6EE.uiAcc0LrnStat || state_6EE.uiAccLrnStat != s_state_6EE.uiAccLrnStat ||
             state_6EE.uiAccMaxLrnStat != s_state_6EE.uiAccMaxLrnStat || state_6EE.uiBrk0LrnStat != s_state_6EE.uiBrk0LrnStat ||
@@ -151,44 +197,38 @@ bool Perception::PublishState_6EE(const canbus::canMsg::ConstPtr &msg)
             state_6EE.uiStrLrnStat != s_state_6EE.uiStrLrnStat || state_6EE.uiStrRLrnStat != s_state_6EE.uiStrRLrnStat ||
             state_6EE.uiUpLrnStat != s_state_6EE.uiUpLrnStat)
     {
-        try
+
+        memcpy(&s_state_6EE, msg->data.c_str(), copySize);
+
+        if(bus_pub_6EE)
         {
-            ros::Publisher publisher = node_handle_->advertise<::perception::aesMsg>(robot::common::SlData, 2000);
-            if(publisher)
-            {
-                ::perception::slMsg msg;
-                msg.Acc0LrnStat = state_6EE.uiAcc0LrnStat;
-                msg.AccLrnStat = state_6EE.uiAccLrnStat;
-                msg.AccMaxLrnStat = state_6EE.uiAccMaxLrnStat;
-                msg.Brk0LrnStat = state_6EE.uiBrk0LrnStat;
-                msg.BrkLrnStat = state_6EE.uiBrkLrnStat;
-                msg.BrkMaxLrnStat = state_6EE.uiBrkMaxLrnStat;
-                msg.Clch0LrnStat = state_6EE.uiClch0LrnStat;
-                msg.ClchLrnStat = state_6EE.uiClchLrnStat;
-                msg.ClchMaxLrnStat = state_6EE.uiClchMaxLrnStat;
-                msg.DLrnStat = state_6EE.uiDLrnStat;
-                msg.DpLrnStat = state_6EE.uiDpLrnStat;
-                msg.MLrnStat = state_6EE.uiMLrnStat;
-                msg.NLrnStat = state_6EE.uiNLrnStat;
-                msg.PLrnStat = state_6EE.uiPLrnStat;
-                msg.RLrnStat = state_6EE.uiRLrnStat;
-                msg.SftLrnStat = state_6EE.uiSftLrnStat;
-                msg.Str0LrnStat = state_6EE.uiStr0LrnStat;
-                msg.StrLLrnStat = state_6EE.uiStrLLrnStat;
-                msg.StrLrnStat = state_6EE.uiStrLrnStat;
-                msg.StrRLrnStat = state_6EE.uiStrRLrnStat;
-                msg.UpLrnStat = state_6EE.uiPLrnStat;
-                publisher.publish(msg);
-            }
-            else
-            {
-                LOG(INFO) << Name() << " PublishState_6EE publisher is unvalid";
-                return false;
-            }
+            ::perception::slMsg msg;
+            msg.Acc0LrnStat = state_6EE.uiAcc0LrnStat;
+            msg.AccLrnStat = state_6EE.uiAccLrnStat;
+            msg.AccMaxLrnStat = state_6EE.uiAccMaxLrnStat;
+            msg.Brk0LrnStat = state_6EE.uiBrk0LrnStat;
+            msg.BrkLrnStat = state_6EE.uiBrkLrnStat;
+            msg.BrkMaxLrnStat = state_6EE.uiBrkMaxLrnStat;
+            msg.Clch0LrnStat = state_6EE.uiClch0LrnStat;
+            msg.ClchLrnStat = state_6EE.uiClchLrnStat;
+            msg.ClchMaxLrnStat = state_6EE.uiClchMaxLrnStat;
+            msg.DLrnStat = state_6EE.uiDLrnStat;
+            msg.DpLrnStat = state_6EE.uiDpLrnStat;
+            msg.MLrnStat = state_6EE.uiMLrnStat;
+            msg.NLrnStat = state_6EE.uiNLrnStat;
+            msg.PLrnStat = state_6EE.uiPLrnStat;
+            msg.RLrnStat = state_6EE.uiRLrnStat;
+            msg.SftLrnStat = state_6EE.uiSftLrnStat;
+            msg.Str0LrnStat = state_6EE.uiStr0LrnStat;
+            msg.StrLLrnStat = state_6EE.uiStrLLrnStat;
+            msg.StrLrnStat = state_6EE.uiStrLrnStat;
+            msg.StrRLrnStat = state_6EE.uiStrRLrnStat;
+            msg.UpLrnStat = state_6EE.uiPLrnStat;
+            bus_pub_6EE.publish(msg);
         }
-        catch(ros::Exception e)
+        else
         {
-            LOG(INFO) << Name() << " PublishState_6EE exception is:" << e.what() ;
+            LOG(INFO) << Name() << " PublishState_6EE publisher is unvalid";
             return false;
         }
         return true;
@@ -203,10 +243,16 @@ bool Perception::PublishState_6EE(const canbus::canMsg::ConstPtr &msg)
 bool Perception::PublishState_6ED(const canbus::canMsg::ConstPtr &msg)
 {
     static robot::canbusSpace::T_STATE_OUT_6ED s_state_6ED;
-    memset(&s_state_6ED, 0 ,sizeof(s_state_6ED));
+
+    int copySize;
+    if(msg->data.size() < sizeof(s_state_6ED))
+        copySize = msg->data.size();
+    else
+        copySize = sizeof(s_state_6ED);
 
     robot::canbusSpace::T_STATE_OUT_6ED state_6ED;
-    memcpy(&state_6ED, msg->data.c_str(), sizeof(state_6ED));
+    memset(&state_6ED, 0 ,sizeof(state_6ED));
+    memcpy(&state_6ED, msg->data.c_str(), copySize);
 
     if(state_6ED.uiAccCtrAlive != s_state_6ED.uiAccCtrAlive || state_6ED.uiAccMotClbr != s_state_6ED.uiAccMotClbr ||
             state_6ED.uiBrkCtrAlive != s_state_6ED.uiBrkCtrAlive || state_6ED.uiBrkMotClbr != s_state_6ED.uiBrkMotClbr ||
@@ -216,38 +262,31 @@ bool Perception::PublishState_6ED(const canbus::canMsg::ConstPtr &msg)
             state_6ED.uiXSftArmCtrAlive != s_state_6ED.uiXSftArmCtrAlive || state_6ED.uiXSftMotClbr != s_state_6ED.uiXSftMotClbr ||
             state_6ED.uiYSftArmCtrAlive != s_state_6ED.uiYSftArmCtrAlive || state_6ED.uiYSftMotClbr != s_state_6ED.uiYSftMotClbr)
     {
-        try
+
+        memcpy(&s_state_6ED, msg->data.c_str(), copySize);
+
+        if(bus_pub_6ED)
         {
-            ros::Publisher publisher = node_handle_->advertise<::perception::si2Msg>(robot::common::Si2Data, 2000);
-            if(publisher)
-            {
-                ::perception::si2Msg msg;
-
-                msg.AccCtrAlive = state_6ED.uiAccCtrAlive;
-                msg.AccMotClbr = state_6ED.uiAccMotClbr;
-                msg.BrkCtrAlive = state_6ED.uiBrkCtrAlive;
-                msg.BrkMotClbr = state_6ED.uiBrkMotClbr;
-                msg.ClchCtrAlive = state_6ED.uiClchCtrAlive;
-                msg.ClchMotClbr = state_6ED.uiClchMotClbr;
-                msg.CtrMdStat = state_6ED.uiCtrMdStat;
-                msg.RCUAlive = state_6ED.uiRCUAlive;
-                msg.StrCtrAlive = state_6ED.uiStrCtrAlive;
-                msg.StrMotClbr = state_6ED.uiStrMotClbr;
-                msg.XSftArmCtrAlive = state_6ED.uiXSftArmCtrAlive;
-                msg.XSftMotClbr = state_6ED.uiXSftMotClbr;
-                msg.YSftArmCtrAlive = state_6ED.uiYSftArmCtrAlive;
-                msg.YSftMotClbr = state_6ED.uiYSftMotClbr;
-                publisher.publish(msg);
-            }
-            else
-            {
-                LOG(INFO) << Name() << " PublishState_6ED publisher is unvalid";
-                return false;
-            }
+            ::perception::si2Msg msg;
+            msg.AccCtrAlive = state_6ED.uiAccCtrAlive;
+            msg.AccMotClbr = state_6ED.uiAccMotClbr;
+            msg.BrkCtrAlive = state_6ED.uiBrkCtrAlive;
+            msg.BrkMotClbr = state_6ED.uiBrkMotClbr;
+            msg.ClchCtrAlive = state_6ED.uiClchCtrAlive;
+            msg.ClchMotClbr = state_6ED.uiClchMotClbr;
+            msg.CtrMdStat = state_6ED.uiCtrMdStat;
+            msg.RCUAlive = state_6ED.uiRCUAlive;
+            msg.StrCtrAlive = state_6ED.uiStrCtrAlive;
+            msg.StrMotClbr = state_6ED.uiStrMotClbr;
+            msg.XSftArmCtrAlive = state_6ED.uiXSftArmCtrAlive;
+            msg.XSftMotClbr = state_6ED.uiXSftMotClbr;
+            msg.YSftArmCtrAlive = state_6ED.uiYSftArmCtrAlive;
+            msg.YSftMotClbr = state_6ED.uiYSftMotClbr;
+            bus_pub_6ED.publish(msg);
         }
-        catch(ros::Exception e)
+        else
         {
-            LOG(INFO) << Name() << " PublishState_6ED exception is:" << e.what() ;
+            LOG(INFO) << Name() << " PublishState_6ED publisher is unvalid";
             return false;
         }
         return true;
@@ -262,10 +301,16 @@ bool Perception::PublishState_6ED(const canbus::canMsg::ConstPtr &msg)
 bool Perception::PublishState_6D1(const canbus::canMsg::ConstPtr &msg)
 {
     static robot::canbusSpace::T_STATE_OUT_6D1 s_state_6D1;
-    memset(&s_state_6D1, 0 ,sizeof(s_state_6D1));
+
+    int copySize;
+    if(msg->data.size() < sizeof(s_state_6D1))
+        copySize = msg->data.size();
+    else
+        copySize = sizeof(s_state_6D1);
 
     robot::canbusSpace::T_STATE_OUT_6D1 state_6D1;
-    memcpy(&state_6D1, msg->data.c_str(), sizeof(state_6D1));
+    memset(&state_6D1, 0 ,sizeof(state_6D1));
+    memcpy(&state_6D1, msg->data.c_str(), copySize);
 
     if(state_6D1.uiSysErra != s_state_6D1.uiSysErra || state_6D1.uiSysErrb != s_state_6D1.uiSysErrb || state_6D1.uiSysErrc != s_state_6D1.uiSysErrc ||
             state_6D1.uiSysErrd != s_state_6D1.uiSysErrd || state_6D1.uiSysErre != s_state_6D1.uiSysErre || state_6D1.uiSysErrf != s_state_6D1.uiSysErrf ||
@@ -277,49 +322,43 @@ bool Perception::PublishState_6D1(const canbus::canMsg::ConstPtr &msg)
             state_6D1.uiSysErrv != s_state_6D1.uiSysErrv || state_6D1.uiSysErrw != s_state_6D1.uiSysErrw || state_6D1.uiSysErrx != s_state_6D1.uiSysErrx ||
             state_6D1.uiSysErry != s_state_6D1.uiSysErry || state_6D1.uiSysErrz != s_state_6D1.uiSysErrz)
     {
-        try
+
+        memcpy(&s_state_6D1, msg->data.c_str(), copySize);
+
+        if(bus_pub_6D1)
         {
-            ros::Publisher publisher = node_handle_->advertise<::perception::seMsg>(robot::common::SeData, 2000);
-            if(publisher)
-            {
-                ::perception::seMsg msg;
-                msg.SysErra = state_6D1.uiSysErra;
-                msg.SysErrb = state_6D1.uiSysErrb;
-                msg.SysErrc = state_6D1.uiSysErrc;
-                msg.SysErrd = state_6D1.uiSysErrd;
-                msg.SysErre = state_6D1.uiSysErre;
-                msg.SysErrf = state_6D1.uiSysErrf;
-                msg.SysErrg = state_6D1.uiSysErrg;
-                msg.SysErrh = state_6D1.uiSysErrh;
-                msg.SysErri = state_6D1.uiSysErri;
-                msg.SysErrj = state_6D1.uiSysErrj;
-                msg.SysErrk = state_6D1.uiSysErrk;
-                msg.SysErrl = state_6D1.uiSysErrl;
-                msg.SysErrm = state_6D1.uiSysErrm;
-                msg.SysErrn = state_6D1.uiSysErrn;
-                msg.SysErro = state_6D1.uiSysErro;
-                msg.SysErrp = state_6D1.uiSysErrp;
-                msg.SysErrq = state_6D1.uiSysErrq;
-                msg.SysErrr = state_6D1.uiSysErrr;
-                msg.SysErrs = state_6D1.uiSysErrs;
-                msg.SysErrt = state_6D1.uiSysErrt;
-                msg.SysErru = state_6D1.uiSysErru;
-                msg.SysErrv = state_6D1.uiSysErrv;
-                msg.SysErrw = state_6D1.uiSysErrw;
-                msg.SysErrx = state_6D1.uiSysErrx;
-                msg.SysErry = state_6D1.uiSysErry;
-                msg.SysErrz = state_6D1.uiSysErrz;
-                publisher.publish(msg);
-            }
-            else
-            {
-                LOG(INFO) << Name() << " PublishState_6D1 publisher is unvalid";
-                return false;
-            }
+            ::perception::seMsg msg;
+            msg.SysErra = state_6D1.uiSysErra;
+            msg.SysErrb = state_6D1.uiSysErrb;
+            msg.SysErrc = state_6D1.uiSysErrc;
+            msg.SysErrd = state_6D1.uiSysErrd;
+            msg.SysErre = state_6D1.uiSysErre;
+            msg.SysErrf = state_6D1.uiSysErrf;
+            msg.SysErrg = state_6D1.uiSysErrg;
+            msg.SysErrh = state_6D1.uiSysErrh;
+            msg.SysErri = state_6D1.uiSysErri;
+            msg.SysErrj = state_6D1.uiSysErrj;
+            msg.SysErrk = state_6D1.uiSysErrk;
+            msg.SysErrl = state_6D1.uiSysErrl;
+            msg.SysErrm = state_6D1.uiSysErrm;
+            msg.SysErrn = state_6D1.uiSysErrn;
+            msg.SysErro = state_6D1.uiSysErro;
+            msg.SysErrp = state_6D1.uiSysErrp;
+            msg.SysErrq = state_6D1.uiSysErrq;
+            msg.SysErrr = state_6D1.uiSysErrr;
+            msg.SysErrs = state_6D1.uiSysErrs;
+            msg.SysErrt = state_6D1.uiSysErrt;
+            msg.SysErru = state_6D1.uiSysErru;
+            msg.SysErrv = state_6D1.uiSysErrv;
+            msg.SysErrw = state_6D1.uiSysErrw;
+            msg.SysErrx = state_6D1.uiSysErrx;
+            msg.SysErry = state_6D1.uiSysErry;
+            msg.SysErrz = state_6D1.uiSysErrz;
+            bus_pub_6D1.publish(msg);
         }
-        catch(ros::Exception e)
+        else
         {
-            LOG(INFO) << Name() << " PublishState_6D1 exception is:" << e.what() ;
+            LOG(INFO) << Name() << " PublishState_6D1 publisher is unvalid";
             return false;
         }
         return true;
@@ -331,9 +370,25 @@ bool Perception::PublishState_6D1(const canbus::canMsg::ConstPtr &msg)
     }
 }
 
+bool Perception::PublishState_6D2(const canbus::canMsg::ConstPtr &msg)
+{
+}
+
+bool Perception::PublishState_6D3(const canbus::canMsg::ConstPtr &msg)
+{
+}
 
+bool Perception::PublishState_6D4(const canbus::canMsg::ConstPtr &msg)
+{
+}
 
+bool Perception::PublishState_6D5(const canbus::canMsg::ConstPtr &msg)
+{
+}
 
+bool Perception::PublishState_6D6(const canbus::canMsg::ConstPtr &msg)
+{
+}
 
 
     }  // namespace perception

src/perception/perception.h

@@ -5,7 +5,7 @@
 #include "ros/ros.h"
 #include "std_msgs/String.h"
 #include <canbus/canMsg.h>
-
+#include <perception/aesMsg.h>
 namespace robot {
 namespace perception {
 
@@ -67,9 +67,30 @@ public:
 
     bool PublishState_6D1(const canbus::canMsg::ConstPtr& msg);
 
+    bool PublishState_6D2(const canbus::canMsg::ConstPtr& msg);
+
+    bool PublishState_6D3(const canbus::canMsg::ConstPtr& msg);
+
+    bool PublishState_6D4(const canbus::canMsg::ConstPtr& msg);
+
+    bool PublishState_6D5(const canbus::canMsg::ConstPtr& msg);
+
+    bool PublishState_6D6(const canbus::canMsg::ConstPtr& msg);
+
+
+    void CanTopicCallbackTest(const ::perception::aesMsg::ConstPtr& msg);
 private:
 
-     //ros::Publisher busData_pub;
+     ros::Publisher bus_pub_6E5;
+     ros::Publisher bus_pub_6ED;
+     ros::Publisher bus_pub_6EE;
+
+     ros::Publisher bus_pub_6D1;
+     ros::Publisher bus_pub_6D2;
+     ros::Publisher bus_pub_6D3;
+     ros::Publisher bus_pub_6D4;
+     ros::Publisher bus_pub_6D5;
+     ros::Publisher bus_pub_6D6;
 };