|
@@ -6,6 +6,9 @@
|
|
#include <perception/slMsg.h>
|
|
#include <perception/slMsg.h>
|
|
#include <perception/seMsg.h>
|
|
#include <perception/seMsg.h>
|
|
#include <perception/si2Msg.h>
|
|
#include <perception/si2Msg.h>
|
|
|
|
+#include <perception/cecMsg.h>
|
|
|
|
+#include <perception/eiMsg.h>
|
|
|
|
+#include <perception/eeMsg.h>
|
|
|
|
|
|
namespace robot {
|
|
namespace robot {
|
|
namespace perception {
|
|
namespace perception {
|
|
@@ -29,13 +32,22 @@ int Perception::Start()
|
|
try
|
|
try
|
|
{
|
|
{
|
|
ros::Subscriber sub = node_handle_->subscribe(robot::common::CanDataAnalysis, 1000, &Perception::CanTopicCallback, this);
|
|
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_6E5 = node_handle_->advertise<::perception::aesMsg>(robot::common::AesData, 2000);
|
|
bus_pub_6EE = node_handle_->advertise<::perception::slMsg>(robot::common::SlData, 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_6ED = node_handle_->advertise<::perception::si2Msg>(robot::common::Si2Data, 2000);
|
|
|
|
|
|
bus_pub_6D1 = node_handle_->advertise<::perception::seMsg>(robot::common::SeData, 2000);
|
|
bus_pub_6D1 = node_handle_->advertise<::perception::seMsg>(robot::common::SeData, 2000);
|
|
|
|
|
|
|
|
+ bus_pub_6D2 = node_handle_->advertise<::perception::cecMsg>(robot::common::AcceData, 2000);
|
|
|
|
+ bus_pub_6D3 = node_handle_->advertise<::perception::cecMsg>(robot::common::BrkeData, 2000);
|
|
|
|
+ bus_pub_6D4 = node_handle_->advertise<::perception::cecMsg>(robot::common::XsfteData, 2000);
|
|
|
|
+ bus_pub_6D5 = node_handle_->advertise<::perception::cecMsg>(robot::common::YsfteData, 2000);
|
|
|
|
+ bus_pub_6D6 = node_handle_->advertise<::perception::cecMsg>(robot::common::StreData, 2000);
|
|
|
|
+
|
|
|
|
+ bus_pub_2E0 = node_handle_->advertise<::perception::eiMsg>(robot::common::EiData, 2000);
|
|
|
|
+ bus_pub_2E1 = node_handle_->advertise<::perception::eeMsg>(robot::common::EeData, 2000);
|
|
|
|
+
|
|
ros::spin();
|
|
ros::spin();
|
|
}
|
|
}
|
|
catch(ros::Exception e)
|
|
catch(ros::Exception e)
|
|
@@ -43,18 +55,10 @@ int Perception::Start()
|
|
LOG(INFO) << Name() << " Start exception is:" << e.what();
|
|
LOG(INFO) << Name() << " Start exception is:" << e.what();
|
|
return ROBOT_FAILTURE;
|
|
return ROBOT_FAILTURE;
|
|
}
|
|
}
|
|
-
|
|
|
|
-
|
|
|
|
-
|
|
|
|
LOG(INFO) << Name() << " Start end.";
|
|
LOG(INFO) << Name() << " Start end.";
|
|
return ROBOT_SUCCESS;
|
|
return ROBOT_SUCCESS;
|
|
}
|
|
}
|
|
|
|
|
|
-void Perception::CanTopicCallbackTest(const ::perception::aesMsg::ConstPtr& msg)
|
|
|
|
-{
|
|
|
|
- LOG(INFO) << " Msg Test :" << "1:" << msg->AccMotEnStat << ",2:" << msg->AccClchCtrStat;
|
|
|
|
-}
|
|
|
|
-
|
|
|
|
void Perception::Stop()
|
|
void Perception::Stop()
|
|
{
|
|
{
|
|
LOG(INFO) << Name() << " Stop start...";
|
|
LOG(INFO) << Name() << " Stop start...";
|
|
@@ -66,8 +70,6 @@ void Perception::CanTopicCallback(const canbus::canMsg::ConstPtr& msg)
|
|
{
|
|
{
|
|
LOG(INFO) << " Msg CanData :" << "ID:" << msg->id << ",Data:" << msg->data;
|
|
LOG(INFO) << " Msg CanData :" << "ID:" << msg->id << ",Data:" << msg->data;
|
|
|
|
|
|
- bool sendFlag = false;
|
|
|
|
-
|
|
|
|
switch (msg->id) {
|
|
switch (msg->id) {
|
|
|
|
|
|
case robot::common::ActuatorEnableStatus:
|
|
case robot::common::ActuatorEnableStatus:
|
|
@@ -92,27 +94,37 @@ void Perception::CanTopicCallback(const canbus::canMsg::ConstPtr& msg)
|
|
break;
|
|
break;
|
|
case robot::common::AccErr:
|
|
case robot::common::AccErr:
|
|
{
|
|
{
|
|
- PublishState_6D2(msg);
|
|
|
|
|
|
+ PublishState_6D2_6(msg, 2, bus_pub_6D2);
|
|
}
|
|
}
|
|
break;
|
|
break;
|
|
case robot::common::BrkErr:
|
|
case robot::common::BrkErr:
|
|
{
|
|
{
|
|
- PublishState_6D3(msg);
|
|
|
|
|
|
+ PublishState_6D2_6(msg, 3, bus_pub_6D3);
|
|
}
|
|
}
|
|
break;
|
|
break;
|
|
case robot::common::XsftErr:
|
|
case robot::common::XsftErr:
|
|
{
|
|
{
|
|
- PublishState_6D4(msg);
|
|
|
|
|
|
+ PublishState_6D2_6(msg, 4, bus_pub_6D4);
|
|
}
|
|
}
|
|
break;
|
|
break;
|
|
case robot::common::YsftErr:
|
|
case robot::common::YsftErr:
|
|
{
|
|
{
|
|
- PublishState_6D5(msg);
|
|
|
|
|
|
+ PublishState_6D2_6(msg, 5, bus_pub_6D5);
|
|
}
|
|
}
|
|
break;
|
|
break;
|
|
case robot::common::StrErr:
|
|
case robot::common::StrErr:
|
|
{
|
|
{
|
|
- PublishState_6D6(msg);
|
|
|
|
|
|
+ PublishState_6D2_6(msg, 6, bus_pub_6D6);
|
|
|
|
+ }
|
|
|
|
+ break;
|
|
|
|
+ case robot::common::EstopInfo:
|
|
|
|
+ {
|
|
|
|
+ PublishState_2E0(msg);
|
|
|
|
+ }
|
|
|
|
+ break;
|
|
|
|
+ case robot::common::EstopErr:
|
|
|
|
+ {
|
|
|
|
+ PublishState_2E1(msg);
|
|
}
|
|
}
|
|
break;
|
|
break;
|
|
default:
|
|
default:
|
|
@@ -142,7 +154,8 @@ bool Perception::PublishState_6E5(const canbus::canMsg::ConstPtr &msg)
|
|
{
|
|
{
|
|
|
|
|
|
memcpy(&s_state_6E5, msg->data.c_str(), copySize);
|
|
memcpy(&s_state_6E5, msg->data.c_str(), copySize);
|
|
-
|
|
|
|
|
|
+ if(msg->data.size() < sizeof(s_state_6E5))
|
|
|
|
+ memset((char*)&s_state_6E5 + msg->data.size(), 0, sizeof(s_state_6E5) - copySize);
|
|
if(bus_pub_6E5)
|
|
if(bus_pub_6E5)
|
|
{
|
|
{
|
|
::perception::aesMsg msg;
|
|
::perception::aesMsg msg;
|
|
@@ -199,7 +212,8 @@ bool Perception::PublishState_6EE(const canbus::canMsg::ConstPtr &msg)
|
|
{
|
|
{
|
|
|
|
|
|
memcpy(&s_state_6EE, msg->data.c_str(), copySize);
|
|
memcpy(&s_state_6EE, msg->data.c_str(), copySize);
|
|
-
|
|
|
|
|
|
+ if(msg->data.size() < sizeof(s_state_6EE))
|
|
|
|
+ memset((char*)&s_state_6EE + msg->data.size(), 0, sizeof(s_state_6EE) - copySize);
|
|
if(bus_pub_6EE)
|
|
if(bus_pub_6EE)
|
|
{
|
|
{
|
|
::perception::slMsg msg;
|
|
::perception::slMsg msg;
|
|
@@ -264,7 +278,8 @@ bool Perception::PublishState_6ED(const canbus::canMsg::ConstPtr &msg)
|
|
{
|
|
{
|
|
|
|
|
|
memcpy(&s_state_6ED, msg->data.c_str(), copySize);
|
|
memcpy(&s_state_6ED, msg->data.c_str(), copySize);
|
|
-
|
|
|
|
|
|
+ if(msg->data.size() < sizeof(s_state_6ED))
|
|
|
|
+ memset((char*)&s_state_6ED + msg->data.size(), 0, sizeof(s_state_6ED) - copySize);
|
|
if(bus_pub_6ED)
|
|
if(bus_pub_6ED)
|
|
{
|
|
{
|
|
::perception::si2Msg msg;
|
|
::perception::si2Msg msg;
|
|
@@ -324,7 +339,8 @@ bool Perception::PublishState_6D1(const canbus::canMsg::ConstPtr &msg)
|
|
{
|
|
{
|
|
|
|
|
|
memcpy(&s_state_6D1, msg->data.c_str(), copySize);
|
|
memcpy(&s_state_6D1, msg->data.c_str(), copySize);
|
|
-
|
|
|
|
|
|
+ if(msg->data.size() < sizeof(s_state_6D1))
|
|
|
|
+ memset((char*)&s_state_6D1 + msg->data.size(), 0, sizeof(s_state_6D1) - copySize);
|
|
if(bus_pub_6D1)
|
|
if(bus_pub_6D1)
|
|
{
|
|
{
|
|
::perception::seMsg msg;
|
|
::perception::seMsg msg;
|
|
@@ -370,26 +386,215 @@ bool Perception::PublishState_6D1(const canbus::canMsg::ConstPtr &msg)
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
-bool Perception::PublishState_6D2(const canbus::canMsg::ConstPtr &msg)
|
|
|
|
|
|
+bool Perception::PublishState_6D2_6(const canbus::canMsg::ConstPtr &msg, int type, const ros::Publisher &pub)
|
|
{
|
|
{
|
|
-}
|
|
|
|
|
|
+ static robot::canbusSpace::T_ACC_ERR s_state_6D2;
|
|
|
|
+ static robot::canbusSpace::T_BRK_ERR s_state_6D3;
|
|
|
|
+ static robot::canbusSpace::T_XSFT_ERR s_state_6D4;
|
|
|
|
+ static robot::canbusSpace::T_YSFT_ERR s_state_6D5;
|
|
|
|
+ static robot::canbusSpace::T_STR_ERR s_state_6D6;
|
|
|
|
+
|
|
|
|
+ robot::canbusSpace::T_ACC_ERR *pErr;
|
|
|
|
+ switch (type) {
|
|
|
|
+ case 2:
|
|
|
|
+ pErr = &s_state_6D2;
|
|
|
|
+ break;
|
|
|
|
+ case 3:
|
|
|
|
+ pErr = (robot::canbusSpace::T_ACC_ERR *)&s_state_6D3;
|
|
|
|
+ break;
|
|
|
|
+ case 4:
|
|
|
|
+ pErr = (robot::canbusSpace::T_ACC_ERR *)&s_state_6D4;
|
|
|
|
+ break;
|
|
|
|
+ case 5:
|
|
|
|
+ pErr = (robot::canbusSpace::T_ACC_ERR *)&s_state_6D5;
|
|
|
|
+ break;
|
|
|
|
+ case 6:
|
|
|
|
+ pErr = (robot::canbusSpace::T_ACC_ERR *)&s_state_6D6;
|
|
|
|
+ break;
|
|
|
|
+ default:
|
|
|
|
+ LOG(INFO) << Name() << " PublishState_6D2_6 " << type << " type is unvalid";
|
|
|
|
+ return false;
|
|
|
|
+ break;
|
|
|
|
+ }
|
|
|
|
|
|
-bool Perception::PublishState_6D3(const canbus::canMsg::ConstPtr &msg)
|
|
|
|
-{
|
|
|
|
-}
|
|
|
|
|
|
+ int copySize;
|
|
|
|
+ if(msg->data.size() < sizeof(s_state_6D2))
|
|
|
|
+ copySize = msg->data.size();
|
|
|
|
+ else
|
|
|
|
+ copySize = sizeof(s_state_6D2);
|
|
|
|
+
|
|
|
|
+ //6D2_2 6D2_3 6D2_4 6D2_5 6D2_6 数据结构相同
|
|
|
|
+ robot::canbusSpace::T_ACC_ERR state_6D2;
|
|
|
|
+ memset(&state_6D2, 0 ,sizeof(state_6D2));
|
|
|
|
+ memcpy(&state_6D2, msg->data.c_str(), copySize);
|
|
|
|
+
|
|
|
|
+ if(state_6D2.uiAccErra != pErr->uiAccErra || state_6D2.uiAccErraa != pErr->uiAccErraa || state_6D2.uiAccErrab != pErr->uiAccErrab ||
|
|
|
|
+ state_6D2.uiAccErrac != pErr->uiAccErrac || state_6D2.uiAccErrad != pErr->uiAccErrad || state_6D2.uiAccErrae != pErr->uiAccErrae ||
|
|
|
|
+ state_6D2.uiAccErraf != pErr->uiAccErraf || state_6D2.uiAccErrag != pErr->uiAccErrag || state_6D2.uiAccErrah != pErr->uiAccErrah ||
|
|
|
|
+ state_6D2.uiAccErrai != pErr->uiAccErrai || state_6D2.uiAccErrb != pErr->uiAccErrb || state_6D2.uiAccErrc != pErr->uiAccErrc ||
|
|
|
|
+ state_6D2.uiAccErrd != pErr->uiAccErrd || state_6D2.uiAccErre != pErr->uiAccErre || state_6D2.uiAccErrf != pErr->uiAccErrf ||
|
|
|
|
+ state_6D2.uiAccErrg != pErr->uiAccErrg || state_6D2.uiAccErrh != pErr->uiAccErrh || state_6D2.uiAccErrai != pErr->uiAccErrai ||
|
|
|
|
+ state_6D2.uiAccErrj != pErr->uiAccErrj || state_6D2.uiAccErrk != pErr->uiAccErrk || state_6D2.uiAccErrl != pErr->uiAccErrl ||
|
|
|
|
+ state_6D2.uiAccErrm != pErr->uiAccErrm || state_6D2.uiAccErrn != pErr->uiAccErrn || state_6D2.uiAccErro != pErr->uiAccErro ||
|
|
|
|
+ state_6D2.uiAccErrp != pErr->uiAccErrp || state_6D2.uiAccErrq != pErr->uiAccErrq || state_6D2.uiAccErrr != pErr->uiAccErrr ||
|
|
|
|
+ state_6D2.uiAccErrs != pErr->uiAccErrs || state_6D2.uiAccErrt != pErr->uiAccErrt || state_6D2.uiAccErru != pErr->uiAccErru ||
|
|
|
|
+ state_6D2.uiAccErrv != pErr->uiAccErrv || state_6D2.uiAccErrw != pErr->uiAccErrw || state_6D2.uiAccErrx != pErr->uiAccErrx ||
|
|
|
|
+ state_6D2.uiAccErry != pErr->uiAccErry || state_6D2.uiAccErrz != pErr->uiAccErrz)
|
|
|
|
+ {
|
|
|
|
|
|
-bool Perception::PublishState_6D4(const canbus::canMsg::ConstPtr &msg)
|
|
|
|
-{
|
|
|
|
|
|
+ memcpy(pErr, msg->data.c_str(), copySize);
|
|
|
|
+ if(msg->data.size() < sizeof(s_state_6D2))
|
|
|
|
+ memset((char*)&s_state_6D2 + msg->data.size(), 0, sizeof(s_state_6D2) - copySize);
|
|
|
|
+ if(pub)
|
|
|
|
+ {
|
|
|
|
+ ::perception::cecMsg msg;
|
|
|
|
+ msg.a = state_6D2.uiAccErra;
|
|
|
|
+ msg.aa = state_6D2.uiAccErraa;
|
|
|
|
+ msg.ab = state_6D2.uiAccErrab;
|
|
|
|
+ msg.ac = state_6D2.uiAccErrac;
|
|
|
|
+ msg.ad = state_6D2.uiAccErrad;
|
|
|
|
+ msg.ae = state_6D2.uiAccErrae;
|
|
|
|
+ msg.af = state_6D2.uiAccErraf;
|
|
|
|
+ msg.ag = state_6D2.uiAccErrag;
|
|
|
|
+ msg.ah = state_6D2.uiAccErrah;
|
|
|
|
+ msg.ai = state_6D2.uiAccErrai;
|
|
|
|
+ msg.b = state_6D2.uiAccErrb;
|
|
|
|
+ msg.c = state_6D2.uiAccErrc;
|
|
|
|
+ msg.d = state_6D2.uiAccErrd;
|
|
|
|
+ msg.e = state_6D2.uiAccErre;
|
|
|
|
+ msg.f = state_6D2.uiAccErrf;
|
|
|
|
+ msg.g = state_6D2.uiAccErrg;
|
|
|
|
+ msg.h = state_6D2.uiAccErrh;
|
|
|
|
+ msg.i = state_6D2.uiAccErri;
|
|
|
|
+ msg.j = state_6D2.uiAccErrj;
|
|
|
|
+ msg.k = state_6D2.uiAccErrk;
|
|
|
|
+ msg.l = state_6D2.uiAccErrl;
|
|
|
|
+ msg.m = state_6D2.uiAccErrm;
|
|
|
|
+ msg.n = state_6D2.uiAccErrn;
|
|
|
|
+ msg.o = state_6D2.uiAccErro;
|
|
|
|
+ msg.p = state_6D2.uiAccErrp;
|
|
|
|
+ msg.q = state_6D2.uiAccErrq;
|
|
|
|
+ msg.r = state_6D2.uiAccErrr;
|
|
|
|
+ msg.s = state_6D2.uiAccErrs;
|
|
|
|
+ msg.t = state_6D2.uiAccErrt;
|
|
|
|
+ msg.u = state_6D2.uiAccErru;
|
|
|
|
+ msg.v = state_6D2.uiAccErrv;
|
|
|
|
+ msg.w = state_6D2.uiAccErrw;
|
|
|
|
+ msg.x = state_6D2.uiAccErrx;
|
|
|
|
+ msg.y = state_6D2.uiAccErry;
|
|
|
|
+ msg.z = state_6D2.uiAccErrz;
|
|
|
|
+ pub.publish(msg);
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+ LOG(INFO) << Name() << " PublishState_6D2_6 publisher " << type <<" is unvalid";
|
|
|
|
+ return false;
|
|
|
|
+ }
|
|
|
|
+ return true;
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+ LOG(INFO) << Name() << " PublishState_6D2_6 " << type << " data is repeat";
|
|
|
|
+ return false;
|
|
|
|
+ }
|
|
}
|
|
}
|
|
|
|
|
|
-bool Perception::PublishState_6D5(const canbus::canMsg::ConstPtr &msg)
|
|
|
|
|
|
+bool Perception::PublishState_2E0(const canbus::canMsg::ConstPtr &msg)
|
|
{
|
|
{
|
|
|
|
+ static robot::canbusSpace::T_ESTOP_2E0 s_state_2E0;
|
|
|
|
+
|
|
|
|
+ int copySize;
|
|
|
|
+ if(msg->data.size() < sizeof(s_state_2E0))
|
|
|
|
+ {
|
|
|
|
+ copySize = msg->data.size();
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ copySize = sizeof(s_state_2E0);
|
|
|
|
+
|
|
|
|
+ robot::canbusSpace::T_ESTOP_2E0 state_2E0;
|
|
|
|
+ memset(&state_2E0, 0 ,sizeof(state_2E0));
|
|
|
|
+ memcpy(&state_2E0, msg->data.c_str(), copySize);
|
|
|
|
+
|
|
|
|
+ if(state_2E0.uiEstpAbnStat != s_state_2E0.uiEstpAbnStat || state_2E0.uiEstpAlive != state_2E0.uiEstpAlive ||
|
|
|
|
+ state_2E0.uiEstpEnStat != s_state_2E0.uiEstpEnStat || state_2E0.uiPmpEnStat != s_state_2E0.uiPmpEnStat )
|
|
|
|
+ {
|
|
|
|
+
|
|
|
|
+ memcpy(&s_state_2E0, msg->data.c_str(), copySize);
|
|
|
|
+ if(msg->data.size() < sizeof(s_state_2E0))
|
|
|
|
+ memset((char*)&s_state_2E0 + msg->data.size(), 0, sizeof(s_state_2E0) - copySize);
|
|
|
|
+ if(bus_pub_2E0)
|
|
|
|
+ {
|
|
|
|
+ ::perception::eiMsg msg;
|
|
|
|
+ msg.EstpAbnStat = state_2E0.uiEstpAbnStat;
|
|
|
|
+ msg.EstpAlive = state_2E0.uiEstpAlive;
|
|
|
|
+ msg.EstpEnStat = state_2E0.uiEstpEnStat;
|
|
|
|
+ msg.PmpEnStat = state_2E0.uiPmpEnStat;
|
|
|
|
+ bus_pub_2E0.publish(msg);
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+ LOG(INFO) << Name() << " PublishState_2E0 publisher is unvalid";
|
|
|
|
+ return false;
|
|
|
|
+ }
|
|
|
|
+ return true;
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+ LOG(INFO) << Name() << " PublishState_2E0 data is repeat";
|
|
|
|
+ return false;
|
|
|
|
+ }
|
|
}
|
|
}
|
|
|
|
|
|
-bool Perception::PublishState_6D6(const canbus::canMsg::ConstPtr &msg)
|
|
|
|
|
|
+bool Perception::PublishState_2E1(const canbus::canMsg::ConstPtr &msg)
|
|
{
|
|
{
|
|
-}
|
|
|
|
|
|
+ static robot::canbusSpace::T_ESTOP_2E1 s_state_2E1;
|
|
|
|
|
|
|
|
+ int copySize;
|
|
|
|
+ if(msg->data.size() < sizeof(s_state_2E1))
|
|
|
|
+ copySize = msg->data.size();
|
|
|
|
+ else
|
|
|
|
+ copySize = sizeof(s_state_2E1);
|
|
|
|
|
|
|
|
+ robot::canbusSpace::T_ESTOP_2E1 state_2E1;
|
|
|
|
+ memset(&state_2E1, 0 ,sizeof(state_2E1));
|
|
|
|
+ memcpy(&state_2E1, msg->data.c_str(), copySize);
|
|
|
|
+
|
|
|
|
+ if(state_2E1.uiEspErra != s_state_2E1.uiEspErra || state_2E1.uiEspErrb != s_state_2E1.uiEspErrb || state_2E1.uiEspErrc != s_state_2E1.uiEspErrc ||
|
|
|
|
+ state_2E1.uiEspErrd != s_state_2E1.uiEspErrd || state_2E1.uiEspErre != s_state_2E1.uiEspErre || state_2E1.uiEspErrf != s_state_2E1.uiEspErrf ||
|
|
|
|
+ state_2E1.uiEspErrg != s_state_2E1.uiEspErrg || state_2E1.uiEspErrh != s_state_2E1.uiEspErrh || state_2E1.uiEspErri != s_state_2E1.uiEspErri ||
|
|
|
|
+ state_2E1.uiEspErrj != s_state_2E1.uiEspErrj)
|
|
|
|
+ {
|
|
|
|
+
|
|
|
|
+ memcpy(&s_state_2E1, msg->data.c_str(), copySize);
|
|
|
|
+ if(msg->data.size() < sizeof(s_state_2E1))
|
|
|
|
+ memset((char*)&s_state_2E1 + msg->data.size(), 0, sizeof(s_state_2E1) - copySize);
|
|
|
|
+ if(bus_pub_2E1)
|
|
|
|
+ {
|
|
|
|
+ ::perception::eeMsg msg;
|
|
|
|
+ msg.uiEspErra = state_2E1.uiEspErra;
|
|
|
|
+ msg.uiEspErrb = state_2E1.uiEspErrb;
|
|
|
|
+ msg.uiEspErrc = state_2E1.uiEspErrc;
|
|
|
|
+ msg.uiEspErrd = state_2E1.uiEspErrd;
|
|
|
|
+ msg.uiEspErre = state_2E1.uiEspErre;
|
|
|
|
+ msg.uiEspErrf = state_2E1.uiEspErrf;
|
|
|
|
+ msg.uiEspErrg = state_2E1.uiEspErrg;
|
|
|
|
+ msg.uiEspErrh = state_2E1.uiEspErrh;
|
|
|
|
+ msg.uiEspErri = state_2E1.uiEspErri;
|
|
|
|
+ msg.uiEspErrj = state_2E1.uiEspErrj;
|
|
|
|
+ bus_pub_2E1.publish(msg);
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+ LOG(INFO) << Name() << " PublishState_2E1 publisher is unvalid";
|
|
|
|
+ return false;
|
|
|
|
+ }
|
|
|
|
+ return true;
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+ LOG(INFO) << Name() << " PublishState_2E1 data is repeat";
|
|
|
|
+ return false;
|
|
|
|
+ }
|
|
|
|
+}
|
|
} // namespace perception
|
|
} // namespace perception
|
|
} // namespace robot
|
|
} // namespace robot
|