|
@@ -2,13 +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>
|
|
|
|
-#include <perception/si2Msg.h>
|
|
|
|
-#include <perception/cecMsg.h>
|
|
|
|
-#include <perception/eiMsg.h>
|
|
|
|
-#include <perception/eeMsg.h>
|
|
|
|
|
|
+#include "perception/aesMsg.h"
|
|
|
|
+#include "perception/slMsg.h"
|
|
|
|
+#include "perception/seMsg.h"
|
|
|
|
+#include "perception/si2Msg.h"
|
|
|
|
+#include "perception/cecMsg.h"
|
|
|
|
+#include "perception/eiMsg.h"
|
|
|
|
+#include "perception/eeMsg.h"
|
|
|
|
+#include "canbus/ActuatorEnableRequest.h"
|
|
|
|
|
|
namespace robot {
|
|
namespace robot {
|
|
namespace perception {
|
|
namespace perception {
|
|
@@ -48,11 +49,13 @@ int Perception::Start()
|
|
bus_pub_2E0 = node_handle_->advertise<::perception::eiMsg>(robot::common::EiData, 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);
|
|
bus_pub_2E1 = node_handle_->advertise<::perception::eeMsg>(robot::common::EeData, 2000);
|
|
|
|
|
|
|
|
+ clientActuatorEnableRequest = node_handle_->serviceClient<canbus::ActuatorEnableRequest>(robot::common::ActuatorEnableService);
|
|
|
|
+
|
|
ros::spin();
|
|
ros::spin();
|
|
}
|
|
}
|
|
catch(ros::Exception e)
|
|
catch(ros::Exception e)
|
|
{
|
|
{
|
|
- LOG(INFO) << Name() << " Start exception is:" << e.what();
|
|
|
|
|
|
+ LOG(ERROR) << Name() << " Start exception is:" << e.what();
|
|
return ROBOT_FAILTURE;
|
|
return ROBOT_FAILTURE;
|
|
}
|
|
}
|
|
LOG(INFO) << Name() << " Start end.";
|
|
LOG(INFO) << Name() << " Start end.";
|
|
@@ -89,31 +92,37 @@ void Perception::CanTopicCallback(const canbus::canMsg::ConstPtr& msg)
|
|
break;
|
|
break;
|
|
case robot::common::SysWarnErr:
|
|
case robot::common::SysWarnErr:
|
|
{
|
|
{
|
|
|
|
+ SoftStopControl();
|
|
PublishState_6D1(msg);
|
|
PublishState_6D1(msg);
|
|
}
|
|
}
|
|
break;
|
|
break;
|
|
case robot::common::AccErr:
|
|
case robot::common::AccErr:
|
|
{
|
|
{
|
|
|
|
+ SoftStopControl();
|
|
PublishState_6D2_6(msg, 2, bus_pub_6D2);
|
|
PublishState_6D2_6(msg, 2, bus_pub_6D2);
|
|
}
|
|
}
|
|
break;
|
|
break;
|
|
case robot::common::BrkErr:
|
|
case robot::common::BrkErr:
|
|
{
|
|
{
|
|
|
|
+ SoftStopControl();
|
|
PublishState_6D2_6(msg, 3, bus_pub_6D3);
|
|
PublishState_6D2_6(msg, 3, bus_pub_6D3);
|
|
}
|
|
}
|
|
break;
|
|
break;
|
|
case robot::common::XsftErr:
|
|
case robot::common::XsftErr:
|
|
{
|
|
{
|
|
|
|
+ SoftStopControl();
|
|
PublishState_6D2_6(msg, 4, bus_pub_6D4);
|
|
PublishState_6D2_6(msg, 4, bus_pub_6D4);
|
|
}
|
|
}
|
|
break;
|
|
break;
|
|
case robot::common::YsftErr:
|
|
case robot::common::YsftErr:
|
|
{
|
|
{
|
|
|
|
+ SoftStopControl();
|
|
PublishState_6D2_6(msg, 5, bus_pub_6D5);
|
|
PublishState_6D2_6(msg, 5, bus_pub_6D5);
|
|
}
|
|
}
|
|
break;
|
|
break;
|
|
case robot::common::StrErr:
|
|
case robot::common::StrErr:
|
|
{
|
|
{
|
|
|
|
+ SoftStopControl();
|
|
PublishState_6D2_6(msg, 6, bus_pub_6D6);
|
|
PublishState_6D2_6(msg, 6, bus_pub_6D6);
|
|
}
|
|
}
|
|
break;
|
|
break;
|
|
@@ -172,7 +181,7 @@ bool Perception::PublishState_6E5(const canbus::canMsg::ConstPtr &msg)
|
|
}
|
|
}
|
|
else
|
|
else
|
|
{
|
|
{
|
|
- LOG(INFO) << Name() << " PublishState_6E5 publisher is unvalid";
|
|
|
|
|
|
+ LOG(ERROR) << Name() << " PublishState_6E5 publisher is unvalid";
|
|
return false;
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
return true;
|
|
@@ -242,7 +251,7 @@ bool Perception::PublishState_6EE(const canbus::canMsg::ConstPtr &msg)
|
|
}
|
|
}
|
|
else
|
|
else
|
|
{
|
|
{
|
|
- LOG(INFO) << Name() << " PublishState_6EE publisher is unvalid";
|
|
|
|
|
|
+ LOG(ERROR) << Name() << " PublishState_6EE publisher is unvalid";
|
|
return false;
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
return true;
|
|
@@ -301,7 +310,7 @@ bool Perception::PublishState_6ED(const canbus::canMsg::ConstPtr &msg)
|
|
}
|
|
}
|
|
else
|
|
else
|
|
{
|
|
{
|
|
- LOG(INFO) << Name() << " PublishState_6ED publisher is unvalid";
|
|
|
|
|
|
+ LOG(ERROR) << Name() << " PublishState_6ED publisher is unvalid";
|
|
return false;
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
return true;
|
|
@@ -374,7 +383,7 @@ bool Perception::PublishState_6D1(const canbus::canMsg::ConstPtr &msg)
|
|
}
|
|
}
|
|
else
|
|
else
|
|
{
|
|
{
|
|
- LOG(INFO) << Name() << " PublishState_6D1 publisher is unvalid";
|
|
|
|
|
|
+ LOG(ERROR) << Name() << " PublishState_6D1 publisher is unvalid";
|
|
return false;
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
return true;
|
|
@@ -487,7 +496,7 @@ bool Perception::PublishState_6D2_6(const canbus::canMsg::ConstPtr &msg, int typ
|
|
}
|
|
}
|
|
else
|
|
else
|
|
{
|
|
{
|
|
- LOG(INFO) << Name() << " PublishState_6D2_6 publisher " << type <<" is unvalid";
|
|
|
|
|
|
+ LOG(ERROR) << Name() << " PublishState_6D2_6 publisher " << type <<" is unvalid";
|
|
return false;
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
return true;
|
|
@@ -533,7 +542,7 @@ bool Perception::PublishState_2E0(const canbus::canMsg::ConstPtr &msg)
|
|
}
|
|
}
|
|
else
|
|
else
|
|
{
|
|
{
|
|
- LOG(INFO) << Name() << " PublishState_2E0 publisher is unvalid";
|
|
|
|
|
|
+ LOG(ERROR) << Name() << " PublishState_2E0 publisher is unvalid";
|
|
return false;
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
return true;
|
|
@@ -585,7 +594,7 @@ bool Perception::PublishState_2E1(const canbus::canMsg::ConstPtr &msg)
|
|
}
|
|
}
|
|
else
|
|
else
|
|
{
|
|
{
|
|
- LOG(INFO) << Name() << " PublishState_2E1 publisher is unvalid";
|
|
|
|
|
|
+ LOG(ERROR) << Name() << " PublishState_2E1 publisher is unvalid";
|
|
return false;
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
return true;
|
|
@@ -596,5 +605,25 @@ bool Perception::PublishState_2E1(const canbus::canMsg::ConstPtr &msg)
|
|
return false;
|
|
return false;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
+
|
|
|
|
+bool Perception::SoftStopControl()
|
|
|
|
+{
|
|
|
|
+ canbus::ActuatorEnableRequest srv;
|
|
|
|
+ // 系统软急停使能: enable
|
|
|
|
+ srv.request.uiPauseEnReq = 1;
|
|
|
|
+ // 系统控制模式请求: 人工驾驶
|
|
|
|
+ srv.request.uiCtrMdReq = 3;
|
|
|
|
+
|
|
|
|
+ if (clientActuatorEnableRequest.call(srv))
|
|
|
|
+ {
|
|
|
|
+ LOG(INFO) << Name() << " SoftStopControl call service return success, value = " << srv.response.uiReturn;
|
|
|
|
+ return true;
|
|
|
|
+ }
|
|
|
|
+ else
|
|
|
|
+ {
|
|
|
|
+ LOG(ERROR) << Name() << " SoftStopControl call service return fail";
|
|
|
|
+ return false;
|
|
|
|
+ }
|
|
|
|
+}
|
|
} // namespace perception
|
|
} // namespace perception
|
|
} // namespace robot
|
|
} // namespace robot
|