|
@@ -7,6 +7,8 @@
|
|
#include <perception/seMsg.h>
|
|
#include <perception/seMsg.h>
|
|
#include <perception/si2Msg.h>
|
|
#include <perception/si2Msg.h>
|
|
#include <perception/cecMsg.h>
|
|
#include <perception/cecMsg.h>
|
|
|
|
+#include <perception/eiMsg.h>
|
|
|
|
+#include <perception/eeMsg.h>
|
|
|
|
|
|
namespace robot {
|
|
namespace robot {
|
|
namespace perception {
|
|
namespace perception {
|
|
@@ -30,7 +32,7 @@ int Perception::Start()
|
|
try
|
|
try
|
|
{
|
|
{
|
|
ros::Subscriber sub = node_handle_->subscribe(robot::common::CanDataAnalysis, 1000, &Perception::CanTopicCallback, this);
|
|
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_6E5 = node_handle_->advertise<::perception::aesMsg>(robot::common::AesData, 2000);
|
|
bus_pub_6EE = node_handle_->advertise<::perception::slMsg>(robot::common::SlData, 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_6ED = node_handle_->advertise<::perception::si2Msg>(robot::common::Si2Data, 2000);
|
|
@@ -43,6 +45,9 @@ int Perception::Start()
|
|
bus_pub_6D5 = node_handle_->advertise<::perception::cecMsg>(robot::common::YsfteData, 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);
|
|
bus_pub_6D6 = node_handle_->advertise<::perception::cecMsg>(robot::common::StreData, 2000);
|
|
|
|
|
|
|
|
+ bus_pub_2E0 = node_handle_->advertise<::perception::eiMsg>(robot::common::EiData, 2000);
|
|
|
|
+ bus_pub_2E1 = node_handle_->advertise<::perception::eeMsg>(robot::common::EeData, 2000);
|
|
|
|
+
|
|
ros::spin();
|
|
ros::spin();
|
|
}
|
|
}
|
|
catch(ros::Exception e)
|
|
catch(ros::Exception e)
|
|
@@ -54,11 +59,6 @@ int Perception::Start()
|
|
return ROBOT_SUCCESS;
|
|
return ROBOT_SUCCESS;
|
|
}
|
|
}
|
|
|
|
|
|
-void Perception::CanTopicCallbackTest(const ::perception::aesMsg::ConstPtr& msg)
|
|
|
|
-{
|
|
|
|
- LOG(INFO) << " Msg Test :" << "1:" << msg->AccMotEnStat << ",2:" << msg->AccClchCtrStat;
|
|
|
|
-}
|
|
|
|
-
|
|
|
|
void Perception::Stop()
|
|
void Perception::Stop()
|
|
{
|
|
{
|
|
LOG(INFO) << Name() << " Stop start...";
|
|
LOG(INFO) << Name() << " Stop start...";
|
|
@@ -70,8 +70,6 @@ void Perception::CanTopicCallback(const canbus::canMsg::ConstPtr& msg)
|
|
{
|
|
{
|
|
LOG(INFO) << " Msg CanData :" << "ID:" << msg->id << ",Data:" << msg->data;
|
|
LOG(INFO) << " Msg CanData :" << "ID:" << msg->id << ",Data:" << msg->data;
|
|
|
|
|
|
- bool sendFlag = false;
|
|
|
|
-
|
|
|
|
switch (msg->id) {
|
|
switch (msg->id) {
|
|
|
|
|
|
case robot::common::ActuatorEnableStatus:
|
|
case robot::common::ActuatorEnableStatus:
|
|
@@ -119,6 +117,16 @@ void Perception::CanTopicCallback(const canbus::canMsg::ConstPtr& msg)
|
|
PublishState_6D2_6(msg, 6, bus_pub_6D6);
|
|
PublishState_6D2_6(msg, 6, bus_pub_6D6);
|
|
}
|
|
}
|
|
break;
|
|
break;
|
|
|
|
+ case robot::common::EstopInfo:
|
|
|
|
+ {
|
|
|
|
+ PublishState_2E0(msg);
|
|
|
|
+ }
|
|
|
|
+ break;
|
|
|
|
+ case robot::common::EstopErr:
|
|
|
|
+ {
|
|
|
|
+ PublishState_2E1(msg);
|
|
|
|
+ }
|
|
|
|
+ break;
|
|
default:
|
|
default:
|
|
break;
|
|
break;
|
|
}
|
|
}
|
|
@@ -146,7 +154,8 @@ bool Perception::PublishState_6E5(const canbus::canMsg::ConstPtr &msg)
|
|
{
|
|
{
|
|
|
|
|
|
memcpy(&s_state_6E5, msg->data.c_str(), copySize);
|
|
memcpy(&s_state_6E5, msg->data.c_str(), copySize);
|
|
-
|
|
|
|
|
|
+ if(msg->data.size() < sizeof(s_state_6E5))
|
|
|
|
+ memset((char*)&s_state_6E5 + msg->data.size(), 0, sizeof(s_state_6E5) - copySize);
|
|
if(bus_pub_6E5)
|
|
if(bus_pub_6E5)
|
|
{
|
|
{
|
|
::perception::aesMsg msg;
|
|
::perception::aesMsg msg;
|
|
@@ -203,7 +212,8 @@ bool Perception::PublishState_6EE(const canbus::canMsg::ConstPtr &msg)
|
|
{
|
|
{
|
|
|
|
|
|
memcpy(&s_state_6EE, msg->data.c_str(), copySize);
|
|
memcpy(&s_state_6EE, msg->data.c_str(), copySize);
|
|
-
|
|
|
|
|
|
+ if(msg->data.size() < sizeof(s_state_6EE))
|
|
|
|
+ memset((char*)&s_state_6EE + msg->data.size(), 0, sizeof(s_state_6EE) - copySize);
|
|
if(bus_pub_6EE)
|
|
if(bus_pub_6EE)
|
|
{
|
|
{
|
|
::perception::slMsg msg;
|
|
::perception::slMsg msg;
|
|
@@ -268,7 +278,8 @@ bool Perception::PublishState_6ED(const canbus::canMsg::ConstPtr &msg)
|
|
{
|
|
{
|
|
|
|
|
|
memcpy(&s_state_6ED, msg->data.c_str(), copySize);
|
|
memcpy(&s_state_6ED, msg->data.c_str(), copySize);
|
|
-
|
|
|
|
|
|
+ if(msg->data.size() < sizeof(s_state_6ED))
|
|
|
|
+ memset((char*)&s_state_6ED + msg->data.size(), 0, sizeof(s_state_6ED) - copySize);
|
|
if(bus_pub_6ED)
|
|
if(bus_pub_6ED)
|
|
{
|
|
{
|
|
::perception::si2Msg msg;
|
|
::perception::si2Msg msg;
|
|
@@ -328,7 +339,8 @@ bool Perception::PublishState_6D1(const canbus::canMsg::ConstPtr &msg)
|
|
{
|
|
{
|
|
|
|
|
|
memcpy(&s_state_6D1, msg->data.c_str(), copySize);
|
|
memcpy(&s_state_6D1, msg->data.c_str(), copySize);
|
|
-
|
|
|
|
|
|
+ if(msg->data.size() < sizeof(s_state_6D1))
|
|
|
|
+ memset((char*)&s_state_6D1 + msg->data.size(), 0, sizeof(s_state_6D1) - copySize);
|
|
if(bus_pub_6D1)
|
|
if(bus_pub_6D1)
|
|
{
|
|
{
|
|
::perception::seMsg msg;
|
|
::perception::seMsg msg;
|
|
@@ -431,7 +443,8 @@ bool Perception::PublishState_6D2_6(const canbus::canMsg::ConstPtr &msg, int typ
|
|
{
|
|
{
|
|
|
|
|
|
memcpy(pErr, msg->data.c_str(), copySize);
|
|
memcpy(pErr, msg->data.c_str(), copySize);
|
|
-
|
|
|
|
|
|
+ if(msg->data.size() < sizeof(s_state_6D2))
|
|
|
|
+ memset((char*)&s_state_6D2 + msg->data.size(), 0, sizeof(s_state_6D2) - copySize);
|
|
if(pub)
|
|
if(pub)
|
|
{
|
|
{
|
|
::perception::cecMsg msg;
|
|
::perception::cecMsg msg;
|
|
@@ -486,6 +499,102 @@ bool Perception::PublishState_6D2_6(const canbus::canMsg::ConstPtr &msg, int typ
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
+bool Perception::PublishState_2E0(const canbus::canMsg::ConstPtr &msg)
|
|
|
|
+{
|
|
|
|
+ static robot::canbusSpace::T_ESTOP_2E0 s_state_2E0;
|
|
|
|
+
|
|
|
|
+ int copySize;
|
|
|
|
+ if(msg->data.size() < sizeof(s_state_2E0))
|
|
|
|
+ {
|
|
|
|
+ copySize = msg->data.size();
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ copySize = sizeof(s_state_2E0);
|
|
|
|
+
|
|
|
|
+ robot::canbusSpace::T_ESTOP_2E0 state_2E0;
|
|
|
|
+ memset(&state_2E0, 0 ,sizeof(state_2E0));
|
|
|
|
+ memcpy(&state_2E0, msg->data.c_str(), copySize);
|
|
|
|
+
|
|
|
|
+ if(state_2E0.uiEstpAbnStat != s_state_2E0.uiEstpAbnStat || state_2E0.uiEstpAlive != state_2E0.uiEstpAlive ||
|
|
|
|
+ state_2E0.uiEstpEnStat != s_state_2E0.uiEstpEnStat || state_2E0.uiPmpEnStat != s_state_2E0.uiPmpEnStat )
|
|
|
|
+ {
|
|
|
|
|
|
|
|
+ memcpy(&s_state_2E0, msg->data.c_str(), copySize);
|
|
|
|
+ if(msg->data.size() < sizeof(s_state_2E0))
|
|
|
|
+ memset((char*)&s_state_2E0 + msg->data.size(), 0, sizeof(s_state_2E0) - copySize);
|
|
|
|
+ if(bus_pub_2E0)
|
|
|
|
+ {
|
|
|
|
+ ::perception::eiMsg msg;
|
|
|
|
+ msg.EstpAbnStat = state_2E0.uiEstpAbnStat;
|
|
|
|
+ msg.EstpAlive = state_2E0.uiEstpAlive;
|
|
|
|
+ msg.EstpEnStat = state_2E0.uiEstpEnStat;
|
|
|
|
+ msg.PmpEnStat = state_2E0.uiPmpEnStat;
|
|
|
|
+ bus_pub_2E0.publish(msg);
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+ LOG(INFO) << Name() << " PublishState_2E0 publisher is unvalid";
|
|
|
|
+ return false;
|
|
|
|
+ }
|
|
|
|
+ return true;
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+ LOG(INFO) << Name() << " PublishState_2E0 data is repeat";
|
|
|
|
+ return false;
|
|
|
|
+ }
|
|
|
|
+}
|
|
|
|
+
|
|
|
|
+bool Perception::PublishState_2E1(const canbus::canMsg::ConstPtr &msg)
|
|
|
|
+{
|
|
|
|
+ static robot::canbusSpace::T_ESTOP_2E1 s_state_2E1;
|
|
|
|
+
|
|
|
|
+ int copySize;
|
|
|
|
+ if(msg->data.size() < sizeof(s_state_2E1))
|
|
|
|
+ copySize = msg->data.size();
|
|
|
|
+ else
|
|
|
|
+ copySize = sizeof(s_state_2E1);
|
|
|
|
+
|
|
|
|
+ robot::canbusSpace::T_ESTOP_2E1 state_2E1;
|
|
|
|
+ memset(&state_2E1, 0 ,sizeof(state_2E1));
|
|
|
|
+ memcpy(&state_2E1, msg->data.c_str(), copySize);
|
|
|
|
+
|
|
|
|
+ if(state_2E1.uiEspErra != s_state_2E1.uiEspErra || state_2E1.uiEspErrb != s_state_2E1.uiEspErrb || state_2E1.uiEspErrc != s_state_2E1.uiEspErrc ||
|
|
|
|
+ state_2E1.uiEspErrd != s_state_2E1.uiEspErrd || state_2E1.uiEspErre != s_state_2E1.uiEspErre || state_2E1.uiEspErrf != s_state_2E1.uiEspErrf ||
|
|
|
|
+ state_2E1.uiEspErrg != s_state_2E1.uiEspErrg || state_2E1.uiEspErrh != s_state_2E1.uiEspErrh || state_2E1.uiEspErri != s_state_2E1.uiEspErri ||
|
|
|
|
+ state_2E1.uiEspErrj != s_state_2E1.uiEspErrj)
|
|
|
|
+ {
|
|
|
|
+
|
|
|
|
+ memcpy(&s_state_2E1, msg->data.c_str(), copySize);
|
|
|
|
+ if(msg->data.size() < sizeof(s_state_2E1))
|
|
|
|
+ memset((char*)&s_state_2E1 + msg->data.size(), 0, sizeof(s_state_2E1) - copySize);
|
|
|
|
+ if(bus_pub_2E1)
|
|
|
|
+ {
|
|
|
|
+ ::perception::eeMsg msg;
|
|
|
|
+ msg.uiEspErra = state_2E1.uiEspErra;
|
|
|
|
+ msg.uiEspErrb = state_2E1.uiEspErrb;
|
|
|
|
+ msg.uiEspErrc = state_2E1.uiEspErrc;
|
|
|
|
+ msg.uiEspErrd = state_2E1.uiEspErrd;
|
|
|
|
+ msg.uiEspErre = state_2E1.uiEspErre;
|
|
|
|
+ msg.uiEspErrf = state_2E1.uiEspErrf;
|
|
|
|
+ msg.uiEspErrg = state_2E1.uiEspErrg;
|
|
|
|
+ msg.uiEspErrh = state_2E1.uiEspErrh;
|
|
|
|
+ msg.uiEspErri = state_2E1.uiEspErri;
|
|
|
|
+ msg.uiEspErrj = state_2E1.uiEspErrj;
|
|
|
|
+ bus_pub_2E1.publish(msg);
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+ LOG(INFO) << Name() << " PublishState_2E1 publisher is unvalid";
|
|
|
|
+ return false;
|
|
|
|
+ }
|
|
|
|
+ return true;
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+ LOG(INFO) << Name() << " PublishState_2E1 data is repeat";
|
|
|
|
+ return false;
|
|
|
|
+ }
|
|
|
|
+}
|
|
} // namespace perception
|
|
} // namespace perception
|
|
} // namespace robot
|
|
} // namespace robot
|