SMACC
Loading...
Searching...
No Matches
Public Member Functions | Public Attributes | List of all members
SmaccTestRuntimeNode Class Reference
Collaboration diagram for SmaccTestRuntimeNode:
Collaboration graph

Public Member Functions

template<typename T >
std::shared_ptr< TestPolicytryTestPolicyFactory (std::string matchname, XmlRpc::XmlRpcValue &initXmlRpcValue)
 
void init ()
 
void success (std::string msg)
 
void failure (std::string msg)
 
void update ()
 
void onRosOutCallback (const rosgraph_msgs::Log &msg)
 
void onSmaccTransitionCallback (const smacc_msgs::SmaccTransitionLogEntry &msg)
 

Public Attributes

ros::NodeHandle nh_
 
ros::Subscriber rosoutSub_
 
ros::Subscriber smaccTransitionSub_
 
std::vector< std::shared_ptr< TestPolicy > > testPolicies
 
boost::signals2::signal< void(const rosgraph_msgs::Log &)> onRosOutMsg
 
boost::signals2::signal< void(const smacc_msgs::SmaccTransitionLogEntry &)> onSmaccTransition
 

Detailed Description

Definition at line 28 of file smacc_runtime_test_node.cpp.

Member Function Documentation

◆ failure()

void SmaccTestRuntimeNode::failure ( std::string  msg)
inline

Definition at line 105 of file smacc_runtime_test_node.cpp.

106 {
107 ROS_FATAL_STREAM("SmaccRuntimeTestNode policy thrown failure exit: " << msg);
108 exit(-1);
109 }

Referenced by TimeoutFailureTestPolicy::update().

Here is the caller graph for this function:

◆ init()

void SmaccTestRuntimeNode::init ( )
inline

Definition at line 61 of file smacc_runtime_test_node.cpp.

62 {
63 ROS_INFO("[SmaccTestruntimeNode] subscribing rosout");
64
65 ros::NodeHandle private_nh("~");
66 std::string stateMachineROSParamWs;
67 if (private_nh.getParam("state_machine_rosparam_ws", stateMachineROSParamWs))
68 {
69 ROS_INFO_STREAM("state machine param ws: " << stateMachineROSParamWs);
70 }
71
72 rosoutSub_ = nh_.subscribe("/rosout", 1, &SmaccTestRuntimeNode::onRosOutCallback, this);
73 smaccTransitionSub_ = nh_.subscribe(stateMachineROSParamWs + "/smacc/transition_log", 1, &SmaccTestRuntimeNode::onSmaccTransitionCallback, this);
74
75 XmlRpc::XmlRpcValue successSwitchParam;
76 private_nh.getParam("success_switch", successSwitchParam);
77 ROS_INFO_STREAM("[SmaccTestruntimeNode] success switch" << successSwitchParam);
78 ROS_ASSERT(successSwitchParam.getType() == XmlRpc::XmlRpcValue::TypeArray);
79
80 for (int32_t i = 0; i < successSwitchParam.size(); ++i)
81 {
82 auto initXmlRpcValue = successSwitchParam[i];
83 if (tryTestPolicyFactory<ReachedStateSuccessTestPolicy>("state_reached", initXmlRpcValue) != nullptr)
84 continue;
85 }
86
87 XmlRpc::XmlRpcValue failureSwitchParam;
88 private_nh.getParam("failure_switch", failureSwitchParam);
89 ROS_INFO_STREAM("[SmaccTestruntimeNode] failure switch" << failureSwitchParam);
90 ROS_ASSERT(failureSwitchParam.getType() == XmlRpc::XmlRpcValue::TypeArray);
91
92 for (int32_t i = 0; i < failureSwitchParam.size(); ++i)
93 {
94 auto initXmlRpcValue = failureSwitchParam[i];
95 if (tryTestPolicyFactory<TimeoutFailureTestPolicy>("timeout", initXmlRpcValue) != nullptr)
96 continue;
97 }
98 }
void onSmaccTransitionCallback(const smacc_msgs::SmaccTransitionLogEntry &msg)
void onRosOutCallback(const rosgraph_msgs::Log &msg)

