-
Notifications
You must be signed in to change notification settings - Fork 195
/
qos.lua
80 lines (71 loc) · 2.28 KB
/
qos.lua
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
-- Copyright (C) 2006-2020 Istituto Italiano di Tecnologia (IIT)
-- All rights reserved.
--
-- This software may be modified and distributed under the terms of the
-- BSD-3-Clause license. See the accompanying LICENSE file for details.
-- loading lua-yarp binding library
require("yarp")
--
-- create is called when the port monitor is created
-- @return Boolean
--
PortMonitor.create = function()
prev_rcv_time = nil
prev_monitor_time = nil
frame_jitter = 0
frame_count = 0
admin_port = yarp.Port()
admin_port:setAdminMode(true)
local ret = admin_port:open("...")
ret = ret and yarp.NetworkBase_connect(admin_port:getName(), "/grabber")
if ret == true then
print("QOS monitor started!")
return true
else
print("Failed to start QOS monitor!")
return false
end
end
--
-- destroy is called when port monitor is destroyed
--
PortMonitor.destroy = function()
yarp.NetworkBase_disconnect(admin_port:getName(), "/grabber")
admin_port:close()
print("QOS monitor finished!")
end
--
-- accept is called when the port receives new data
-- @param thing The Things abstract data type
-- @return Boolean
-- if false is returned, the data will be ignored
-- and update() will never be called
PortMonitor.accept = function(thing)
if prev_rcv_time == nil then
prev_rcv_time = yarp.Time_now()
prev_monitor_time = yarp.Time_now()
return true
end
frame_jitter = frame_jitter + (yarp.Time_now() - prev_rcv_time)
frame_count = frame_count + 1;
prev_rcv_time = yarp.Time_now()
if (yarp.Time_now() - prev_monitor_time) > 5.0 then
prev_monitor_time = yarp.Time_now()
local frame_rate = 1.0 / (frame_jitter/frame_count)
frame_jitter = 0
frame_count = 0
print("Frame rate:", frame_rate)
if frame_rate < 28.0 then
print(string.format("QOS low! frame rate = %d", frame_rate))
local cmd = yarp.Bottle()
cmd:fromString("prop set /view (qos ((tos 16)))")
print(cmd:toString())
admin_port:write(cmd)
cmd:clear()
cmd:fromString("prop set /view (sched ((priority 10) (policy 1)))")
print(cmd:toString())
admin_port:write(cmd)
end
end
return true
end