rcll_refbox_peer_node.cpp
Go to the documentation of this file.
1 /***************************************************************************
2  * rcll_refbox_peer_node.cpp - RCLL Referee Box Peer as ROS node
3  *
4  * Created: Fri May 27 21:38:32 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 
22 #include <ros/ros.h>
23 
24 #include <protobuf_comm/peer.h>
25 
26 #include <llsf_msgs/BeaconSignal.pb.h>
27 #include <llsf_msgs/VersionInfo.pb.h>
28 #include <llsf_msgs/GameState.pb.h>
29 #include <llsf_msgs/MachineInfo.pb.h>
30 #include <llsf_msgs/RobotInfo.pb.h>
31 #include <llsf_msgs/ExplorationInfo.pb.h>
32 #include <llsf_msgs/MachineReport.pb.h>
33 #include <llsf_msgs/OrderInfo.pb.h>
34 #include <llsf_msgs/RingInfo.pb.h>
35 #include <llsf_msgs/MachineInstructions.pb.h>
36 
37 #include <rcll_ros_msgs/BeaconSignal.h>
38 #include <rcll_ros_msgs/GameState.h>
39 #include <rcll_ros_msgs/Team.h>
40 #include <rcll_ros_msgs/MachineInfo.h>
41 #include <rcll_ros_msgs/ExplorationInfo.h>
42 #include <rcll_ros_msgs/MachineReportInfo.h>
43 #include <rcll_ros_msgs/OrderInfo.h>
44 #include <rcll_ros_msgs/RingInfo.h>
45 
46 #include <rcll_ros_msgs/SendBeaconSignal.h>
47 #include <rcll_ros_msgs/SendMachineReport.h>
48 #include <rcll_ros_msgs/SendPrepareMachine.h>
49 
50 #include <tf2/LinearMath/Quaternion.h>
51 #include <tf2/utils.h>
52 
53 #include <mutex>
54 #include <condition_variable>
55 #include <chrono>
56 
57 #define GET_PRIV_PARAM(P) \
58  { \
59  if (! ros::param::get("~" #P, cfg_ ## P ## _)) { \
60  ROS_ERROR("Failed to retrieve parameter " #P ", aborting"); \
61  exit(-1); \
62  } \
63  }
64 
65 using namespace protobuf_comm;
66 using namespace llsf_msgs;
67 
68 std::string cfg_team_name_;
69 std::string cfg_robot_name_;
70 int cfg_robot_number_;
71 int cfg_team_color_;
72 std::string cfg_crypto_key_;
73 std::string cfg_crypto_cipher_;
74 std::string cfg_peer_address_;
75 bool cfg_peer_public_local_;
76 int cfg_peer_public_port_;
77 int cfg_peer_public_send_port_;
78 int cfg_peer_public_recv_port_;
79 bool cfg_peer_cyan_local_;
80 int cfg_peer_cyan_port_;
81 int cfg_peer_cyan_send_port_;
82 int cfg_peer_cyan_recv_port_;
83 bool cfg_peer_magenta_local_;
84 int cfg_peer_magenta_port_;
85 int cfg_peer_magenta_send_port_;
86 int cfg_peer_magenta_recv_port_;
87 
88 ros::Publisher pub_beacon_;
89 ros::Publisher pub_game_state_;
90 ros::Publisher pub_machine_info_;
91 ros::Publisher pub_exploration_info_;
92 ros::Publisher pub_machine_report_info_;
93 ros::Publisher pub_order_info_;
94 ros::Publisher pub_ring_info_;
95 
96 ros::ServiceServer srv_send_beacon_;
97 ros::ServiceServer srv_send_machine_report_;
98 ros::ServiceServer srv_send_prepare_machine_;
99 
100 ProtobufBroadcastPeer *peer_public_ = NULL;
101 ProtobufBroadcastPeer *peer_private_ = NULL;
102 
103 std::mutex mtx_machine_info_;
104 std::condition_variable cdv_machine_info_;
105 std::shared_ptr<MachineInfo> msg_machine_info_;
106 
107 void setup_private_peer(llsf_msgs::Team team_color);
108 
109 void
110 handle_recv_error(boost::asio::ip::udp::endpoint &endpoint, std::string msg)
111 {
112  ROS_WARN("Receive error from %s:%u: %s",
113  endpoint.address().to_string().c_str(), endpoint.port(), msg.c_str());
114 }
115 
116 void
117 handle_send_error(std::string msg)
118 {
119  ROS_ERROR("Send error: %s", msg.c_str());
120 }
121 
122 
123 int
124 pb_to_ros_team_color(const llsf_msgs::Team &team_color)
125 {
126  if (team_color == llsf_msgs::CYAN)
127  return (int)rcll_ros_msgs::Team::CYAN;
128  else
129  return (int)rcll_ros_msgs::Team::MAGENTA;
130 }
131 
132 llsf_msgs::Team
133 ros_to_pb_team_color(int team_color)
134 {
135  if (team_color == rcll_ros_msgs::Team::CYAN)
136  return llsf_msgs::CYAN;
137  else
138  return llsf_msgs::MAGENTA;
139 }
140 
141 void
142 handle_message(boost::asio::ip::udp::endpoint &sender,
143  uint16_t component_id, uint16_t msg_type,
144  std::shared_ptr<google::protobuf::Message> msg)
145 {
146  {
147  // This is only sent right after starting the refbox and not
148  // terribly interesting, hence only log
149  static bool version_info_printed = false;
150  std::shared_ptr<VersionInfo> v;
151  if (! version_info_printed && (v = std::dynamic_pointer_cast<VersionInfo>(msg))) {
152  ROS_INFO("Referee Box %s detected", v->version_string().c_str());
153  version_info_printed = true;
154  }
155  }
156 
157  {
158  std::shared_ptr<BeaconSignal> b;
159  if ((b = std::dynamic_pointer_cast<BeaconSignal>(msg))) {
160  //ROS_INFO("Received beacon signal from %s:%s", b->team_name().c_str(), b->peer_name().c_str());
161 
162  rcll_ros_msgs::BeaconSignal rb;
163  rb.header.seq = b->seq();
164  rb.header.stamp = ros::Time(b->time().sec(), b->time().nsec());
165  rb.number = b->number();
166  rb.team_name = b->team_name();
167  rb.peer_name = b->peer_name();
168  rb.team_color = pb_to_ros_team_color(b->team_color());
169  if (b->has_pose()) {
170  rb.pose.header.frame_id = "/map";
171  rb.pose.header.stamp = ros::Time(b->pose().timestamp().sec(), b->pose().timestamp().nsec());
172  rb.pose.pose.position.x = b->pose().x();
173  rb.pose.pose.position.y = b->pose().y();
174  rb.pose.pose.position.z = 0.;
175  tf2::Quaternion q;
176  q.setRPY(0., 0., b->pose().ori());
177  rb.pose.pose.orientation.x = q.x();
178  rb.pose.pose.orientation.y = q.y();
179  rb.pose.pose.orientation.z = q.z();
180  rb.pose.pose.orientation.w = q.w();
181  }
182  pub_beacon_.publish(rb);
183  }
184  }
185 
186  {
187  std::shared_ptr<GameState> gs;
188  if ((gs = std::dynamic_pointer_cast<GameState>(msg))) {
189  rcll_ros_msgs::GameState rgs;
190  rgs.game_time.sec = gs->game_time().sec();
191  rgs.game_time.nsec = gs->game_time().nsec();
192  rgs.state = (int)gs->state();
193  rgs.phase = (int)gs->phase();
194  if (gs->has_points_cyan()) rgs.points_cyan = gs->points_cyan();
195  if (gs->has_points_magenta()) rgs.points_magenta = gs->points_magenta();
196 
197  if (gs->has_team_cyan()) {
198  if (gs->team_cyan() == cfg_team_name_ && cfg_team_color_ != (int)rcll_ros_msgs::Team::CYAN) {
199  setup_private_peer(llsf_msgs::CYAN);
200  cfg_team_color_ = (int)rcll_ros_msgs::Team::CYAN;
201  }
202  rgs.team_cyan = gs->team_cyan();
203  }
204  if (gs->has_team_magenta()) {
205  if (gs->team_magenta() == cfg_team_name_ && cfg_team_color_ != (int)rcll_ros_msgs::Team::MAGENTA) {
206  setup_private_peer(llsf_msgs::MAGENTA);
207  cfg_team_color_ = (int)rcll_ros_msgs::Team::MAGENTA;
208  }
209  rgs.team_magenta = gs->team_magenta();
210  }
211 
212  pub_game_state_.publish(rgs);
213  }
214  }
215 
216  {
217  std::shared_ptr<MachineInfo> mi;
218  if ((mi = std::dynamic_pointer_cast<MachineInfo>(msg))) {
219  rcll_ros_msgs::MachineInfo rmi;
220  for (int i = 0; i < mi->machines_size(); ++i) {
221  const llsf_msgs::Machine &m = mi->machines(i);
222  rcll_ros_msgs::Machine rm;
223  rm.name = m.name();
224  if (m.has_type()) rm.type = m.type();
225  if (m.has_state()) rm.state = m.state();
226  if (m.has_team_color()) pb_to_ros_team_color(m.team_color());
227  if (m.has_zone()) rm.zone = (int)m.zone();
228  for (int j = 0; j < m.ring_colors_size(); ++j) {
229  rm.rs_ring_colors.push_back((int)m.ring_colors(j));
230  }
231  rmi.machines.push_back(rm);
232  }
233  pub_machine_info_.publish(rmi);
234 
235  { // Wake if anyone is waiting for this, i.e., the prepare svc callback
236  std::unique_lock<std::mutex> lock(mtx_machine_info_);
237  msg_machine_info_ = mi;
238  cdv_machine_info_.notify_all();
239  }
240  }
241  }
242 
243  {
244  std::shared_ptr<ExplorationInfo> ei;
245  if ((ei = std::dynamic_pointer_cast<ExplorationInfo>(msg))) {
246  rcll_ros_msgs::ExplorationInfo rei;
247  for (int i = 0; i < ei->signals_size(); ++i) {
248  const llsf_msgs::ExplorationSignal &es = ei->signals(i);
249  rcll_ros_msgs::ExplorationSignal res;
250  res.type = es.type();
251  for (int j = 0; j < es.lights_size(); ++j) {
252  const llsf_msgs::LightSpec &l = es.lights(j);
253  rcll_ros_msgs::LightSpec rl;
254 
255  rl.light_color = (int)l.color();
256  rl.light_state = (int)l.state();
257  res.lights.push_back(rl);
258  }
259  rei.signals.push_back(res);
260  }
261  for (int i = 0; i < ei->zones_size(); ++i) {
262  const llsf_msgs::ExplorationZone &ez = ei->zones(i);
263  rcll_ros_msgs::ExplorationZone rez;
264  rez.zone = (int)ez.zone();
265  rez.team = pb_to_ros_team_color(ez.team_color());
266  rei.zones.push_back(rez);
267  }
268  pub_exploration_info_.publish(rei);
269  }
270  }
271 
272  {
273  std::shared_ptr<MachineReportInfo> mri;
274  if ((mri = std::dynamic_pointer_cast<MachineReportInfo>(msg))) {
275  rcll_ros_msgs::MachineReportInfo rmri;
276  for (int i = 0; i < mri->reported_machines_size(); ++i) {
277  rmri.reported_machines.push_back(mri->reported_machines(i));
278  }
279  rmri.team_color = pb_to_ros_team_color(mri->team_color());
280  pub_machine_report_info_.publish(rmri);
281  }
282  }
283 
284  {
285  std::shared_ptr<OrderInfo> oi;
286  if ((oi = std::dynamic_pointer_cast<OrderInfo>(msg))) {
287  rcll_ros_msgs::OrderInfo roi;
288  for (int i = 0; i < oi->orders_size(); ++i) {
289  const llsf_msgs::Order &o = oi->orders(i);
290  rcll_ros_msgs::Order ro;
291  ro.id = o.id();
292  ro.complexity = (int)o.complexity();
293  ro.base_color = (int)o.base_color();
294  ro.cap_color = (int)o.cap_color();
295  for (int j = 0; j < o.ring_colors_size(); ++j) {
296  ro.ring_colors.push_back((int)o.ring_colors(j));
297  }
298  ro.quantity_requested = o.quantity_requested();
299  ro.quantity_delivered_cyan = o.quantity_delivered_cyan();
300  ro.quantity_delivered_magenta = o.quantity_delivered_magenta();
301  ro.delivery_period_begin = o.delivery_period_begin();
302  ro.delivery_period_end = o.delivery_period_end();
303  ro.delivery_gate = o.delivery_gate();
304  roi.orders.push_back(ro);
305  }
306  pub_order_info_.publish(roi);
307  }
308  }
309 
310  {
311  std::shared_ptr<RingInfo> ri;
312  if ((ri = std::dynamic_pointer_cast<RingInfo>(msg))) {
313  rcll_ros_msgs::RingInfo rri;
314  for (int i = 0; i < ri->rings_size(); ++i) {
315  const llsf_msgs::Ring &r = ri->rings(i);
316  rcll_ros_msgs::Ring rr;
317  rr.ring_color = (int)r.ring_color();
318  rr.raw_material = r.raw_material();
319  rri.rings.push_back(rr);
320  }
321  pub_ring_info_.publish(rri);
322  }
323  }
324 
325 }
326 
327 
328 void
329 setup_private_peer(llsf_msgs::Team team_color)
330 {
331  delete peer_private_;
332  peer_private_ = NULL;
333 
334  if (team_color == llsf_msgs::CYAN) {
335  ROS_INFO("Creating private peer for CYAN");
336 
337  if (cfg_peer_cyan_local_) {
338  peer_private_ = new ProtobufBroadcastPeer(cfg_peer_address_,
339  cfg_peer_cyan_send_port_,
340  cfg_peer_cyan_recv_port_,
341  &peer_public_->message_register(),
342  cfg_crypto_key_, cfg_crypto_cipher_);
343  } else {
344  peer_private_ = new ProtobufBroadcastPeer(cfg_peer_address_, cfg_peer_cyan_port_,
345  &peer_public_->message_register(),
346  cfg_crypto_key_, cfg_crypto_cipher_);
347  }
348 
349  } else {
350  ROS_INFO("Creating private peer for MAGENTA");
351 
352  if (cfg_peer_magenta_local_) {
353  peer_private_ = new ProtobufBroadcastPeer(cfg_peer_address_,
354  cfg_peer_magenta_send_port_,
355  cfg_peer_magenta_recv_port_,
356  &peer_public_->message_register(),
357  cfg_crypto_key_, cfg_crypto_cipher_);
358  } else {
359  peer_private_ = new ProtobufBroadcastPeer(cfg_peer_address_, cfg_peer_magenta_port_,
360  &peer_public_->message_register(),
361  cfg_crypto_key_, cfg_crypto_cipher_);
362  }
363  }
364 
365  peer_private_->signal_received().connect(handle_message);
366  peer_private_->signal_recv_error().connect(handle_recv_error);
367  peer_private_->signal_send_error().connect(handle_send_error);
368 }
369 
370 bool
371 srv_cb_send_beacon(rcll_ros_msgs::SendBeaconSignal::Request &req,
372  rcll_ros_msgs::SendBeaconSignal::Response &res)
373 {
374  if (! peer_private_) {
375  res.ok = false;
376  res.error_msg = "Cannot send beacon signal: private peer not setup, team not set in refbox?";
377  return true;
378  }
379 
380  llsf_msgs::BeaconSignal b;
381  b.mutable_time()->set_sec(req.header.stamp.sec);
382  b.mutable_time()->set_nsec(req.header.stamp.nsec);
383  b.set_seq(req.header.seq);
384  b.set_number(cfg_robot_number_);
385  b.set_team_name(cfg_team_name_);
386  b.set_peer_name(cfg_robot_name_);
387  b.set_team_color(ros_to_pb_team_color(cfg_team_color_));
388  if (req.pose.pose.position.x != 0. || req.pose.pose.position.y != 0. ||
389  req.pose.pose.orientation.x != 0. || req.pose.pose.orientation.y != 0. ||
390  req.pose.pose.orientation.z != 0. || req.pose.pose.orientation.w != 0.)
391  {
392  if (req.pose.pose.position.z != 0.) {
393  ROS_WARN("Poses must be 2.5D pose in ground support plane (x,y,ori)");
394  res.ok = false;
395  res.error_msg = "Poses must be 2.5D pose in ground support plane (x,y,ori)";
396  return true;
397  } else {
398  b.mutable_pose()->mutable_timestamp()->set_sec(req.pose.header.stamp.sec);
399  b.mutable_pose()->mutable_timestamp()->set_nsec(req.pose.header.stamp.nsec);
400  b.mutable_pose()->set_x(req.pose.pose.position.x);
401  b.mutable_pose()->set_y(req.pose.pose.position.y);
402  tf2::Quaternion q(req.pose.pose.orientation.x, req.pose.pose.orientation.y,
403  req.pose.pose.orientation.z, req.pose.pose.orientation.w);
404  b.mutable_pose()->set_ori(tf2::getYaw(q));
405  }
406  }
407 
408  try {
409  ROS_DEBUG("Sending beacon %s:%s (seq %lu)", b.team_name().c_str(), b.peer_name().c_str(), b.seq());
410  peer_private_->send(b);
411  res.ok = true;
412  } catch (std::runtime_error &e) {
413  res.ok = false;
414  res.error_msg = e.what();
415  }
416 
417  return true;
418 }
419 
420 bool
421 srv_cb_send_machine_report(rcll_ros_msgs::SendMachineReport::Request &req,
422  rcll_ros_msgs::SendMachineReport::Response &res)
423 {
424  if (! peer_private_) {
425  res.ok = false;
426  res.error_msg = "Cannot send machine report: private peer not setup, team not set in refbox?";
427  return true;
428  }
429 
430  llsf_msgs::MachineReport mr;
431  mr.set_team_color(ros_to_pb_team_color(cfg_team_color_));
432 
433  std::string machines_sent;
434 
435  for (size_t i = 0; i < req.machines.size(); ++i) {
436  const rcll_ros_msgs::MachineReportEntry &rmre = req.machines[i];
437  llsf_msgs::MachineReportEntry *mre = mr.add_machines();
438  mre->set_name(rmre.name);
439  mre->set_type(rmre.type);
440 
441  if (! Zone_IsValid(rmre.zone)) {
442  res.ok = false;
443  res.error_msg = std::string("Invalid zone value for machine") + rmre.name;
444  return true;
445  }
446 
447  mre->set_zone((llsf_msgs::Zone)rmre.zone);
448 
449  if (! machines_sent.empty()) machines_sent += ", ";
450  machines_sent += rmre.name;
451  }
452 
453  try {
454  ROS_DEBUG("Sending machine report for machines: %s", machines_sent.c_str());
455  peer_private_->send(mr);
456  res.ok = true;
457  } catch (std::runtime_error &e) {
458  res.ok = false;
459  res.error_msg = e.what();
460  }
461 
462  return true;
463 }
464 
465 bool
466 srv_cb_send_prepare_machine(rcll_ros_msgs::SendPrepareMachine::Request &req,
467  rcll_ros_msgs::SendPrepareMachine::Response &res)
468 {
469  if (! peer_private_) {
470  res.ok = false;
471  res.error_msg = "private peer not setup, team not set in refbox?";
472  return true;
473  }
474 
475  if (req.machine.length() < 4) {
476  res.ok = false;
477  res.error_msg = "Invalid machine name '" + req.machine + "'";
478  return true;
479  }
480 
481  // parse machine name
482  std::string machine_team = req.machine.substr(0, 1);
483  std::string machine_type = req.machine.substr(2, 2);
484 
485  if (machine_team != "C" && machine_team != "M") {
486  res.ok = false;
487  res.error_msg = "Invalid team prefix '" + machine_team + "', must be C or M";
488  return true;
489  }
490  if (machine_team == "C" && cfg_team_color_ == (int)rcll_ros_msgs::Team::MAGENTA) {
491  res.ok = false;
492  res.error_msg = "Invalid team prefix 'C', own team is MAGENTA";
493  return true;
494  }
495  if (machine_team == "M" && cfg_team_color_ == (int)rcll_ros_msgs::Team::CYAN) {
496  res.ok = false;
497  res.error_msg = "Invalid team prefix 'M', own team is CYAN";
498  return true;
499  }
500  if (machine_type != "BS" && machine_type != "DS" && machine_type != "CS" && machine_type != "RS") {
501  res.ok = false;
502  res.error_msg = "Invalid machine type '" + machine_type + "' in name";
503  return true;
504  }
505 
506  llsf_msgs::PrepareMachine pm;
507  pm.set_team_color(ros_to_pb_team_color(cfg_team_color_));
508  pm.set_machine(req.machine);
509 
510  if (machine_type == "BS") {
511  if (! llsf_msgs::MachineSide_IsValid(req.bs_side)) {
512  res.ok = false;
513  res.error_msg = "Invalid BS machine side";
514  return true;
515  }
516  if (! llsf_msgs::BaseColor_IsValid(req.bs_base_color)) {
517  res.ok = false;
518  res.error_msg = "Invalid BS base color";
519  return true;
520  }
521  llsf_msgs::PrepareInstructionBS *bsi = pm.mutable_instruction_bs();
522  bsi->set_side((llsf_msgs::MachineSide)req.bs_side);
523  bsi->set_color((llsf_msgs::BaseColor)req.bs_base_color);
524  } else if (machine_type == "DS") {
525  llsf_msgs::PrepareInstructionDS *dsi = pm.mutable_instruction_ds();
526  dsi->set_gate(req.ds_gate);
527  } else if (machine_type == "CS") {
528  if (! llsf_msgs::CsOp_IsValid(req.cs_operation)) {
529  res.ok = false;
530  res.error_msg = "Invalid CS operation";
531  return true;
532  }
533  llsf_msgs::PrepareInstructionCS *csi = pm.mutable_instruction_cs();
534  csi->set_operation((llsf_msgs::CsOp)req.cs_operation);
535  } else if (machine_type == "RS") {
536  if (! llsf_msgs::RingColor_IsValid(req.rs_ring_color)) {
537  res.ok = false;
538  res.error_msg = "Invalid RS ring color";
539  return true;
540  }
541  llsf_msgs::PrepareInstructionRS *rsi = pm.mutable_instruction_rs();
542  rsi->set_ring_color((llsf_msgs::RingColor)req.rs_ring_color);
543  }
544 
545  if (req.wait) {
546  std::unique_lock<std::mutex> lock(mtx_machine_info_);
547 
548  bool need_to_wait = false;
549  for (int i = 0; i < msg_machine_info_->machines_size(); ++i) {
550  const llsf_msgs::Machine &m = msg_machine_info_->machines(i);
551  if (m.name() == req.machine) {
552  if (m.has_state() && m.state() != "IDLE") {
553  ROS_INFO("Machine '%s' is not in IDLE state, waiting briefly", req.machine.c_str());
554  need_to_wait = true;
555  }
556  break;
557  }
558  }
559  if (need_to_wait) {
560  int machine_idx = 0;
561  cdv_machine_info_.wait_for(lock, std::chrono::seconds(10),
562  [&req, &machine_idx]() -> bool {
563  if (! msg_machine_info_) return false;
564  rcll_ros_msgs::MachineInfo rmi;
565  for (int i = 0; i < msg_machine_info_->machines_size(); ++i) {
566  const llsf_msgs::Machine &m = msg_machine_info_->machines(i);
567  if (m.name() == req.machine && m.has_state() && m.state() == "IDLE") {
568  machine_idx = i;
569  return true;
570  }
571  }
572  return false;
573  });
574 
575  const llsf_msgs::Machine &m = msg_machine_info_->machines(machine_idx);
576  if (m.state() != "IDLE") {
577  ROS_INFO("Machine '%s' went into '%s' state, not IDLE", req.machine.c_str(), m.state().c_str());
578  res.ok = false;
579  res.error_msg = "Machine '" + req.machine + "' went into '" + m.state() + "' state, not IDLE";
580  return true;
581  } else {
582  ROS_INFO("Machine '%s' is now in IDLE state", req.machine.c_str());
583  }
584  }
585  }
586 
587  try {
588  ROS_DEBUG("Sending prepare machine instruction for machine: %s", req.machine.c_str());
589  peer_private_->send(pm);
590  res.ok = true;
591  } catch (std::runtime_error &e) {
592  res.ok = false;
593  res.error_msg = e.what();
594  }
595 
596  if (req.wait) {
597  ROS_INFO("Waiting for machine '%s' state != IDLE", req.machine.c_str());
598  std::unique_lock<std::mutex> lock(mtx_machine_info_);
599 
600  int machine_idx = 0;
601  cdv_machine_info_.wait(lock,
602  [&req, &machine_idx, &pm]() -> bool {
603  if (! msg_machine_info_) return false;
604  rcll_ros_msgs::MachineInfo rmi;
605  for (int i = 0; i < msg_machine_info_->machines_size(); ++i) {
606  const llsf_msgs::Machine &m = msg_machine_info_->machines(i);
607  if (m.name() == req.machine && m.has_state() && m.state() != "IDLE") {
608  machine_idx = i;
609  return true;
610  }
611  }
612  peer_private_->send(pm);
613  return false;
614  });
615 
616  const llsf_msgs::Machine &m = msg_machine_info_->machines(machine_idx);
617  if (m.state() == "BROKEN") {
618  ROS_WARN("Machine '%s' went into 'BROKEN' state", req.machine.c_str());
619  res.ok = false;
620  res.error_msg = "Machine '" + req.machine + "' went into '" + m.state() + "' state";
621  } else {
622  ROS_INFO("Machine '%s' went into '%s' state", req.machine.c_str(), m.state().c_str());
623  res.ok = true;
624  }
625  }
626 
627  return true;
628 }
629 
630 int
631 main(int argc, char **argv)
632 {
633  ros::init(argc, argv, "rcll_refbox_peer");
634 
635  ros::NodeHandle n;
636 
637  ROS_INFO("%s starting up", ros::this_node::getName().c_str());
638 
639  // Parameter parsing
640  cfg_team_color_ = 0;
641 
642  GET_PRIV_PARAM(team_name);
643  GET_PRIV_PARAM(robot_name);
644  GET_PRIV_PARAM(robot_number);
645 
646  GET_PRIV_PARAM(peer_address);
647 
648  if (ros::param::has("~peer_public_recv_port") && ros::param::has("~peer_public_send_port")) {
649  cfg_peer_public_local_ = true;
650  GET_PRIV_PARAM(peer_public_recv_port);
651  GET_PRIV_PARAM(peer_public_send_port);
652  } else {
653  cfg_peer_public_local_ = false;
654  GET_PRIV_PARAM(peer_public_port);
655  }
656 
657  if (ros::param::has("~peer_cyan_recv_port") && ros::param::has("~peer_cyan_send_port")) {
658  cfg_peer_cyan_local_ = true;
659  if (! ros::param::get("~peer_cyan_recv_port", cfg_peer_cyan_recv_port_)) {
660  ROS_ERROR("Failed to retrieve parameter cyan_recv_port, aborting");
661  exit(-1);
662  }
663  if (! ros::param::get("~peer_cyan_send_port", cfg_peer_cyan_send_port_)) {
664  ROS_ERROR("Failed to retrieve parameter cyan_send_port, aborting");
665  exit(-1);
666  }
667  } else {
668  cfg_peer_cyan_local_ = false;
669  if (! ros::param::get("~peer_cyan_port", cfg_peer_cyan_port_)) {
670  ROS_ERROR("Failed to retrieve parameter cyan_port, aborting");
671  exit(-1);
672  }
673  }
674  if (ros::param::has("~peer_magenta_recv_port") && ros::param::has("~peer_magenta_send_port")) {
675  cfg_peer_magenta_local_ = true;
676  if (! ros::param::get("~peer_magenta_recv_port", cfg_peer_magenta_recv_port_)) {
677  ROS_ERROR("Failed to retrieve parameter magenta_recv_port, aborting");
678  exit(-1);
679  }
680  if (! ros::param::get("~peer_magenta_send_port", cfg_peer_magenta_send_port_)) {
681  ROS_ERROR("Failed to retrieve parameter magenta_send_port, aborting");
682  exit(-1);
683  }
684  } else {
685  cfg_peer_magenta_local_ = false;
686  if (! ros::param::get("~peer_magenta_port", cfg_peer_magenta_port_)) {
687  ROS_ERROR("Failed to retrieve parameter magenta_port, aborting");
688  exit(-1);
689  }
690  }
691 
692  GET_PRIV_PARAM(crypto_key);
693  GET_PRIV_PARAM(crypto_cipher);
694 
695  // Setup ROS topics
696  pub_beacon_ = n.advertise<rcll_ros_msgs::BeaconSignal>("rcll/beacon", 100);
697  pub_game_state_ = n.advertise<rcll_ros_msgs::GameState>("rcll/game_state", 10);
698  pub_machine_info_ = n.advertise<rcll_ros_msgs::MachineInfo>("rcll/machine_info", 10);
699  pub_exploration_info_ = n.advertise<rcll_ros_msgs::ExplorationInfo>("rcll/exploration_info", 10);
700  pub_machine_report_info_ = n.advertise<rcll_ros_msgs::MachineReportInfo>("rcll/machine_report_info", 10);
701  pub_order_info_ = n.advertise<rcll_ros_msgs::OrderInfo>("rcll/order_info", 10);
702  pub_ring_info_ = n.advertise<rcll_ros_msgs::RingInfo>("rcll/ring_info", 10);
703 
704  // Setup basic communication
705  if (cfg_peer_public_local_) {
706  ROS_WARN("Creating public peer: addr: %s send: %u recv: %u",
707  cfg_peer_address_.c_str(), cfg_peer_public_send_port_, cfg_peer_public_recv_port_);
708  peer_public_ = new ProtobufBroadcastPeer(cfg_peer_address_,
709  cfg_peer_public_send_port_,
710  cfg_peer_public_recv_port_);
711  } else {
712  ROS_WARN("Creating public peer: addr: %s port: %u",
713  cfg_peer_address_.c_str(), cfg_peer_public_port_);
714  peer_public_ = new ProtobufBroadcastPeer(cfg_peer_address_, cfg_peer_public_port_);
715  }
716 
717  MessageRegister & message_register = peer_public_->message_register();
718  message_register.add_message_type<llsf_msgs::VersionInfo>();
719  message_register.add_message_type<llsf_msgs::BeaconSignal>();
720  message_register.add_message_type<llsf_msgs::GameState>();
721  message_register.add_message_type<llsf_msgs::MachineInfo>();
722  message_register.add_message_type<llsf_msgs::ExplorationInfo>();
723  message_register.add_message_type<llsf_msgs::MachineReportInfo>();
724  message_register.add_message_type<llsf_msgs::OrderInfo>();
725  message_register.add_message_type<llsf_msgs::RingInfo>();
726  message_register.add_message_type<llsf_msgs::RobotInfo>();
727  message_register.add_message_type<llsf_msgs::PrepareMachine>();
728 
729  peer_public_->signal_received().connect(handle_message);
730  peer_public_->signal_recv_error().connect(handle_recv_error);
731  peer_public_->signal_send_error().connect(handle_send_error);
732 
733  // provide services
734  srv_send_beacon_ = n.advertiseService("rcll/send_beacon", srv_cb_send_beacon);
735  srv_send_machine_report_ = n.advertiseService("rcll/send_machine_report", srv_cb_send_machine_report);
736  srv_send_prepare_machine_ = n.advertiseService("rcll/send_prepare_machine", srv_cb_send_prepare_machine);
737 
738  ros::spin();
739 
740  return 0;
741 }
cfg_team_name_
std::string cfg_team_name_
Definition: rcll_refbox_peer_node.cpp:68
msg_machine_info_
std::shared_ptr< MachineInfo > msg_machine_info_
Definition: rcll_refbox_peer_node.cpp:105
tf2::Quaternion::setRPY
void setRPY(const tf2Scalar &roll, const tf2Scalar &pitch, const tf2Scalar &yaw)
peer_public_
ProtobufBroadcastPeer * peer_public_
Definition: rcll_refbox_peer_node.cpp:100
ros::NodeHandle
cfg_peer_magenta_recv_port_
int cfg_peer_magenta_recv_port_
Definition: rcll_refbox_peer_node.cpp:86
setup_private_peer
void setup_private_peer(llsf_msgs::Team team_color)
Definition: rcll_refbox_peer_node.cpp:329
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
cfg_peer_public_recv_port_
int cfg_peer_public_recv_port_
Definition: rcll_refbox_peer_node.cpp:78
cfg_robot_name_
std::string cfg_robot_name_
Definition: rcll_refbox_peer_node.cpp:69
handle_send_error
void handle_send_error(std::string msg)
Definition: rcll_refbox_peer_node.cpp:117
srv_send_machine_report_
ros::ServiceServer srv_send_machine_report_
Definition: rcll_refbox_peer_node.cpp:97
pb_to_ros_team_color
int pb_to_ros_team_color(const llsf_msgs::Team &team_color)
Definition: rcll_refbox_peer_node.cpp:124
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
cfg_peer_magenta_send_port_
int cfg_peer_magenta_send_port_
Definition: rcll_refbox_peer_node.cpp:85
ros::this_node::getName
ROSCPP_DECL const std::string & getName()
pub_order_info_
ros::Publisher pub_order_info_
Definition: rcll_refbox_peer_node.cpp:93
cfg_peer_cyan_local_
bool cfg_peer_cyan_local_
Definition: rcll_refbox_peer_node.cpp:79
cfg_peer_magenta_port_
int cfg_peer_magenta_port_
Definition: rcll_refbox_peer_node.cpp:84
protobuf_comm
ros::NodeHandle::advertiseService
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
cfg_peer_public_port_
int cfg_peer_public_port_
Definition: rcll_refbox_peer_node.cpp:76
ROS_WARN
#define ROS_WARN(...)
pub_machine_info_
ros::Publisher pub_machine_info_
Definition: rcll_refbox_peer_node.cpp:90
srv_send_prepare_machine_
ros::ServiceServer srv_send_prepare_machine_
Definition: rcll_refbox_peer_node.cpp:98
llsf_msgs
srv_send_beacon_
ros::ServiceServer srv_send_beacon_
Definition: rcll_refbox_peer_node.cpp:96
pub_game_state_
ros::Publisher pub_game_state_
Definition: rcll_refbox_peer_node.cpp:89
cfg_peer_cyan_recv_port_
int cfg_peer_cyan_recv_port_
Definition: rcll_refbox_peer_node.cpp:82
cfg_peer_magenta_local_
bool cfg_peer_magenta_local_
Definition: rcll_refbox_peer_node.cpp:83
ros::param::get
ROSCPP_DECL bool get(const std::string &key, std::string &s)
ROS_INFO
#define ROS_INFO(...)
handle_recv_error
void handle_recv_error(boost::asio::ip::udp::endpoint &endpoint, std::string msg)
Definition: rcll_refbox_peer_node.cpp:110
srv_cb_send_machine_report
bool srv_cb_send_machine_report(rcll_ros_msgs::SendMachineReport::Request &req, rcll_ros_msgs::SendMachineReport::Response &res)
Definition: rcll_refbox_peer_node.cpp:421
ros_to_pb_team_color
llsf_msgs::Team ros_to_pb_team_color(int team_color)
Definition: rcll_refbox_peer_node.cpp:133
pub_machine_report_info_
ros::Publisher pub_machine_report_info_
Definition: rcll_refbox_peer_node.cpp:92
cdv_machine_info_
std::condition_variable cdv_machine_info_
Definition: rcll_refbox_peer_node.cpp:104
ros::param::has
ROSCPP_DECL bool has(const std::string &key)
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::spin
ROSCPP_DECL void spin()
pub_ring_info_
ros::Publisher pub_ring_info_
Definition: rcll_refbox_peer_node.cpp:94
cfg_peer_address_
std::string cfg_peer_address_
Definition: rcll_refbox_peer_node.cpp:74
tf2::getYaw
double getYaw(const A &a)
cfg_peer_cyan_port_
int cfg_peer_cyan_port_
Definition: rcll_refbox_peer_node.cpp:80
cfg_peer_cyan_send_port_
int cfg_peer_cyan_send_port_
Definition: rcll_refbox_peer_node.cpp:81
cfg_peer_public_send_port_
int cfg_peer_public_send_port_
Definition: rcll_refbox_peer_node.cpp:77
cfg_crypto_key_
std::string cfg_crypto_key_
Definition: rcll_refbox_peer_node.cpp:72
cfg_peer_public_local_
bool cfg_peer_public_local_
Definition: rcll_refbox_peer_node.cpp:75
srv_cb_send_beacon
bool srv_cb_send_beacon(rcll_ros_msgs::SendBeaconSignal::Request &req, rcll_ros_msgs::SendBeaconSignal::Response &res)
Definition: rcll_refbox_peer_node.cpp:371
pub_beacon_
ros::Publisher pub_beacon_
Definition: rcll_refbox_peer_node.cpp:88
handle_message
void handle_message(boost::asio::ip::udp::endpoint &sender, uint16_t component_id, uint16_t msg_type, std::shared_ptr< google::protobuf::Message > msg)
Definition: rcll_refbox_peer_node.cpp:142
srv_cb_send_prepare_machine
bool srv_cb_send_prepare_machine(rcll_ros_msgs::SendPrepareMachine::Request &req, rcll_ros_msgs::SendPrepareMachine::Response &res)
Definition: rcll_refbox_peer_node.cpp:466
cfg_crypto_cipher_
std::string cfg_crypto_cipher_
Definition: rcll_refbox_peer_node.cpp:73
cfg_team_color_
int cfg_team_color_
Definition: rcll_refbox_peer_node.cpp:71
main
int main(int argc, char **argv)
Definition: rcll_refbox_peer_node.cpp:631
cfg_robot_number_
int cfg_robot_number_
Definition: rcll_refbox_peer_node.cpp:70
ROS_ERROR
#define ROS_ERROR(...)
peer_private_
ProtobufBroadcastPeer * peer_private_
Definition: rcll_refbox_peer_node.cpp:101
mtx_machine_info_
std::mutex mtx_machine_info_
Definition: rcll_refbox_peer_node.cpp:103
pub_exploration_info_
ros::Publisher pub_exploration_info_
Definition: rcll_refbox_peer_node.cpp:91
ROS_DEBUG
#define ROS_DEBUG(...)
GET_PRIV_PARAM
#define GET_PRIV_PARAM(P)
Definition: rcll_refbox_peer_node.cpp:57

rcll_refbox_peer
Author(s): Tim Niemueller
autogenerated on Mon Jun 10 2019 14:30:04