References nh_, onRosOutCallback(), onSmaccTransitionCallback(), rosoutSub_, and smaccTransitionSub_.

Referenced by main().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ onRosOutCallback()

void SmaccTestRuntimeNode::onRosOutCallback ( const rosgraph_msgs::Log &  msg)
inline

Definition at line 119 of file smacc_runtime_test_node.cpp.

120 {
121 this->onRosOutMsg(msg);
122 }
boost::signals2::signal< void(const rosgraph_msgs::Log &)> onRosOutMsg

References onRosOutMsg.

Referenced by init().

Here is the caller graph for this function:

◆ onSmaccTransitionCallback()

void SmaccTestRuntimeNode::onSmaccTransitionCallback ( const smacc_msgs::SmaccTransitionLogEntry &  msg)
inline

Definition at line 124 of file smacc_runtime_test_node.cpp.

125 {
126 this->onSmaccTransition(msg);
127 }
boost::signals2::signal< void(const smacc_msgs::SmaccTransitionLogEntry &)> onSmaccTransition

References onSmaccTransition.

Referenced by init().

Here is the caller graph for this function:

◆ success()

void SmaccTestRuntimeNode::success ( std::string  msg)
inline

Definition at line 99 of file smacc_runtime_test_node.cpp.

100 {
101 ROS_INFO_STREAM("SmaccRuntimeTestNode policy thrown success exit: " << msg);
102 exit(-1);
103 }

Referenced by ReachedStateSuccessTestPolicy::init().

Here is the caller graph for this function:

◆ tryTestPolicyFactory()

template<typename T >
std::shared_ptr< TestPolicy > SmaccTestRuntimeNode::tryTestPolicyFactory ( std::string  matchname,
XmlRpc::XmlRpcValue &  initXmlRpcValue 
)
inline

Definition at line 41 of file smacc_runtime_test_node.cpp.

42 {
43 std::shared_ptr<TestPolicy> policy = nullptr;
44 ROS_INFO_STREAM(initXmlRpcValue);
45
46 auto type = (std::string)initXmlRpcValue["type"];
47
48 if (type == matchname)
49 {
50 auto specificPolicy = std::make_shared<T>();
51 policy = std::dynamic_pointer_cast<TestPolicy>(specificPolicy);
52 testPolicies.push_back(policy);
53
54 policy->owner_ = this;
55 policy->init(initXmlRpcValue);
56 }
57
58 return policy;
59 }
std::vector< std::shared_ptr< TestPolicy > > testPolicies

References testPolicies.

◆ update()

void SmaccTestRuntimeNode::update ( )
inline

Definition at line 111 of file smacc_runtime_test_node.cpp.

112 {
113 for (auto& policy : testPolicies)
114 {
115 policy->update();
116 }
117 }

References testPolicies.

Referenced by main().

Here is the caller graph for this function:

Member Data Documentation

◆ nh_

ros::NodeHandle SmaccTestRuntimeNode::nh_

Definition at line 31 of file smacc_runtime_test_node.cpp.

Referenced by init().

◆ onRosOutMsg

boost::signals2::signal<void(const rosgraph_msgs::Log&)> SmaccTestRuntimeNode::onRosOutMsg

Definition at line 37 of file smacc_runtime_test_node.cpp.

Referenced by onRosOutCallback().

◆ onSmaccTransition

boost::signals2::signal<void(const smacc_msgs::SmaccTransitionLogEntry&)> SmaccTestRuntimeNode::onSmaccTransition

◆ rosoutSub_

ros::Subscriber SmaccTestRuntimeNode::rosoutSub_

Definition at line 32 of file smacc_runtime_test_node.cpp.

Referenced by init().

◆ smaccTransitionSub_

ros::Subscriber SmaccTestRuntimeNode::smaccTransitionSub_

Definition at line 33 of file smacc_runtime_test_node.cpp.

Referenced by init().

◆ testPolicies

std::vector<std::shared_ptr<TestPolicy> > SmaccTestRuntimeNode::testPolicies

Definition at line 36 of file smacc_runtime_test_node.cpp.

Referenced by tryTestPolicyFactory(), and update().


The documentation for this class was generated from the following file: