|
@@ -2,7 +2,7 @@
|
|
|
#include "../common/message.h"
|
|
|
#include "../common/canmsgobdmsgid.h"
|
|
|
#include "../canbus/candatatype.h"
|
|
|
-
|
|
|
+#include <perception/aesMsg.h>
|
|
|
|
|
|
|
|
|
|
|
@@ -10,8 +10,6 @@ namespace robot {
|
|
|
namespace perception {
|
|
|
|
|
|
|
|
|
-ros::Publisher Perception::s_perData_pub;
|
|
|
-
|
|
|
std::string Perception::Name() const { return "perception"; }
|
|
|
|
|
|
int Perception::Init()
|
|
@@ -20,14 +18,19 @@ int Perception::Init()
|
|
|
|
|
|
node_handle_.reset(new ros::NodeHandle());
|
|
|
|
|
|
- s_perData_pub = node_handle_->advertise<canbus::canMsg>(robot::common::PerceptionData, 2000);
|
|
|
-
|
|
|
LOG(INFO) << Name() << " Init end.";
|
|
|
}
|
|
|
|
|
|
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();
|
|
|
}
|
|
|
|
|
@@ -44,50 +47,65 @@ void Perception::CanTopicCallback(const canbus::canMsg::ConstPtr& msg)
|
|
|
bool sendFlag = false;
|
|
|
|
|
|
switch (msg->id) {
|
|
|
- case robot::common::PowerOnReq:
|
|
|
- {
|
|
|
- static robot::canbusSpace::T_CONTROL_CMD_5F1 cmd_5F1_PRO;
|
|
|
- memset(&cmd_5F1_PRO, 0 ,sizeof(cmd_5F1_PRO));
|
|
|
-
|
|
|
- robot::canbusSpace::T_CONTROL_CMD_5F1 cmd_5F1_REV;
|
|
|
- memcpy(&cmd_5F1_REV, msg->data.c_str(), sizeof(cmd_5F1_REV));
|
|
|
|
|
|
- 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)
|
|
|
- {
|
|
|
- sendFlag = true;
|
|
|
- }
|
|
|
-
|
|
|
- memcpy(&cmd_5F1_PRO, msg->data.c_str(), sizeof(cmd_5F1_PRO));
|
|
|
- }
|
|
|
- break;
|
|
|
case robot::common::ActuatorEnableStatus:
|
|
|
{
|
|
|
- static robot::canbusSpace::T_STATE_OUT_6E5 s_state_6E5;
|
|
|
- memset(&s_state_6E5, 0 ,sizeof(s_state_6E5));
|
|
|
-
|
|
|
- robot::canbusSpace::T_STATE_OUT_6E5 state_6E5;
|
|
|
- memcpy(&state_6E5, msg->data.c_str(), sizeof(state_6E5));
|
|
|
-
|
|
|
- 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);
|
|
|
- {
|
|
|
- sendFlag = true;
|
|
|
- }
|
|
|
+ PublishState_6E5(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));
|
|
|
|
|
|
- if (sendFlag)
|
|
|
+ robot::canbusSpace::T_STATE_OUT_6E5 state_6E5;
|
|
|
+ memcpy(&state_6E5, msg->data.c_str(), sizeof(state_6E5));
|
|
|
+
|
|
|
+ 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
|
|
|
+ {
|
|
|
+ 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;
|
|
|
+ }
|
|
|
+ }
|
|
|
+ catch(ros::Exception e)
|
|
|
+ {
|
|
|
+ LOG(INFO) << Name() << " PublishState_6E5 exception is:" << e.what() ;
|
|
|
+ return false;
|
|
|
+ }
|
|
|
+ return true;
|
|
|
+ }
|
|
|
+ else
|
|
|
{
|
|
|
- s_perData_pub.publish(msg);
|
|
|
+ LOG(INFO) << Name() << " PublishState_6E5 data is repeat";
|
|
|
+ return false;
|
|
|
}
|
|
|
}
|
|
|
|