|
@@ -41,8 +41,8 @@ int Perception::Start()
|
|
|
|
|
|
bus_pub_6D1 = node_handle_->advertise<::perception::seMsg>(robot::common::SeData, 1);
|
|
|
|
|
|
- bus_pub_6D2 = node_handle_->advertise<::perception::cecMsg>(robot::common::AcceData, 1);
|
|
|
- bus_pub_6D3 = node_handle_->advertise<::perception::cecMsg>(robot::common::BrkeData, 1);
|
|
|
+ bus_pub_6D2 = node_handle_->advertise<::perception::cecMsg2>(robot::common::AcceData, 1);
|
|
|
+ bus_pub_6D3 = node_handle_->advertise<::perception::cecMsg2>(robot::common::BrkeData, 1);
|
|
|
bus_pub_6D4 = node_handle_->advertise<::perception::cecMsg>(robot::common::XsfteData, 1);
|
|
|
bus_pub_6D5 = node_handle_->advertise<::perception::cecMsg>(robot::common::YsfteData, 1);
|
|
|
bus_pub_6D6 = node_handle_->advertise<::perception::cecMsg>(robot::common::StreData, 1);
|
|
@@ -107,23 +107,23 @@ LOG(INFO)<<Name()<<" Receive 6EE";
|
|
|
break;
|
|
|
case robot::common::AccErr:
|
|
|
//SoftStopControl();
|
|
|
- PublishState_6D2_6(ucCanMsgSrc, 2, bus_pub_6D2);
|
|
|
+ PublishState_6D2_3(ucCanMsgSrc, 2, bus_pub_6D2);
|
|
|
break;
|
|
|
case robot::common::BrkErr:
|
|
|
//SoftStopControl();
|
|
|
- PublishState_6D2_6(ucCanMsgSrc, 3, bus_pub_6D3);
|
|
|
+ PublishState_6D2_3(ucCanMsgSrc, 3, bus_pub_6D3);
|
|
|
break;
|
|
|
case robot::common::XsftErr:
|
|
|
//SoftStopControl();
|
|
|
- PublishState_6D2_6(ucCanMsgSrc, 4, bus_pub_6D4);
|
|
|
+ PublishState_6D4_6(ucCanMsgSrc, 4, bus_pub_6D4);
|
|
|
break;
|
|
|
case robot::common::YsftErr:
|
|
|
//SoftStopControl();
|
|
|
- PublishState_6D2_6(ucCanMsgSrc, 5, bus_pub_6D5);
|
|
|
+ PublishState_6D4_6(ucCanMsgSrc, 5, bus_pub_6D5);
|
|
|
break;
|
|
|
case robot::common::StrErr:
|
|
|
//SoftStopControl();
|
|
|
- PublishState_6D2_6(ucCanMsgSrc, 6, bus_pub_6D6);
|
|
|
+ PublishState_6D4_6(ucCanMsgSrc, 6, bus_pub_6D6);
|
|
|
break;
|
|
|
case robot::common::EstopInfo:
|
|
|
PublishState_2E0(ucCanMsgSrc);
|
|
@@ -402,29 +402,20 @@ bool Perception::PublishState_6D1(unsigned char* msg)
|
|
|
}
|
|
|
}
|
|
|
|
|
|
-bool Perception::PublishState_6D2_6(unsigned char* msg, int type, const ros::Publisher &pub)
|
|
|
+bool Perception::PublishState_6D2_3(unsigned char* msg, int type, const ros::Publisher &pub)
|
|
|
{
|
|
|
|
|
|
|
|
|
- robot::canbusSpace::T_ACC_ERR *pErr;
|
|
|
+ robot::canbusSpace::T_MOT_CTR_ERR_1 *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;
|
|
|
+ pErr = &s_state_6D3;
|
|
|
break;
|
|
|
default:
|
|
|
- LOG(INFO) << Name() << " PublishState_6D2_6 " << type << " type is unvalid";
|
|
|
+ LOG(INFO) << Name() << " PublishState_6D2_3 " << type << " type is unvalid";
|
|
|
return false;
|
|
|
break;
|
|
|
}
|
|
@@ -435,23 +426,27 @@ bool Perception::PublishState_6D2_6(unsigned char* msg, int type, const ros::Pub
|
|
|
// else
|
|
|
// copySize = sizeof(s_state_6D2);
|
|
|
|
|
|
- //6D2_2 6D2_3 6D2_4 6D2_5 6D2_6 数据结构相同
|
|
|
- robot::canbusSpace::T_ACC_ERR state_6D2;
|
|
|
+ //6D2_2 6D2_3 数据结构相同
|
|
|
+ robot::canbusSpace::T_MOT_CTR_ERR_1 state_6D2;
|
|
|
memset(&state_6D2, 0 ,sizeof(state_6D2));
|
|
|
memcpy(&state_6D2, msg, CAN_MSG_LEN);
|
|
|
|
|
|
- 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)
|
|
|
+ if(state_6D2.a != pErr->a || state_6D2.aa != pErr->aa || state_6D2.ab != pErr->ab || state_6D2.ac != pErr->ac
|
|
|
+ || state_6D2.ad != pErr->ad || state_6D2.ae != pErr->ae || state_6D2.af != pErr->af || state_6D2.ag != pErr->ag
|
|
|
+ || state_6D2.ah != pErr->ah || state_6D2.ai != pErr->ai || state_6D2.aj != pErr->aj || state_6D2.ak != pErr->ak
|
|
|
+ || state_6D2.al != pErr->al || state_6D2.am != pErr->am || state_6D2.an != pErr->an || state_6D2.ao != pErr->ao
|
|
|
+ || state_6D2.ap != pErr->ap || state_6D2.aq != pErr->aq || state_6D2.ar != pErr->ar || state_6D2.as != pErr->as
|
|
|
+ || state_6D2.at != pErr->at || state_6D2.au != pErr->au || state_6D2.av != pErr->av || state_6D2.aw != pErr->aw
|
|
|
+ || state_6D2.ax != pErr->ax || state_6D2.ay != pErr->ay || state_6D2.az != pErr->az || state_6D2.b != pErr->b
|
|
|
+ || state_6D2.ba != pErr->ba || state_6D2.bb != pErr->bb || state_6D2.bc != pErr->bc || state_6D2.bd != pErr->be
|
|
|
+ || state_6D2.bf != pErr->bf || state_6D2.bg != pErr->bg || state_6D2.bh != pErr->bh || state_6D2.bi != pErr->bi
|
|
|
+ || state_6D2.bj != pErr->bj || state_6D2.bk != pErr->bk || state_6D2.bl != pErr->bl || state_6D2.c != pErr->c
|
|
|
+ || state_6D2.d != pErr->d || state_6D2.e != pErr->e || state_6D2.f != pErr->f || state_6D2.g != pErr->g
|
|
|
+ || state_6D2.h != pErr->h || state_6D2.i != pErr->i || state_6D2.j != pErr->j || state_6D2.k != pErr->k
|
|
|
+ || state_6D2.l != pErr->l || state_6D2.m != pErr->m || state_6D2.n != pErr->n || state_6D2.o != pErr->o
|
|
|
+ || state_6D2.p != pErr->p || state_6D2.q != pErr->q || state_6D2.r != pErr->r || state_6D2.s != pErr->s
|
|
|
+ || state_6D2.t != pErr->t || state_6D2.u != pErr->u || state_6D2.v != pErr->v || state_6D2.w != pErr->w
|
|
|
+ || state_6D2.x != pErr->x || state_6D2.y != pErr->y || state_6D2.z != pErr->z)
|
|
|
{
|
|
|
|
|
|
|
|
@@ -462,6 +457,204 @@ bool Perception::PublishState_6D2_6(unsigned char* msg, int type, const ros::Pub
|
|
|
case 3:
|
|
|
memcpy(&s_state_6D3, msg, CAN_MSG_LEN);
|
|
|
break;
|
|
|
+ default:
|
|
|
+ LOG(INFO) << Name() << " PublishState_6D2_3 " << type << " type is unvalid";
|
|
|
+ return false;
|
|
|
+ break;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(type == 2)
|
|
|
+ {
|
|
|
+ cec_msg22.Erra = state_6D2.a;
|
|
|
+ cec_msg22.Erraa = state_6D2.aa;
|
|
|
+ cec_msg22.Errab = state_6D2.ab;
|
|
|
+ cec_msg22.Errac = state_6D2.ac;
|
|
|
+ cec_msg22.Errad = state_6D2.ad;
|
|
|
+ cec_msg22.Errae = state_6D2.ae;
|
|
|
+ cec_msg22.Erraf = state_6D2.af;
|
|
|
+ cec_msg22.Errag = state_6D2.ag;
|
|
|
+ cec_msg22.Errah = state_6D2.ah;
|
|
|
+ cec_msg22.Errai = state_6D2.ai;
|
|
|
+ cec_msg22.Erraj = state_6D2.aj;
|
|
|
+ cec_msg22.Errak = state_6D2.ak;
|
|
|
+ cec_msg22.Erral = state_6D2.al;
|
|
|
+ cec_msg22.Erram = state_6D2.am;
|
|
|
+ cec_msg22.Erran = state_6D2.an;
|
|
|
+ cec_msg22.Errao = state_6D2.ao;
|
|
|
+ cec_msg22.Errap = state_6D2.ap;
|
|
|
+ cec_msg22.Erraq = state_6D2.aq;
|
|
|
+ cec_msg22.Errar = state_6D2.ar;
|
|
|
+ cec_msg22.Erras = state_6D2.as;
|
|
|
+ cec_msg22.Errat = state_6D2.at;
|
|
|
+ cec_msg22.Errau = state_6D2.au;
|
|
|
+ cec_msg22.Errav = state_6D2.av;
|
|
|
+ cec_msg22.Erraw = state_6D2.aw;
|
|
|
+ cec_msg22.Errax = state_6D2.ax;
|
|
|
+ cec_msg22.Erray = state_6D2.ay;
|
|
|
+ cec_msg22.Erraz = state_6D2.az;
|
|
|
+ cec_msg22.Errb = state_6D2.b;
|
|
|
+ cec_msg22.Errba = state_6D2.ba;
|
|
|
+ cec_msg22.Errbb = state_6D2.bb;
|
|
|
+ cec_msg22.Errbc = state_6D2.bc;
|
|
|
+ cec_msg22.Errbd = state_6D2.bd;
|
|
|
+ cec_msg22.Errbe = state_6D2.be;
|
|
|
+ cec_msg22.Errbf = state_6D2.bf;
|
|
|
+ cec_msg22.Errbg = state_6D2.bg;
|
|
|
+ cec_msg22.Errbh = state_6D2.bh;
|
|
|
+ cec_msg22.Errbi = state_6D2.bi;
|
|
|
+ cec_msg22.Errbk = state_6D2.bk;
|
|
|
+ cec_msg22.Errbj = state_6D2.bj;
|
|
|
+ cec_msg22.Errbl = state_6D2.bl;
|
|
|
+ cec_msg22.Errc = state_6D2.c;
|
|
|
+ cec_msg22.Errd = state_6D2.d;
|
|
|
+ cec_msg22.Erre = state_6D2.e;
|
|
|
+ cec_msg22.Errf = state_6D2.f;
|
|
|
+ cec_msg22.Errg = state_6D2.g;
|
|
|
+ cec_msg22.Errh = state_6D2.h;
|
|
|
+ cec_msg22.Erri = state_6D2.i;
|
|
|
+ cec_msg22.Errj = state_6D2.j;
|
|
|
+ cec_msg22.Errk = state_6D2.k;
|
|
|
+ cec_msg22.Errl = state_6D2.l;
|
|
|
+ cec_msg22.Errm = state_6D2.m;
|
|
|
+ cec_msg22.Errn = state_6D2.n;
|
|
|
+ cec_msg22.Erro = state_6D2.o;
|
|
|
+ cec_msg22.Errp = state_6D2.p;
|
|
|
+ cec_msg22.Errq = state_6D2.q;
|
|
|
+ cec_msg22.Errr = state_6D2.r;
|
|
|
+ cec_msg22.Errs = state_6D2.s;
|
|
|
+ cec_msg22.Errt = state_6D2.t;
|
|
|
+ cec_msg22.Erru = state_6D2.u;
|
|
|
+ cec_msg22.Errv = state_6D2.v;
|
|
|
+ cec_msg22.Errw = state_6D2.w;
|
|
|
+ cec_msg22.Errx = state_6D2.x;
|
|
|
+ cec_msg22.Erry = state_6D2.y;
|
|
|
+ cec_msg22.Errz = state_6D2.z;
|
|
|
+ }
|
|
|
+ else if(type == 3)
|
|
|
+ {
|
|
|
+ cec_msg23.Erra = state_6D2.a;
|
|
|
+ cec_msg23.Erraa = state_6D2.aa;
|
|
|
+ cec_msg23.Errab = state_6D2.ab;
|
|
|
+ cec_msg23.Errac = state_6D2.ac;
|
|
|
+ cec_msg23.Errad = state_6D2.ad;
|
|
|
+ cec_msg23.Errae = state_6D2.ae;
|
|
|
+ cec_msg23.Erraf = state_6D2.af;
|
|
|
+ cec_msg23.Errag = state_6D2.ag;
|
|
|
+ cec_msg23.Errah = state_6D2.ah;
|
|
|
+ cec_msg23.Errai = state_6D2.ai;
|
|
|
+ cec_msg23.Erraj = state_6D2.aj;
|
|
|
+ cec_msg23.Errak = state_6D2.ak;
|
|
|
+ cec_msg23.Erral = state_6D2.al;
|
|
|
+ cec_msg23.Erram = state_6D2.am;
|
|
|
+ cec_msg23.Erran = state_6D2.an;
|
|
|
+ cec_msg23.Errao = state_6D2.ao;
|
|
|
+ cec_msg23.Errap = state_6D2.ap;
|
|
|
+ cec_msg23.Erraq = state_6D2.aq;
|
|
|
+ cec_msg23.Errar = state_6D2.ar;
|
|
|
+ cec_msg23.Erras = state_6D2.as;
|
|
|
+ cec_msg23.Errat = state_6D2.at;
|
|
|
+ cec_msg23.Errau = state_6D2.au;
|
|
|
+ cec_msg23.Errav = state_6D2.av;
|
|
|
+ cec_msg23.Erraw = state_6D2.aw;
|
|
|
+ cec_msg23.Errax = state_6D2.ax;
|
|
|
+ cec_msg23.Erray = state_6D2.ay;
|
|
|
+ cec_msg23.Erraz = state_6D2.az;
|
|
|
+ cec_msg23.Errb = state_6D2.b;
|
|
|
+ cec_msg23.Errba = state_6D2.ba;
|
|
|
+ cec_msg23.Errbb = state_6D2.bb;
|
|
|
+ cec_msg23.Errbc = state_6D2.bc;
|
|
|
+ cec_msg23.Errbd = state_6D2.bd;
|
|
|
+ cec_msg23.Errbe = state_6D2.be;
|
|
|
+ cec_msg23.Errbf = state_6D2.bf;
|
|
|
+ cec_msg23.Errbg = state_6D2.bg;
|
|
|
+ cec_msg23.Errbh = state_6D2.bh;
|
|
|
+ cec_msg23.Errbi = state_6D2.bi;
|
|
|
+ cec_msg23.Errbk = state_6D2.bk;
|
|
|
+ cec_msg23.Errbj = state_6D2.bj;
|
|
|
+ cec_msg23.Errbl = state_6D2.bl;
|
|
|
+ cec_msg23.Errc = state_6D2.c;
|
|
|
+ cec_msg23.Errd = state_6D2.d;
|
|
|
+ cec_msg23.Erre = state_6D2.e;
|
|
|
+ cec_msg23.Errf = state_6D2.f;
|
|
|
+ cec_msg23.Errg = state_6D2.g;
|
|
|
+ cec_msg23.Errh = state_6D2.h;
|
|
|
+ cec_msg23.Erri = state_6D2.i;
|
|
|
+ cec_msg23.Errj = state_6D2.j;
|
|
|
+ cec_msg23.Errk = state_6D2.k;
|
|
|
+ cec_msg23.Errl = state_6D2.l;
|
|
|
+ cec_msg23.Errm = state_6D2.m;
|
|
|
+ cec_msg23.Errn = state_6D2.n;
|
|
|
+ cec_msg23.Erro = state_6D2.o;
|
|
|
+ cec_msg23.Errp = state_6D2.p;
|
|
|
+ cec_msg23.Errq = state_6D2.q;
|
|
|
+ cec_msg23.Errr = state_6D2.r;
|
|
|
+ cec_msg23.Errs = state_6D2.s;
|
|
|
+ cec_msg23.Errt = state_6D2.t;
|
|
|
+ cec_msg23.Erru = state_6D2.u;
|
|
|
+ cec_msg23.Errv = state_6D2.v;
|
|
|
+ cec_msg23.Errw = state_6D2.w;
|
|
|
+ cec_msg23.Errx = state_6D2.x;
|
|
|
+ cec_msg23.Erry = state_6D2.y;
|
|
|
+ cec_msg23.Errz = state_6D2.z;
|
|
|
+ }
|
|
|
+
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ // LOG(INFO) << Name() << " PublishState_6D2_3 " << type << " data is repeat";
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+bool Perception::PublishState_6D4_6(unsigned char* msg, int type, const ros::Publisher &pub)
|
|
|
+{
|
|
|
+
|
|
|
+
|
|
|
+ robot::canbusSpace::T_ACC_ERR *pErr;
|
|
|
+ switch (type) {
|
|
|
+ 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_6D4_6 " << type << " type is unvalid";
|
|
|
+ return false;
|
|
|
+ break;
|
|
|
+ }
|
|
|
+
|
|
|
+// int copySize;
|
|
|
+// if(msg->data.size() < sizeof(s_state_6D2))
|
|
|
+// copySize = msg->data.size();
|
|
|
+// else
|
|
|
+// copySize = sizeof(s_state_6D2);
|
|
|
+
|
|
|
+ // 6D2_4 6D2_5 6D2_6 数据结构相同
|
|
|
+ robot::canbusSpace::T_ACC_ERR state_6D4;
|
|
|
+ memset(&state_6D4, 0 ,sizeof(state_6D4));
|
|
|
+ memcpy(&state_6D4, msg, CAN_MSG_LEN);
|
|
|
+
|
|
|
+ if(state_6D4.uiAccErra != pErr->uiAccErra || state_6D4.uiAccErraa != pErr->uiAccErraa || state_6D4.uiAccErrab != pErr->uiAccErrab ||
|
|
|
+ state_6D4.uiAccErrac != pErr->uiAccErrac || state_6D4.uiAccErrad != pErr->uiAccErrad || state_6D4.uiAccErrae != pErr->uiAccErrae ||
|
|
|
+ state_6D4.uiAccErraf != pErr->uiAccErraf || state_6D4.uiAccErrag != pErr->uiAccErrag || state_6D4.uiAccErrah != pErr->uiAccErrah ||
|
|
|
+ state_6D4.uiAccErrai != pErr->uiAccErrai || state_6D4.uiAccErrb != pErr->uiAccErrb || state_6D4.uiAccErrc != pErr->uiAccErrc ||
|
|
|
+ state_6D4.uiAccErrd != pErr->uiAccErrd || state_6D4.uiAccErre != pErr->uiAccErre || state_6D4.uiAccErrf != pErr->uiAccErrf ||
|
|
|
+ state_6D4.uiAccErrg != pErr->uiAccErrg || state_6D4.uiAccErrh != pErr->uiAccErrh || state_6D4.uiAccErrai != pErr->uiAccErrai ||
|
|
|
+ state_6D4.uiAccErrj != pErr->uiAccErrj || state_6D4.uiAccErrk != pErr->uiAccErrk || state_6D4.uiAccErrl != pErr->uiAccErrl ||
|
|
|
+ state_6D4.uiAccErrm != pErr->uiAccErrm || state_6D4.uiAccErrn != pErr->uiAccErrn || state_6D4.uiAccErro != pErr->uiAccErro ||
|
|
|
+ state_6D4.uiAccErrp != pErr->uiAccErrp || state_6D4.uiAccErrq != pErr->uiAccErrq || state_6D4.uiAccErrr != pErr->uiAccErrr ||
|
|
|
+ state_6D4.uiAccErrs != pErr->uiAccErrs || state_6D4.uiAccErrt != pErr->uiAccErrt || state_6D4.uiAccErru != pErr->uiAccErru ||
|
|
|
+ state_6D4.uiAccErrv != pErr->uiAccErrv || state_6D4.uiAccErrw != pErr->uiAccErrw || state_6D4.uiAccErrx != pErr->uiAccErrx ||
|
|
|
+ state_6D4.uiAccErry != pErr->uiAccErry || state_6D4.uiAccErrz != pErr->uiAccErrz)
|
|
|
+ {
|
|
|
+
|
|
|
+
|
|
|
+ switch (type) {
|
|
|
case 4:
|
|
|
memcpy(&s_state_6D4, msg, CAN_MSG_LEN);
|
|
|
break;
|
|
@@ -472,207 +665,131 @@ bool Perception::PublishState_6D2_6(unsigned char* msg, int type, const ros::Pub
|
|
|
memcpy(&s_state_6D6, msg, CAN_MSG_LEN);
|
|
|
break;
|
|
|
default:
|
|
|
- LOG(INFO) << Name() << " PublishState_6D2_6 " << type << " type is unvalid";
|
|
|
+ LOG(INFO) << Name() << " PublishState_6D4_6 " << type << " type is unvalid";
|
|
|
return false;
|
|
|
break;
|
|
|
}
|
|
|
|
|
|
- if(type == 2)
|
|
|
- {
|
|
|
- cec_msg2.Erra = state_6D2.uiAccErra;
|
|
|
- cec_msg2.Erraa = state_6D2.uiAccErraa;
|
|
|
- cec_msg2.Errab = state_6D2.uiAccErrab;
|
|
|
- cec_msg2.Errac = state_6D2.uiAccErrac;
|
|
|
- cec_msg2.Errad = state_6D2.uiAccErrad;
|
|
|
- cec_msg2.Errae = state_6D2.uiAccErrae;
|
|
|
- cec_msg2.Erraf = state_6D2.uiAccErraf;
|
|
|
- cec_msg2.Errag = state_6D2.uiAccErrag;
|
|
|
- cec_msg2.Errah = state_6D2.uiAccErrah;
|
|
|
- cec_msg2.Errai = state_6D2.uiAccErrai;
|
|
|
- cec_msg2.Errb = state_6D2.uiAccErrb;
|
|
|
- cec_msg2.Errc = state_6D2.uiAccErrc;
|
|
|
- cec_msg2.Errd = state_6D2.uiAccErrd;
|
|
|
- cec_msg2.Erre = state_6D2.uiAccErre;
|
|
|
- cec_msg2.Errf = state_6D2.uiAccErrf;
|
|
|
- cec_msg2.Errg = state_6D2.uiAccErrg;
|
|
|
- cec_msg2.Errh = state_6D2.uiAccErrh;
|
|
|
- cec_msg2.Erri = state_6D2.uiAccErri;
|
|
|
- cec_msg2.Errj = state_6D2.uiAccErrj;
|
|
|
- cec_msg2.Errk = state_6D2.uiAccErrk;
|
|
|
- cec_msg2.Errl = state_6D2.uiAccErrl;
|
|
|
- cec_msg2.Errm = state_6D2.uiAccErrm;
|
|
|
- cec_msg2.Errn = state_6D2.uiAccErrn;
|
|
|
- cec_msg2.Erro = state_6D2.uiAccErro;
|
|
|
- cec_msg2.Errp = state_6D2.uiAccErrp;
|
|
|
- cec_msg2.Errq = state_6D2.uiAccErrq;
|
|
|
- cec_msg2.Errr = state_6D2.uiAccErrr;
|
|
|
- cec_msg2.Errs = state_6D2.uiAccErrs;
|
|
|
- cec_msg2.Errt = state_6D2.uiAccErrt;
|
|
|
- cec_msg2.Erru = state_6D2.uiAccErru;
|
|
|
- cec_msg2.Errv = state_6D2.uiAccErrv;
|
|
|
- cec_msg2.Errw = state_6D2.uiAccErrw;
|
|
|
- cec_msg2.Errx = state_6D2.uiAccErrx;
|
|
|
- cec_msg2.Erry = state_6D2.uiAccErry;
|
|
|
- cec_msg2.Errz = state_6D2.uiAccErrz;
|
|
|
- }
|
|
|
- else if(type == 3)
|
|
|
- {
|
|
|
- cec_msg3.Erra = state_6D2.uiAccErra;
|
|
|
- cec_msg3.Erraa = state_6D2.uiAccErraa;
|
|
|
- cec_msg3.Errab = state_6D2.uiAccErrab;
|
|
|
- cec_msg3.Errac = state_6D2.uiAccErrac;
|
|
|
- cec_msg3.Errad = state_6D2.uiAccErrad;
|
|
|
- cec_msg3.Errae = state_6D2.uiAccErrae;
|
|
|
- cec_msg3.Erraf = state_6D2.uiAccErraf;
|
|
|
- cec_msg3.Errag = state_6D2.uiAccErrag;
|
|
|
- cec_msg3.Errah = state_6D2.uiAccErrah;
|
|
|
- cec_msg3.Errai = state_6D2.uiAccErrai;
|
|
|
- cec_msg3.Errb = state_6D2.uiAccErrb;
|
|
|
- cec_msg3.Errc = state_6D2.uiAccErrc;
|
|
|
- cec_msg3.Errd = state_6D2.uiAccErrd;
|
|
|
- cec_msg3.Erre = state_6D2.uiAccErre;
|
|
|
- cec_msg3.Errf = state_6D2.uiAccErrf;
|
|
|
- cec_msg3.Errg = state_6D2.uiAccErrg;
|
|
|
- cec_msg3.Errh = state_6D2.uiAccErrh;
|
|
|
- cec_msg3.Erri = state_6D2.uiAccErri;
|
|
|
- cec_msg3.Errj = state_6D2.uiAccErrj;
|
|
|
- cec_msg3.Errk = state_6D2.uiAccErrk;
|
|
|
- cec_msg3.Errl = state_6D2.uiAccErrl;
|
|
|
- cec_msg3.Errm = state_6D2.uiAccErrm;
|
|
|
- cec_msg3.Errn = state_6D2.uiAccErrn;
|
|
|
- cec_msg3.Erro = state_6D2.uiAccErro;
|
|
|
- cec_msg3.Errp = state_6D2.uiAccErrp;
|
|
|
- cec_msg3.Errq = state_6D2.uiAccErrq;
|
|
|
- cec_msg3.Errr = state_6D2.uiAccErrr;
|
|
|
- cec_msg3.Errs = state_6D2.uiAccErrs;
|
|
|
- cec_msg3.Errt = state_6D2.uiAccErrt;
|
|
|
- cec_msg3.Erru = state_6D2.uiAccErru;
|
|
|
- cec_msg3.Errv = state_6D2.uiAccErrv;
|
|
|
- cec_msg3.Errw = state_6D2.uiAccErrw;
|
|
|
- cec_msg3.Errx = state_6D2.uiAccErrx;
|
|
|
- cec_msg3.Erry = state_6D2.uiAccErry;
|
|
|
- cec_msg3.Errz = state_6D2.uiAccErrz;
|
|
|
- }
|
|
|
- else if(type ==4)
|
|
|
+ if(type ==4)
|
|
|
{
|
|
|
- cec_msg4.Erra = state_6D2.uiAccErra;
|
|
|
- cec_msg4.Erraa = state_6D2.uiAccErraa;
|
|
|
- cec_msg4.Errab = state_6D2.uiAccErrab;
|
|
|
- cec_msg4.Errac = state_6D2.uiAccErrac;
|
|
|
- cec_msg4.Errad = state_6D2.uiAccErrad;
|
|
|
- cec_msg4.Errae = state_6D2.uiAccErrae;
|
|
|
- cec_msg4.Erraf = state_6D2.uiAccErraf;
|
|
|
- cec_msg4.Errag = state_6D2.uiAccErrag;
|
|
|
- cec_msg4.Errah = state_6D2.uiAccErrah;
|
|
|
- cec_msg4.Errai = state_6D2.uiAccErrai;
|
|
|
- cec_msg4.Errb = state_6D2.uiAccErrb;
|
|
|
- cec_msg4.Errc = state_6D2.uiAccErrc;
|
|
|
- cec_msg4.Errd = state_6D2.uiAccErrd;
|
|
|
- cec_msg4.Erre = state_6D2.uiAccErre;
|
|
|
- cec_msg4.Errf = state_6D2.uiAccErrf;
|
|
|
- cec_msg4.Errg = state_6D2.uiAccErrg;
|
|
|
- cec_msg4.Errh = state_6D2.uiAccErrh;
|
|
|
- cec_msg4.Erri = state_6D2.uiAccErri;
|
|
|
- cec_msg4.Errj = state_6D2.uiAccErrj;
|
|
|
- cec_msg4.Errk = state_6D2.uiAccErrk;
|
|
|
- cec_msg4.Errl = state_6D2.uiAccErrl;
|
|
|
- cec_msg4.Errm = state_6D2.uiAccErrm;
|
|
|
- cec_msg4.Errn = state_6D2.uiAccErrn;
|
|
|
- cec_msg4.Erro = state_6D2.uiAccErro;
|
|
|
- cec_msg4.Errp = state_6D2.uiAccErrp;
|
|
|
- cec_msg4.Errq = state_6D2.uiAccErrq;
|
|
|
- cec_msg4.Errr = state_6D2.uiAccErrr;
|
|
|
- cec_msg4.Errs = state_6D2.uiAccErrs;
|
|
|
- cec_msg4.Errt = state_6D2.uiAccErrt;
|
|
|
- cec_msg4.Erru = state_6D2.uiAccErru;
|
|
|
- cec_msg4.Errv = state_6D2.uiAccErrv;
|
|
|
- cec_msg4.Errw = state_6D2.uiAccErrw;
|
|
|
- cec_msg4.Errx = state_6D2.uiAccErrx;
|
|
|
- cec_msg4.Erry = state_6D2.uiAccErry;
|
|
|
- cec_msg4.Errz = state_6D2.uiAccErrz;
|
|
|
+ cec_msg4.Erra = state_6D4.uiAccErra;
|
|
|
+ cec_msg4.Erraa = state_6D4.uiAccErraa;
|
|
|
+ cec_msg4.Errab = state_6D4.uiAccErrab;
|
|
|
+ cec_msg4.Errac = state_6D4.uiAccErrac;
|
|
|
+ cec_msg4.Errad = state_6D4.uiAccErrad;
|
|
|
+ cec_msg4.Errae = state_6D4.uiAccErrae;
|
|
|
+ cec_msg4.Erraf = state_6D4.uiAccErraf;
|
|
|
+ cec_msg4.Errag = state_6D4.uiAccErrag;
|
|
|
+ cec_msg4.Errah = state_6D4.uiAccErrah;
|
|
|
+ cec_msg4.Errai = state_6D4.uiAccErrai;
|
|
|
+ cec_msg4.Errb = state_6D4.uiAccErrb;
|
|
|
+ cec_msg4.Errc = state_6D4.uiAccErrc;
|
|
|
+ cec_msg4.Errd = state_6D4.uiAccErrd;
|
|
|
+ cec_msg4.Erre = state_6D4.uiAccErre;
|
|
|
+ cec_msg4.Errf = state_6D4.uiAccErrf;
|
|
|
+ cec_msg4.Errg = state_6D4.uiAccErrg;
|
|
|
+ cec_msg4.Errh = state_6D4.uiAccErrh;
|
|
|
+ cec_msg4.Erri = state_6D4.uiAccErri;
|
|
|
+ cec_msg4.Errj = state_6D4.uiAccErrj;
|
|
|
+ cec_msg4.Errk = state_6D4.uiAccErrk;
|
|
|
+ cec_msg4.Errl = state_6D4.uiAccErrl;
|
|
|
+ cec_msg4.Errm = state_6D4.uiAccErrm;
|
|
|
+ cec_msg4.Errn = state_6D4.uiAccErrn;
|
|
|
+ cec_msg4.Erro = state_6D4.uiAccErro;
|
|
|
+ cec_msg4.Errp = state_6D4.uiAccErrp;
|
|
|
+ cec_msg4.Errq = state_6D4.uiAccErrq;
|
|
|
+ cec_msg4.Errr = state_6D4.uiAccErrr;
|
|
|
+ cec_msg4.Errs = state_6D4.uiAccErrs;
|
|
|
+ cec_msg4.Errt = state_6D4.uiAccErrt;
|
|
|
+ cec_msg4.Erru = state_6D4.uiAccErru;
|
|
|
+ cec_msg4.Errv = state_6D4.uiAccErrv;
|
|
|
+ cec_msg4.Errw = state_6D4.uiAccErrw;
|
|
|
+ cec_msg4.Errx = state_6D4.uiAccErrx;
|
|
|
+ cec_msg4.Erry = state_6D4.uiAccErry;
|
|
|
+ cec_msg4.Errz = state_6D4.uiAccErrz;
|
|
|
}
|
|
|
else if(type == 5)
|
|
|
{
|
|
|
- cec_msg5.Erra = state_6D2.uiAccErra;
|
|
|
- cec_msg5.Erraa = state_6D2.uiAccErraa;
|
|
|
- cec_msg5.Errab = state_6D2.uiAccErrab;
|
|
|
- cec_msg5.Errac = state_6D2.uiAccErrac;
|
|
|
- cec_msg5.Errad = state_6D2.uiAccErrad;
|
|
|
- cec_msg5.Errae = state_6D2.uiAccErrae;
|
|
|
- cec_msg5.Erraf = state_6D2.uiAccErraf;
|
|
|
- cec_msg5.Errag = state_6D2.uiAccErrag;
|
|
|
- cec_msg5.Errah = state_6D2.uiAccErrah;
|
|
|
- cec_msg5.Errai = state_6D2.uiAccErrai;
|
|
|
- cec_msg5.Errb = state_6D2.uiAccErrb;
|
|
|
- cec_msg5.Errc = state_6D2.uiAccErrc;
|
|
|
- cec_msg5.Errd = state_6D2.uiAccErrd;
|
|
|
- cec_msg5.Erre = state_6D2.uiAccErre;
|
|
|
- cec_msg5.Errf = state_6D2.uiAccErrf;
|
|
|
- cec_msg5.Errg = state_6D2.uiAccErrg;
|
|
|
- cec_msg5.Errh = state_6D2.uiAccErrh;
|
|
|
- cec_msg5.Erri = state_6D2.uiAccErri;
|
|
|
- cec_msg5.Errj = state_6D2.uiAccErrj;
|
|
|
- cec_msg5.Errk = state_6D2.uiAccErrk;
|
|
|
- cec_msg5.Errl = state_6D2.uiAccErrl;
|
|
|
- cec_msg5.Errm = state_6D2.uiAccErrm;
|
|
|
- cec_msg5.Errn = state_6D2.uiAccErrn;
|
|
|
- cec_msg5.Erro = state_6D2.uiAccErro;
|
|
|
- cec_msg5.Errp = state_6D2.uiAccErrp;
|
|
|
- cec_msg5.Errq = state_6D2.uiAccErrq;
|
|
|
- cec_msg5.Errr = state_6D2.uiAccErrr;
|
|
|
- cec_msg5.Errs = state_6D2.uiAccErrs;
|
|
|
- cec_msg5.Errt = state_6D2.uiAccErrt;
|
|
|
- cec_msg5.Erru = state_6D2.uiAccErru;
|
|
|
- cec_msg5.Errv = state_6D2.uiAccErrv;
|
|
|
- cec_msg5.Errw = state_6D2.uiAccErrw;
|
|
|
- cec_msg5.Errx = state_6D2.uiAccErrx;
|
|
|
- cec_msg5.Erry = state_6D2.uiAccErry;
|
|
|
- cec_msg5.Errz = state_6D2.uiAccErrz;
|
|
|
+ cec_msg5.Erra = state_6D4.uiAccErra;
|
|
|
+ cec_msg5.Erraa = state_6D4.uiAccErraa;
|
|
|
+ cec_msg5.Errab = state_6D4.uiAccErrab;
|
|
|
+ cec_msg5.Errac = state_6D4.uiAccErrac;
|
|
|
+ cec_msg5.Errad = state_6D4.uiAccErrad;
|
|
|
+ cec_msg5.Errae = state_6D4.uiAccErrae;
|
|
|
+ cec_msg5.Erraf = state_6D4.uiAccErraf;
|
|
|
+ cec_msg5.Errag = state_6D4.uiAccErrag;
|
|
|
+ cec_msg5.Errah = state_6D4.uiAccErrah;
|
|
|
+ cec_msg5.Errai = state_6D4.uiAccErrai;
|
|
|
+ cec_msg5.Errb = state_6D4.uiAccErrb;
|
|
|
+ cec_msg5.Errc = state_6D4.uiAccErrc;
|
|
|
+ cec_msg5.Errd = state_6D4.uiAccErrd;
|
|
|
+ cec_msg5.Erre = state_6D4.uiAccErre;
|
|
|
+ cec_msg5.Errf = state_6D4.uiAccErrf;
|
|
|
+ cec_msg5.Errg = state_6D4.uiAccErrg;
|
|
|
+ cec_msg5.Errh = state_6D4.uiAccErrh;
|
|
|
+ cec_msg5.Erri = state_6D4.uiAccErri;
|
|
|
+ cec_msg5.Errj = state_6D4.uiAccErrj;
|
|
|
+ cec_msg5.Errk = state_6D4.uiAccErrk;
|
|
|
+ cec_msg5.Errl = state_6D4.uiAccErrl;
|
|
|
+ cec_msg5.Errm = state_6D4.uiAccErrm;
|
|
|
+ cec_msg5.Errn = state_6D4.uiAccErrn;
|
|
|
+ cec_msg5.Erro = state_6D4.uiAccErro;
|
|
|
+ cec_msg5.Errp = state_6D4.uiAccErrp;
|
|
|
+ cec_msg5.Errq = state_6D4.uiAccErrq;
|
|
|
+ cec_msg5.Errr = state_6D4.uiAccErrr;
|
|
|
+ cec_msg5.Errs = state_6D4.uiAccErrs;
|
|
|
+ cec_msg5.Errt = state_6D4.uiAccErrt;
|
|
|
+ cec_msg5.Erru = state_6D4.uiAccErru;
|
|
|
+ cec_msg5.Errv = state_6D4.uiAccErrv;
|
|
|
+ cec_msg5.Errw = state_6D4.uiAccErrw;
|
|
|
+ cec_msg5.Errx = state_6D4.uiAccErrx;
|
|
|
+ cec_msg5.Erry = state_6D4.uiAccErry;
|
|
|
+ cec_msg5.Errz = state_6D4.uiAccErrz;
|
|
|
}
|
|
|
else if(type == 6)
|
|
|
{
|
|
|
- cec_msg6.Erra = state_6D2.uiAccErra;
|
|
|
- cec_msg6.Erraa = state_6D2.uiAccErraa;
|
|
|
- cec_msg6.Errab = state_6D2.uiAccErrab;
|
|
|
- cec_msg6.Errac = state_6D2.uiAccErrac;
|
|
|
- cec_msg6.Errad = state_6D2.uiAccErrad;
|
|
|
- cec_msg6.Errae = state_6D2.uiAccErrae;
|
|
|
- cec_msg6.Erraf = state_6D2.uiAccErraf;
|
|
|
- cec_msg6.Errag = state_6D2.uiAccErrag;
|
|
|
- cec_msg6.Errah = state_6D2.uiAccErrah;
|
|
|
- cec_msg6.Errai = state_6D2.uiAccErrai;
|
|
|
- cec_msg6.Errb = state_6D2.uiAccErrb;
|
|
|
- cec_msg6.Errc = state_6D2.uiAccErrc;
|
|
|
- cec_msg6.Errd = state_6D2.uiAccErrd;
|
|
|
- cec_msg6.Erre = state_6D2.uiAccErre;
|
|
|
- cec_msg6.Errf = state_6D2.uiAccErrf;
|
|
|
- cec_msg6.Errg = state_6D2.uiAccErrg;
|
|
|
- cec_msg6.Errh = state_6D2.uiAccErrh;
|
|
|
- cec_msg6.Erri = state_6D2.uiAccErri;
|
|
|
- cec_msg6.Errj = state_6D2.uiAccErrj;
|
|
|
- cec_msg6.Errk = state_6D2.uiAccErrk;
|
|
|
- cec_msg6.Errl = state_6D2.uiAccErrl;
|
|
|
- cec_msg6.Errm = state_6D2.uiAccErrm;
|
|
|
- cec_msg6.Errn = state_6D2.uiAccErrn;
|
|
|
- cec_msg6.Erro = state_6D2.uiAccErro;
|
|
|
- cec_msg6.Errp = state_6D2.uiAccErrp;
|
|
|
- cec_msg6.Errq = state_6D2.uiAccErrq;
|
|
|
- cec_msg6.Errr = state_6D2.uiAccErrr;
|
|
|
- cec_msg6.Errs = state_6D2.uiAccErrs;
|
|
|
- cec_msg6.Errt = state_6D2.uiAccErrt;
|
|
|
- cec_msg6.Erru = state_6D2.uiAccErru;
|
|
|
- cec_msg6.Errv = state_6D2.uiAccErrv;
|
|
|
- cec_msg6.Errw = state_6D2.uiAccErrw;
|
|
|
- cec_msg6.Errx = state_6D2.uiAccErrx;
|
|
|
- cec_msg6.Erry = state_6D2.uiAccErry;
|
|
|
- cec_msg6.Errz = state_6D2.uiAccErrz;
|
|
|
+ cec_msg6.Erra = state_6D4.uiAccErra;
|
|
|
+ cec_msg6.Erraa = state_6D4.uiAccErraa;
|
|
|
+ cec_msg6.Errab = state_6D4.uiAccErrab;
|
|
|
+ cec_msg6.Errac = state_6D4.uiAccErrac;
|
|
|
+ cec_msg6.Errad = state_6D4.uiAccErrad;
|
|
|
+ cec_msg6.Errae = state_6D4.uiAccErrae;
|
|
|
+ cec_msg6.Erraf = state_6D4.uiAccErraf;
|
|
|
+ cec_msg6.Errag = state_6D4.uiAccErrag;
|
|
|
+ cec_msg6.Errah = state_6D4.uiAccErrah;
|
|
|
+ cec_msg6.Errai = state_6D4.uiAccErrai;
|
|
|
+ cec_msg6.Errb = state_6D4.uiAccErrb;
|
|
|
+ cec_msg6.Errc = state_6D4.uiAccErrc;
|
|
|
+ cec_msg6.Errd = state_6D4.uiAccErrd;
|
|
|
+ cec_msg6.Erre = state_6D4.uiAccErre;
|
|
|
+ cec_msg6.Errf = state_6D4.uiAccErrf;
|
|
|
+ cec_msg6.Errg = state_6D4.uiAccErrg;
|
|
|
+ cec_msg6.Errh = state_6D4.uiAccErrh;
|
|
|
+ cec_msg6.Erri = state_6D4.uiAccErri;
|
|
|
+ cec_msg6.Errj = state_6D4.uiAccErrj;
|
|
|
+ cec_msg6.Errk = state_6D4.uiAccErrk;
|
|
|
+ cec_msg6.Errl = state_6D4.uiAccErrl;
|
|
|
+ cec_msg6.Errm = state_6D4.uiAccErrm;
|
|
|
+ cec_msg6.Errn = state_6D4.uiAccErrn;
|
|
|
+ cec_msg6.Erro = state_6D4.uiAccErro;
|
|
|
+ cec_msg6.Errp = state_6D4.uiAccErrp;
|
|
|
+ cec_msg6.Errq = state_6D4.uiAccErrq;
|
|
|
+ cec_msg6.Errr = state_6D4.uiAccErrr;
|
|
|
+ cec_msg6.Errs = state_6D4.uiAccErrs;
|
|
|
+ cec_msg6.Errt = state_6D4.uiAccErrt;
|
|
|
+ cec_msg6.Erru = state_6D4.uiAccErru;
|
|
|
+ cec_msg6.Errv = state_6D4.uiAccErrv;
|
|
|
+ cec_msg6.Errw = state_6D4.uiAccErrw;
|
|
|
+ cec_msg6.Errx = state_6D4.uiAccErrx;
|
|
|
+ cec_msg6.Erry = state_6D4.uiAccErry;
|
|
|
+ cec_msg6.Errz = state_6D4.uiAccErrz;
|
|
|
}
|
|
|
|
|
|
return true;
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
- // LOG(INFO) << Name() << " PublishState_6D2_6 " << type << " data is repeat";
|
|
|
+ // LOG(INFO) << Name() << " PublishState_6D4_6 " << type << " data is repeat";
|
|
|
return false;
|
|
|
}
|
|
|
}
|
|
@@ -1075,7 +1192,7 @@ void Perception::OnTimer(const ros::TimerEvent &)
|
|
|
|
|
|
if(bus_pub_6D2)
|
|
|
{
|
|
|
- bus_pub_6D2.publish(cec_msg2);
|
|
|
+ bus_pub_6D2.publish(cec_msg22);
|
|
|
}
|
|
|
else
|
|
|
{
|
|
@@ -1084,7 +1201,7 @@ void Perception::OnTimer(const ros::TimerEvent &)
|
|
|
|
|
|
if(bus_pub_6D3)
|
|
|
{
|
|
|
- bus_pub_6D3.publish(cec_msg3);
|
|
|
+ bus_pub_6D3.publish(cec_msg23);
|
|
|
}
|
|
|
else
|
|
|
{
|