00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022 #include <ros/ros.h>
00023
00024 #include <protobuf_comm/peer.h>
00025
00026 #include <llsf_msgs/BeaconSignal.pb.h>
00027 #include <llsf_msgs/VersionInfo.pb.h>
00028 #include <llsf_msgs/GameState.pb.h>
00029 #include <llsf_msgs/MachineInfo.pb.h>
00030 #include <llsf_msgs/RobotInfo.pb.h>
00031 #include <llsf_msgs/ExplorationInfo.pb.h>
00032 #include <llsf_msgs/MachineReport.pb.h>
00033 #include <llsf_msgs/OrderInfo.pb.h>
00034 #include <llsf_msgs/RingInfo.pb.h>
00035 #include <llsf_msgs/MachineInstructions.pb.h>
00036
00037 #include <rcll_ros_msgs/BeaconSignal.h>
00038 #include <rcll_ros_msgs/GameState.h>
00039 #include <rcll_ros_msgs/Team.h>
00040 #include <rcll_ros_msgs/MachineInfo.h>
00041 #include <rcll_ros_msgs/ExplorationInfo.h>
00042 #include <rcll_ros_msgs/MachineReportInfo.h>
00043 #include <rcll_ros_msgs/OrderInfo.h>
00044 #include <rcll_ros_msgs/RingInfo.h>
00045
00046 #include <rcll_ros_msgs/SendBeaconSignal.h>
00047 #include <rcll_ros_msgs/SendMachineReport.h>
00048 #include <rcll_ros_msgs/SendPrepareMachine.h>
00049
00050 #include <tf2/LinearMath/Quaternion.h>
00051 #include <tf2/utils.h>
00052
00053 #include <mutex>
00054 #include <condition_variable>
00055 #include <chrono>
00056
00057 #define GET_PRIV_PARAM(P) \
00058 { \
00059 if (! ros::param::get("~" #P, cfg_ ## P ## _)) { \
00060 ROS_ERROR("Failed to retrieve parameter " #P ", aborting"); \
00061 exit(-1); \
00062 } \
00063 }
00064
00065 using namespace protobuf_comm;
00066 using namespace llsf_msgs;
00067
00068 std::string cfg_team_name_;
00069 std::string cfg_robot_name_;
00070 int cfg_robot_number_;
00071 int cfg_team_color_;
00072 std::string cfg_crypto_key_;
00073 std::string cfg_crypto_cipher_;
00074 std::string cfg_peer_address_;
00075 bool cfg_peer_public_local_;
00076 int cfg_peer_public_port_;
00077 int cfg_peer_public_send_port_;
00078 int cfg_peer_public_recv_port_;
00079 bool cfg_peer_cyan_local_;
00080 int cfg_peer_cyan_port_;
00081 int cfg_peer_cyan_send_port_;
00082 int cfg_peer_cyan_recv_port_;
00083 bool cfg_peer_magenta_local_;
00084 int cfg_peer_magenta_port_;
00085 int cfg_peer_magenta_send_port_;
00086 int cfg_peer_magenta_recv_port_;
00087
00088 ros::Publisher pub_beacon_;
00089 ros::Publisher pub_game_state_;
00090 ros::Publisher pub_machine_info_;
00091 ros::Publisher pub_exploration_info_;
00092 ros::Publisher pub_machine_report_info_;
00093 ros::Publisher pub_order_info_;
00094 ros::Publisher pub_ring_info_;
00095
00096 ros::ServiceServer srv_send_beacon_;
00097 ros::ServiceServer srv_send_machine_report_;
00098 ros::ServiceServer srv_send_prepare_machine_;
00099
00100 ProtobufBroadcastPeer *peer_public_ = NULL;
00101 ProtobufBroadcastPeer *peer_private_ = NULL;
00102
00103 std::mutex mtx_machine_info_;
00104 std::condition_variable cdv_machine_info_;
00105 std::shared_ptr<MachineInfo> msg_machine_info_;
00106
00107 void setup_private_peer(llsf_msgs::Team team_color);
00108
00109 void
00110 handle_recv_error(boost::asio::ip::udp::endpoint &endpoint, std::string msg)
00111 {
00112 ROS_WARN("Receive error from %s:%u: %s",
00113 endpoint.address().to_string().c_str(), endpoint.port(), msg.c_str());
00114 }
00115
00116 void
00117 handle_send_error(std::string msg)
00118 {
00119 ROS_ERROR("Send error: %s", msg.c_str());
00120 }
00121
00122
00123 int
00124 pb_to_ros_team_color(const llsf_msgs::Team &team_color)
00125 {
00126 if (team_color == llsf_msgs::CYAN)
00127 return (int)rcll_ros_msgs::Team::CYAN;
00128 else
00129 return (int)rcll_ros_msgs::Team::MAGENTA;
00130 }
00131
00132 llsf_msgs::Team
00133 ros_to_pb_team_color(int team_color)
00134 {
00135 if (team_color == rcll_ros_msgs::Team::CYAN)
00136 return llsf_msgs::CYAN;
00137 else
00138 return llsf_msgs::MAGENTA;
00139 }
00140
00141 void
00142 handle_message(boost::asio::ip::udp::endpoint &sender,
00143 uint16_t component_id, uint16_t msg_type,
00144 std::shared_ptr<google::protobuf::Message> msg)
00145 {
00146 {
00147
00148
00149 static bool version_info_printed = false;
00150 std::shared_ptr<VersionInfo> v;
00151 if (! version_info_printed && (v = std::dynamic_pointer_cast<VersionInfo>(msg))) {
00152 ROS_INFO("Referee Box %s detected", v->version_string().c_str());
00153 version_info_printed = true;
00154 }
00155 }
00156
00157 {
00158 std::shared_ptr<BeaconSignal> b;
00159 if ((b = std::dynamic_pointer_cast<BeaconSignal>(msg))) {
00160
00161
00162 rcll_ros_msgs::BeaconSignal rb;
00163 rb.header.seq = b->seq();
00164 rb.header.stamp = ros::Time(b->time().sec(), b->time().nsec());
00165 rb.number = b->number();
00166 rb.team_name = b->team_name();
00167 rb.peer_name = b->peer_name();
00168 rb.team_color = pb_to_ros_team_color(b->team_color());
00169 if (b->has_pose()) {
00170 rb.pose.header.frame_id = "/map";
00171 rb.pose.header.stamp = ros::Time(b->pose().timestamp().sec(), b->pose().timestamp().nsec());
00172 rb.pose.pose.position.x = b->pose().x();
00173 rb.pose.pose.position.y = b->pose().y();
00174 rb.pose.pose.position.z = 0.;
00175 tf2::Quaternion q;
00176 q.setRPY(0., 0., b->pose().ori());
00177 rb.pose.pose.orientation.x = q.x();
00178 rb.pose.pose.orientation.y = q.y();
00179 rb.pose.pose.orientation.z = q.z();
00180 rb.pose.pose.orientation.w = q.w();
00181 }
00182 pub_beacon_.publish(rb);
00183 }
00184 }
00185
00186 {
00187 std::shared_ptr<GameState> gs;
00188 if ((gs = std::dynamic_pointer_cast<GameState>(msg))) {
00189 rcll_ros_msgs::GameState rgs;
00190 rgs.game_time.sec = gs->game_time().sec();
00191 rgs.game_time.nsec = gs->game_time().nsec();
00192 rgs.state = (int)gs->state();
00193 rgs.phase = (int)gs->phase();
00194 if (gs->has_points_cyan()) rgs.points_cyan = gs->points_cyan();
00195 if (gs->has_points_magenta()) rgs.points_magenta = gs->points_magenta();
00196
00197 if (gs->has_team_cyan()) {
00198 if (gs->team_cyan() == cfg_team_name_ && cfg_team_color_ != (int)rcll_ros_msgs::Team::CYAN) {
00199 setup_private_peer(llsf_msgs::CYAN);
00200 cfg_team_color_ = (int)rcll_ros_msgs::Team::CYAN;
00201 }
00202 rgs.team_cyan = gs->team_cyan();
00203 }
00204 if (gs->has_team_magenta()) {
00205 if (gs->team_magenta() == cfg_team_name_ && cfg_team_color_ != (int)rcll_ros_msgs::Team::MAGENTA) {
00206 setup_private_peer(llsf_msgs::MAGENTA);
00207 cfg_team_color_ = (int)rcll_ros_msgs::Team::MAGENTA;
00208 }
00209 rgs.team_magenta = gs->team_magenta();
00210 }
00211
00212 pub_game_state_.publish(rgs);
00213 }
00214 }
00215
00216 {
00217 std::shared_ptr<MachineInfo> mi;
00218 if ((mi = std::dynamic_pointer_cast<MachineInfo>(msg))) {
00219 rcll_ros_msgs::MachineInfo rmi;
00220 for (int i = 0; i < mi->machines_size(); ++i) {
00221 const llsf_msgs::Machine &m = mi->machines(i);
00222 rcll_ros_msgs::Machine rm;
00223 rm.name = m.name();
00224 if (m.has_type()) rm.type = m.type();
00225 if (m.has_state()) rm.state = m.state();
00226 if (m.has_team_color()) pb_to_ros_team_color(m.team_color());
00227 if (m.has_zone()) rm.zone = (int)m.zone();
00228 for (int j = 0; j < m.ring_colors_size(); ++j) {
00229 rm.rs_ring_colors.push_back((int)m.ring_colors(j));
00230 }
00231 rmi.machines.push_back(rm);
00232 }
00233 pub_machine_info_.publish(rmi);
00234
00235 {
00236 std::unique_lock<std::mutex> lock(mtx_machine_info_);
00237 msg_machine_info_ = mi;
00238 cdv_machine_info_.notify_all();
00239 }
00240 }
00241 }
00242
00243 {
00244 std::shared_ptr<ExplorationInfo> ei;
00245 if ((ei = std::dynamic_pointer_cast<ExplorationInfo>(msg))) {
00246 rcll_ros_msgs::ExplorationInfo rei;
00247 for (int i = 0; i < ei->signals_size(); ++i) {
00248 const llsf_msgs::ExplorationSignal &es = ei->signals(i);
00249 rcll_ros_msgs::ExplorationSignal res;
00250 res.type = es.type();
00251 for (int j = 0; j < es.lights_size(); ++j) {
00252 const llsf_msgs::LightSpec &l = es.lights(j);
00253 rcll_ros_msgs::LightSpec rl;
00254
00255 rl.light_color = (int)l.color();
00256 rl.light_state = (int)l.state();
00257 res.lights.push_back(rl);
00258 }
00259 rei.signals.push_back(res);
00260 }
00261 for (int i = 0; i < ei->zones_size(); ++i) {
00262 const llsf_msgs::ExplorationZone &ez = ei->zones(i);
00263 rcll_ros_msgs::ExplorationZone rez;
00264 rez.zone = (int)ez.zone();
00265 rez.team = pb_to_ros_team_color(ez.team_color());
00266 rei.zones.push_back(rez);
00267 }
00268 pub_exploration_info_.publish(rei);
00269 }
00270 }
00271
00272 {
00273 std::shared_ptr<MachineReportInfo> mri;
00274 if ((mri = std::dynamic_pointer_cast<MachineReportInfo>(msg))) {
00275 rcll_ros_msgs::MachineReportInfo rmri;
00276 for (int i = 0; i < mri->reported_machines_size(); ++i) {
00277 rmri.reported_machines.push_back(mri->reported_machines(i));
00278 }
00279 rmri.team_color = pb_to_ros_team_color(mri->team_color());
00280 pub_machine_report_info_.publish(rmri);
00281 }
00282 }
00283
00284 {
00285 std::shared_ptr<OrderInfo> oi;
00286 if ((oi = std::dynamic_pointer_cast<OrderInfo>(msg))) {
00287 rcll_ros_msgs::OrderInfo roi;
00288 for (int i = 0; i < oi->orders_size(); ++i) {
00289 const llsf_msgs::Order &o = oi->orders(i);
00290 rcll_ros_msgs::Order ro;
00291 ro.id = o.id();
00292 ro.complexity = (int)o.complexity();
00293 ro.base_color = (int)o.base_color();
00294 ro.cap_color = (int)o.cap_color();
00295 for (int j = 0; j < o.ring_colors_size(); ++j) {
00296 ro.ring_colors.push_back((int)o.ring_colors(j));
00297 }
00298 ro.quantity_requested = o.quantity_requested();
00299 ro.quantity_delivered_cyan = o.quantity_delivered_cyan();
00300 ro.quantity_delivered_magenta = o.quantity_delivered_magenta();
00301 ro.delivery_period_begin = o.delivery_period_begin();
00302 ro.delivery_period_end = o.delivery_period_end();
00303 ro.delivery_gate = o.delivery_gate();
00304 roi.orders.push_back(ro);
00305 }
00306 pub_order_info_.publish(roi);
00307 }
00308 }
00309
00310 {
00311 std::shared_ptr<RingInfo> ri;
00312 if ((ri = std::dynamic_pointer_cast<RingInfo>(msg))) {
00313 rcll_ros_msgs::RingInfo rri;
00314 for (int i = 0; i < ri->rings_size(); ++i) {
00315 const llsf_msgs::Ring &r = ri->rings(i);
00316 rcll_ros_msgs::Ring rr;
00317 rr.ring_color = (int)r.ring_color();
00318 rr.raw_material = r.raw_material();
00319 rri.rings.push_back(rr);
00320 }
00321 pub_ring_info_.publish(rri);
00322 }
00323 }
00324
00325 }
00326
00327
00328 void
00329 setup_private_peer(llsf_msgs::Team team_color)
00330 {
00331 delete peer_private_;
00332 peer_private_ = NULL;
00333
00334 if (team_color == llsf_msgs::CYAN) {
00335 ROS_INFO("Creating private peer for CYAN");
00336
00337 if (cfg_peer_cyan_local_) {
00338 peer_private_ = new ProtobufBroadcastPeer(cfg_peer_address_,
00339 cfg_peer_cyan_send_port_,
00340 cfg_peer_cyan_recv_port_,
00341 &peer_public_->message_register(),
00342 cfg_crypto_key_, cfg_crypto_cipher_);
00343 } else {
00344 peer_private_ = new ProtobufBroadcastPeer(cfg_peer_address_, cfg_peer_cyan_port_,
00345 &peer_public_->message_register(),
00346 cfg_crypto_key_, cfg_crypto_cipher_);
00347 }
00348
00349 } else {
00350 ROS_INFO("Creating private peer for MAGENTA");
00351
00352 if (cfg_peer_magenta_local_) {
00353 peer_private_ = new ProtobufBroadcastPeer(cfg_peer_address_,
00354 cfg_peer_magenta_send_port_,
00355 cfg_peer_magenta_recv_port_,
00356 &peer_public_->message_register(),
00357 cfg_crypto_key_, cfg_crypto_cipher_);
00358 } else {
00359 peer_private_ = new ProtobufBroadcastPeer(cfg_peer_address_, cfg_peer_magenta_port_,
00360 &peer_public_->message_register(),
00361 cfg_crypto_key_, cfg_crypto_cipher_);
00362 }
00363 }
00364
00365 peer_private_->signal_received().connect(handle_message);
00366 peer_private_->signal_recv_error().connect(handle_recv_error);
00367 peer_private_->signal_send_error().connect(handle_send_error);
00368 }
00369
00370 bool
00371 srv_cb_send_beacon(rcll_ros_msgs::SendBeaconSignal::Request &req,
00372 rcll_ros_msgs::SendBeaconSignal::Response &res)
00373 {
00374 if (! peer_private_) {
00375 res.ok = false;
00376 res.error_msg = "Cannot send beacon signal: private peer not setup, team not set in refbox?";
00377 return true;
00378 }
00379
00380 llsf_msgs::BeaconSignal b;
00381 b.mutable_time()->set_sec(req.header.stamp.sec);
00382 b.mutable_time()->set_nsec(req.header.stamp.nsec);
00383 b.set_seq(req.header.seq);
00384 b.set_number(cfg_robot_number_);
00385 b.set_team_name(cfg_team_name_);
00386 b.set_peer_name(cfg_robot_name_);
00387 b.set_team_color(ros_to_pb_team_color(cfg_team_color_));
00388 if (req.pose.pose.position.x != 0. || req.pose.pose.position.y != 0. ||
00389 req.pose.pose.orientation.x != 0. || req.pose.pose.orientation.y != 0. ||
00390 req.pose.pose.orientation.z != 0. || req.pose.pose.orientation.w != 0.)
00391 {
00392 if (req.pose.pose.position.z != 0.) {
00393 ROS_WARN("Poses must be 2.5D pose in ground support plane (x,y,ori)");
00394 res.ok = false;
00395 res.error_msg = "Poses must be 2.5D pose in ground support plane (x,y,ori)";
00396 return true;
00397 } else {
00398 b.mutable_pose()->mutable_timestamp()->set_sec(req.pose.header.stamp.sec);
00399 b.mutable_pose()->mutable_timestamp()->set_nsec(req.pose.header.stamp.nsec);
00400 b.mutable_pose()->set_x(req.pose.pose.position.x);
00401 b.mutable_pose()->set_y(req.pose.pose.position.y);
00402 tf2::Quaternion q(req.pose.pose.orientation.x, req.pose.pose.orientation.y,
00403 req.pose.pose.orientation.z, req.pose.pose.orientation.w);
00404 b.mutable_pose()->set_ori(tf2::getYaw(q));
00405 }
00406 }
00407
00408 try {
00409 ROS_DEBUG("Sending beacon %s:%s (seq %lu)", b.team_name().c_str(), b.peer_name().c_str(), b.seq());
00410 peer_private_->send(b);
00411 res.ok = true;
00412 } catch (std::runtime_error &e) {
00413 res.ok = false;
00414 res.error_msg = e.what();
00415 }
00416
00417 return true;
00418 }
00419
00420 bool
00421 srv_cb_send_machine_report(rcll_ros_msgs::SendMachineReport::Request &req,
00422 rcll_ros_msgs::SendMachineReport::Response &res)
00423 {
00424 if (! peer_private_) {
00425 res.ok = false;
00426 res.error_msg = "Cannot send machine report: private peer not setup, team not set in refbox?";
00427 return true;
00428 }
00429
00430 llsf_msgs::MachineReport mr;
00431 mr.set_team_color(ros_to_pb_team_color(cfg_team_color_));
00432
00433 std::string machines_sent;
00434
00435 for (size_t i = 0; i < req.machines.size(); ++i) {
00436 const rcll_ros_msgs::MachineReportEntry &rmre = req.machines[i];
00437 llsf_msgs::MachineReportEntry *mre = mr.add_machines();
00438 mre->set_name(rmre.name);
00439 mre->set_type(rmre.type);
00440
00441 if (! Zone_IsValid(rmre.zone)) {
00442 res.ok = false;
00443 res.error_msg = std::string("Invalid zone value for machine") + rmre.name;
00444 return true;
00445 }
00446
00447 mre->set_zone((llsf_msgs::Zone)rmre.zone);
00448
00449 if (! machines_sent.empty()) machines_sent += ", ";
00450 machines_sent += rmre.name;
00451 }
00452
00453 try {
00454 ROS_DEBUG("Sending machine report for machines: %s", machines_sent.c_str());
00455 peer_private_->send(mr);
00456 res.ok = true;
00457 } catch (std::runtime_error &e) {
00458 res.ok = false;
00459 res.error_msg = e.what();
00460 }
00461
00462 return true;
00463 }
00464
00465 bool
00466 srv_cb_send_prepare_machine(rcll_ros_msgs::SendPrepareMachine::Request &req,
00467 rcll_ros_msgs::SendPrepareMachine::Response &res)
00468 {
00469 if (! peer_private_) {
00470 res.ok = false;
00471 res.error_msg = "private peer not setup, team not set in refbox?";
00472 return true;
00473 }
00474
00475 if (req.machine.length() < 4) {
00476 res.ok = false;
00477 res.error_msg = "Invalid machine name '" + req.machine + "'";
00478 return true;
00479 }
00480
00481
00482 std::string machine_team = req.machine.substr(0, 1);
00483 std::string machine_type = req.machine.substr(2, 2);
00484
00485 if (machine_team != "C" && machine_team != "M") {
00486 res.ok = false;
00487 res.error_msg = "Invalid team prefix '" + machine_team + "', must be C or M";
00488 return true;
00489 }
00490 if (machine_team == "C" && cfg_team_color_ == (int)rcll_ros_msgs::Team::MAGENTA) {
00491 res.ok = false;
00492 res.error_msg = "Invalid team prefix 'C', own team is MAGENTA";
00493 return true;
00494 }
00495 if (machine_team == "M" && cfg_team_color_ == (int)rcll_ros_msgs::Team::CYAN) {
00496 res.ok = false;
00497 res.error_msg = "Invalid team prefix 'M', own team is CYAN";
00498 return true;
00499 }
00500 if (machine_type != "BS" && machine_type != "DS" && machine_type != "CS" && machine_type != "RS") {
00501 res.ok = false;
00502 res.error_msg = "Invalid machine type '" + machine_type + "' in name";
00503 return true;
00504 }
00505
00506 llsf_msgs::PrepareMachine pm;
00507 pm.set_team_color(ros_to_pb_team_color(cfg_team_color_));
00508 pm.set_machine(req.machine);
00509
00510 if (machine_type == "BS") {
00511 if (! llsf_msgs::MachineSide_IsValid(req.bs_side)) {
00512 res.ok = false;
00513 res.error_msg = "Invalid BS machine side";
00514 return true;
00515 }
00516 if (! llsf_msgs::BaseColor_IsValid(req.bs_base_color)) {
00517 res.ok = false;
00518 res.error_msg = "Invalid BS base color";
00519 return true;
00520 }
00521 llsf_msgs::PrepareInstructionBS *bsi = pm.mutable_instruction_bs();
00522 bsi->set_side((llsf_msgs::MachineSide)req.bs_side);
00523 bsi->set_color((llsf_msgs::BaseColor)req.bs_base_color);
00524 } else if (machine_type == "DS") {
00525 llsf_msgs::PrepareInstructionDS *dsi = pm.mutable_instruction_ds();
00526 dsi->set_gate(req.ds_gate);
00527 } else if (machine_type == "CS") {
00528 if (! llsf_msgs::CsOp_IsValid(req.cs_operation)) {
00529 res.ok = false;
00530 res.error_msg = "Invalid CS operation";
00531 return true;
00532 }
00533 llsf_msgs::PrepareInstructionCS *csi = pm.mutable_instruction_cs();
00534 csi->set_operation((llsf_msgs::CsOp)req.cs_operation);
00535 } else if (machine_type == "RS") {
00536 if (! llsf_msgs::RingColor_IsValid(req.rs_ring_color)) {
00537 res.ok = false;
00538 res.error_msg = "Invalid RS ring color";
00539 return true;
00540 }
00541 llsf_msgs::PrepareInstructionRS *rsi = pm.mutable_instruction_rs();
00542 rsi->set_ring_color((llsf_msgs::RingColor)req.rs_ring_color);
00543 }
00544
00545 if (req.wait) {
00546 std::unique_lock<std::mutex> lock(mtx_machine_info_);
00547
00548 bool need_to_wait = false;
00549 for (int i = 0; i < msg_machine_info_->machines_size(); ++i) {
00550 const llsf_msgs::Machine &m = msg_machine_info_->machines(i);
00551 if (m.name() == req.machine) {
00552 if (m.has_state() && m.state() != "IDLE") {
00553 ROS_INFO("Machine '%s' is not in IDLE state, waiting briefly", req.machine.c_str());
00554 need_to_wait = true;
00555 }
00556 break;
00557 }
00558 }
00559 if (need_to_wait) {
00560 int machine_idx = 0;
00561 cdv_machine_info_.wait_for(lock, std::chrono::seconds(10),
00562 [&req, &machine_idx]() -> bool {
00563 if (! msg_machine_info_) return false;
00564 rcll_ros_msgs::MachineInfo rmi;
00565 for (int i = 0; i < msg_machine_info_->machines_size(); ++i) {
00566 const llsf_msgs::Machine &m = msg_machine_info_->machines(i);
00567 if (m.name() == req.machine && m.has_state() && m.state() == "IDLE") {
00568 machine_idx = i;
00569 return true;
00570 }
00571 }
00572 return false;
00573 });
00574
00575 const llsf_msgs::Machine &m = msg_machine_info_->machines(machine_idx);
00576 if (m.state() != "IDLE") {
00577 ROS_INFO("Machine '%s' went into '%s' state, not IDLE", req.machine.c_str(), m.state().c_str());
00578 res.ok = false;
00579 res.error_msg = "Machine '" + req.machine + "' went into '" + m.state() + "' state, not IDLE";
00580 return true;
00581 } else {
00582 ROS_INFO("Machine '%s' is now in IDLE state", req.machine.c_str());
00583 }
00584 }
00585 }
00586
00587 try {
00588 ROS_DEBUG("Sending prepare machine instruction for machine: %s", req.machine.c_str());
00589 peer_private_->send(pm);
00590 res.ok = true;
00591 } catch (std::runtime_error &e) {
00592 res.ok = false;
00593 res.error_msg = e.what();
00594 }
00595
00596 if (req.wait) {
00597 ROS_INFO("Waiting for machine '%s' state != IDLE", req.machine.c_str());
00598 std::unique_lock<std::mutex> lock(mtx_machine_info_);
00599
00600 int machine_idx = 0;
00601 cdv_machine_info_.wait(lock,
00602 [&req, &machine_idx, &pm]() -> bool {
00603 if (! msg_machine_info_) return false;
00604 rcll_ros_msgs::MachineInfo rmi;
00605 for (int i = 0; i < msg_machine_info_->machines_size(); ++i) {
00606 const llsf_msgs::Machine &m = msg_machine_info_->machines(i);
00607 if (m.name() == req.machine && m.has_state() && m.state() != "IDLE") {
00608 machine_idx = i;
00609 return true;
00610 }
00611 }
00612 peer_private_->send(pm);
00613 return false;
00614 });
00615
00616 const llsf_msgs::Machine &m = msg_machine_info_->machines(machine_idx);
00617 if (m.state() == "BROKEN") {
00618 ROS_WARN("Machine '%s' went into 'BROKEN' state", req.machine.c_str());
00619 res.ok = false;
00620 res.error_msg = "Machine '" + req.machine + "' went into '" + m.state() + "' state";
00621 } else {
00622 ROS_INFO("Machine '%s' went into '%s' state", req.machine.c_str(), m.state().c_str());
00623 res.ok = true;
00624 }
00625 }
00626
00627 return true;
00628 }
00629
00630 int
00631 main(int argc, char **argv)
00632 {
00633 ros::init(argc, argv, "rcll_refbox_peer");
00634
00635 ros::NodeHandle n;
00636
00637 ROS_INFO("%s starting up", ros::this_node::getName().c_str());
00638
00639
00640 cfg_team_color_ = 0;
00641
00642 GET_PRIV_PARAM(team_name);
00643 GET_PRIV_PARAM(robot_name);
00644 GET_PRIV_PARAM(robot_number);
00645
00646 GET_PRIV_PARAM(peer_address);
00647
00648 if (ros::param::has("~peer_public_recv_port") && ros::param::has("~peer_public_send_port")) {
00649 cfg_peer_public_local_ = true;
00650 GET_PRIV_PARAM(peer_public_recv_port);
00651 GET_PRIV_PARAM(peer_public_send_port);
00652 } else {
00653 cfg_peer_public_local_ = false;
00654 GET_PRIV_PARAM(peer_public_port);
00655 }
00656
00657 if (ros::param::has("~peer_cyan_recv_port") && ros::param::has("~peer_cyan_send_port")) {
00658 cfg_peer_cyan_local_ = true;
00659 if (! ros::param::get("~peer_cyan_recv_port", cfg_peer_cyan_recv_port_)) {
00660 ROS_ERROR("Failed to retrieve parameter cyan_recv_port, aborting");
00661 exit(-1);
00662 }
00663 if (! ros::param::get("~peer_cyan_send_port", cfg_peer_cyan_send_port_)) {
00664 ROS_ERROR("Failed to retrieve parameter cyan_send_port, aborting");
00665 exit(-1);
00666 }
00667 } else {
00668 cfg_peer_cyan_local_ = false;
00669 if (! ros::param::get("~peer_cyan_port", cfg_peer_cyan_port_)) {
00670 ROS_ERROR("Failed to retrieve parameter cyan_port, aborting");
00671 exit(-1);
00672 }
00673 }
00674 if (ros::param::has("~peer_magenta_recv_port") && ros::param::has("~peer_magenta_send_port")) {
00675 cfg_peer_magenta_local_ = true;
00676 if (! ros::param::get("~peer_magenta_recv_port", cfg_peer_magenta_recv_port_)) {
00677 ROS_ERROR("Failed to retrieve parameter magenta_recv_port, aborting");
00678 exit(-1);
00679 }
00680 if (! ros::param::get("~peer_magenta_send_port", cfg_peer_magenta_send_port_)) {
00681 ROS_ERROR("Failed to retrieve parameter magenta_send_port, aborting");
00682 exit(-1);
00683 }
00684 } else {
00685 cfg_peer_magenta_local_ = false;
00686 if (! ros::param::get("~peer_magenta_port", cfg_peer_magenta_port_)) {
00687 ROS_ERROR("Failed to retrieve parameter magenta_port, aborting");
00688 exit(-1);
00689 }
00690 }
00691
00692 GET_PRIV_PARAM(crypto_key);
00693 GET_PRIV_PARAM(crypto_cipher);
00694
00695
00696 pub_beacon_ = n.advertise<rcll_ros_msgs::BeaconSignal>("rcll/beacon", 100);
00697 pub_game_state_ = n.advertise<rcll_ros_msgs::GameState>("rcll/game_state", 10);
00698 pub_machine_info_ = n.advertise<rcll_ros_msgs::MachineInfo>("rcll/machine_info", 10);
00699 pub_exploration_info_ = n.advertise<rcll_ros_msgs::ExplorationInfo>("rcll/exploration_info", 10);
00700 pub_machine_report_info_ = n.advertise<rcll_ros_msgs::MachineReportInfo>("rcll/machine_report_info", 10);
00701 pub_order_info_ = n.advertise<rcll_ros_msgs::OrderInfo>("rcll/order_info", 10);
00702 pub_ring_info_ = n.advertise<rcll_ros_msgs::RingInfo>("rcll/ring_info", 10);
00703
00704
00705 if (cfg_peer_public_local_) {
00706 ROS_WARN("Creating public peer: addr: %s send: %u recv: %u",
00707 cfg_peer_address_.c_str(), cfg_peer_public_send_port_, cfg_peer_public_recv_port_);
00708 peer_public_ = new ProtobufBroadcastPeer(cfg_peer_address_,
00709 cfg_peer_public_send_port_,
00710 cfg_peer_public_recv_port_);
00711 } else {
00712 ROS_WARN("Creating public peer: addr: %s port: %u",
00713 cfg_peer_address_.c_str(), cfg_peer_public_port_);
00714 peer_public_ = new ProtobufBroadcastPeer(cfg_peer_address_, cfg_peer_public_port_);
00715 }
00716
00717 MessageRegister & message_register = peer_public_->message_register();
00718 message_register.add_message_type<llsf_msgs::VersionInfo>();
00719 message_register.add_message_type<llsf_msgs::BeaconSignal>();
00720 message_register.add_message_type<llsf_msgs::GameState>();
00721 message_register.add_message_type<llsf_msgs::MachineInfo>();
00722 message_register.add_message_type<llsf_msgs::ExplorationInfo>();
00723 message_register.add_message_type<llsf_msgs::MachineReportInfo>();
00724 message_register.add_message_type<llsf_msgs::OrderInfo>();
00725 message_register.add_message_type<llsf_msgs::RingInfo>();
00726 message_register.add_message_type<llsf_msgs::RobotInfo>();
00727 message_register.add_message_type<llsf_msgs::PrepareMachine>();
00728
00729 peer_public_->signal_received().connect(handle_message);
00730 peer_public_->signal_recv_error().connect(handle_recv_error);
00731 peer_public_->signal_send_error().connect(handle_send_error);
00732
00733
00734 srv_send_beacon_ = n.advertiseService("rcll/send_beacon", srv_cb_send_beacon);
00735 srv_send_machine_report_ = n.advertiseService("rcll/send_machine_report", srv_cb_send_machine_report);
00736 srv_send_prepare_machine_ = n.advertiseService("rcll/send_prepare_machine", srv_cb_send_prepare_machine);
00737
00738 ros::spin();
00739
00740 return 0;
00741 }