header.h
Go to the documentation of this file.
00001 /*
00002  * \header.h
00003  *
00004  * \date 13.08.2013
00005  * \author G├╝nhter Cwioro
00006  * g.cwioro@gmx.net
00007  */
00008 
00009 #ifndef HEADER_H_
00010 #define HEADER_H_
00011 
00012 
00013 /*General includes*/
00014 #include "ros/ros.h"
00015 
00016 //#include <iwlib.h>
00017 //#include <wireless.h>
00018 #include <stdint.h>
00019 #include <stdio.h>
00020 #include <time.h>
00021 
00022 #include <stdio.h>
00023 #include <sys/ioctl.h>
00024 #include <boost/thread.hpp>
00025 #include <boost/date_time.hpp>
00026 #include <boost/crc.hpp>
00027 using std::list;
00028 using std::string;
00029 
00030 
00031 /*Network specific includes*/
00032 #include <sys/socket.h>
00033 #include <netinet/in.h>
00034 #include <linux/if.h>
00035 #include <netpacket/packet.h>
00036 #include <net/ethernet.h>
00037 #include <asm/types.h>
00038 
00039 /* Custom MSG Types*/
00040 #include "adhoc_communication/ExpAuction.h"
00041 #include "adhoc_communication/ExpFrontierElement.h"
00042 #include "adhoc_communication/MmListOfPoints.h"
00043 #include "adhoc_communication/MmRobotPosition.h"
00044 #include "adhoc_communication/ExpFrontier.h"
00045 #include "adhoc_communication/MmMapUpdate.h"
00046 #include "adhoc_communication/RecvString.h"
00047 #include "adhoc_communication/MmControl.h"
00048 #include "adhoc_communication/MmPoint.h"
00049 #include "adhoc_communication/ExpCluster.h"
00050 #include "adhoc_communication/CMgrDimensions.h"
00051 #include "adhoc_communication/CMgrRobotUpdate.h"
00052 
00053 /* Custom SRV Types*/
00054 #include "adhoc_communication/ChangeMCMembership.h"
00055 #include "adhoc_communication/SendExpCluster.h"
00056 #include "adhoc_communication/SendMmMapUpdate.h"
00057 #include "adhoc_communication/SendOccupancyGrid.h"
00058 #include "adhoc_communication/GetNeighbors.h"
00059 #include "adhoc_communication/SendExpFrontier.h"
00060 #include "adhoc_communication/SendMmPoint.h"
00061 #include "adhoc_communication/SendQuaternion.h"
00062 #include "adhoc_communication/SendExpAuction.h"
00063 #include "adhoc_communication/SendMmControl.h"
00064 #include "adhoc_communication/SendMmRobotPosition.h"
00065 #include "adhoc_communication/SendString.h"
00066 #include "adhoc_communication/SendTwist.h"
00067 #include "adhoc_communication/GetGroupState.h"
00068 #include "adhoc_communication/SendCMgrRobotUpdate.h"
00069 #include "adhoc_communication/ShutDown.h"
00070 #include "adhoc_communication/BroadcastCMgrRobotUpdate.h"
00071 #include "adhoc_communication/BroadcastString.h"
00072 
00073 
00074 #include "nav_msgs/Odometry.h"
00075 #include "std_msgs/String.h"
00076 #include "geometry_msgs/Twist.h"
00077 
00078 #include "functions.h"
00079 
00080 /*Defines for timers and retransmits)*/
00081 #include "defines.h"
00082 
00083 
00084 #include "EthernetFrame.h"
00085 #include "EthernetFrame.cpp"
00086 /*Struct defines*/
00087 #include "structs.h"
00088 
00089 /*Classes for the AD-HOC protocol*/
00090 
00091 #include "Beacon.cpp"
00092 #include "Beacon.h"
00093 #include "AckLinkFrame.h"
00094 #include "AckLinkFrame.cpp"
00095 #include "McAckFrame.h"
00096 #include "McAckFrame.cpp"
00097 #include "RouteRequest.h"
00098 #include "RouteRequest.cpp"
00099 #include "RouteResponse.cpp"
00100 #include "RouteResponse.h"
00101 #include "AckRoutedFrame.h"
00102 #include "AckRoutedFrame.cpp"
00103 #include "McRouteActivationFrame.h"
00104 #include "McRouteActivationFrame.cpp"
00105 #include "Packet.cpp"
00106 #include "PositionSubscriber.h"
00107 #include "PositionSubscriber.cpp"
00108 #include "McDisconnectFrame.h"
00109 #include "McDisconnectFrame.cpp"
00110 #include "MultiHopBroadcastFrame.h"
00111 #include "MultiHopBroadcastFrame.cpp"
00112 #include "McNackFrame.h"
00113 #include "McNackFrame.cpp"
00114 
00115 #include "McHandler.h"
00116 #include "McHandler.cpp"
00117 #include "McPosAckObj.h"
00118 #include "McPosAckObj.cpp"
00119 
00120 
00121 #include <execinfo.h>
00122 #include <signal.h>
00123 #include <stdlib.h>
00124 #include <unistd.h>
00125 
00126 
00127 
00128 std::map<uint16_t, std::string> frame_types;
00129 
00130 void handler(int sig)
00131 {
00132     void *array[10];
00133     size_t size;
00134 
00135     // get void*'s for all entries on the stack
00136     size = backtrace(array, 10);
00137 
00138     // print out all the frames to stderr
00139     fprintf(stderr, "Error: signal %d:\n", sig);
00140     backtrace_symbols_fd(array, size, STDERR_FILENO);
00141     exit(1);
00142 }
00143 
00144 
00145 
00146 /*VARS DECLARATION*/
00147 unsigned char* interface = NULL; // using outgoing interface
00148 std::string hostname; // hostname of the node
00149 std::string mac_as_string = "";
00150 std::string interface_as_string = "";
00151 uint32_t packet_id_ = -1;
00152 
00153 
00154 void socketSend(string network_string);
00155 
00156 void deliverMcAckFrame(stc_RoutedFrame& stc_rf, routing_entry& r, bool send_ack);
00157 geometry_msgs::PoseStamped getPoseStampFromNetworkString(unsigned char* serializedPoseStamp, uint32_t length);
00158 nav_msgs::OccupancyGrid getMapFromNetworkString(unsigned char* serializedMap, uint32_t length);
00159 bool joinMCGroup(adhoc_communication::ChangeMCMembership::Request &req, adhoc_communication::ChangeMCMembership::Response &res);
00160 
00161 #include "Logging.h"
00162 #include "Logging.cpp"
00163 
00164 void processMcAckFrame(McAckFrame* f);
00165 
00166 list<McTree*> mc_groups_l;
00167 McHandler mc_handler(&mc_groups_l);
00168 list<McPosAckObj*> mc_ack_l;
00169 
00170 list<stc_frame> unack_link_frames_l;
00171 list<ack_cr_info> unack_cr_frames_l;
00172 list<ack_cr_info> unack_mc_cr_frames_l;
00173 list<bcasts> bcasts_l;
00174 
00175 stc_RoutedFrame* unack_routed_frame; // there can be just one unacknowledged routed frame at a time, because the protocol waits for the ack until the next routed frame will be send
00176 list<route_request> route_requests_l;
00177 list<routing_entry> routing_table_l;
00178 
00179 std::map<unsigned char*, string> mac_hname_dict;
00180 list<hostname_mac> neighbors_l; // list with all reachable direct neighbors. This list is not used for the routing process. Nodes detect each other by "hello frames"
00181 list<Packet> packets_incomplete_l; // list to cache incomplete packets
00182 list<Packet> cached_mc_packets_l;
00183 list<RoutedFrame> inc_frames_l;
00184 list<McNackFrame> requested_frames_l;
00185 
00186 list<stc_packet> published_packets_l;
00187 
00188 list<PositionSubscriber*> robot_positions_l;
00189 PositionSubscriber* my_sim_position;
00190 
00191 
00192 
00193 
00194 RoutedFrame frames_received_recently_a[MAX_FRAMES_CACHED]; // static array to cache frames incoming frames. This is needed, because frames are always received twice by the receive function
00195 uint16_t frr_index = 0; // current index to insert incoming frame.
00196 uint16_t frr_count = 0; // count to know how many frames are cached. This is only important at the beginning, later frr_count will always be MAX_FRAMES_CACHED
00197 
00198 /*MUTEXES to share data over different threads*/
00199 boost::mutex mtx_relays;
00200 boost::mutex mtx_routing_table;
00201 boost::mutex mtx_cr_entries;
00202 boost::mutex mtx_route_requests;
00203 boost::mutex mtx_unack_routed_frame;
00204 boost::mutex mtx_neighbors;
00205 boost::mutex mtx_unack_link_frames;
00206 boost::mutex mtx_unack_cr_frames;
00207 boost::mutex mtx_unack_mc_cr_frames;
00208 
00209 boost::mutex mtx_frames_received_recently;
00210 boost::mutex mtx_inc_rf_frames;
00211 boost::mutex mtx_requested_frames;
00212 boost::mutex mtx_packets_incomplete;
00213 boost::mutex mtx_cached_mc_packets;
00214 
00215 boost::mutex mtx_mc_ack_l;
00216 boost::mutex mtx_mc_groups;
00217 
00218 // Vars for communication
00219 int raw_socket = 0; // Socket descriptor
00220 struct sockaddr_ll socket_address;
00221 unsigned char src_mac[6]; /*Local host NIC MAC address*/
00222 struct eh_header* ethernet_header;
00223 
00224 
00225 //VARS for the configurable parameter
00226 int num_link_retrans = 3;
00227 int num_e2e_retrans = 10; //1;
00228 int num_rreq = 1;
00229 int max_frame_size = 1500;
00230 int hop_limit_min = 0;
00231 int hop_limit_max = 0;
00232 int hop_limit_increment = 3;
00233 int max_packet_size = 10000;
00234 int beacon_interval = 500;
00235 bool enable_cooperative_relaying = false;
00236 bool rebuild_mc_tree = false;
00237 bool recursive_mc_ack = false;
00238 double loss_ratio = 0;
00239 std::string topic_new_robot = "new_robot";
00240 std::string topic_remove_robot = "remove_robot";
00241 std::string log_path = "";
00242 std::string sim_robot_macs = "";
00243 
00244 /*Simulation*/
00245 bool simulation_mode = false;
00246 int robots_in_simulation = 10;
00247 int p_thres = -75;
00248 int p_tx = 15; //[dBm]
00249 int n_model = 4;
00250 int l_0_model = 33;
00251 #ifdef DELAY
00252 int delay = 1; //[us]
00253 #endif
00254 
00255 std::list<ros::Publisher> publishers_l;
00256 
00257 ros::NodeHandle *n_priv;
00258 ros::NodeHandle *n_pub;
00259 
00260 boost::mutex mtx_notify;
00261 
00262 boost::condition_variable got_request_response;
00263 boost::condition_variable got_rframe_ack;
00264 
00265 
00266 std::string node_prefix = "adhoc_communication/";
00267 
00268 
00269 std::string getHostnameFromMac(unsigned char mac[6]);
00270 
00271 bool isReachable(unsigned char mac[6]);
00272 void initParams(ros::NodeHandle* n);
00273 
00274 bool getRoute(routing_entry& r, std::string hostname);
00275 void safeFrame(RoutedFrame incomingFrame);
00276 
00277 void sendLinkAck(unsigned char* dest, unsigned char* confirmer_mac, uint32_t id, string source, bool cr, uint8_t type);
00278 bool gotAck(AckLinkFrame* frame);
00279 bool gotFrameRecently(RoutedFrame incomingFrame);
00280 void publishMap(nav_msgs::OccupancyGrid map, std::string topic);
00281 void publishPacket(Packet* p);
00282 
00283 void addDownlinkToMcTree(mc_tree tree, mac downlink);
00284 mc_tree getMCTree(std::string mc_name);
00285 void mcLostConnection(hostname_mac host);
00286 bool connectedWith(unsigned char mac[6]);
00287 
00288 void resendLinkFrame(stc_frame f);
00289 void processAckRoutedFrame(AckRoutedFrame* fr);
00290 void processRoutedFrame(RoutedFrame* f);
00291 void processBroadcastFrame(RoutedFrame* f);
00292 void processRouteRequest(RouteRequest req);
00293 void processRouteResponse(RouteResponse rr);
00294 void processBeacon(Beacon* helloF);
00295 void processMcActivationFrame(McRouteActivationFrame * f);
00296 
00297 void resendMcFrame(boost::condition_variable &condi, stc_RoutedFrame* rf, routing_entry route);
00298 void resendRouteRequest(route_request &req, boost::condition_variable &con);
00299 void resendRoutedFrame(boost::condition_variable &condi, routing_entry route);
00300 void reconnectToMcGroup(std::string group_name);
00301 void deleteObsoleteRequests();
00302 void deleteOldPackets();
00303 
00304 void sendBeacons();
00305 void receiveFrames();
00306 void processIncomingFrames();
00307 void close_raw_socket();
00308 int eth_raw_init();
00309 
00310 McPosAckObj* getMcAckObj(std::string* group, uint32_t p_id, uint32_t seq);
00311 
00312 void resendRequestedFrame(uint32_t* seq_num, uint32_t* packet_id, string* source_h, string* mc_group);
00313 void resendRequestedFrames();
00314 
00315 
00316 bool sendPacket(std::string &hostname_destination, std::string& payload, uint8_t data_type_, std::string& topic);
00317 
00318 void initRobotMacList(std::string* robot_mac);
00319 template<class message>
00320 void publishMessage(message m, string topic);
00321 
00322 
00323 std::string getListAsString(std::list<uint32_t> l);
00324 
00325 #ifdef DEBUG
00326 #include "DebugFunctions.h"
00327 #endif
00328 
00329 uint32_t incoming_frame_count = 0;
00330 uint32_t incoming_frames_lost = 0;
00331 
00332 int eth_raw_init()
00333 {
00334 
00335 
00336     /* Vars */
00337     int i;
00338     struct ifreq ifr;
00339     int ifindex = 0; /*Ethernet Interface index*/
00340 
00341     /* Open socket */
00342     raw_socket = socket(PF_PACKET, SOCK_RAW, htons(ETH_TYPE));
00343     if (raw_socket == -1)
00344     {
00345         perror("socket():");
00346         exit(1);
00347     }
00348     ROS_INFO("Successfully opened socket: %i\n", raw_socket);
00349 
00350     /*Get ethernet interface index*/
00351     strncpy(ifr.ifr_name, (const char*) interface, IFNAMSIZ);
00352     if (ioctl(raw_socket, SIOCGIFINDEX, &ifr) == -1)
00353     {
00354         perror("SIOCGIFINDEX");
00355         exit(1);
00356     }
00357     ifindex = ifr.ifr_ifindex;
00358     ROS_INFO("Successfully got interface index: %i\n", ifindex);
00359 
00360     /*retrieve corresponding MAC*/
00361     if (ioctl(raw_socket, SIOCGIFHWADDR, &ifr) == -1)
00362     {
00363         perror("SIOCGIFINDEX");
00364         exit(1);
00365     }
00366 
00367     if (mac_as_string.compare("") == 0)
00368     {
00369         for (i = 0; i < 6; i++)
00370         {
00371             src_mac[i] = ifr.ifr_hwaddr.sa_data[i];
00372         }
00373         ROS_INFO("Host MAC address: %02X:%02X:%02X:%02X:%02X:%02X\n",
00374                 src_mac[0], src_mac[1], src_mac[2], src_mac[3], src_mac[4], src_mac[5]);
00375     }
00376 
00377     /*prepare sockaddr_ll*/
00378     socket_address.sll_family = PF_INET;
00379     socket_address.sll_protocol = htons(ETH_TYPE);
00380     socket_address.sll_ifindex = ifindex;
00381     //socket_address.sll_hatype   = ARPHRD_ETHER;
00382     socket_address.sll_pkttype = PACKET_BROADCAST;
00383     socket_address.sll_halen = ETH_ALEN;
00384     socket_address.sll_addr[0] = bcast_mac[0];
00385     socket_address.sll_addr[1] = bcast_mac[1];
00386     socket_address.sll_addr[2] = bcast_mac[2];
00387     socket_address.sll_addr[3] = bcast_mac[3];
00388     socket_address.sll_addr[4] = bcast_mac[4];
00389     socket_address.sll_addr[5] = bcast_mac[5];
00390     socket_address.sll_addr[6] = 0x00;
00391     socket_address.sll_addr[7] = 0x00;
00392 
00393     return 0;
00394 
00395 }
00396 
00397 void close_raw_socket()
00398 {
00399     /*Clean up.......*/
00400 
00401     struct ifreq ifr;
00402 
00403     if (raw_socket == -1)
00404         return; /* No socket has beeen created */
00405 
00406     strncpy(ifr.ifr_name, (const char*) interface, IFNAMSIZ);
00407     ioctl(raw_socket, SIOCGIFFLAGS, &ifr);
00408     ifr.ifr_flags &= ~IFF_PROMISC;
00409     ioctl(raw_socket, SIOCSIFFLAGS, &ifr);
00410     close(raw_socket);
00411 
00412 }
00413 
00414 bool getRoute(routing_entry& r, std::string hostname)
00415 {
00416     /* Description:
00417      * Search a route to the destination host in the routing table.
00418      * The parameter of the type routing_entry will be initialized, if a route can be found.
00419      *
00420      * Returns:
00421      *          (true)  If route has been found
00422      *
00423      *          (false) If route has not been found
00424      */
00425     boost::unique_lock<boost::mutex> lock(mtx_routing_table);
00426 
00427     for (std::list<routing_entry>::iterator it = routing_table_l.begin(); it != routing_table_l.end(); ++it)
00428     {
00429 
00430         if ((*it).hostname_destination.compare(hostname) == 0)
00431         {
00432             r = *it;
00433             r.ts = getMillisecondsTime();
00434             return true;
00435         }
00436     }
00437     return false;
00438 }
00439 
00440 void socketSend(string network_string)
00441 {
00442     sendto(raw_socket, network_string.data(), network_string.length(), 0, (struct sockaddr*) &socket_address, sizeof (socket_address));
00443 
00444 #ifdef DELAY
00445 
00446     if (simulation_mode)
00447 
00448         boost::this_thread::sleep(boost::posix_time::milliseconds(delay)); //usleep(delay);
00449 #endif
00450 }
00451 
00452 void resendUnackLinkFrame(string hostname_src, uint32_t id, unsigned char* mac, string network_string, uint8_t type)
00453 {
00454     /*SAFE the mc activation as unacknowledged*/
00455     stc_frame unack_link_frame;
00456     unack_link_frame.frame_id = id;
00457     memcpy(unack_link_frame.mac, mac, 6);
00458     unack_link_frame.hostname_source = hostname_src;
00459     unack_link_frame.type = type;
00460     unack_link_frame.network_string = network_string;
00461 
00462     {
00463         boost::unique_lock<boost::mutex> lock(mtx_unack_link_frames);
00464         unack_link_frames_l.push_back(unack_link_frame);
00465     }
00466 
00467     boost::thread cr(&resendLinkFrame, unack_link_frame);
00468 }
00469 
00470 void cleanRoutingTable()
00471 {
00472     unsigned long now = getMillisecondsTime();
00473     for (list<routing_entry>::iterator i = routing_table_l.begin(); i != routing_table_l.end();)
00474     {
00475 
00476         if (now - (*i).ts >= MAX_TIME_CACHE_UNUSED_ROUTES)
00477             i = routing_table_l.erase(i);
00478         else
00479             i++;
00480     }
00481 }
00482 
00483 void initFrameTypes()
00484 {
00485     frame_types[0x61] = "Link Ack";
00486     frame_types[0x42] = "Beacon";
00487     frame_types[0x52] = "RReq";
00488     frame_types[0x72] = "RRep";
00489     frame_types[0x44] = "Data";
00490     frame_types[0x41] = "Transport Ack";
00491     frame_types[0x64] = "Relay Detection";
00492     frame_types[0x53] = "Reley Delection";
00493     frame_types[0x5d] = "MC ACK";
00494     frame_types[0x59] = "MC ACTIVATION";
00495     frame_types[0x50] = "MC DISCONNECTION";
00496 }
00497 
00498 /* disconnection frame that comes from a downlink to disconnect from an uplink*/
00499 void processMcDisconnectUplink(McDisconnectFrame* f)
00500 {
00501     if (!compareMac(f->header_.mac_destination, src_mac))
00502         return;
00503 
00504     sendLinkAck(f->eh_h_.eh_dest, src_mac, f->header_.id, "", false, FRAME_TYPE_MC_DISCONNECT);
00505 
00506     boost::unique_lock<boost::mutex> groups_lock(mtx_mc_groups);
00507 
00508     McTree* group = mc_handler.getMcGroup(&f->mc_group_);
00509 
00510     if (group == NULL)
00511     {
00512         return;
00513     }
00514 
00515 #ifdef PERFORMANCE_LOGGING_MC_LINK_SUMMARY
00516     groups_lock.unlock();
00517     Logging::increaseProperty("num_mc_total_frames_received");
00518     Logging::increaseProperty("num_mc_total_bytes_received ", f->buffer_str_len_);
00519     groups_lock.lock();
00520 #endif
00521 
00522     if (!group->removeMacIfExsists(f->eh_h_.eh_source))
00523     {
00524         ROS_ERROR("Unexpected Error: %s is not a downlink for group %s", getMacAsStr(f->eh_h_.eh_source).c_str(), f->mc_group_.c_str());
00525         return;
00526     }
00527 
00528     if (group->member)
00529     {
00530         groups_lock.unlock();
00531        // ROS_ERROR("GOT DISCONNECT MSG FROM DOWNLINK: GROUP[%s] DOWNLINK[%s]", f->mc_group_.c_str(), getHostnameFromMac(f->eh_h_.eh_source).c_str());
00532         groups_lock.lock();
00533     } else
00534     {
00535         adhoc_communication::ChangeMCMembership::Request req;
00536         adhoc_communication::ChangeMCMembership::Response res;
00537         req.action = false;
00538         req.group_name = f->mc_group_;
00539         groups_lock.unlock();
00540         joinMCGroup(req, res);
00541         groups_lock.lock();
00542     }
00543 }
00544 
00545 /* disconnection frame that comes from a uplink to rebuild the whole mc path*/
00546 void processMcDisconnectDownlink(McDisconnectFrame* f)
00547 {
00548     sendLinkAck(f->eh_h_.eh_dest, src_mac, f->header_.id, "", false, FRAME_TYPE_MC_DISCONNECT);
00549 
00550     boost::unique_lock<boost::mutex> groups_lock(mtx_mc_groups);
00551     McTree* group = mc_handler.getMcGroup(&f->mc_group_);
00552 
00553     if (group == NULL)
00554         return;
00555 
00556 #ifdef PERFORMANCE_LOGGING_MC_LINK_SUMMARY
00557     groups_lock.unlock();
00558     Logging::increaseProperty("num_mc_total_frames_received");
00559     Logging::increaseProperty("num_mc_total_bytes_received ", f->buffer_str_len_);
00560     groups_lock.lock();
00561 #endif
00562 
00563     if (!compareMac(f->eh_h_.eh_source, group->route_uplink_->next_hop))
00564     {
00565         return;
00566     }
00567 
00568     group->connected = false;
00569     //ROS_ERROR("GOT MC PRUNE MESSAGE GROUP[%s] HOST[%s]", f->mc_group_.c_str(), getHostnameFromMac(f->eh_h_.eh_source).c_str());
00570 
00571     if (!group->downlinks_l_.empty())
00572     {
00573       //  ROS_ERROR("FORWARD MC PRUNE GROUP[%s] HOST[%s]", group->group_name_.c_str(), getHostnameFromMac(f->eh_h_.eh_source).c_str());
00574         group->downlinks_l_.clear();
00575         mtx_mc_groups.unlock();
00576         McDisconnectFrame dis_f(bcast_mac, f->mc_group_);
00577         dis_f.disconnect_downlink = true;
00578         string network_string = dis_f.getFrameAsNetworkString(src_mac);
00579 
00580         socketSend(network_string);
00581         mtx_mc_groups.lock();
00582     }
00583 
00584     /*Try to reconn*/
00585     if (group->member)
00586     {
00587         /* set connection to false*/
00588         boost::thread tryToJoin(&reconnectToMcGroup, f->mc_group_);
00589 
00590     } else
00591         mc_handler.removeGroup(&f->mc_group_);
00592 }
00593 
00594 void testPacket()
00595 {
00596     int packet_size = 10;
00597     int packet_id = 0;
00598     Packet p;
00599     std::vector<RoutedFrame> l;
00600 
00601     for (int packet_seq = 0; packet_seq < packet_size; packet_seq++)
00602     {
00603         RoutedFrame f("topic", "random_data", 2, packet_id, packet_seq, packet_size);
00604         l.push_back(f);
00605 
00606         if (packet_seq == 0)
00607             p = Packet(f);
00608 
00609         if (packet_seq % 2 == 0)
00610         {
00611             ROS_ERROR("add %u", packet_seq);
00612             p.addFrame(f);
00613         } else
00614             ROS_ERROR("skip %u", packet_seq);
00615     }
00616 
00617     ROS_ERROR("missings seq: %s %u %lu %lu", getListAsString(p.missed_sequences_l_).c_str(), p.size_, p.frames_l_.size(), p.missed_sequences_l_.size());
00618 
00619     p.refreshLists();
00620 
00621     ROS_ERROR("missings seq: %s %u %lu %lu", getListAsString(p.missed_sequences_l_).c_str(), p.size_, p.frames_l_.size(), p.missed_sequences_l_.size());
00622 }
00623 
00624 bool gotFrameRecently(RoutedFrame rf)
00625 {
00626     /* Description:
00627      * Checks if frame is in frames_received_recently_a.
00628      *
00629      * Returns:
00630      *          (true)  If got frame already
00631      *
00632      *          (false) If frame is new
00633      */
00634     //ROS_DEBUG("");
00635 
00636     /* Allways process frames with the resend flag*/
00637     if (rf.resend_flag && rf.mc_flag == false)
00638     {
00639         return false;
00640     }
00641 
00642     boost::unique_lock<boost::mutex> lock(mtx_frames_received_recently);
00643 
00644     int interations = 0;
00645     //  ROS_DEBUG("COMPARE FRAME SEQ NUM[%u] PACKET ID[%u] SOURCE[%s] ",rf.header_.packet_sequence_num,rf.header_.packet_id,rf.hostname_source_.c_str());
00646     /*The first index to search will be the last of insert this makes search faster, because it happens often that a frame will be received twice from the network interface*/
00647     for (int i = frr_index > 0 ? frr_index - 1 : frr_count - 1; interations < frr_count;)
00648     {
00649 
00650         //      ROS_DEBUG("IT SEQ[%u] PACKET ID[%u] SOURCE[%s] ",frames_received_recently_a[i].header_.packet_sequence_num,frames_received_recently_a[i].header_.packet_id,frames_received_recently_a[i].hostname_source_.c_str());
00651         /* UNICAST */
00652         if (!rf.mc_flag && !rf.resend_flag &&
00653                 frames_received_recently_a[i].header_.packet_sequence_num == rf.header_.packet_sequence_num &&
00654                 frames_received_recently_a[i].header_.packet_id == rf.header_.packet_id &&
00655                 frames_received_recently_a[i].hostname_source_.compare(rf.hostname_source_) == 0)// &&
00656         {
00657             //  ROS_DEBUG("GOT FRAME RECENTLY: SEQ NUM[%u] PACKET ID[%u] SOURCE[%s] ",rf.header_.packet_sequence_num,rf.header_.packet_id,rf.hostname_source_.c_str());
00658             return true;
00659         }
00660 
00661         if (rf.mc_flag && frames_received_recently_a[i].header_.packet_id == rf.header_.packet_id &&
00662                 frames_received_recently_a[i].header_.packet_sequence_num == rf.header_.packet_sequence_num &&
00663                 frames_received_recently_a[i].hostname_source_.compare(rf.hostname_source_) == 0 &&
00664                 frames_received_recently_a[i].mc_g_name_.compare(rf.mc_g_name_) == 0
00665                 )
00666         {
00667             //ROS_DEBUG("GOT MC FRAME RECENTLY: SEQ NUM[%u] PACKET ID[%u] SOURCE[%s] ",rf.header_.packet_sequence_num,rf.header_.packet_id,rf.hostname_source_.c_str());
00668             return true;
00669         }
00670 
00671         interations++;
00672 
00673         if (i - 1 > 0)
00674             i--;
00675         else
00676             i = frr_count - 1;
00677 
00678     }
00679 
00680     return false;
00681 }
00682 
00683 void safeFrame(RoutedFrame incomingFrame)
00684 {
00685     /* Description:
00686      * Safe frame in frames_received_recently_a
00687      */
00688 
00689     boost::unique_lock<boost::mutex> lock(mtx_frames_received_recently);
00690     //  ROS_DEBUG("safe frame %u %s",incomingFrame.frame_id,incomingFrame.hostname_source.c_str());
00691 
00692     frames_received_recently_a[frr_index].header_.packet_sequence_num = incomingFrame.header_.packet_sequence_num;
00693     frames_received_recently_a[frr_index].header_.packet_id = incomingFrame.header_.packet_id;
00694     frames_received_recently_a[frr_index].header_.frame_id = incomingFrame.header_.frame_id;
00695     frames_received_recently_a[frr_index].hostname_source_ = incomingFrame.hostname_source_;
00696     frames_received_recently_a[frr_index].mc_g_name_ = incomingFrame.mc_g_name_;
00697     memcpy(frames_received_recently_a[frr_index++].header_.mac_destination_, incomingFrame.header_.mac_destination_, 6);
00698 
00699 
00700     //frames_received_recently_a[frr_index] = RoutedFrame(incomingFrame);
00701 
00702     if (frr_count < MAX_FRAMES_CACHED)
00703         frr_count++;
00704 
00705     if (frr_index >= MAX_FRAMES_CACHED)
00706         frr_index = 0;
00707 
00708 }
00709 
00710 std::string getHostnameFromMac(unsigned char mac[6])
00711 {
00712     /* Description:
00713      * Returns a hostname of a mac using neighbor table.
00714      *
00715      * NOTE: This function is not used in this version.. but in earlier implementations.
00716      */
00717 
00718     // boost::unique_lock<boost::mutex> lock(mtx_neighbors);
00719     hostname_mac hm;
00720     for (std::list<hostname_mac>::iterator it = neighbors_l.begin(); it != neighbors_l.end(); ++it)
00721     {
00722         hm = *it;
00723 
00724         if (compareMac(hm.mac, mac))
00725         {
00726             return hm.hostname;
00727         }
00728     }
00729     return getMacAsStr(mac);
00730 }
00731 
00732 void initParams(ros::NodeHandle* n)
00733 {
00734     /*GET PRIVATE PARAMETER*/
00735     /*          NAME                                            VARIABLE                                        DEFAULT VALUE*/
00736 
00737     n->param("interface", interface_as_string, std::string("wlan0"));
00738     n->param("mac", mac_as_string, std::string(""));
00739     n->param("num_link_retrans", num_link_retrans, 10);
00740     n->param("num_e2e_retrans", num_e2e_retrans, 10);
00741     n->param("num_rreq", num_rreq, 1);
00742     n->param("max_frame_size", max_frame_size, 1500);
00743     n->param("hop_limit_min", hop_limit_min, 0);
00744     n->param("hop_limit_max", hop_limit_max, 0);
00745     n->param("robot_name", hostname, hostname);
00746     n->param("hop_limit_increment", hop_limit_increment, 3);
00747     n->param("beacon_interval", beacon_interval, 250);
00748     n->param("max_packet_size", max_packet_size, 10000000);
00749     n->param("simulation_mode", simulation_mode, false);
00750     n->param("robots_in_simulation", robots_in_simulation, 10);
00751     n->param("p_thres", p_thres, -75);
00752     n->param("p_tx", p_tx, 15);
00753     n->param("n_model", n_model, 4);
00754     n->param("l_0_model", l_0_model, 33);
00755     n->param("topic_new_robot", topic_new_robot, std::string("new_robot"));
00756     n->param("topic_remove_robot", topic_remove_robot, std::string("remove_robot"));
00757    
00758     n->param("rebuild_mc_tree", rebuild_mc_tree, false);
00759     n->param("recursive_mc_ack", recursive_mc_ack, false);
00760     n->param("loss_ratio", loss_ratio, loss_ratio);
00761     n->param("nack_threshold", Packet::NACK_THRESHOLD, Packet::NACK_THRESHOLD);
00762 
00763     n->param("sim_robot_macs", sim_robot_macs, std::string("")); // exp: "robot_0,00:11:00:00:00:00!robot_0,00:11:00:00:00:00
00764     RoutedFrame::enable_cooperative_relaying = enable_cooperative_relaying;
00765 
00766     //ROS_ERROR("mac from param %s",mac_as_string.c_str());
00767 
00768     if (mac_as_string.compare("") != 0)
00769     {
00770 
00771         initMacFromString(src_mac, mac_as_string.data());
00772     }
00773     //  ROS_ERROR("parameters int: %s mac: %s real mac %s",interface_as_string.c_str(),mac_as_string.c_str(), getMacAsCStr(src_mac));
00774 
00775     interface = (unsigned char*) interface_as_string.data();
00776 
00777     initRobotMacList(&sim_robot_macs);
00778 
00779     /*SET TX POWER ON HARDWARE IF SIMULATION MODE IS OFF*/
00780     if (simulation_mode == false)
00781     {
00782         std::string command = "iwconfig " + std::string((const char*) interface) + " txpower ";
00783         command.append(getIntAsString(p_tx) + "db");
00784         if (system(command.data()) == 0)
00785             ROS_INFO("TX POWER CHANGED IN HARDWARE TO %u dbm", p_tx);
00786 
00787     }
00788 }
00789 
00790 bool isReachable(unsigned char mac[6])
00791 {
00792     if (simulation_mode)
00793     {
00794         PositionSubscriber other_robot;
00795         other_robot.robot_name_ = getHostnameFromMac(mac);
00796 
00797         for (std::list<PositionSubscriber*>::iterator it = robot_positions_l.begin(); it != robot_positions_l.end(); ++it)
00798         {
00799             if (other_robot.robot_name_.compare((*it)->robot_name_) == 0)
00800             {
00801                 double d = my_sim_position->calcDistance(*it);
00802 
00803                 //ROS_ERROR("d: %f",d);
00804                 if (d == -1)
00805                 {
00806 
00807                     return false;
00808                 }
00809 
00810                 double p_rx = p_tx - (l_0_model + 10 * n_model * log10(d));
00811 
00812 
00813                 if (p_thres <= p_rx)
00814                 {
00815                     //ROS_DEBUG("connected with %s %f %f",other_robot->robot_name_.c_str(),d,p_rx);
00816                     return true;
00817                 } else
00818                     return false;
00819 
00820             }
00821         }
00822 
00823     } else if (sim_robot_macs.compare("") != 0)
00824     {
00825         boost::unique_lock<boost::mutex> lock(mtx_neighbors);
00826         hostname_mac n(mac);
00827 
00828         std::list<hostname_mac>::iterator searchNeighbor = std::find(neighbors_l.begin(), neighbors_l.end(), n);
00829 
00830         return searchNeighbor != neighbors_l.end();
00831 
00832     } else
00833         return true;
00834 }
00835 
00836 bool connectedWith(unsigned char mac[6])
00837 {
00838     hostname_mac n;
00839     memcpy((unsigned char*) n.mac, (unsigned char*) mac, 6);
00840     boost::unique_lock<boost::mutex> lock(mtx_neighbors);
00841     std::list<hostname_mac>::iterator searchNeighbor = std::find(neighbors_l.begin(), neighbors_l.end(), n);
00842 
00843     // ROS_DEBUG("reachable %u %s", ((hostname_mac) * searchNeighbor).reachable, getMacAsCStr((unsigned char*) mac));
00844 
00845     return searchNeighbor != neighbors_l.end() && ((hostname_mac) * searchNeighbor).reachable;
00846 }
00847 
00848 void disconnectDownlinks(McTree* t)
00849 {
00850     McDisconnectFrame disc_f(bcast_mac, t->group_name_);
00851     disc_f.disconnect_downlink = true;
00852     string network_string = disc_f.getFrameAsNetworkString(src_mac);
00853 
00854     for (list<mac*>::iterator i = t->downlinks_l_.begin(); i != t->downlinks_l_.end(); i++)
00855     {
00856         resendUnackLinkFrame("", disc_f.header_.id, (*i)->mac_adr, network_string, FRAME_TYPE_MC_DISCONNECT);
00857     }
00858 
00859     t->downlinks_l_.clear();
00860 
00861     socketSend(network_string);
00862 }
00863 
00864 void mcLostConnection(hostname_mac host)
00865 {
00866     std::string mc_name = "";
00867 
00868     boost::unique_lock<boost::mutex> lock_g(mtx_mc_groups);
00869 
00870     std::vector<McTree*> trees = mc_handler.lostConnectionDownlinks(host.mac);
00871 
00872     while (!trees.empty())
00873     {
00874         McTree* t = trees.back();
00875 
00876         ROS_ERROR("REMOVE DOWNLINK: GROUP[%s]  HOST[%s]", t->group_name_.c_str(), host.hostname.c_str());
00877         trees.pop_back();
00878 
00879         /*disconnent from uplink if tree is just a connector*/
00880         if (t->downlinks_l_.empty() && !t->member)
00881         {
00882             adhoc_communication::ChangeMCMembership::Request req;
00883             adhoc_communication::ChangeMCMembership::Response res;
00884             req.action = false;
00885             req.group_name = t->group_name_;
00886             lock_g.unlock();
00887             joinMCGroup(req, res);
00888             lock_g.lock();
00889         }
00890     }
00891     trees = mc_handler.lostConnectionUplinks(host.mac);
00892     while (!trees.empty())
00893     {
00894         McTree* t = trees.back();
00895         trees.pop_back();
00896 
00897        // ROS_ERROR("LOST CONNECTION TO UPLINK GROUP[%s] HOST[%s]", t->group_name_.c_str(), host.hostname.c_str());
00898 
00899         if (rebuild_mc_tree)
00900         {
00901             /*disconnect from downlinks*/
00902             if (!t->downlinks_l_.empty())
00903             {
00904                 ROS_INFO("SEND PRUNE MESSAGE TO TREE LEAFES GROUP[%s]", t->group_name_.c_str());
00905                 disconnectDownlinks(t);
00906             } else
00907             {
00908                 ROS_INFO("NO OTHER DOWNLINKS: GROUP[%s]", t->group_name_.c_str());
00909             }
00910 
00911             if (t->member)
00912             {
00913                 // ROS_ERROR("try rejoin");
00914                 t->connected = false;
00915                 boost::thread tryToJoin(&reconnectToMcGroup, std::string(t->group_name_));
00916             } else
00917             {
00918                 //  ROS_ERROR("delete group");
00919                 mc_handler.removeGroup(&t->group_name_);
00920             }
00921         } else
00922         {
00923             t->connected = false;
00924             boost::thread tryToJoin(&reconnectToMcGroup, std::string(t->group_name_));
00925         }
00926     }
00927 }
00928 
00929 void cacheNackMcFrame(RoutedFrame rf)
00930 {
00931     bool packet_exsists = false;
00932     //ROS_DEBUG("safe frame with %u",rf.packet_sequence_num_);
00933     boost::unique_lock<boost::mutex> lock(mtx_cached_mc_packets);
00934 
00935     for (std::list<Packet>::iterator it_p = cached_mc_packets_l.begin(); it_p != cached_mc_packets_l.end(); ++it_p)
00936     {
00937         Packet & p(*it_p);
00938         
00939         if (p.id_ == rf.header_.packet_id)
00940         {
00941             packet_exsists = true;
00942             if (rf.hostname_source_.compare(p.hostname_source_) == 0 && rf.mc_g_name_.compare(p.mc_group_) == 0)
00943             {
00944                 bool frame_exsists = false;
00945                 for (std::list<RoutedFrame>::iterator it_f = p.frames_l_.begin(); it_f != p.frames_l_.end(); ++it_f)
00946                 {
00947                     RoutedFrame cur_f = *it_f;
00948                     if (cur_f.header_.packet_sequence_num == rf.header_.packet_sequence_num)
00949                     {
00950                         frame_exsists = true;
00951                         break;
00952                     }
00953                 }
00954                 //ROS_DEBUG("push front %u",p.frames_l_.size());
00955                 if (!frame_exsists)
00956                     p.frames_l_.push_front(rf);
00957 
00958                 return;
00959             }
00960         }
00961     }
00962 
00963     /*create new packet*/
00964     if (!packet_exsists)
00965     {
00966         Packet p = Packet(rf);
00967         p.addFrame(rf);
00968         cached_mc_packets_l.push_front(p);
00969     }
00970 }
00971 
00972 bool iAmMember(string group)
00973 {
00974     boost::unique_lock<boost::mutex> lock_groups(mtx_mc_groups);
00975     McTree* t = mc_handler.getMcGroup(&group);
00976 
00977     return (t != NULL && t->activated && t->member);
00978 }
00979 
00980 void resendRequestedFrameFromPacket(McNackFrame nack)
00981 {
00982     boost::unique_lock<boost::mutex> lock(mtx_cached_mc_packets);
00983     //list<Packet> cachedc_mc_packets_copy = list<Packet>(cached_mc_packets_l);
00984 
00985     for (std::list<Packet>::iterator it_p = cached_mc_packets_l.begin(); it_p != cached_mc_packets_l.end(); ++it_p)
00986     {
00987         /* packet found*/
00988         if ((*it_p).id_ == nack.header_.packet_id && nack.hostname_source_.compare((*it_p).hostname_source_) == 0 && nack.mc_group_.compare((*it_p).mc_group_) == 0)
00989         {
00990             bool one_frame_resent = false;
00991 
00992             std::vector<RoutedFrame> frames_in_packet((*it_p).frames_l_.begin(), (*it_p).frames_l_.end());
00993             lock.unlock();
00994 
00995             //ROS_DEBUG("pid %u size %u",p.id_,p.frames_l_.size());
00996             for (std::vector<RoutedFrame>::iterator it_f = frames_in_packet.begin(); it_f != frames_in_packet.end(); ++it_f)
00997             {
00998                 RoutedFrame f = *it_f;
00999 
01000                 list<uint32_t> l(nack.req_seq_nums_.begin(), nack.req_seq_nums_.end());
01001 
01002                 //    ROS_ERROR("%s", getListAsString(l).c_str());
01003 
01004                 if (std::find(nack.req_seq_nums_.begin(), nack.req_seq_nums_.end(), f.header_.packet_sequence_num) != nack.req_seq_nums_.end())
01005                 {
01006                     one_frame_resent = true;
01007                     f.resend_flag = true;
01008                     string network_string = f.getFrameAsNetworkString(f.header_.route_id, bcast_mac, f.hostname_source_, src_mac);
01009                     //    ROS_ERROR("RESEND REQUESTED FRAME: SEQ NUM[%u] PACKET ID[%u] GROUP[%s] SOURCE[%s]", f.header_.packet_sequence_num, f.header_.packet_id, f.mc_g_name_.c_str(), f.hostname_source_.c_str());
01010                     socketSend(network_string);
01011 #ifdef PERFORMANCE_LOGGING_MC_LINK_SUMMARY
01012                     Logging::increaseProperty("num_mc_data_frames_resent");
01013                     Logging::increaseProperty("num_mc_total_bytes_sent", network_string.length());
01014 #endif          
01015                 }
01016             }
01017 
01018             lock.lock();
01019 
01020             if (one_frame_resent)
01021             {
01022                 (*it_p).ts_ = getMillisecondsTime();
01023             }
01024 
01025             return;
01026         }
01027     }
01028     // ROS_ERROR("COULD NOT FIND REQ PACKET: PACKET ID[%u] GROUP[%s] SOURCE[%s]", nack.header_.packet_id, nack.mc_group_.c_str(), nack.hostname_source_.c_str());
01029 }
01030 
01031 void resendRequestedFrames()
01032 {
01033     while (ros::ok())
01034     {
01035         list<McNackFrame> frames;
01036         {
01037             frames.clear();
01038             boost::lock_guard<boost::mutex> lock(mtx_requested_frames);
01039             frames = list<McNackFrame>(requested_frames_l);
01040             requested_frames_l.clear();
01041 
01042             //  ROS_DEBUG("list :%u",frames.size());
01043         }
01044 
01045         if (frames.size() > 0)
01046         {
01047             list<McNackFrame>::iterator it = frames.begin();
01048             while (!frames.empty())
01049             {
01050                 McNackFrame f = *it;
01051                 resendRequestedFrameFromPacket(f);
01052                 // resendRequestedFrame(&f.header_.frame_seq_num, &f.header_.packet_id, &f.hostname_source_, &f.mc_group_);
01053                 it = frames.erase(it);
01054                 // ros::Duration(0, INTERVAL_BETWEEN_RESEND_REQUESTED_FRAMES * 1000).sleep();
01055             }
01056         } else
01057         {
01058             ros::Duration(0, INTERVAL_BETWEEN_RESEND_REQUESTED_FRAMES * 1000).sleep();
01059         }
01060     }
01061 }
01062 //todo unicast_link_transmission.csv -> check // nur gesendete data frames lala
01063 
01064 
01065 void reconnectToMcGroup(std::string group_name)
01066 {
01067     if (rebuild_mc_tree)
01068     {
01069         //if a node gets a prune message, it will will a little to give the network time for synconization
01070         sleepMS(INTERVAL_WAIT_MC_RECONN);
01071     }
01072 
01073     int reconnect_tries = 0;
01074     adhoc_communication::ChangeMCMembership::Request req;
01075     adhoc_communication::ChangeMCMembership::Response res;
01076     req.action = false;
01077     while (ros::ok() && reconnect_tries <= MAX_JOIN_ATTEMPS)
01078     {
01079         {
01080             boost::unique_lock<boost::mutex> lock_groups(mtx_mc_groups);
01081             McTree* t = mc_handler.getMcGroup(&group_name);
01082 
01083             if (t != NULL && t->activated && (!t->downlinks_l_.empty() || t->member))
01084             {
01085                 if (!t->connected)
01086                 {
01087                     ROS_INFO("TRY RECONNECTION TO MC GROUP[%s]", group_name.c_str());
01088                     reconnect_tries++;
01089                     req.group_name = group_name;
01090                     req.action = true;
01091                     lock_groups.unlock();
01092                     joinMCGroup(req, res);
01093                     lock_groups.lock();
01094 
01095                 } else
01096                     return;
01097             }
01098         }
01099         sleepMS(INTERWAL_RECONNECT_MC_G);
01100     }
01101 
01102     if (reconnect_tries <= MAX_JOIN_ATTEMPS)
01103     {
01104         /*downlink disconnection*/
01105         ROS_ERROR("STOP TRYING TO CONNECT TO MC GROUP: NAME[%s]", group_name.c_str());
01106         routing_entry route;
01107         route.hostname_source = group_name;
01108         memcpy(route.next_hop, bcast_mac, 6);
01109 
01110         boost::unique_lock<boost::mutex> lock_groups(mtx_mc_groups);
01111         McTree* t = mc_handler.getMcGroup(&group_name);
01112         if (t != NULL)
01113         {
01114             disconnectDownlinks(t);
01115             req.action = false;
01116             lock_groups.unlock();
01117             joinMCGroup(req, res);
01118             lock_groups.lock();
01119         }
01120     }
01121 }
01122 
01123 void joinAllMcGroups()
01124 {
01125     ros::Duration(3, 0).sleep();
01126     adhoc_communication::ChangeMCMembership::Request req;
01127     req.action = true;
01128     adhoc_communication::ChangeMCMembership::Response res;
01129     for (int c = 0; c < robots_in_simulation; c++)
01130     { //robots_in_simulation
01131 
01132 
01133 
01134         req.group_name = "mc_robot_" + getIntAsString(c);
01135 
01136         ROS_ERROR("join %s", req.group_name.c_str());
01137         joinMCGroup(req, res);
01138         sleepMS(100);
01139 
01140         {
01141             boost::unique_lock<boost::mutex> lock_mc_groups(mtx_mc_groups);
01142 
01143             if (!mc_handler.getMcGroup(&req.group_name)->member)
01144             {
01145                 lock_mc_groups.unlock();
01146                 joinMCGroup(req, res);
01147                 lock_mc_groups.lock();
01148             }
01149         }
01150     }
01151     ROS_ERROR("JOINING FINISHED!");
01152     mc_handler.printMcGroups();
01153 #ifdef TRY_REJOIN
01154 
01155     mc_handler.printMcGroups();
01156 
01157     /*
01158    ros::Duration(2, 0).sleep();
01159    for (int c = 3; c < 4; c++) {//robots_in_simulation
01160 
01161 
01162        std::string g_name = "mc_robot_" + convertInt(c);
01163        McTree* mc_t = mc_handler.getMcGroup(&g_name);
01164        if(mc_t == NULL )
01165            ROS_ERROR("FATAL: %s", g_name.c_str());
01166        else if(!mc_t->activated)
01167             ROS_ERROR("NOT CONNECTED: %s", g_name.c_str());
01168       /*
01169        uint8_t joinAttemps = 0;
01170        while ((t.group_name.compare("") == 0 || !t.member) && joinAttemps++ < 10) {
01171            req.action = true;
01172            req.group_name = "mc_robot_" + convertInt(c);
01173            joinMCGroup(req, res);
01174            t = getActivatedMCTree("mc_robot_" + convertInt(c));
01175            ;
01176            ROS_ERROR("Join mc_robot_%u again", c);
01177        }
01178    }*/
01179 #endif
01180 
01181 
01182 }
01183 
01184 struct frame_packet_info
01185 {
01186     string source;
01187     string group_name;
01188     uint32_t seq_num;
01189     uint32_t packet_id;
01190 
01191     bool operator ==(const frame_packet_info& st)
01192     {
01193         if (seq_num == st.seq_num && packet_id == st.packet_id && group_name.compare(st.group_name) == 0 && source.compare(st.source))
01194             return true;
01195 
01196         return false;
01197     }
01198 };
01199 
01200 std::vector<McNackFrame> getMissingFrames(Packet * p)
01201 {
01202     std::vector<McNackFrame> l;
01203 
01204     if (p->missed_sequences_l_.empty())
01205         p->refreshLists();
01206 
01207     std::vector<uint32_t> list_4_nack;
01208 
01209     for (std::list<uint32_t>::iterator i = p->missed_sequences_l_.begin(); i != p->missed_sequences_l_.end(); i++)
01210     {
01211         list_4_nack.push_back(*i);
01212 
01213         if (list_4_nack.size() % MAX_REQUESTED_FRAMES_IN_NACK == 0)
01214         {
01215             McNackFrame nack(src_mac, bcast_mac, p->hostname_source_, p->mc_group_, p->id_, list_4_nack);
01216             l.push_back(nack);
01217             list_4_nack.clear();
01218         }
01219     }
01220 
01221     if (list_4_nack.size() > 0)
01222     {
01223         McNackFrame nack(src_mac, bcast_mac, p->hostname_source_, p->mc_group_, p->id_, list_4_nack);
01224         l.push_back(nack);
01225     }
01226 
01227     return l;
01228 }
01229 
01230 Packet * getFirstMcFrame(list<Packet>* packets)
01231 {
01232     for (std::list<Packet>::iterator p = packets->begin(); p != packets->end(); p++)
01233     {
01234         if ((*p).isNack() && (*p).size_ > (*p).frames_l_.size())
01235             return &(*p);
01236     }
01237 
01238     return NULL;
01239 }
01240 
01241 std::string getListAsString(std::vector<uint32_t> l)
01242 {
01243     string r = "";
01244 
01245     for (std::vector<uint32_t>::iterator i = l.begin(); i != l.end(); i++)
01246         r.append(getIntAsString(*i) + ",");
01247 
01248     return r;
01249 }
01250 
01251 std::string getListAsString(std::list<uint32_t> l)
01252 {
01253     string r = "";
01254 
01255     for (std::list<uint32_t>::iterator i = l.begin(); i != l.end(); i++)
01256         r.append(getIntAsString(*i) + ",");
01257 
01258     return r;
01259 }
01260 
01261 void updateTsRequestFrames(Packet p)
01262 {
01263     boost::unique_lock<boost::mutex> lock(mtx_packets_incomplete);
01264     for (std::list<Packet>::iterator pi = packets_incomplete_l.begin(); pi != packets_incomplete_l.end(); pi++)
01265     {
01266         if ((*pi).id_ == p.id_ && (*pi).hostname_source_.compare(p.hostname_source_) == 0 && (*pi).mc_group_.compare(p.mc_group_) == 0)
01267         {
01268             (*pi).ts_last_frame_request = getMillisecondsTime();
01269             return;
01270         }
01271     }
01272 }
01273 
01274 void requestPendingFrames()
01275 {
01276     std::list<Packet> pack_inco_copy;
01277     while (ros::ok())
01278     {
01279         sleepMS(INTERVAL_REQUEST_FRAMES);
01280 
01281         int requested_frames = 0;
01282 
01283         {
01284             pack_inco_copy.clear();
01285             boost::unique_lock<boost::mutex> lock(mtx_packets_incomplete);
01286             pack_inco_copy = std::list<Packet>(packets_incomplete_l);
01287         }
01288 
01289         for (std::list<Packet>::iterator p = pack_inco_copy.begin(); p != pack_inco_copy.end(); p++)
01290         {
01291             if (!(*p).isNack() || (*p).size_ <= (*p).frames_l_.size())
01292                 continue;
01293 
01294             float perc_frames_got = (float) (*p).frames_l_.size() / (float) (*p).size_;
01295             //ROS_DEBUG("%f %u",perc_frames_got,p.id_);
01296             // if ((perc_frames_got >= START_RQUESTING_PACKET_FILL_LEVEL || getMillisecondsTime() - (*p).ts_ >= START_RQUESTING_INTERVAL_AFTER_LAST_FRAME_GOT) || !(*p).missed_sequences_l_.empty())
01297 
01298             if (getMillisecondsTime() - (*p).ts_ >= START_RQUESTING_INTERVAL_AFTER_LAST_FRAME_GOT || (!(*p).missed_sequences_l_.empty() && getMillisecondsTime() - (*p).ts_last_frame_request >= 200))
01299             {
01300                 std::vector<McNackFrame> nacks = getMissingFrames(&(*p));
01301 
01302                 while (!nacks.empty())
01303                 {
01304                     updateTsRequestFrames(*p);
01305 
01306                     //   ROS_ERROR("%u %u %u ", (*p).size_, (*p).frames_l_.size(), (*p).missed_sequences_l_.size());
01307 
01308                     //  ROS_ERROR("REQUEST FRAMES: PACKET ID[%u] SEQ[%s] GROUP[%s] SOURCE[%s]", (*p).id_, getListAsString(nacks.back().req_seq_nums_).c_str(), (*p).mc_group_.c_str(), (*p).hostname_source_.c_str());
01309 
01310                     string network_string = nacks.back().getFrameAsNetworkString();
01311                     //    ROS_ERROR("%u %u %u %u", network_string.length(), nacks.back().req_seq_nums_.size(), nacks.size(), (*p).missed_sequences_l_.size());
01312                     nacks.pop_back();
01313                     socketSend(network_string);
01314 #ifdef PERFORMANCE_LOGGING_MC_LINK_SUMMARY
01315                     Logging::increaseProperty("num_mc_acknowledgments_sent");
01316                     Logging::increaseProperty("num_mc_total_bytes_sent", network_string.length());
01317 #endif      
01318                     sleepMS(10);
01319 
01320                 }
01321             }
01322         }
01323     }
01324 }
01325 
01326 void initRobotMacList(std::string * robot_mac)
01327 {
01328     if (robot_mac->compare("") == 0)
01329         return;
01330 
01331     //input string should have the following format: hostname,mac!hostname2,mac2
01332 
01333     robot_mac->insert(0, "!");
01334 
01335     std::vector<std::string> hostnames_macs;
01336     boost::split(hostnames_macs, *robot_mac, boost::is_any_of("!"));
01337 
01338     for (std::vector<std::string>::iterator i = hostnames_macs.begin(); i != hostnames_macs.end(); i++)
01339     {
01340         std::string & h(*i);
01341         if (h.compare("") == 0)
01342             continue;
01343 
01344         try
01345         {
01346             //ROS_ERROR("hostname macs: %s", h.c_str());
01347             std::vector<std::string> hostname_mac_l;
01348             boost::split(hostname_mac_l, h, boost::is_any_of(","));
01349             //macToString(hostname_mac[1]);
01350             unsigned char* mac_p = new unsigned char[6];
01351             hostname_mac hm;
01352             hm.reachable = false;
01353             hm.hostname = hostname_mac_l[0];
01354             initMacFromString(hm.mac, hostname_mac_l[1].data());
01355 
01356             neighbors_l.push_back(hm);
01357             ROS_ERROR("ADD TO WHITELIST: %s [%s]", hm.hostname.c_str(), getMacAsStr(hm.mac).c_str());
01358 
01359         } catch (const std::out_of_range& oor)
01360         {
01361             ROS_ERROR("SYNTAX ERROR IN PARAMETER 'sim_robot_macs' %s ", robot_mac->c_str());
01362         }
01363     }
01364 }
01365 
01366 template <class t>
01367 void desializeObject(unsigned char* serialized_pose_stamp, uint32_t length, t * obj)
01368 {
01369     /* Description:
01370      * de-serialized a ROS message from a buffer.
01371      */
01372     // Fill buffer with a serialized UInt32
01373     try
01374     {
01375         ros::serialization::IStream stream(serialized_pose_stamp, length);
01376         ros::serialization::deserialize(stream, *obj);
01377     } catch (ros::serialization::StreamOverrunException e)
01378     {
01379         ROS_ERROR("IN desializeObject: NODE THROWS EXCEPTION: %s ", e.what());
01380         ROS_ERROR("PARAMETERS: length=[%u] object type=[%s]", length, typeid (*obj).name());
01381     }
01382 }
01383 
01384 
01385 #endif /* HEADER_H_ */


adhoc_communication
Author(s): Guenter Cwioro , Torsten Andre
autogenerated on Thu Aug 27 2015 11:56:40