rcll_refbox_peer_node.cpp
Go to the documentation of this file.
00001 /***************************************************************************
00002  *  rcll_refbox_peer_node.cpp - RCLL Referee Box Peer as ROS node
00003  *
00004  *  Created: Fri May 27 21:38:32 2016
00005  *  Copyright  2016  Tim Niemueller [www.niemueller.de]
00006  ****************************************************************************/
00007 
00008 /*  This program is free software; you can redistribute it and/or modify
00009  *  it under the terms of the GNU General Public License as published by
00010  *  the Free Software Foundation; either version 2 of the License, or
00011  *  (at your option) any later version.
00012  *
00013  *  This program is distributed in the hope that it will be useful,
00014  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00015  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00016  *  GNU Library General Public License for more details.
00017  *
00018  *  Read the full text in the LICENSE.GPL file in the doc directory.
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                 // This is only sent right after starting the refbox and not
00148                 // terribly interesting, hence only log
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                         //ROS_INFO("Received beacon signal from %s:%s", b->team_name().c_str(), b->peer_name().c_str());
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                         { // Wake if anyone is waiting for this, i.e., the prepare svc callback
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         // parse machine name
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         // Parameter parsing    
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         // Setup ROS topics
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         // Setup basic communication
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   // provide services
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 }


rcll_refbox_peer
Author(s): Tim Niemueller
autogenerated on Wed May 17 2017 02:47:55