void
init()
{
ifc_zone_ = blackboard_->open_for_reading<fawkes::ZoneInterface>("/explore-zone/info");
ifc_tag_vision_ = blackboard_->open_for_reading<fawkes::TagVisionInterface>("/tag-vision/info");
for (size_t i = 0; i < std::min(ifc_tag_vision_->maxlenof_tag_id(), ifcs_tag_pos_.size()); ++i) {
std::string ifc_id = "/tag-vision/" + std::to_string(i);
ifcs_tag_pos_[i] = blackboard_->open_for_reading<fawkes::Position3DInterface>(ifc_id.c_str());
// do not listen for events on these interfaces, we read them
// whenever the tag vision info interface changes
}
ifc_machine_signal_ = blackboard_->open_for_reading<fawkes::RobotinoLightInterface>("/machine-signal/best");
ifc_navgraph_gen_ =
blackboard_->open_for_reading<fawkes::NavGraphWithMPSGeneratorInterface>("/navgraph-generator-mps");
ifc_pose_ = blackboard_->open_for_reading<fawkes::Position3DInterface>("Pose");
bbil_add_data_interface(ifc_zone_);
bbil_add_data_interface(ifc_tag_vision_);
bbil_add_data_interface(ifc_machine_signal_);
bbil_add_data_interface(ifc_pose_);
// Setup ROS topics
pub_expl_zone_info_ =
rosnode.advertise<rcll_fawkes_sim_msgs::ExplorationZoneInfo>("rcll_sim/explore_zone_info", 10);
pub_mps_marker_array_ =
rosnode.advertise<rcll_fawkes_sim_msgs::MPSMarkerArray>("rcll_sim/mps_marker_array", 10);
pub_mps_light_state_ =
rosnode.advertise<rcll_fawkes_sim_msgs::MPSLightState>("rcll_sim/mps_light_state", 10);
pub_pose_ =
rosnode.advertise<geometry_msgs::PoseWithCovarianceStamped>("rcll_sim/amcl_pose", 10);
blackboard_->register_listener(this, fawkes::BlackBoard::BBIL_FLAG_DATA);
// provide services
srv_navgraph_gen_ = rosnode.advertiseService("rcll_sim/navgraph_generate",
&RcllFawkesSimNode::srv_cb_navgraph_gen, this);
}
/** Constructor.
* @param blackboard local BlackBoard
* @param interface interface to care about
* @param hub Fawkes network hub to use to send messages
* @param clid client ID of the client which opened this interface
*/
BlackBoardNetHandlerInterfaceListener::BlackBoardNetHandlerInterfaceListener(BlackBoard *blackboard,
Interface *interface,
FawkesNetworkHub *hub,
unsigned int clid)
: BlackBoardInterfaceListener("NetIL/%s", interface->uid())
{
bbil_add_data_interface(interface);
bbil_add_reader_interface(interface);
bbil_add_writer_interface(interface);
if ( interface->is_writer() ) {
bbil_add_message_interface(interface);
}
__blackboard = blackboard;
__interface = interface;
__fnh = hub;
__clid = clid;
__blackboard->register_listener(this, BlackBoard::BBIL_FLAG_ALL);
}
/** Constructor.
* @param listener_name name of the listener
* @param iface interface to watch for data changes. Register this dispatcher as
* listener by yourself!
* @param message_enqueueing true to enqueue messages after the message received
* event handler has been called, false to drop the message afterwards.
*/
InterfaceDispatcher::InterfaceDispatcher(const char *listener_name,
Interface *iface,
bool message_enqueueing)
: BlackBoardInterfaceListener(listener_name)
{
__message_enqueueing = message_enqueueing;
bbil_add_data_interface(iface);
if ( iface->is_writer() ) {
bbil_add_message_interface(iface);
}
bbil_add_writer_interface(iface);
bbil_add_reader_interface(iface);
__dispatcher_data_changed.connect(sigc::mem_fun(*this, &InterfaceDispatcher::on_data_changed));
__dispatcher_message_received.connect(sigc::mem_fun(*this, &InterfaceDispatcher::on_message_received));
__dispatcher_writer_added.connect(sigc::mem_fun(*this, &InterfaceDispatcher::on_writer_added));
__dispatcher_writer_removed.connect(sigc::mem_fun(*this, &InterfaceDispatcher::on_writer_removed));
__dispatcher_reader_added.connect(sigc::mem_fun(*this, &InterfaceDispatcher::on_reader_added));
__dispatcher_reader_removed.connect(sigc::mem_fun(*this, &InterfaceDispatcher::on_writer_removed));
}
