7 void state_callback (
const smach_msgs::SmachContainerStatus::ConstPtr& msg)
9 ROS_DEBUG(
"Received state machine path %s", msg->path.c_str());
16 state = msg->active_states[0];
27 string uuid = msg.swarmio.node;
32 member_state.
uuid = uuid;
47 int main (
int argc,
char **argv)
50 init(argc, argv,
"state_exchanger");
55 nh.
param(this_node::getName() +
"/loop_rate", loop_rate, 1.5);
57 nh.
param(this_node::getName() +
"/queue_size", queue_size, 10);
59 nh.
param(this_node::getName() +
"/timeout", timeout, 20.0);
69 Publisher outgoing_state_publisher = nh.
advertise<cpswarm_msgs::StateEvent>(
"state", queue_size);
70 Publisher incoming_state_publisher = nh.
advertise<cpswarm_msgs::ArrayOfStates>(
"swarm_state", queue_size);
83 cpswarm_msgs::ArrayOfStates swarm_state_msg;
88 swarm_state_msg.states.clear();
93 if ((Time::now() - member->second.stamp) >
Duration(timeout)) {
99 cpswarm_msgs::StateEvent state_event;
100 state_event.header.stamp = Time::now();
101 state_event.swarmio.node = member->first;
102 state_event.state = member->second.state;
103 swarm_state_msg.states.push_back(state_event);
110 incoming_state_publisher.publish(swarm_state_msg);
113 cpswarm_msgs::StateEvent state_event;
114 state_event.header.stamp = Time::now();
115 state_event.swarmio.name =
"state";
116 state_event.state =
state;
117 outgoing_state_publisher.
publish(state_event);
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void init(const M_string &remappings)
void swarm_state_callback(cpswarm_msgs::StateEvent msg)
Callback function for state updates from other swarm members.
#define ROS_DEBUG_ONCE(...)
string sm_path
The path of the smach state machine whose state shall be exchanged.
bool state_valid
Whether a valid state has been received.
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
string state
Current state of the CPS.
int main(int argc, char **argv)
A ROS node that exchanges the behavioral state between CPSs in a swarm.
A STATE type containing UUID and state of the corresponding CPS together with last updated time stamp...
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
map< string, state_t > swarm_state
The state of all known swarm members.
void state_callback(const smach_msgs::SmachContainerStatus::ConstPtr &msg)
Callback function for state updates.
ROSCPP_DECL void spinOnce()