rcll_fawkes_sim_node.cpp
Go to the documentation of this file.
1 /***************************************************************************
2  * rcll_fawkes_sim_node.cpp - RCLL simulation access through Fawkes
3  *
4  * Created: Sun May 29 15:36:18 2016
5  * Copyright 2016 Tim Niemueller [www.niemueller.de]
6  ****************************************************************************/
7 
8 /* This program is free software; you can redistribute it and/or modify
9  * it under the terms of the GNU General Public License as published by
10  * the Free Software Foundation; either version 2 of the License, or
11  * (at your option) any later version.
12  *
13  * This program is distributed in the hope that it will be useful,
14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16  * GNU Library General Public License for more details.
17  *
18  * Read the full text in the LICENSE.GPL file in the doc directory.
19  */
20 
21 #include <ros/ros.h>
22 #include <ros/callback_queue.h>
23 
24 #include <blackboard/remote.h>
25 #include <blackboard/interface_listener.h>
26 
27 #include <interfaces/ZoneInterface.h>
28 #include <interfaces/Position3DInterface.h>
29 #include <interfaces/TagVisionInterface.h>
30 #include <interfaces/RobotinoLightInterface.h>
31 #include <interfaces/NavGraphWithMPSGeneratorInterface.h>
32 // these are Fawkes' utils
33 #include <utils/time/time.h>
34 
35 #include <rcll_fawkes_sim_msgs/ExplorationZoneInfo.h>
36 #include <rcll_fawkes_sim_msgs/MPSMarkerArray.h>
37 #include <rcll_fawkes_sim_msgs/MPSLightState.h>
38 #include <rcll_fawkes_sim_msgs/NavgraphWithMPSGenerate.h>
39 #include <geometry_msgs/PoseWithCovarianceStamped.h>
40 
41 #include <memory>
42 
43 #define GET_PRIV_PARAM(P) \
44  { \
45  if (! ros::param::get("~" #P, cfg_ ## P ## _)) { \
46  ROS_ERROR("%s: Failed to retrieve parameter " #P ", aborting", ros::this_node::getName().c_str()); \
47  exit(-1); \
48  } \
49  }
50 
51 int
52 fawkes_to_ros_light_state(const fawkes::RobotinoLightInterface::LightState &state)
53 {
54  switch (state) {
55  case fawkes::RobotinoLightInterface::ON:
56  return rcll_fawkes_sim_msgs::MPSLightState::LIGHT_STATE_ON;
57  case fawkes::RobotinoLightInterface::OFF:
58  return rcll_fawkes_sim_msgs::MPSLightState::LIGHT_STATE_OFF;
59  case fawkes::RobotinoLightInterface::BLINKING:
60  return rcll_fawkes_sim_msgs::MPSLightState::LIGHT_STATE_BLINKING;
61  }
62  return rcll_fawkes_sim_msgs::MPSLightState::LIGHT_STATE_UNKNOWN;
63 }
64 
65 fawkes::NavGraphWithMPSGeneratorInterface::Side
66 ros_to_fawkes_mps_side(int side)
67 {
68  if (side == rcll_fawkes_sim_msgs::NavgraphMPSStation::SIDE_INPUT) {
69  return fawkes::NavGraphWithMPSGeneratorInterface::INPUT;
70  } else {
71  return fawkes::NavGraphWithMPSGeneratorInterface::OUTPUT;
72  }
73 }
74 
75 class RcllFawkesSimNode : public fawkes::BlackBoardInterfaceListener
76 {
77 public:
78  RcllFawkesSimNode(ros::NodeHandle rn, std::shared_ptr<fawkes::BlackBoard> bb)
79  : BlackBoardInterfaceListener("RcllFawkesSimNode"),
80  rosnode(rn), blackboard_(bb)
81  {}
82 
83  void
84  init()
85  {
86  ifc_zone_ = blackboard_->open_for_reading<fawkes::ZoneInterface>("/explore-zone/info");
87  ifc_tag_vision_ = blackboard_->open_for_reading<fawkes::TagVisionInterface>("/tag-vision/info");
88  for (size_t i = 0; i < std::min(ifc_tag_vision_->maxlenof_tag_id(), ifcs_tag_pos_.size()); ++i) {
89  std::string ifc_id = "/tag-vision/" + std::to_string(i);
90  ifcs_tag_pos_[i] = blackboard_->open_for_reading<fawkes::Position3DInterface>(ifc_id.c_str());
91  // do not listen for events on these interfaces, we read them
92  // whenever the tag vision info interface changes
93  }
94  ifc_machine_signal_ = blackboard_->open_for_reading<fawkes::RobotinoLightInterface>("/machine-signal/best");
95  ifc_navgraph_gen_ =
96  blackboard_->open_for_reading<fawkes::NavGraphWithMPSGeneratorInterface>("/navgraph-generator-mps");
97  ifc_pose_ = blackboard_->open_for_reading<fawkes::Position3DInterface>("Pose");
98 
99 
100  bbil_add_data_interface(ifc_zone_);
101  bbil_add_data_interface(ifc_tag_vision_);
102  bbil_add_data_interface(ifc_machine_signal_);
103  bbil_add_data_interface(ifc_pose_);
104 
105  // Setup ROS topics
106  pub_expl_zone_info_ =
107  rosnode.advertise<rcll_fawkes_sim_msgs::ExplorationZoneInfo>("rcll_sim/explore_zone_info", 10);
108  pub_mps_marker_array_ =
109  rosnode.advertise<rcll_fawkes_sim_msgs::MPSMarkerArray>("rcll_sim/mps_marker_array", 10);
110  pub_mps_light_state_ =
111  rosnode.advertise<rcll_fawkes_sim_msgs::MPSLightState>("rcll_sim/mps_light_state", 10);
112  pub_pose_ =
113  rosnode.advertise<geometry_msgs::PoseWithCovarianceStamped>("rcll_sim/amcl_pose", 10);
114 
115  blackboard_->register_listener(this, fawkes::BlackBoard::BBIL_FLAG_DATA);
116 
117  // provide services
118  srv_navgraph_gen_ = rosnode.advertiseService("rcll_sim/navgraph_generate",
119  &RcllFawkesSimNode::srv_cb_navgraph_gen, this);
120  }
121 
122  void
123  finalize()
124  {
125  blackboard_->unregister_listener(this);
126 
127  blackboard_->close(ifc_zone_);
128  blackboard_->close(ifc_tag_vision_);
129  for (const auto &i : ifcs_tag_pos_) {
130  blackboard_->close(i);
131  }
132  blackboard_->close(ifc_machine_signal_);
133  blackboard_->close(ifc_navgraph_gen_);
134  blackboard_->close(ifc_pose_);
135 
136  pub_expl_zone_info_.shutdown();
137  pub_mps_marker_array_.shutdown();
138  pub_mps_light_state_.shutdown();
139  pub_pose_.shutdown();
140  srv_navgraph_gen_.shutdown();
141  }
142 
143  virtual void bb_interface_data_changed(fawkes::Interface *interface) throw()
144  {
145  interface->read();
146  if (strcmp(interface->uid(), ifc_pose_->uid()) == 0) {
147  const fawkes::Time *ts = ifc_pose_->timestamp();
148  geometry_msgs::PoseWithCovarianceStamped p;
149  p.header.frame_id = ifc_pose_->frame();
150  p.header.stamp.sec = ts->get_sec();
151  p.header.stamp.nsec = ts->get_nsec();
152  p.pose.pose.position.x = ifc_pose_->translation(0);
153  p.pose.pose.position.y = ifc_pose_->translation(1);
154  p.pose.pose.orientation.x = ifc_pose_->rotation(0);
155  p.pose.pose.orientation.y = ifc_pose_->rotation(1);
156  p.pose.pose.orientation.z = ifc_pose_->rotation(2);
157  p.pose.pose.orientation.w = ifc_pose_->rotation(3);
158 
159  for (int i = 0; i < ifc_pose_->maxlenof_covariance(); ++i) {
160  p.pose.covariance[i] = ifc_pose_->covariance(i);
161  }
162  pub_pose_.publish(p);
163 
164  } else if (strcmp(interface->uid(), ifc_zone_->uid()) == 0) {
165  rcll_fawkes_sim_msgs::ExplorationZoneInfo rezi;
166  rezi.result = rcll_fawkes_sim_msgs::ExplorationZoneInfo::MPS_IN_ZONE_UNKNOWN;
167  switch (ifc_zone_->search_state()) {
168  case fawkes::ZoneInterface::NO:
169  rezi.result = rcll_fawkes_sim_msgs::ExplorationZoneInfo::MPS_IN_ZONE_NO;
170  break;
171  case fawkes::ZoneInterface::YES:
172  rezi.result = rcll_fawkes_sim_msgs::ExplorationZoneInfo::MPS_IN_ZONE_YES;
173  break;
174  case fawkes::ZoneInterface::MAYBE:
175  rezi.result = rcll_fawkes_sim_msgs::ExplorationZoneInfo::MPS_IN_ZONE_MAYBE;
176  break;
177  }
178  rezi.marker_id = ifc_zone_->tag_id();
179  ROS_DEBUG("%s: Publishing zone info, marker ID %i", ros::this_node::getName().c_str(), ifc_zone_->tag_id());
180  pub_expl_zone_info_.publish(rezi);
181  } else if (strcmp(interface->uid(), ifc_tag_vision_->uid()) == 0) {
182  rcll_fawkes_sim_msgs::MPSMarkerArray rma;
183  for (size_t i = 0; i < std::min(ifc_tag_vision_->maxlenof_tag_id(), ifcs_tag_pos_.size()); ++i) {
184  if (ifc_tag_vision_->tag_id(i) > 0) {
185  ifcs_tag_pos_[i]->read();
186  const fawkes::Time *ts = ifcs_tag_pos_[i]->timestamp();
187  rcll_fawkes_sim_msgs::MPSMarker rm;
188  rm.id = ifc_tag_vision_->tag_id(i);
189  rm.pose.header.stamp.sec = ts->get_sec();
190  rm.pose.header.stamp.nsec = ts->get_nsec();
191  rm.pose.header.frame_id = ifcs_tag_pos_[i]->frame();
192  rm.pose.name = ifcs_tag_pos_[i]->id();
193  rm.pose.visibility_history = ifcs_tag_pos_[i]->visibility_history();
194  rm.pose.pose.position.x = ifcs_tag_pos_[i]->translation(0);
195  rm.pose.pose.position.y = ifcs_tag_pos_[i]->translation(1);
196  rm.pose.pose.position.z = ifcs_tag_pos_[i]->translation(2);
197  rm.pose.pose.orientation.x = ifcs_tag_pos_[i]->rotation(0);
198  rm.pose.pose.orientation.y = ifcs_tag_pos_[i]->rotation(1);
199  rm.pose.pose.orientation.z = ifcs_tag_pos_[i]->rotation(2);
200  rm.pose.pose.orientation.w = ifcs_tag_pos_[i]->rotation(3);
201  rma.markers.push_back(rm);
202  }
203  }
204  pub_mps_marker_array_.publish(rma);
205  } else if (strcmp(interface->uid(), ifc_machine_signal_->uid()) == 0) {
206  rcll_fawkes_sim_msgs::MPSLightState rls;
207  rls.ready = ifc_machine_signal_->is_ready();
208  rls.light_state_red = fawkes_to_ros_light_state(ifc_machine_signal_->red());
209  rls.light_state_yellow = fawkes_to_ros_light_state(ifc_machine_signal_->yellow());
210  rls.light_state_green = fawkes_to_ros_light_state(ifc_machine_signal_->green());
211  rls.visibility_history = ifc_machine_signal_->visibility_history();
212  pub_mps_light_state_.publish(rls);
213  }
214 
215  }
216 
217  bool
218  srv_cb_navgraph_gen(rcll_fawkes_sim_msgs::NavgraphWithMPSGenerate::Request &req,
219  rcll_fawkes_sim_msgs::NavgraphWithMPSGenerate::Response &res)
220  {
221  if (! blackboard_->is_alive()) {
222  res.ok = false;
223  res.error_msg = "Blackboard is not connected";
224  return true;
225  }
226 
227  if (! ifc_navgraph_gen_->has_writer()) {
228  res.ok = false;
229  res.error_msg = "No writer for navgraph generator with MPS interface";
230  return true;
231  }
232 
233  std::queue<fawkes::Message *> msgs;
234 
235  ifc_navgraph_gen_->msgq_enqueue(new fawkes::NavGraphWithMPSGeneratorInterface::ClearMessage());
236 
237  for (size_t i = 0; i < req.mps_stations.size(); ++i) {
238  const rcll_fawkes_sim_msgs::NavgraphMPSStation &mps = req.mps_stations[i];
239  fawkes::NavGraphWithMPSGeneratorInterface::UpdateStationByTagMessage *upm =
240  new fawkes::NavGraphWithMPSGeneratorInterface::UpdateStationByTagMessage();
241 #ifdef HAVE_OLD_NAVGRAPH_GENMPS_INTERFACE
242  upm->set_id(mps.name.c_str());
243 #else
244  upm->set_name(mps.name.c_str());
245 #endif
246  upm->set_side(ros_to_fawkes_mps_side(mps.marker_side));
247  upm->set_frame(mps.marker_frame.c_str());
248  upm->set_tag_translation(0, mps.marker_pose.position.x);
249  upm->set_tag_translation(1, mps.marker_pose.position.y);
250  upm->set_tag_translation(2, mps.marker_pose.position.z);
251  upm->set_tag_rotation(0, mps.marker_pose.orientation.x);
252  upm->set_tag_rotation(1, mps.marker_pose.orientation.y);
253  upm->set_tag_rotation(2, mps.marker_pose.orientation.z);
254  upm->set_tag_rotation(3, mps.marker_pose.orientation.w);
255  msgs.push(upm);
256  }
257 
258  {
259  fawkes::NavGraphWithMPSGeneratorInterface::SetExplorationZonesMessage *sezm =
260  new fawkes::NavGraphWithMPSGeneratorInterface::SetExplorationZonesMessage();
261  bool zones[sezm->maxlenof_zones()];
262  for (int i = 0; i < sezm->maxlenof_zones(); ++i) zones[i] = false;
263  for (size_t i = 0; i < req.exploration_zones.size(); ++i) {
264  int zone_idx = req.exploration_zones[i] - 1;
265  if (zone_idx < 0 || zone_idx > sezm->maxlenof_zones()) {
266  delete sezm;
267  res.ok = false;
268  res.error_msg = "Exploration zone index out of bounds";
269  return true;
270  }
271  zones[zone_idx] = true;
272  }
273  sezm->set_zones(zones);
274  msgs.push(sezm);
275  }
276 
277  {
278  fawkes::NavGraphWithMPSGeneratorInterface::SetWaitZonesMessage *swzm =
279  new fawkes::NavGraphWithMPSGeneratorInterface::SetWaitZonesMessage();
280  bool zones[swzm->maxlenof_zones()];
281  for (int i = 0; i < swzm->maxlenof_zones(); ++i) zones[i] = false;
282  for (size_t i = 0; i < req.wait_zones.size(); ++i) {
283  int zone_idx = req.wait_zones[i] - 1;
284  if (zone_idx < 0 || zone_idx > swzm->maxlenof_zones()) {
285  delete swzm;
286  res.ok = false;
287  res.error_msg = "Wait zone index out of bounds";
288  return true;
289  }
290  zones[zone_idx] = true;
291  }
292  swzm->set_zones(zones);
293  msgs.push(swzm);
294  }
295 
296  while (! msgs.empty()) {
297  fawkes::Message *m = msgs.front();
298  ifc_navgraph_gen_->msgq_enqueue(m);
299  msgs.pop();
300  }
301 
302  fawkes::NavGraphWithMPSGeneratorInterface::ComputeMessage *compmsg =
303  new fawkes::NavGraphWithMPSGeneratorInterface::ComputeMessage();
304  compmsg->ref();
305  ifc_navgraph_gen_->msgq_enqueue(compmsg);
306  unsigned int msgid = compmsg->id();
307  compmsg->unref();
308  bool finished = false;
309  while (! finished && ifc_navgraph_gen_->has_writer()) {
310  ifc_navgraph_gen_->read();
311  if (ifc_navgraph_gen_->msgid() == msgid && ifc_navgraph_gen_->is_final()) {
312  res.ok = true;
313  finished = true;
314  }
315  ros::Duration(0.1 /* sec */).sleep();
316  }
317 
318  if (finished) {
319  res.ok = true;
320  } else {
321  res.ok = false;
322  res.error_msg = "Navgraph generator with MPS interface writer disappeared while waiting";
323  }
324 
325  return true;
326  }
327 
328 private:
329  ros::NodeHandle rosnode;
330 
331  std::shared_ptr<fawkes::BlackBoard> blackboard_;
332 
333  fawkes::ZoneInterface *ifc_zone_;
334  fawkes::TagVisionInterface *ifc_tag_vision_;
335  std::array<fawkes::Position3DInterface *, 16> ifcs_tag_pos_;
336  fawkes::RobotinoLightInterface *ifc_machine_signal_;
337  fawkes::NavGraphWithMPSGeneratorInterface *ifc_navgraph_gen_;
338  fawkes::Position3DInterface *ifc_pose_;
339 
340  ros::Publisher pub_expl_zone_info_;
341  ros::Publisher pub_mps_marker_array_;
342  ros::Publisher pub_mps_light_state_;
343  ros::Publisher pub_pose_;
344  ros::ServiceServer srv_navgraph_gen_;
345 };
346 
347 
348 int
349 main(int argc, char **argv)
350 {
351  ros::init(argc, argv, "rcll_fawkes_sim");
352 
353  ros::NodeHandle n;
354 
355  std::string cfg_fawkes_host_;
356  int cfg_fawkes_port_;
357  std::shared_ptr<fawkes::BlackBoard> blackboard_;
358  std::shared_ptr<RcllFawkesSimNode> node;
359 
360  GET_PRIV_PARAM(fawkes_host);
361  GET_PRIV_PARAM(fawkes_port);
362 
363  while (ros::ok()) {
364  if (!blackboard_) {
365  try {
366  blackboard_ =
367  std::make_shared<fawkes::RemoteBlackBoard>(cfg_fawkes_host_.c_str(), cfg_fawkes_port_);
368  node = std::make_shared<RcllFawkesSimNode>(n, blackboard_);
369  node->init();
370  ROS_INFO("%s: Blackboard connected and initialized", ros::this_node::getName().c_str());
371  } catch (fawkes::Exception &e) {
372  ROS_WARN_THROTTLE(10, "%s: Initialization failed, retrying", ros::this_node::getName().c_str());
373  if (node) {
374  node->finalize();
375  node.reset();
376  }
377  blackboard_.reset();
378  }
379  } else if (! blackboard_->is_alive()) {
380  ROS_WARN_THROTTLE(30, "%s: blackboard connection lost, retrying", ros::this_node::getName().c_str());
381  if (blackboard_->try_aliveness_restore()) {
382  ROS_INFO("%s: Blackboard re-connected", ros::this_node::getName().c_str());
383  }
384  }
385 
386  ros::getGlobalCallbackQueue()->callAvailable(ros::WallDuration(0.1));
387  }
388 
389  if (node) node->finalize();
390 
391  return 0;
392 }
RcllFawkesSimNode::ifc_pose_
fawkes::Position3DInterface * ifc_pose_
Definition: rcll_fawkes_sim_node.cpp:338
RcllFawkesSimNode::blackboard_
std::shared_ptr< fawkes::BlackBoard > blackboard_
Definition: rcll_fawkes_sim_node.cpp:331
ros::NodeHandle
RcllFawkesSimNode::pub_mps_marker_array_
ros::Publisher pub_mps_marker_array_
Definition: rcll_fawkes_sim_node.cpp:341
ros_to_fawkes_mps_side
fawkes::NavGraphWithMPSGeneratorInterface::Side ros_to_fawkes_mps_side(int side)
Definition: rcll_fawkes_sim_node.cpp:66
ROS_WARN_THROTTLE
#define ROS_WARN_THROTTLE(rate,...)
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
main
int main(int argc, char **argv)
Definition: rcll_fawkes_sim_node.cpp:349
RcllFawkesSimNode::ifc_navgraph_gen_
fawkes::NavGraphWithMPSGeneratorInterface * ifc_navgraph_gen_
Definition: rcll_fawkes_sim_node.cpp:337
RcllFawkesSimNode::bb_interface_data_changed
virtual void bb_interface_data_changed(fawkes::Interface *interface)
Definition: rcll_fawkes_sim_node.cpp:143
ros::Duration::sleep
bool sleep() const
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ros::this_node::getName
ROSCPP_DECL const std::string & getName()
ros::NodeHandle::advertiseService
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
GET_PRIV_PARAM
#define GET_PRIV_PARAM(P)
Definition: rcll_fawkes_sim_node.cpp:43
RcllFawkesSimNode::ifc_machine_signal_
fawkes::RobotinoLightInterface * ifc_machine_signal_
Definition: rcll_fawkes_sim_node.cpp:336
RcllFawkesSimNode::pub_mps_light_state_
ros::Publisher pub_mps_light_state_
Definition: rcll_fawkes_sim_node.cpp:342
fawkes_to_ros_light_state
int fawkes_to_ros_light_state(const fawkes::RobotinoLightInterface::LightState &state)
Definition: rcll_fawkes_sim_node.cpp:52
ros::getGlobalCallbackQueue
ROSCPP_DECL CallbackQueue * getGlobalCallbackQueue()
RcllFawkesSimNode::ifc_tag_vision_
fawkes::TagVisionInterface * ifc_tag_vision_
Definition: rcll_fawkes_sim_node.cpp:334
ROS_INFO
#define ROS_INFO(...)
ros::ok
ROSCPP_DECL bool ok()
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
RcllFawkesSimNode::srv_cb_navgraph_gen
bool srv_cb_navgraph_gen(rcll_fawkes_sim_msgs::NavgraphWithMPSGenerate::Request &req, rcll_fawkes_sim_msgs::NavgraphWithMPSGenerate::Response &res)
Definition: rcll_fawkes_sim_node.cpp:218
RcllFawkesSimNode::ifc_zone_
fawkes::ZoneInterface * ifc_zone_
Definition: rcll_fawkes_sim_node.cpp:333
RcllFawkesSimNode::srv_navgraph_gen_
ros::ServiceServer srv_navgraph_gen_
Definition: rcll_fawkes_sim_node.cpp:344
RcllFawkesSimNode::RcllFawkesSimNode
RcllFawkesSimNode(ros::NodeHandle rn, std::shared_ptr< fawkes::BlackBoard > bb)
Definition: rcll_fawkes_sim_node.cpp:78
RcllFawkesSimNode::ifcs_tag_pos_
std::array< fawkes::Position3DInterface *, 16 > ifcs_tag_pos_
Definition: rcll_fawkes_sim_node.cpp:335
callback_queue.h
RcllFawkesSimNode::pub_expl_zone_info_
ros::Publisher pub_expl_zone_info_
Definition: rcll_fawkes_sim_node.cpp:340
ROS_DEBUG
#define ROS_DEBUG(...)

rcll_fawkes_sim
Author(s): Tim Niemueller
autogenerated on Mon Jun 10 2019 14:31:10