|
@@ -6,6 +6,7 @@
|
|
|
#include <perception/slMsg.h>
|
|
|
#include <perception/seMsg.h>
|
|
|
#include <perception/si2Msg.h>
|
|
|
+#include <perception/cecMsg.h>
|
|
|
|
|
|
namespace robot {
|
|
|
namespace perception {
|
|
@@ -36,6 +37,12 @@ int Perception::Start()
|
|
|
|
|
|
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);
|
|
|
+
|
|
|
ros::spin();
|
|
|
}
|
|
|
catch(ros::Exception e)
|
|
@@ -43,9 +50,6 @@ int Perception::Start()
|
|
|
LOG(INFO) << Name() << " Start exception is:" << e.what();
|
|
|
return ROBOT_FAILTURE;
|
|
|
}
|
|
|
-
|
|
|
-
|
|
|
-
|
|
|
LOG(INFO) << Name() << " Start end.";
|
|
|
return ROBOT_SUCCESS;
|
|
|
}
|
|
@@ -92,27 +96,27 @@ void Perception::CanTopicCallback(const canbus::canMsg::ConstPtr& msg)
|
|
|
break;
|
|
|
case robot::common::AccErr:
|
|
|
{
|
|
|
- PublishState_6D2(msg);
|
|
|
+ PublishState_6D2_6(msg, 2, bus_pub_6D2);
|
|
|
}
|
|
|
break;
|
|
|
case robot::common::BrkErr:
|
|
|
{
|
|
|
- PublishState_6D3(msg);
|
|
|
+ PublishState_6D2_6(msg, 3, bus_pub_6D3);
|
|
|
}
|
|
|
break;
|
|
|
case robot::common::XsftErr:
|
|
|
{
|
|
|
- PublishState_6D4(msg);
|
|
|
+ PublishState_6D2_6(msg, 4, bus_pub_6D4);
|
|
|
}
|
|
|
break;
|
|
|
case robot::common::YsftErr:
|
|
|
{
|
|
|
- PublishState_6D5(msg);
|
|
|
+ PublishState_6D2_6(msg, 5, bus_pub_6D5);
|
|
|
}
|
|
|
break;
|
|
|
case robot::common::StrErr:
|
|
|
{
|
|
|
- PublishState_6D6(msg);
|
|
|
+ PublishState_6D2_6(msg, 6, bus_pub_6D6);
|
|
|
}
|
|
|
break;
|
|
|
default:
|
|
@@ -370,24 +374,116 @@ 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_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_6D4(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_6D5(const canbus::canMsg::ConstPtr &msg)
|
|
|
-{
|
|
|
-}
|
|
|
+ memcpy(pErr, msg->data.c_str(), copySize);
|
|
|
|
|
|
-bool Perception::PublishState_6D6(const canbus::canMsg::ConstPtr &msg)
|
|
|
-{
|
|
|
+ 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;
|
|
|
+ }
|
|
|
}
|
|
|
|
|
|
|