|
@@ -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
|