|
@@ -2,16 +2,14 @@
|
|
#include "../common/message.h"
|
|
#include "../common/message.h"
|
|
#include "../common/canmsgobdmsgid.h"
|
|
#include "../common/canmsgobdmsgid.h"
|
|
#include "../canbus/candatatype.h"
|
|
#include "../canbus/candatatype.h"
|
|
-
|
|
|
|
-
|
|
|
|
-
|
|
|
|
|
|
+#include <perception/aesMsg.h>
|
|
|
|
+#include <perception/slMsg.h>
|
|
|
|
+#include <perception/seMsg.h>
|
|
|
|
|
|
namespace robot {
|
|
namespace robot {
|
|
namespace perception {
|
|
namespace perception {
|
|
|
|
|
|
|
|
|
|
-ros::Publisher Perception::s_perData_pub;
|
|
|
|
-
|
|
|
|
std::string Perception::Name() const { return "perception"; }
|
|
std::string Perception::Name() const { return "perception"; }
|
|
|
|
|
|
int Perception::Init()
|
|
int Perception::Init()
|
|
@@ -20,14 +18,19 @@ int Perception::Init()
|
|
|
|
|
|
node_handle_.reset(new ros::NodeHandle());
|
|
node_handle_.reset(new ros::NodeHandle());
|
|
|
|
|
|
- s_perData_pub = node_handle_->advertise<canbus::canMsg>(robot::common::PerceptionData, 2000);
|
|
|
|
-
|
|
|
|
LOG(INFO) << Name() << " Init end.";
|
|
LOG(INFO) << Name() << " Init end.";
|
|
}
|
|
}
|
|
|
|
|
|
int Perception::Start()
|
|
int Perception::Start()
|
|
{
|
|
{
|
|
- ros::Subscriber sub = node_handle_->subscribe(robot::common::CanDataAnalysis, 1000, CanTopicCallback);
|
|
|
|
|
|
+ try
|
|
|
|
+ {
|
|
|
|
+ ros::Subscriber sub = node_handle_->subscribe(robot::common::CanDataAnalysis, 1000, &Perception::CanTopicCallback, this);
|
|
|
|
+ }
|
|
|
|
+ catch(ros::Exception e)
|
|
|
|
+ {
|
|
|
|
+ LOG(INFO) << Name() << " Start exception is:" << e.what() ;
|
|
|
|
+ }
|
|
ros::spin();
|
|
ros::spin();
|
|
}
|
|
}
|
|
|
|
|
|
@@ -44,52 +47,223 @@ void Perception::CanTopicCallback(const canbus::canMsg::ConstPtr& msg)
|
|
bool sendFlag = false;
|
|
bool sendFlag = false;
|
|
|
|
|
|
switch (msg->id) {
|
|
switch (msg->id) {
|
|
- case robot::common::PowerOnReq:
|
|
|
|
|
|
+
|
|
|
|
+ case robot::common::ActuatorEnableStatus:
|
|
{
|
|
{
|
|
- static robot::canbusSpace::T_CONTROL_CMD_5F1 cmd_5F1_PRO;
|
|
|
|
- memset(&cmd_5F1_PRO, 0 ,sizeof(cmd_5F1_PRO));
|
|
|
|
|
|
+ PublishState_6E5(msg);
|
|
|
|
+ }
|
|
|
|
+ break;
|
|
|
|
+ case robot::common::SysLearnBack:
|
|
|
|
+ {
|
|
|
|
+ PublishState_6EE(msg);
|
|
|
|
+ }
|
|
|
|
+ break;
|
|
|
|
+ case robot::common::SysWarnErr:
|
|
|
|
+ {
|
|
|
|
+ PublishState_6D1(msg);
|
|
|
|
+ }
|
|
|
|
+ break;
|
|
|
|
+ default:
|
|
|
|
+ break;
|
|
|
|
+ }
|
|
|
|
+}
|
|
|
|
+
|
|
|
|
+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));
|
|
|
|
|
|
- robot::canbusSpace::T_CONTROL_CMD_5F1 cmd_5F1_REV;
|
|
|
|
- memcpy(&cmd_5F1_REV, msg->data.c_str(), sizeof(cmd_5F1_REV));
|
|
|
|
|
|
+ robot::canbusSpace::T_STATE_OUT_6E5 state_6E5;
|
|
|
|
+ memcpy(&state_6E5, msg->data.c_str(), sizeof(state_6E5));
|
|
|
|
|
|
- if(cmd_5F1_REV.uiAccCtrPowReq != cmd_5F1_PRO.uiAccCtrPowReq && cmd_5F1_REV.uiBrkCtrPowReq != cmd_5F1_PRO.uiBrkCtrPowReq &&
|
|
|
|
- cmd_5F1_REV.uiClchCtrPowReq != cmd_5F1_PRO.uiClchCtrPowReq && cmd_5F1_REV.uiRCUPowReq != cmd_5F1_PRO.uiRCUPowReq &&
|
|
|
|
- cmd_5F1_REV.uiStrCtrPowReq != cmd_5F1_PRO.uiStrCtrPowReq && cmd_5F1_REV.uiXSftArmCtrPowReq != cmd_5F1_PRO.uiXSftArmCtrPowReq &&
|
|
|
|
- cmd_5F1_REV.uiYSftArmCtrPowReq != cmd_5F1_PRO.uiYSftArmCtrPowReq)
|
|
|
|
|
|
+ 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 ||
|
|
|
|
+ state_6E5.uiClchMotEnStat != s_state_6E5.uiClchMotEnStat || state_6E5.uiPauseEnStat != s_state_6E5.uiPauseEnStat ||
|
|
|
|
+ state_6E5.uiStrMotEnStat != s_state_6E5.uiStrMotEnStat || state_6E5.uiXSftMotEnStat != s_state_6E5.uiXSftMotEnStat ||
|
|
|
|
+ state_6E5.uiYSftMotEnStat != s_state_6E5.uiYSftMotEnStat)
|
|
|
|
+ {
|
|
|
|
+ try
|
|
{
|
|
{
|
|
- sendFlag = true;
|
|
|
|
|
|
+ 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;
|
|
|
|
+ }
|
|
}
|
|
}
|
|
-
|
|
|
|
- memcpy(&cmd_5F1_PRO, msg->data.c_str(), sizeof(cmd_5F1_PRO));
|
|
|
|
|
|
+ catch(ros::Exception e)
|
|
|
|
+ {
|
|
|
|
+ LOG(INFO) << Name() << " PublishState_6E5 exception is:" << e.what() ;
|
|
|
|
+ return false;
|
|
|
|
+ }
|
|
|
|
+ return true;
|
|
}
|
|
}
|
|
- break;
|
|
|
|
- case robot::common::ActuatorEnableStatus:
|
|
|
|
|
|
+ else
|
|
{
|
|
{
|
|
- static robot::canbusSpace::T_STATE_OUT_6E5 s_state_6E5;
|
|
|
|
- memset(&s_state_6E5, 0 ,sizeof(s_state_6E5));
|
|
|
|
|
|
+ LOG(INFO) << Name() << " PublishState_6E5 data is repeat";
|
|
|
|
+ return false;
|
|
|
|
+ }
|
|
|
|
+}
|
|
|
|
|
|
- robot::canbusSpace::T_STATE_OUT_6E5 state_6E5;
|
|
|
|
- memcpy(&state_6E5, msg->data.c_str(), sizeof(state_6E5));
|
|
|
|
|
|
+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));
|
|
|
|
+
|
|
|
|
+ robot::canbusSpace::T_STATE_OUT_6EE state_6EE;
|
|
|
|
+ memcpy(&state_6EE, msg->data.c_str(), sizeof(state_6EE));
|
|
|
|
|
|
- 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 &&
|
|
|
|
- state_6E5.uiClchMotEnStat != s_state_6E5.uiClchMotEnStat && state_6E5.uiPauseEnStat != s_state_6E5.uiPauseEnStat &&
|
|
|
|
- state_6E5.uiStrMotEnStat != s_state_6E5.uiStrMotEnStat && state_6E5.uiXSftMotEnStat != s_state_6E5.uiXSftMotEnStat &&
|
|
|
|
- state_6E5.uiYSftMotEnStat != s_state_6E5.uiYSftMotEnStat);
|
|
|
|
|
|
+ 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 ||
|
|
|
|
+ state_6EE.uiBrkLrnStat != s_state_6EE.uiBrkLrnStat || state_6EE.uiBrkMaxLrnStat != s_state_6EE.uiBrkMaxLrnStat ||
|
|
|
|
+ state_6EE.uiClch0LrnStat != s_state_6EE.uiClch0LrnStat || state_6EE.uiClchLrnStat != s_state_6EE.uiClchLrnStat ||
|
|
|
|
+ state_6EE.uiClchMaxLrnStat != s_state_6EE.uiClchMaxLrnStat || state_6EE.uiDLrnStat != s_state_6EE.uiDLrnStat ||
|
|
|
|
+ state_6EE.uiDpLrnStat != s_state_6EE.uiDpLrnStat || state_6EE.uiMLrnStat != s_state_6EE.uiMLrnStat ||
|
|
|
|
+ state_6EE.uiNLrnStat != s_state_6EE.uiNLrnStat || state_6EE.uiPLrnStat != s_state_6EE.uiPLrnStat ||
|
|
|
|
+ state_6EE.uiRLrnStat != s_state_6EE.uiRLrnStat || state_6EE.uiSftLrnStat != s_state_6EE.uiSftLrnStat ||
|
|
|
|
+ state_6EE.uiStr0LrnStat != s_state_6EE.uiStr0LrnStat || state_6EE.uiStrLLrnStat != s_state_6EE.uiStrLLrnStat ||
|
|
|
|
+ state_6EE.uiStrLrnStat != s_state_6EE.uiStrLrnStat || state_6EE.uiStrRLrnStat != s_state_6EE.uiStrRLrnStat ||
|
|
|
|
+ state_6EE.uiUpLrnStat != s_state_6EE.uiUpLrnStat)
|
|
|
|
+ {
|
|
|
|
+ try
|
|
{
|
|
{
|
|
- sendFlag = true;
|
|
|
|
|
|
+ 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;
|
|
|
|
+ }
|
|
}
|
|
}
|
|
|
|
+ catch(ros::Exception e)
|
|
|
|
+ {
|
|
|
|
+ LOG(INFO) << Name() << " PublishState_6EE exception is:" << e.what() ;
|
|
|
|
+ return false;
|
|
|
|
+ }
|
|
|
|
+ return true;
|
|
}
|
|
}
|
|
- break;
|
|
|
|
- default:
|
|
|
|
- break;
|
|
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+ LOG(INFO) << Name() << " PublishState_6EE data is repeat";
|
|
|
|
+ return false;
|
|
}
|
|
}
|
|
|
|
+}
|
|
|
|
|
|
- if (sendFlag)
|
|
|
|
|
|
+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));
|
|
|
|
+
|
|
|
|
+ robot::canbusSpace::T_STATE_OUT_6D1 state_6D1;
|
|
|
|
+ memcpy(&state_6D1, msg->data.c_str(), sizeof(state_6D1));
|
|
|
|
+
|
|
|
|
+ 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 ||
|
|
|
|
+ state_6D1.uiSysErrg != s_state_6D1.uiSysErrg || state_6D1.uiSysErrh != s_state_6D1.uiSysErrh || state_6D1.uiSysErri != s_state_6D1.uiSysErri ||
|
|
|
|
+ state_6D1.uiSysErrj != s_state_6D1.uiSysErrj || state_6D1.uiSysErrk != s_state_6D1.uiSysErrk || state_6D1.uiSysErrl != s_state_6D1.uiSysErrl ||
|
|
|
|
+ state_6D1.uiSysErrm != s_state_6D1.uiSysErrm || state_6D1.uiSysErrn != s_state_6D1.uiSysErrn || state_6D1.uiSysErro != s_state_6D1.uiSysErro ||
|
|
|
|
+ state_6D1.uiSysErrp != s_state_6D1.uiSysErrp || state_6D1.uiSysErrq != s_state_6D1.uiSysErrq || state_6D1.uiSysErrr != s_state_6D1.uiSysErrr ||
|
|
|
|
+ state_6D1.uiSysErrs != s_state_6D1.uiSysErrs || state_6D1.uiSysErrt != s_state_6D1.uiSysErrt || state_6D1.uiSysErru != s_state_6D1.uiSysErru ||
|
|
|
|
+ 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
|
|
|
|
+ {
|
|
|
|
+ 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;
|
|
|
|
+ }
|
|
|
|
+ }
|
|
|
|
+ catch(ros::Exception e)
|
|
|
|
+ {
|
|
|
|
+ LOG(INFO) << Name() << " PublishState_6D1 exception is:" << e.what() ;
|
|
|
|
+ return false;
|
|
|
|
+ }
|
|
|
|
+ return true;
|
|
|
|
+ }
|
|
|
|
+ else
|
|
{
|
|
{
|
|
- s_perData_pub.publish(msg);
|
|
|
|
|
|
+ LOG(INFO) << Name() << " PublishState_6D1 data is repeat";
|
|
|
|
+ return false;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
-} // namespace perception
|
|
|
|
|
|
+
|
|
|
|
+
|
|
|
|
+
|
|
|
|
+
|
|
|
|
+
|
|
|
|
+ } // namespace perception
|
|
} // namespace robot
|
|
} // namespace robot
|