void schedule_and_send(int link){
if(links[link].timeout_occurred == false)
return;
//printf("QUEUE SIZES for link %d: %d(a) %d(s) %d(f)\n", link, queue_nitems(links[link].ack_sender), queue_nitems(links[link].sender), queue_nitems(links[link].forwarding_queue));
//Send ack if it is there -- TOP PRIORITY
if(queue_nitems(links[link].ack_sender) > 0){
send_acks(link);
return;
}
//If ack queue is empty send packets from sender queue OR forwarding queue in priority basis
if(queue_nitems(links[link].forwarding_queue) == 0 && queue_nitems(links[link].sender) > 0){
send_frames(link);
}
else if (queue_nitems(links[link].forwarding_queue) > 0 && queue_nitems(links[link].sender) == 0){
forward_frames(link);
}
else if (queue_nitems(links[link].forwarding_queue) == 0 && queue_nitems(links[link].sender) == 0){
return;
}
else {
links[link].packet_to_send = (links[link].packet_to_send + 1) % (PRIORITY + 1);
//printf("packet to send is ----------------------------------------- %d\n", links[link].packet_to_send);
//if(links[link].packet_to_send == PRIORITY)
if(queue_nitems(links[link].forwarding_queue) > queue_nitems(links[link].sender))
//send forwarding
forward_frames(link);
else
//send normal
send_frames(link);
}
}
void send_frames(int link){
//printf("Send frames called for link : %d\n", link);
size_t len = sizeof(FRAME);
CnetTime timeout;
FRAME *f = queue_remove(links[link].sender, &len);
switch(f->payload.kind){
case DL_DATA :
if(!links[link].ack_received[f->payload.A]) {
CHECK(CNET_write_physical(link, (char*)f, &len));
timeout = (len*((CnetTime)8000000)/linkinfo[link].bandwidth + linkinfo[link].propagationdelay);
start_timer(link, timeout);
queue_add(links[link].sender, f, len);
}
else {
if(queue_nitems(links[link].sender) > 0)
send_frames(link);
}
break;
case DL_ACK :
CHECK(CNET_write_physical(link, (char*)f, &len));
timeout = (len*((CnetTime)8000000)/linkinfo[link].bandwidth + linkinfo[link].propagationdelay);
start_timer(link, timeout);
break;
case RT_DATA:
//printf("RT packet sending on link : %d\n", link);
CHECK(CNET_write_physical(link, (char*)f, &len));
timeout = (len*((CnetTime)8000000)/linkinfo[link].bandwidth + linkinfo[link].propagationdelay);
start_timer(link, timeout);
break;
}
free(f);
}
int main(int argc, char *argv[]) {
(void) argc;
(void) argv;
struct in_addr remote_ip;
inet_aton("172.16.50.1", &remote_ip);
int mySocket = create_socket(&remote_ip, 2000);
send_frames(mySocket);
printf("Sender finished\n");
}
int main(int argc, char *argv[]) {
if (argc < 2) {
printf("Syntax: %s <remote ip address>\n", argv[0]);
return 1;
}
struct in_addr remote_ip;
int port = 7653;
printf("Remote IP: '%s', port %d\n", argv[1], port);
inet_aton(argv[1], &remote_ip);
int mySocket = create_socket(&remote_ip, port);
send_frames(mySocket);
printf("Sender finished\n");
return 0;
}
void datacb(const puppeteer_msgs::Robots &bots)
{
ROS_DEBUG("coordinator datacb triggered with OC = %d",
operating_condition);
static bool first_flag = true;
static ros::Time time;
// if we aren't calibrating or running, let's just exit
// this cb
if (operating_condition != 2 && operating_condition != 1)
return;
puppeteer_msgs::Robots b;
b.robots.resize(bots.robots.size());
b = bots;
tstamp = ros::Time::now();
// correct the points in bots
b = adjust_for_robot_size(b);
// store the values that we received:
if (first_flag) {
ROS_DEBUG("First call!");
current_bots = b;
prev_bots = b;
first_flag = false;
time = ros::Time::now();
return;
}
// check for timeout:
ros::Duration dt = ros::Time::now()-time;
time = ros::Time::now();
if (dt.toSec() > 1.0/MIN_FREQ)
ROS_WARN("Coordinator frequency dropping - %f Hz",
1/dt.toSec());
// store arrangements:
prev_bots = current_bots;
current_bots = b;
// do we need to calibrate?
if ( !calibrated_flag )
{
if ((int) current_bots.robots.size() == nr)
{
prev_bots_sorted = current_bots_sorted;
current_bots_sorted = calibrate_routine();
}
return;
}
// send all relevant transforms
if (calibrated_flag)
send_frames();
// If we got here, we are calibrated. That means we can
// sort robots based on previous locations
puppeteer_msgs::Robots tmp;
tmp.robots.resize(current_bots_sorted.robots.size());
tmp = current_bots_sorted;
// print_bots("tmp_bots (before)",tmp);
current_bots_sorted = associate_robots(current_bots, prev_bots_sorted);
prev_bots_sorted = tmp;
return;
}
