00001
00002
00003 import time
00004
00005 import roslib; roslib.load_manifest('network_control_tests')
00006 import rospy
00007
00008 from network_monitor_udp.linktest import UdpmonsourceHandle
00009 from network_monitor_udp.linktest import LinkTest
00010 from network_monitor_udp.msg import LinktestGoal
00011 from network_traffic_control.projected_link_metrics import get_projected_link_metrics
00012 import dynamic_reconfigure.client
00013
00014 def measure_link(tx_bandwidth, bandwidth_limit, latency, loss,
00015 packet_size, max_allowed_latency, max_return_time,
00016 sink_ip, direction, duration):
00017 (proj_bandwidth, proj_latency, proj_loss) = get_projected_link_metrics(bandwidth_limit, latency, loss, \
00018 packet_size, tx_bandwidth)
00019 if proj_latency > max_allowed_latency:
00020 rospy.logerr("Max allowed latency %.2fms is smaller than projected latency %.2fms,"
00021 "results will be flawed, exiting", max_allowed_latency * 1e3, proj_latency * 1e3)
00022 exit(1)
00023
00024 dynclient = dynamic_reconfigure.client.Client("tc")
00025 config = dynclient.update_configuration({"bandwidth_egress" : 0.0, "bandwidth_ingress" : 0.0,
00026 "latency_egress" : 0.0, "latency_ingress" : 0.0,
00027 "loss_egress" : 0.0, "loss_ingress" : 0.0 })
00028 if config['status'] != "OK":
00029 rospy.logerr("Initalizing tc node failed: " + config['errmsg'])
00030 exit(1)
00031 config = dynclient.update_configuration({"bandwidth_" + direction: bandwidth_limit,
00032 "latency_" + direction: latency,
00033 "loss_" + direction: loss })
00034 if config['status'] != "OK":
00035 rospy.logerr("Setting tc node config failed: " + config['errmsg'])
00036
00037 srcnode = UdpmonsourceHandle('performance_test')
00038 test = srcnode.create_test(bw = tx_bandwidth, pktsize = packet_size, duration = duration,
00039 sink_ip = sink_ip, sink_port = 12345,
00040 bw_type = LinktestGoal.BW_CONSTANT, max_return_time = max_return_time,
00041 latencybins = [ max_allowed_latency/4, max_allowed_latency/2, max_allowed_latency])
00042 test.start()
00043
00044 time.sleep(duration + 0.5)
00045
00046 rospy.loginfo("Link measurement completed!")
00047 rospy.loginfo("Link parameters: bandwidth_limit %.2fkbit/s latency %.2fms loss %.2f%% tx_bandwidth %.2fkbit/s\n"
00048 " packet_size %dbytes max_allowed_latency %.2fms max_return_time %.2fms\n"
00049 " direction %s duration %.2fs",
00050 bandwidth_limit/1e3, latency*1e3, loss, tx_bandwidth/1e3, packet_size, max_allowed_latency*1e3,
00051 max_return_time*1e3, direction, duration)
00052 rospy.loginfo("\nRESULTS: measured_bandwidth %.2fkbit/s measured_latency %.2fms measured_loss %.2f%%",
00053 test.bandwidth.avg()/1e3, test.latency.avg() * 1e3, test.loss.avg())
00054
00055 if __name__ == '__main__':
00056 rospy.init_node('measure_link_node')
00057
00058 tx_bandwidth = rospy.get_param("~tx_bandwidth")
00059
00060 args = dict()
00061 args['bandwidth_limit'] = rospy.get_param("~bandwidth_limit")
00062 args['latency'] = rospy.get_param("~latency")
00063 args['loss'] = rospy.get_param("~loss")
00064 args['packet_size'] = rospy.get_param("~packet_size")
00065 args['max_allowed_latency'] = rospy.get_param("~max_allowed_latency")
00066 args['max_return_time'] = rospy.get_param("~max_return_time")
00067 args['sink_ip'] = rospy.get_param("~sink_ip")
00068 args['direction'] = rospy.get_param("~direction")
00069 args['duration'] = rospy.get_param("~duration")
00070
00071 measure_link(tx_bandwidth, **args)