forked from strands-project/strands_webtools
-
Notifications
You must be signed in to change notification settings - Fork 0
/
marathon.html
291 lines (252 loc) · 9.96 KB
/
marathon.html
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
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
<!DOCTYPE html>
<html>
<head>
<meta charset="utf-8" />
<META HTTP-EQUIV="Pragma" CONTENT="no-cache">
<META HTTP-EQUIV="Expires" CONTENT="-1">
<!-- The default styling is provided by http://getbootstrap.com -->
<link href="css/bootstrap.css" rel="stylesheet">
<link href="css/main.css" rel="stylesheet">
<link rel="stylesheet" type="text/css" href="css/jquery-ui.css" />
<!-- jQuery and display.js are conveniences for interacting with the DOM -->
<script src="js/jquery.js"></script>
<script src="js/display.js"></script>
<script type="text/javascript" src="js/jquery-ui.js"></script>
<!-- ROSLIBJS -->
<!-- EventEmitter2 is the sole dependency of roslibjs -->
<script src="roslibjs/include/EventEmitter2/eventemitter2.js"></script>
<!-- Roslibjs handles core ROS functionality in the browser -->
<script src="roslibjs/build/roslib.js"></script>
<!-- ROS2DJS -->
<!-- EaselJS is a dependency of ros2djs -->
<script src="ros2djs/include/EaselJS/easeljs.js"></script>
<!-- Ros2djs provides 2D scene support, including mapping and more -->
<script src="ros2djs/build/ros2d.js"></script>
<!-- ROS3DJS -->
<!-- Three.js is the WebGL rendering library -->
<script src="ros3djs/include/threejs/three.js"></script>
<!-- ColladaLoader2 loads collada models of the robot -->
<script src="ros3djs/include/ColladaAnimationCompress/ColladaLoader2.js"></script>
<!-- Ros3djs provides 3D scene support, including mapping and more -->
<script src="ros3djs/build/ros3d.min.js"></script>
<script type="text/javascript" src="mjpegcanvasjs/build/mjpegcanvas.min.js"></script>
<script type="text/javascript" src="keyboardteleopjs/build/keyboardteleop.min.js"></script>
<script type="text/javascript" src="js/ptu_keyboardteleop.js"></script>
<script type="text/javascript">
var hostname = location.hostname;
/**
* Setup all GUI elements when the page is loaded.
*/
function init() {
// Connecting to ROS.
var ros = new ROSLIB.Ros({
url : 'ws://'+hostname+'/linda/rosws'
});
// ----------------------------------------------------------------------
// Initialize the teleop.
// ----------------------------------------------------------------------
//var teleop = new KEYBOARDTELEOP.Teleop({
//ros : ros,
//topic : '/cmd_vel'
//});
// ----------------------------------------------------------------------
// Initialize the PTU teleop.
// ----------------------------------------------------------------------
//var teleop = new PTU_KEYBOARDTELEOP.Teleop({
//ros : ros,
//topic : '/ptu_jointstate'
//});
// ----------------------------------------------------------------------
// Create a UI slider using JQuery UI.
// ----------------------------------------------------------------------
/*
$('#speed-slider').slider({
range : 'min',
min : 0,
max : 100,
value : 90,
slide : function(event, ui) {
// Change the speed label.
$('#speed-label').html('Speed: ' + ui.value + '%');
// Scale the speed.
teleop.scale = (ui.value / 100.0);
}
});
// ----------------------------------------------------------------------
// Set the initial speed .
// ----------------------------------------------------------------------
$('#speed-label').html('Speed: ' + ($('#speed-slider').slider('value')) + '%');
teleop.scale = ($('#speed-slider').slider('value') / 100.0);
*/
// ----------------------------------------------------------------------
// Subscribing to the robot's Pose
// ----------------------------------------------------------------------
// The ROSLIB.Topic handles subscribing and publishing a ROS topic. This
// topic interacts with the /robot_pose topic, published by the robot.
var poseTopic = new ROSLIB.Topic({
ros : ros,
name : '/amcl_pose',
messageType : 'geometry_msgs/PoseWithCovarianceStamped'
});
// Subscribes to the robot's pose. When rosbridge receives the pose
// message from ROS, it forwards the message to roslibjs, which calls this
// callback.
poseTopic.subscribe(function(message) {
// Formats the pose for outputting.
var now = new Date();
var position = 'x: ' + message.pose.pose.position.x
+ ', y: ' + message.pose.pose.position.y
+ ', z: 0.0';
var orientation = 'x: ' + message.pose.pose.orientation.x
+ ', y: ' + message.pose.pose.orientation.y
+ ', z: ' + message.pose.pose.orientation.z
+ ', w: ' + message.pose.pose.orientation.w;
// Prepends a row to the "poses" table with the formatted pose.
$('#poses > tbody > tr:first').after('<tr>'
+ '<td>' + now.toLocaleTimeString() + '</td>'
+ '<td>' + position + '</td>'
+ '<td>' + orientation + '</td>');
});
var mileTopic = new ROSLIB.Topic({
ros : ros,
name : '/mileage',
messageType : 'std_msgs/Float32'
});
// Subscribes to the robot's pose. When rosbridge receives the pose
// message from ROS, it forwards the message to roslibjs, which calls this
// callback.
mileTopic.subscribe(function(message) {
// Formats the pose for outputting.
var mileage = 'current mileage: ' + message.data + ' metres travelled';
$('#mileage').html(mileage);
});
// ----------------------------------------------------------------------
// Create the camera viewer.
// ----------------------------------------------------------------------
var viewer = new MJPEGCANVAS.Viewer({
divID : 'mjpeg',
host : hostname,
port : '80/linda/video',
width : 560,
height : 420,
topic : '/head_xtion/rgb/image_mono'
});
// ----------------------------------------------------------------------
// Rendering the robot in 3D
// ----------------------------------------------------------------------
// Create the scene manager and view port for the 3D world.
var viewer3D = new ROS3D.Viewer({
divID : 'threed-map',
width : 560,
height : 420,
antialias : true,
background : '#EEEEEE'
});
// Create a TF client that subscribes to the fixed frame.
var tfClient = new ROSLIB.TFClient({
ros : ros,
angularThres : 0.01,
transThres : 0.01,
rate : 10.0,
fixedFrame : '/map'
});
// Add the URDF model of the robot.
var urdfClient = new ROS3D.UrdfClient({
ros : ros,
tfClient : tfClient,
rootObject : viewer3D.scene
});
var colorMaterial = ROS3D.makeColorMaterial(1, 0, 0,0.5);
// Track robot pose with an arrow
var arrowNode = new ROS3D.SceneNode({
tfClient : tfClient,
frameID : '/base_link',
object : new ROS3D.Arrow({
length : 0.3,
material : colorMaterial
}),
});
viewer3D.scene.add(arrowNode);
// ----------------------------------------------------------------------
// Rendering the map in 3D
// ----------------------------------------------------------------------
// Add the Occupancy Grid map.
var grid = new ROS3D.OccupancyGridClient({
ros : ros,
tfClient : tfClient,
rootObject : viewer3D.scene
});
grid.on('change', function() {
// Change the opacity level.
grid.currentGrid.children[0].material.transparent = false;
grid.currentGrid.children[0].material.opacity = 0.85;
grid.currentGrid.children[0].material.needsUpdate = true;
});
}
</script>
</head>
<body onload="init()">
<div class="navbar navbar-inverse navbar-fixed-top">
<a class="navbar-brand" href="#">strands_webtools</a>
<ul class="nav pull-right">
<li class="active"><a href="marathon.html">Marathon</a></li>
<!--<li><a href="navigation.html">Autonomous</a></li>-->
</ul>
</div>
<div class="container">
<!--<h1><small>Simple keyboard control</small></h1>
<div class="row">
<div class="col-span-6">
<h2><small>Robot</small></h2>
<ul>
<li><strong>w/s</strong> to move forward and backward</li>
<li><strong>a/d</strong> to move left and right</li>
</ul>
</div>
<div class="col-span-6">
<h2><small>PTU</small></h2>
<ul>
<li><strong>j/k</strong> to move the PTU down and up</li>
<li><strong>h/l</strong> to move the PTU left and right</li>
<li><strong>o</strong> to move the PTU to the default pose</li>
</ul>
</div>
</div>
-->
<!-- <div id="speed-label"></div>
<div id="speed-slider"></div>-->
<div class="row">
<div class="col-span-6">
<h1><small>3D map</small></h1>
<div id="threed-map"></div>
</div>
<div class="col-span-6">
<h1><small>Robot cam</small></h1>
<div id="mjpeg"></div>
</div>
</div>
<div class="row" id="misc-data">
<h1><small>Mileage</small></h1>
<div id="mileage">Mileage</div>
</div>
<!-- Pose Table -->
<div class="row" id="pose-data">
<div class="col-span-12">
<h1><small>Robot's Pose</small></h1>
<table id="poses" class="table table-condensed">
<thead>
<tr>
<th>Time</th>
<th>Position</th>
<th>Orientation</th>
</tr>
</thead>
<tbody>
<tr></tr>
</tbody>
</table>
</div>
</div>
</div>
</body>
</html>