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
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250 | <?xml version="1.0" encoding="UTF-8"?>
<!--
- Copyright (c) 2024 MBARI
- MBARI Proprietary Information. Confidential. All Rights Reserved
- Unauthorized copying or distribution of this file via any medium is strictly
- prohibited.
-
- WARNING - This file contains information whose export is restricted by the
- Export Administration Act of 1979 (Title 50, U.S.C., App. 2401 et seq.), as
- amended. Violations of these export laws are subject to severe civil and/or
- criminal penalties.
-->
<DefineBehavior xmlns="Tethys"
xmlns:Control="Tethys/Control"
xmlns:BehaviorScripts="Tethys/BehaviorScripts"
xmlns:Units="Tethys/Units"
xmlns:Universal="Tethys/Universal"
xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
xsi:schemaLocation="Tethys http://okeanids.mbari.org/tethys/Xml/Tethys.xsd
Tethys/Control http://okeanids.mbari.org/tethys/Xml/Control.xsd
Tethys/BehaviorScripts ./BehaviorScripts.xsd
Tethys/Units http://okeanids.mbari.org/tethys/Xml/Units.xsd
Tethys/Universal http://okeanids.mbari.org/tethys/Xml/Universal.xsd"
Name="LawnMower" Language="Lua">
<Description>
Drives the vehicle in a pattern like a lawnmower search pattern.
Settings allow forward progression along a bearing, to extend to the
left and/or right of the start point, and to start to the left of right
of the start point.
</Description>
<!--
After the optional Description, any number of DefineSetting entries follow.
These are mapped to the Setting elements of the behavior when it is used.
Note that the Name attribute is the name of the variable passed to the script
Also note that the element following DefineSetting can be a URI defined in a
schema file, or a CustomUri element (See Demos/LawnMowerEmbedded.xml).
-->
<DefineSetting Name="forwardBearingSet">
<BehaviorScripts:LawnMower.forwardBearing/>
<Description>
Bearing followed when moving forward in the survey.
</Description><Units:radian/>
<DefaultValue>0</DefaultValue>
</DefineSetting>
<DefineSetting Name="forwardMovesSet">
<BehaviorScripts:LawnMower.forwardMoves/>
<Description>
Number of times to move forward in the survey.
</Description><Units:count/>
<DefaultValue>4</DefaultValue>
</DefineSetting>
<DefineSetting Name="leftWidthSet">
<BehaviorScripts:LawnMower.leftWidth/>
<Description>
Desired width of each leg, to the left of the starting point.
</Description><Units:meter/>
<DefaultValue>0</DefaultValue>
</DefineSetting>
<DefineSetting Name="rightWidthSet">
<BehaviorScripts:LawnMower.rightWidth/>
<Description>
Desired width of each leg, to the right of the starting point.
</Description><Units:meter/>
<DefaultValue>0</DefaultValue>
</DefineSetting>
<DefineSetting Name="spacingSet">
<BehaviorScripts:LawnMower.spacing/>
<Description>
Desired distance to move forward at the end of each leg.
</Description><Units:meter/>
<DefaultValue>1000</DefaultValue>
</DefineSetting>
<DefineSetting Name="startToLeftSet">
<BehaviorScripts:LawnMower.startToLeft/>
<Description>
Set to true to start survey by driving to the left. Otherwise drive
to the right. If this is set to true and leftWidth is zero or unset,
start moving forward along the left edge. If this is not set to true
and rightWidth is zero or unset, start moving forward along the
right edge.
</Description><Units:bool/>
<DefaultValue>0</DefaultValue>
</DefineSetting>
<!--
The optional Construct element defines a script that is run once during the
first initialization of the script. It takes no inputs, and provides not outputs
except for an optional syslog entry string followed by a syslog severity string.
-->
<Construct>
<Description>
Define functions for use in the behavior
</Description>
<Script><![CDATA[
atBearing = function(lat,lon,distance,bearing)
r = distance / 6371000.0 -- EARTH_RADIUS
newLat = math.asin( math.sin( lat ) * math.cos( r )
+ math.cos( lat ) * math.sin( r ) * math.cos( bearing ) )
newLon = lon + math.atan2( math.sin( bearing ) * math.sin( r ) * math.cos( lat ),
math.cos( r ) - math.sin( lat ) * math.sin( newLat ) )
return newLat,newLon
end
phaseFunctions = {
--[[outward to the left]] function(lat,lon,bear,left,right,spacing) lat,lon = atBearing(lat,lon,left,bear-math.pi/2) return lat, lon, bear-math.pi/2, left>0 end,
--[[forward on the left]] function(lat,lon,bear,left,right,spacing) lat,lon = atBearing(lat,lon,spacing,bear) return lat, lon, bear, true end,
--[[inward from the left]] function(lat,lon,bear,left,right,spacing) lat,lon = atBearing(lat,lon,left,bear+math.pi/2) return lat, lon, bear+math.pi/2, left>0 end,
--[[outward to the right]] function(lat,lon,bear,left,right,spacing) lat,lon = atBearing(lat,lon,right,bear+math.pi/2) return lat, lon, bear+math.pi/2, right>0 end,
--[[forward on the right]] function(lat,lon,bear,left,right,spacing) lat,lon = atBearing(lat,lon,spacing,bear) return lat, lon, bear, true end,
--[[inward from the right]] function(lat,lon,bear,left,right,spacing) lat,lon = atBearing(lat,lon,right,bear-math.pi/2) return lat, lon, bear-math.pi/2, right>0 end
}
advancePhase = function(phase,lat,lon,bear,left,right,spacing)
local done = false
local newLat = lat
local newLon = lon
local newBear = bear;
local perpSlope = 0
local startOvLn = false;
local innerSteps = 0;
while not done do
phase=phase + 1
if phase > 6 then phase = 1 end
newLat,newLon,newBear,done = phaseFunctions[phase](lat,lon,bear,left,right,spacing)
innerSteps = innerSteps + 1;
if innerSteps > 6 then
gCritical = "Error in advancePhase, too many steps"
done = true
end
end
if newLat ~= lat then
perpSlope = -( newLon - lon ) / ( newLat - lat )
startOvLn = lat > ( newLat + ( ( lon - newLon ) * perpSlope ) )
else
perpSlope = nil
startOvLn = lon > newLon
end
return phase,newLat,newLon,newBear,perpSlope,startOvLn
end
return "Constructed LawnMower", "IMPORTANT"
]]></Script>
</Construct>
<!--
The optional Initialize element defines a script that is run whenever
the behavior is initialized. It may have Input elements, which are provided
as function arguments, followed by all the setting values. It may also have
Output elements, which define how return values should be interpreted. Note
that if the last two return values are strings, then they are interpreted as
a syslog string and syslog severity.
-->
<Initialize>
<!--
Note in the Input element below, the Name attribute is the name of the variable
passed to the script
-->
<Input Name="latitude"><Universal:latitude/><Units:radian/><DefaultValue>NaN</DefaultValue></Input>
<Input Name="longitude"><Universal:longitude/><Units:radian/><DefaultValue>NaN</DefaultValue></Input>
<Script><![CDATA[
if startToLeft then
phase = 0
else
phase = 3
end
forwardCount = 0
gCritical = nil
phase,goalLat,goalLon,goalBear,perpendicularSlope,startOverLine=advancePhase(phase,latitude,longitude,forwardBearingSet,leftWidthSet,rightWidthSet,spacingSet)
if nil ~= gCritical then return gCritical, "CRITICAL" end
]]></Script>
</Initialize>
<!--
The mandatory Run element defines a script that is run each cycle while the
behavior is active. It may have Input elements, which are provided
as function arguments, followed by all the setting values. It may also have
Output elements, which define how return values should be interpreted
(nil or missing values are ignored). Note that if the last two return values
are strings, then they are interpreted as a syslog string and syslog severity.
And the Run element may have OutputArg elements, which map to OutputArg
elements when the Behavior is used in a script. OutputArg values should follow
any Output values in the script return value
-->
<Run>
<Input Name="latitude"><Universal:latitude/><Units:radian/><DefaultValue>NaN</DefaultValue></Input>
<Input Name="longitude"><Universal:longitude/><Units:radian/><DefaultValue>NaN</DefaultValue></Input>
<Output><Control:HorizontalControl.horizontalMode/><Units:enum/></Output>
<Output><Control:HorizontalControl.latitudeCmd/><Units:radian/></Output>
<Output><Control:HorizontalControl.longitudeCmd/><Units:radian/></Output>
<Output><Control:HorizontalControl.bearingCmd/><Units:radian/></Output>
<!--
No OutputArg elements here, but there is one defined in the FiniteDifference.xml behavior
-->
<Script><![CDATA[
local isOverLine = false
if perpendicularSlope ~= nil then
isOverLine = latitude > (goalLat
+ ( ( longitude - goalLon ) * perpendicularSlope))
else
isOverLine = longitude > goalLon;
end
if isOverLine ~= startOverLine then
if phase == 3 or (leftWidthSet == 0 and phase == 2)
or phase == 6 or (rightWidthSet == 0 and phase == 5)
or (leftWidthSet==0 and rightWidthSet==0 and (phase == 2 or phase == 5)) then
forwardCount = forwardCount + 1;
end
phase,goalLat,goalLon,goalBear,perpendicularSlope,startOverLine=
advancePhase(phase,goalLat,goalLon,forwardBearingSet,leftWidthSet,rightWidthSet,spacingSet)
end
if nil ~= gCritical then return gCritical, "CRITICAL" end
-- 1.0 is the HorizontalControl.horizontalMode value for Waypoint
return 1.0, goalLat, goalLon, goalBear;
]]></Script>
</Run>
<!--
The IsSatisfied element is required if the behavior will be defines a script
that may be run in Sequence or Progression. It may have Input elements, which are provided
as function arguments, followed by all the setting values. It may not have
Output elements, and should only provide a boolean return that indicates that
the behavior is done or otherwise satisfied (although it can also return a syslog
string and syslog severity).
-->
<IsSatisfied>
<Script><![CDATA[
if forwardMovesSet <= forwardCount then
return true
end
return false
]]></Script>
</IsSatisfied>
<!--
The optional Uninitialize element is not specified in this behavior, but if it were,
it would follow the conventions of the Initialize element.
-->
</DefineBehavior>
|