#include "ros/ros.h"
#include <stdint.h>
#include <stdio.h>
#include <time.h>
#include <sys/ioctl.h>
#include <boost/thread.hpp>
#include <boost/date_time.hpp>
#include <boost/crc.hpp>
#include <sys/socket.h>
#include <netinet/in.h>
#include <linux/if.h>
#include <netpacket/packet.h>
#include <net/ethernet.h>
#include <asm/types.h>
#include "adhoc_communication/ExpAuction.h"
#include "adhoc_communication/ExpFrontierElement.h"
#include "adhoc_communication/MmListOfPoints.h"
#include "adhoc_communication/MmRobotPosition.h"
#include "adhoc_communication/ExpFrontier.h"
#include "adhoc_communication/MmMapUpdate.h"
#include "adhoc_communication/RecvString.h"
#include "adhoc_communication/MmControl.h"
#include "adhoc_communication/MmPoint.h"
#include "adhoc_communication/ExpCluster.h"
#include "adhoc_communication/CMgrDimensions.h"
#include "adhoc_communication/CMgrRobotUpdate.h"
#include "adhoc_communication/ChangeMCMembership.h"
#include "adhoc_communication/SendExpCluster.h"
#include "adhoc_communication/SendMmMapUpdate.h"
#include "adhoc_communication/SendOccupancyGrid.h"
#include "adhoc_communication/GetNeighbors.h"
#include "adhoc_communication/SendExpFrontier.h"
#include "adhoc_communication/SendMmPoint.h"
#include "adhoc_communication/SendQuaternion.h"
#include "adhoc_communication/SendExpAuction.h"
#include "adhoc_communication/SendMmControl.h"
#include "adhoc_communication/SendMmRobotPosition.h"
#include "adhoc_communication/SendString.h"
#include "adhoc_communication/SendTwist.h"
#include "adhoc_communication/GetGroupState.h"
#include "adhoc_communication/SendCMgrRobotUpdate.h"
#include "adhoc_communication/ShutDown.h"
#include "adhoc_communication/BroadcastCMgrRobotUpdate.h"
#include "adhoc_communication/BroadcastString.h"
#include "nav_msgs/Odometry.h"
#include "std_msgs/String.h"
#include "geometry_msgs/Twist.h"
#include "functions.h"
#include "defines.h"
#include "EthernetFrame.h"
#include "EthernetFrame.cpp"
#include "structs.h"
#include "Beacon.cpp"
#include "Beacon.h"
#include "AckLinkFrame.h"
#include "AckLinkFrame.cpp"
#include "McAckFrame.h"
#include "McAckFrame.cpp"
#include "RouteRequest.h"
#include "RouteRequest.cpp"
#include "RouteResponse.cpp"
#include "RouteResponse.h"
#include "AckRoutedFrame.h"
#include "AckRoutedFrame.cpp"
#include "McRouteActivationFrame.h"
#include "McRouteActivationFrame.cpp"
#include "Packet.cpp"
#include "PositionSubscriber.h"
#include "PositionSubscriber.cpp"
#include "McDisconnectFrame.h"
#include "McDisconnectFrame.cpp"
#include "MultiHopBroadcastFrame.h"
#include "MultiHopBroadcastFrame.cpp"
#include "McNackFrame.h"
#include "McNackFrame.cpp"
#include "McHandler.h"
#include "McHandler.cpp"
#include "McPosAckObj.h"
#include "McPosAckObj.cpp"
#include <execinfo.h>
#include <signal.h>
#include <stdlib.h>
#include <unistd.h>
#include "Logging.h"
#include "Logging.cpp"
#include "DebugFunctions.h"
Go to the source code of this file.
Classes | |
struct | frame_packet_info |
Functions | |
void | addDownlinkToMcTree (mc_tree tree, mac downlink) |
void | cacheNackMcFrame (RoutedFrame rf) |
void | cleanRoutingTable () |
void | close_raw_socket () |
bool | connectedWith (unsigned char mac[6]) |
void | deleteObsoleteRequests () |
void | deleteOldPackets () |
void | deliverMcAckFrame (stc_RoutedFrame &stc_rf, routing_entry &r, bool send_ack) |
template<class t > | |
void | desializeObject (unsigned char *serialized_pose_stamp, uint32_t length, t *obj) |
void | disconnectDownlinks (McTree *t) |
int | eth_raw_init () |
Packet * | getFirstMcFrame (list< Packet > *packets) |
std::string | getHostnameFromMac (unsigned char mac[6]) |
std::string | getListAsString (std::list< uint32_t > l) |
std::string | getListAsString (std::vector< uint32_t > l) |
nav_msgs::OccupancyGrid | getMapFromNetworkString (unsigned char *serializedMap, uint32_t length) |
McPosAckObj * | getMcAckObj (std::string *group, uint32_t p_id, uint32_t seq) |
mc_tree | getMCTree (std::string mc_name) |
std::vector< McNackFrame > | getMissingFrames (Packet *p) |
geometry_msgs::PoseStamped | getPoseStampFromNetworkString (unsigned char *serializedPoseStamp, uint32_t length) |
bool | getRoute (routing_entry &r, std::string hostname) |
bool | gotAck (AckLinkFrame *frame) |
bool | gotFrameRecently (RoutedFrame incomingFrame) |
void | handler (int sig) |
bool | iAmMember (string group) |
void | initFrameTypes () |
void | initParams (ros::NodeHandle *n) |
void | initRobotMacList (std::string *robot_mac) |
bool | isReachable (unsigned char mac[6]) |
void | joinAllMcGroups () |
bool | joinMCGroup (adhoc_communication::ChangeMCMembership::Request &req, adhoc_communication::ChangeMCMembership::Response &res) |
Service to tell the node to attempt to connect to a multicast group. | |
void | mcLostConnection (hostname_mac host) |
void | processAckRoutedFrame (AckRoutedFrame *fr) |
void | processBeacon (Beacon *helloF) |
void | processBroadcastFrame (RoutedFrame *f) |
void | processIncomingFrames () |
void | processMcAckFrame (McAckFrame *f) |
void | processMcActivationFrame (McRouteActivationFrame *f) |
void | processMcDisconnectDownlink (McDisconnectFrame *f) |
void | processMcDisconnectUplink (McDisconnectFrame *f) |
void | processRoutedFrame (RoutedFrame *f) |
void | processRouteRequest (RouteRequest req) |
void | processRouteResponse (RouteResponse rr) |
void | publishMap (nav_msgs::OccupancyGrid map, std::string topic) |
template<class message > | |
void | publishMessage (message m, string topic) |
void | publishPacket (Packet *p) |
void | receiveFrames () |
void | reconnectToMcGroup (std::string group_name) |
void | requestPendingFrames () |
void | resendLinkFrame (stc_frame f) |
void | resendMcFrame (boost::condition_variable &condi, stc_RoutedFrame *rf, routing_entry route) |
void | resendRequestedFrame (uint32_t *seq_num, uint32_t *packet_id, string *source_h, string *mc_group) |
void | resendRequestedFrameFromPacket (McNackFrame nack) |
void | resendRequestedFrames () |
void | resendRoutedFrame (boost::condition_variable &condi, routing_entry route) |
void | resendRouteRequest (route_request &req, boost::condition_variable &con) |
void | resendUnackLinkFrame (string hostname_src, uint32_t id, unsigned char *mac, string network_string, uint8_t type) |
void | safeFrame (RoutedFrame incomingFrame) |
void | sendBeacons () |
void | sendLinkAck (unsigned char *dest, unsigned char *confirmer_mac, uint32_t id, string source, bool cr, uint8_t type) |
bool | sendPacket (std::string &hostname_destination, std::string &payload, uint8_t data_type_, std::string &topic) |
void | socketSend (string network_string) |
void | testPacket () |
void | updateTsRequestFrames (Packet p) |
Variables | |
list< bcasts > | bcasts_l |
int | beacon_interval = 500 |
list< Packet > | cached_mc_packets_l |
int | delay = 1 |
bool | enable_cooperative_relaying = false |
struct eh_header * | ethernet_header |
std::map< uint16_t, std::string > | frame_types |
RoutedFrame | frames_received_recently_a [MAX_FRAMES_CACHED] |
uint16_t | frr_count = 0 |
uint16_t | frr_index = 0 |
boost::condition_variable | got_request_response |
boost::condition_variable | got_rframe_ack |
int | hop_limit_increment = 3 |
int | hop_limit_max = 0 |
int | hop_limit_min = 0 |
std::string | hostname |
list< RoutedFrame > | inc_frames_l |
uint32_t | incoming_frame_count = 0 |
uint32_t | incoming_frames_lost = 0 |
unsigned char * | interface = NULL |
std::string | interface_as_string = "" |
int | l_0_model = 33 |
std::string | log_path = "" |
double | loss_ratio = 0 |
std::string | mac_as_string = "" |
std::map< unsigned char *, string > | mac_hname_dict |
int | max_frame_size = 1500 |
int | max_packet_size = 10000 |
list< McPosAckObj * > | mc_ack_l |
list< McTree * > | mc_groups_l |
boost::mutex | mtx_cached_mc_packets |
boost::mutex | mtx_cr_entries |
boost::mutex | mtx_frames_received_recently |
boost::mutex | mtx_inc_rf_frames |
boost::mutex | mtx_mc_ack_l |
boost::mutex | mtx_mc_groups |
boost::mutex | mtx_neighbors |
boost::mutex | mtx_notify |
boost::mutex | mtx_packets_incomplete |
boost::mutex | mtx_relays |
boost::mutex | mtx_requested_frames |
boost::mutex | mtx_route_requests |
boost::mutex | mtx_routing_table |
boost::mutex | mtx_unack_cr_frames |
boost::mutex | mtx_unack_link_frames |
boost::mutex | mtx_unack_mc_cr_frames |
boost::mutex | mtx_unack_routed_frame |
PositionSubscriber * | my_sim_position |
int | n_model = 4 |
ros::NodeHandle * | n_priv |
ros::NodeHandle * | n_pub |
list< hostname_mac > | neighbors_l |
std::string | node_prefix = "adhoc_communication/" |
int | num_e2e_retrans = 10 |
int | num_link_retrans = 3 |
int | num_rreq = 1 |
int | p_thres = -75 |
int | p_tx = 15 |
uint32_t | packet_id_ = -1 |
list< Packet > | packets_incomplete_l |
list< stc_packet > | published_packets_l |
std::list< ros::Publisher > | publishers_l |
int | raw_socket = 0 |
bool | rebuild_mc_tree = false |
bool | recursive_mc_ack = false |
list< McNackFrame > | requested_frames_l |
list< PositionSubscriber * > | robot_positions_l |
int | robots_in_simulation = 10 |
list< route_request > | route_requests_l |
list< routing_entry > | routing_table_l |
std::string | sim_robot_macs = "" |
bool | simulation_mode = false |
struct sockaddr_ll | socket_address |
unsigned char | src_mac [6] |
std::string | topic_new_robot = "new_robot" |
std::string | topic_remove_robot = "remove_robot" |
list< ack_cr_info > | unack_cr_frames_l |
list< stc_frame > | unack_link_frames_l |
list< ack_cr_info > | unack_mc_cr_frames_l |
stc_RoutedFrame * | unack_routed_frame |
void addDownlinkToMcTree | ( | mc_tree | tree, |
mac | downlink | ||
) |
void cacheNackMcFrame | ( | RoutedFrame | rf | ) |
void cleanRoutingTable | ( | ) |
void close_raw_socket | ( | ) |
bool connectedWith | ( | unsigned char | mac[6] | ) |
void deleteObsoleteRequests | ( | ) |
Definition at line 3302 of file adhoc_communication.cpp.
void deleteOldPackets | ( | ) |
Definition at line 3332 of file adhoc_communication.cpp.
void deliverMcAckFrame | ( | stc_RoutedFrame & | stc_rf, |
routing_entry & | r, | ||
bool | send_ack | ||
) |
Definition at line 2035 of file adhoc_communication.cpp.
void desializeObject | ( | unsigned char * | serialized_pose_stamp, |
uint32_t | length, | ||
t * | obj | ||
) |
void disconnectDownlinks | ( | McTree * | t | ) |
int eth_raw_init | ( | ) |
Packet* getFirstMcFrame | ( | list< Packet > * | packets | ) |
std::string getHostnameFromMac | ( | unsigned char | mac[6] | ) |
std::string getListAsString | ( | std::list< uint32_t > | l | ) |
std::string getListAsString | ( | std::vector< uint32_t > | l | ) |
nav_msgs::OccupancyGrid getMapFromNetworkString | ( | unsigned char * | serializedMap, |
uint32_t | length | ||
) |
McPosAckObj* getMcAckObj | ( | std::string * | group, |
uint32_t | p_id, | ||
uint32_t | seq | ||
) |
Definition at line 1725 of file adhoc_communication.cpp.
std::vector<McNackFrame> getMissingFrames | ( | Packet * | p | ) |
geometry_msgs::PoseStamped getPoseStampFromNetworkString | ( | unsigned char * | serializedPoseStamp, |
uint32_t | length | ||
) |
bool getRoute | ( | routing_entry & | r, |
std::string | hostname | ||
) |
bool gotAck | ( | AckLinkFrame * | frame | ) |
Definition at line 1247 of file adhoc_communication.cpp.
bool gotFrameRecently | ( | RoutedFrame | incomingFrame | ) |
void initFrameTypes | ( | ) |
void initParams | ( | ros::NodeHandle * | n | ) |
void initRobotMacList | ( | std::string * | robot_mac | ) |
bool isReachable | ( | unsigned char | mac[6] | ) |
void joinAllMcGroups | ( | ) |
bool joinMCGroup | ( | adhoc_communication::ChangeMCMembership::Request & | req, |
adhoc_communication::ChangeMCMembership::Response & | res | ||
) |
Service to tell the node to attempt to connect to a multicast group.
[in] | req | The service request object. |
[out] | res | The service response object. |
Implements the ROS service join_mc_group to tell a node to connect to a multicast group.
Definition at line 108 of file adhoc_communication.cpp.
void mcLostConnection | ( | hostname_mac | host | ) |
void processAckRoutedFrame | ( | AckRoutedFrame * | fr | ) |
Definition at line 1790 of file adhoc_communication.cpp.
void processBeacon | ( | Beacon * | helloF | ) |
Definition at line 2646 of file adhoc_communication.cpp.
void processBroadcastFrame | ( | RoutedFrame * | f | ) |
Definition at line 2072 of file adhoc_communication.cpp.
void processIncomingFrames | ( | ) |
Definition at line 2794 of file adhoc_communication.cpp.
void processMcAckFrame | ( | McAckFrame * | f | ) |
Definition at line 1754 of file adhoc_communication.cpp.
void processMcActivationFrame | ( | McRouteActivationFrame * | f | ) |
Definition at line 2700 of file adhoc_communication.cpp.
void processMcDisconnectDownlink | ( | McDisconnectFrame * | f | ) |
void processMcDisconnectUplink | ( | McDisconnectFrame * | f | ) |
void processRoutedFrame | ( | RoutedFrame * | f | ) |
Definition at line 1898 of file adhoc_communication.cpp.
void processRouteRequest | ( | RouteRequest | req | ) |
Definition at line 2252 of file adhoc_communication.cpp.
void processRouteResponse | ( | RouteResponse | rr | ) |
Definition at line 2481 of file adhoc_communication.cpp.
void publishMap | ( | nav_msgs::OccupancyGrid | map, |
std::string | topic | ||
) |
void publishMessage | ( | message | m, |
string | topic | ||
) |
Definition at line 1628 of file adhoc_communication.cpp.
void publishPacket | ( | Packet * | p | ) |
Definition at line 2994 of file adhoc_communication.cpp.
void receiveFrames | ( | ) |
Definition at line 1330 of file adhoc_communication.cpp.
void reconnectToMcGroup | ( | std::string | group_name | ) |
void requestPendingFrames | ( | ) |
void resendLinkFrame | ( | stc_frame | f | ) |
Definition at line 3380 of file adhoc_communication.cpp.
void resendMcFrame | ( | boost::condition_variable & | condi, |
stc_RoutedFrame * | rf, | ||
routing_entry | route | ||
) |
Definition at line 3273 of file adhoc_communication.cpp.
void resendRequestedFrame | ( | uint32_t * | seq_num, |
uint32_t * | packet_id, | ||
string * | source_h, | ||
string * | mc_group | ||
) |
void resendRequestedFrameFromPacket | ( | McNackFrame | nack | ) |
void resendRequestedFrames | ( | ) |
void resendRoutedFrame | ( | boost::condition_variable & | condi, |
routing_entry | route | ||
) |
Definition at line 3218 of file adhoc_communication.cpp.
void resendRouteRequest | ( | route_request & | req, |
boost::condition_variable & | con | ||
) |
Definition at line 3155 of file adhoc_communication.cpp.
void resendUnackLinkFrame | ( | string | hostname_src, |
uint32_t | id, | ||
unsigned char * | mac, | ||
string | network_string, | ||
uint8_t | type | ||
) |
void safeFrame | ( | RoutedFrame | incomingFrame | ) |
void sendBeacons | ( | ) |
Definition at line 1662 of file adhoc_communication.cpp.
void sendLinkAck | ( | unsigned char * | dest, |
unsigned char * | confirmer_mac, | ||
uint32_t | id, | ||
string | source, | ||
bool | cr, | ||
uint8_t | type | ||
) |
Definition at line 1739 of file adhoc_communication.cpp.
bool sendPacket | ( | std::string & | hostname_destination, |
std::string & | payload, | ||
uint8_t | data_type_, | ||
std::string & | topic | ||
) |
void socketSend | ( | string | network_string | ) |
void testPacket | ( | ) |
void updateTsRequestFrames | ( | Packet | p | ) |
int beacon_interval = 500 |
list<Packet> cached_mc_packets_l |
bool enable_cooperative_relaying = false |
struct eh_header* ethernet_header |
std::map<uint16_t, std::string> frame_types |
boost::condition_variable got_request_response |
boost::condition_variable got_rframe_ack |
int hop_limit_increment = 3 |
int hop_limit_max = 0 |
int hop_limit_min = 0 |
list<RoutedFrame> inc_frames_l |
uint32_t incoming_frame_count = 0 |
uint32_t incoming_frames_lost = 0 |
std::string interface_as_string = "" |
double loss_ratio = 0 |
std::string mac_as_string = "" |
std::map<unsigned char*, string> mac_hname_dict |
int max_frame_size = 1500 |
int max_packet_size = 10000 |
list<McPosAckObj*> mc_ack_l |
McHandler mc_handler & mc_groups_l |
boost::mutex mtx_cached_mc_packets |
boost::mutex mtx_cr_entries |
boost::mutex mtx_frames_received_recently |
boost::mutex mtx_inc_rf_frames |
boost::mutex mtx_mc_ack_l |
boost::mutex mtx_mc_groups |
boost::mutex mtx_neighbors |
boost::mutex mtx_notify |
boost::mutex mtx_packets_incomplete |
boost::mutex mtx_relays |
boost::mutex mtx_requested_frames |
boost::mutex mtx_route_requests |
boost::mutex mtx_routing_table |
boost::mutex mtx_unack_cr_frames |
boost::mutex mtx_unack_link_frames |
boost::mutex mtx_unack_mc_cr_frames |
boost::mutex mtx_unack_routed_frame |
list<hostname_mac> neighbors_l |
std::string node_prefix = "adhoc_communication/" |
int num_e2e_retrans = 10 |
int num_link_retrans = 3 |
uint32_t packet_id_ = -1 |
list<Packet> packets_incomplete_l |
std::list<ros::Publisher> publishers_l |
int raw_socket = 0 |
bool rebuild_mc_tree = false |
bool recursive_mc_ack = false |
int robots_in_simulation = 10 |
list<routing_entry> routing_table_l |
std::string sim_robot_macs = "" |
bool simulation_mode = false |
struct sockaddr_ll socket_address |
std::string topic_new_robot = "new_robot" |
std::string topic_remove_robot = "remove_robot" |
list<ack_cr_info> unack_cr_frames_l |
list<stc_frame> unack_link_frames_l |