|
@@ -5,6 +5,7 @@
|
|
|
#include <perception/aesMsg.h>
|
|
|
#include <perception/slMsg.h>
|
|
|
#include <perception/seMsg.h>
|
|
|
+#include <perception/si2Msg.h>
|
|
|
|
|
|
namespace robot {
|
|
|
namespace perception {
|
|
@@ -53,6 +54,11 @@ void Perception::CanTopicCallback(const canbus::canMsg::ConstPtr& msg)
|
|
|
PublishState_6E5(msg);
|
|
|
}
|
|
|
break;
|
|
|
+ case robot::common::RSysInfoBack2:
|
|
|
+ {
|
|
|
+ PublishState_6ED(msg);
|
|
|
+ }
|
|
|
+ break;
|
|
|
case robot::common::SysLearnBack:
|
|
|
{
|
|
|
PublishState_6EE(msg);
|
|
@@ -188,6 +194,65 @@ 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));
|
|
|
+
|
|
|
+ robot::canbusSpace::T_STATE_OUT_6ED state_6ED;
|
|
|
+ memcpy(&state_6ED, msg->data.c_str(), sizeof(state_6ED));
|
|
|
+
|
|
|
+ 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 ||
|
|
|
+ state_6ED.uiClchCtrAlive != s_state_6ED.uiClchCtrAlive || state_6ED.uiClchMotClbr != s_state_6ED.uiClchMotClbr ||
|
|
|
+ state_6ED.uiCtrMdStat != s_state_6ED.uiCtrMdStat || state_6ED.uiRCUAlive != s_state_6ED.uiRCUAlive ||
|
|
|
+ state_6ED.uiStrCtrAlive != s_state_6ED.uiStrCtrAlive || state_6ED.uiStrMotClbr != s_state_6ED.uiStrMotClbr ||
|
|
|
+ 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
|
|
|
+ {
|
|
|
+ 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;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ catch(ros::Exception e)
|
|
|
+ {
|
|
|
+ LOG(INFO) << Name() << " PublishState_6ED exception is:" << e.what() ;
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+ else
|
|
|
+ {
|
|
|
+ LOG(INFO) << Name() << " PublishState_6ED data is repeat";
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
bool Perception::PublishState_6D1(const canbus::canMsg::ConstPtr &msg)
|
|
|
{
|
|
|
static robot::canbusSpace::T_STATE_OUT_6D1 s_state_6D1;
|