]> defiant.homedns.org Git - ros_wild_thumper.git/blob - ros2d.js
d5c433e4288d8212334a1e349db884417057bde3
[ros_wild_thumper.git] / ros2d.js
1 /**
2  * @author Russell Toris - rctoris@wpi.edu
3  */
4
5 var ROS2D = ROS2D || {
6   REVISION : '0.9.0'
7 };
8
9 // convert the given global Stage coordinates to ROS coordinates
10 createjs.Stage.prototype.globalToRos = function(x, y) {
11   var rosX = (x - this.x) / this.scaleX;
12   var rosY = (this.y - y) / this.scaleY;
13   return new ROSLIB.Vector3({
14     x : rosX,
15     y : rosY
16   });
17 };
18
19 // convert the given ROS coordinates to global Stage coordinates
20 createjs.Stage.prototype.rosToGlobal = function(pos) {
21   var x = pos.x * this.scaleX + this.x;
22   var y = pos.y * this.scaleY + this.y;
23   return {
24     x : x,
25     y : y
26   };
27 };
28
29 // convert a ROS quaternion to theta in degrees
30 createjs.Stage.prototype.rosQuaternionToGlobalTheta = function(orientation) {
31   // See https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Rotation_matrices
32   // here we use [x y z] = R * [1 0 0]
33   var q0 = orientation.w;
34   var q1 = orientation.x;
35   var q2 = orientation.y;
36   var q3 = orientation.z;
37   // Canvas rotation is clock wise and in degrees
38   return -Math.atan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2 * q2 + q3 * q3)) * 180.0 / Math.PI;
39 };
40
41 /**
42  * @author Russell Toris - rctoris@wpi.edu
43  */
44
45 /**
46  * An image map is a PNG image scaled to fit to the dimensions of a OccupancyGrid.
47  *
48  * @constructor
49  * @param options - object with following keys:
50  *   * message - the occupancy grid map meta data message
51  *   * image - the image URL to load
52  */
53 ROS2D.ImageMap = function(options) {
54   options = options || {};
55   var message = options.message;
56   var image = options.image;
57
58   // save the metadata we need
59   this.pose = new ROSLIB.Pose({
60     position : message.origin.position,
61     orientation : message.origin.orientation
62   });
63
64   // set the size
65   this.width = message.width;
66   this.height = message.height;
67
68   // create the bitmap
69   createjs.Bitmap.call(this, image);
70   // change Y direction
71   this.y = -this.height * message.resolution;
72
73   // scale the image
74   this.scaleX = message.resolution;
75   this.scaleY = message.resolution;
76   this.width *= this.scaleX;
77   this.height *= this.scaleY;
78
79   // set the pose
80   this.x += this.pose.position.x;
81   this.y -= this.pose.position.y;
82 };
83 ROS2D.ImageMap.prototype.__proto__ = createjs.Bitmap.prototype;
84
85 /**
86  * @author Russell Toris - rctoris@wpi.edu
87  */
88
89 /**
90  * A image map is a PNG image scaled to fit to the dimensions of a OccupancyGrid.
91  *
92  * Emits the following events:
93  *   * 'change' - there was an update or change in the map
94  *
95  * @constructor
96  * @param options - object with following keys:
97  *   * ros - the ROSLIB.Ros connection handle
98  *   * topic (optional) - the map meta data topic to listen to
99  *   * image - the image URL to load
100  *   * rootObject (optional) - the root object to add this marker to
101  */
102 ROS2D.ImageMapClient = function(options) {
103   var that = this;
104   options = options || {};
105   var ros = options.ros;
106   var topic = options.topic || '/map_metadata';
107   this.image = options.image;
108   this.rootObject = options.rootObject || new createjs.Container();
109
110   // create an empty shape to start with
111   this.currentImage = new createjs.Shape();
112
113   // subscribe to the topic
114   var rosTopic = new ROSLIB.Topic({
115     ros : ros,
116     name : topic,
117     messageType : 'nav_msgs/MapMetaData'
118   });
119
120   rosTopic.subscribe(function(message) {
121     // we only need this once
122     rosTopic.unsubscribe();
123
124     // create the image
125     that.currentImage = new ROS2D.ImageMap({
126       message : message,
127       image : that.image
128     });
129     that.rootObject.addChild(that.currentImage);
130     // work-around for a bug in easeljs -- needs a second object to render correctly
131     that.rootObject.addChild(new ROS2D.Grid({size:1}));
132
133     that.emit('change');
134   });
135 };
136 ROS2D.ImageMapClient.prototype.__proto__ = EventEmitter2.prototype;
137
138 /**
139  * @author Russell Toris - rctoris@wpi.edu
140  */
141
142 /**
143  * An OccupancyGrid can convert a ROS occupancy grid message into a createjs Bitmap object.
144  *
145  * @constructor
146  * @param options - object with following keys:
147  *   * message - the occupancy grid message
148  */
149 ROS2D.OccupancyGrid = function(options) {
150   options = options || {};
151   var message = options.message;
152
153   // internal drawing canvas
154   var canvas = document.createElement('canvas');
155   var context = canvas.getContext('2d');
156
157   // save the metadata we need
158   this.pose = new ROSLIB.Pose({
159     position : message.info.origin.position,
160     orientation : message.info.origin.orientation
161   });
162
163   // set the size
164   this.width = message.info.width;
165   this.height = message.info.height;
166   canvas.width = this.width;
167   canvas.height = this.height;
168
169   var imageData = context.createImageData(this.width, this.height);
170   for ( var row = 0; row < this.height; row++) {
171     for ( var col = 0; col < this.width; col++) {
172       // determine the index into the map data
173       var mapI = col + ((this.height - row - 1) * this.width);
174       // determine the value
175       var data = message.data[mapI];
176       var val;
177       if (data === 100) {
178         val = 0;
179       } else if (data === 0) {
180         val = 255;
181       } else {
182         val = 127;
183       }
184
185       // determine the index into the image data array
186       var i = (col + (row * this.width)) * 4;
187       // r
188       imageData.data[i] = val;
189       // g
190       imageData.data[++i] = val;
191       // b
192       imageData.data[++i] = val;
193       // a
194       imageData.data[++i] = 255;
195     }
196   }
197   context.putImageData(imageData, 0, 0);
198
199   // create the bitmap
200   createjs.Bitmap.call(this, canvas);
201   // change Y direction
202   this.y = -this.height * message.info.resolution;
203   
204   // scale the image
205   this.scaleX = message.info.resolution;
206   this.scaleY = message.info.resolution;
207   this.width *= this.scaleX;
208   this.height *= this.scaleY;
209
210   // set the pose
211   this.x += this.pose.position.x;
212   this.y -= this.pose.position.y;
213 };
214 ROS2D.OccupancyGrid.prototype.__proto__ = createjs.Bitmap.prototype;
215
216 /**
217  * @author Russell Toris - rctoris@wpi.edu
218  */
219
220 /**
221  * A map that listens to a given occupancy grid topic.
222  *
223  * Emits the following events:
224  *   * 'change' - there was an update or change in the map
225  *
226  * @constructor
227  * @param options - object with following keys:
228  *   * ros - the ROSLIB.Ros connection handle
229  *   * topic (optional) - the map topic to listen to
230  *   * rootObject (optional) - the root object to add this marker to
231  *   * continuous (optional) - if the map should be continuously loaded (e.g., for SLAM)
232  */
233 ROS2D.OccupancyGridClient = function(options) {
234   var that = this;
235   options = options || {};
236   var ros = options.ros;
237   var topic = options.topic || '/map';
238   this.continuous = options.continuous;
239   this.rootObject = options.rootObject || new createjs.Container();
240
241   // current grid that is displayed
242   // create an empty shape to start with, so that the order remains correct.
243   this.currentGrid = new createjs.Shape();
244   this.rootObject.addChild(this.currentGrid);
245   // work-around for a bug in easeljs -- needs a second object to render correctly
246   this.rootObject.addChild(new ROS2D.Grid({size:1}));
247
248   // subscribe to the topic
249   var rosTopic = new ROSLIB.Topic({
250     ros : ros,
251     name : topic,
252     messageType : 'nav_msgs/OccupancyGrid',
253     compression : 'png'
254   });
255
256   rosTopic.subscribe(function(message) {
257     // check for an old map
258     var index = null;
259     if (that.currentGrid) {
260       index = that.rootObject.getChildIndex(that.currentGrid);
261       that.rootObject.removeChild(that.currentGrid);
262     }
263
264     that.currentGrid = new ROS2D.OccupancyGrid({
265       message : message
266     });
267     if (index !== null) {
268       that.rootObject.addChildAt(that.currentGrid, index);
269     }
270     else {
271       that.rootObject.addChild(that.currentGrid);
272     }
273
274     that.emit('change');
275
276     // check if we should unsubscribe
277     if (!that.continuous) {
278       rosTopic.unsubscribe();
279     }
280   });
281 };
282 ROS2D.OccupancyGridClient.prototype.__proto__ = EventEmitter2.prototype;
283
284 /**
285  * @author Jihoon Lee- jihoonlee.in@gmail.com
286  * @author Russell Toris - rctoris@wpi.edu
287  */
288
289 /**
290  * A static map that receives from map_server.
291  *
292  * Emits the following events:
293  *   * 'change' - there was an update or change in the map
294  *
295  * @constructor
296  * @param options - object with following keys:
297  *   * ros - the ROSLIB.Ros connection handle
298  *   * service (optional) - the map topic to listen to, like '/static_map'
299  *   * rootObject (optional) - the root object to add this marker to
300  */
301 ROS2D.OccupancyGridSrvClient = function(options) {
302   var that = this;
303   options = options || {};
304   var ros = options.ros;
305   var service = options.service || '/static_map';
306   this.rootObject = options.rootObject || new createjs.Container();
307
308   // current grid that is displayed
309   this.currentGrid = null;
310
311   // Setting up to the service
312   var rosService = new ROSLIB.Service({
313     ros : ros,
314     name : service,
315     serviceType : 'nav_msgs/GetMap',
316     compression : 'png'
317   });
318
319   rosService.callService(new ROSLIB.ServiceRequest(),function(response) {
320     // check for an old map
321     if (that.currentGrid) {
322       that.rootObject.removeChild(that.currentGrid);
323     }
324
325     that.currentGrid = new ROS2D.OccupancyGrid({
326       message : response.map
327     });
328     that.rootObject.addChild(that.currentGrid);
329
330     that.emit('change', that.currentGrid);
331   });
332 };
333 ROS2D.OccupancyGridSrvClient.prototype.__proto__ = EventEmitter2.prototype;
334
335 /**
336  * @author Bart van Vliet - bart@dobots.nl
337  */
338
339 /**
340  * An arrow with line and triangular head, based on the navigation arrow.
341  * Aims to the left at 0 rotation, as would be expected.
342  *
343  * @constructor
344  * @param options - object with following keys:
345  *   * size (optional) - the size of the marker
346  *   * strokeSize (optional) - the size of the outline
347  *   * strokeColor (optional) - the createjs color for the stroke
348  *   * fillColor (optional) - the createjs color for the fill
349  *   * pulse (optional) - if the marker should "pulse" over time
350  */
351 ROS2D.ArrowShape = function(options) {
352         var that = this;
353         options = options || {};
354         var size = options.size || 10;
355         var strokeSize = options.strokeSize || 3;
356         var strokeColor = options.strokeColor || createjs.Graphics.getRGB(0, 0, 0);
357         var fillColor = options.fillColor || createjs.Graphics.getRGB(255, 0, 0);
358         var pulse = options.pulse;
359         
360         // draw the arrow
361         var graphics = new createjs.Graphics();
362         
363         var headLen = size / 3.0;
364         var headWidth = headLen * 2.0 / 3.0;
365         
366         graphics.setStrokeStyle(strokeSize);
367         graphics.beginStroke(strokeColor);
368         graphics.moveTo(0, 0);
369         graphics.lineTo(size-headLen, 0);
370         
371         graphics.beginFill(fillColor);
372         graphics.moveTo(size, 0);
373         graphics.lineTo(size-headLen, headWidth / 2.0);
374         graphics.lineTo(size-headLen, -headWidth / 2.0);
375         graphics.closePath();
376         graphics.endFill();
377         graphics.endStroke();
378         
379         // create the shape
380         createjs.Shape.call(this, graphics);
381         
382         // check if we are pulsing
383         if (pulse) {
384                 // have the model "pulse"
385                 var growCount = 0;
386                 var growing = true;
387                 createjs.Ticker.addEventListener('tick', function() {
388                         if (growing) {
389                                 that.scaleX *= 1.035;
390                                 that.scaleY *= 1.035;
391                                 growing = (++growCount < 10);
392                         } else {
393                                 that.scaleX /= 1.035;
394                                 that.scaleY /= 1.035;
395                                 growing = (--growCount < 0);
396                         }
397                 });
398         }
399 };
400 ROS2D.ArrowShape.prototype.__proto__ = createjs.Shape.prototype;
401
402 /**
403  * @author Raffaello Bonghi - raffaello.bonghi@officinerobotiche.it
404  */
405
406 /**
407  * A Grid object draw in map.
408  *
409  * @constructor
410  * @param options - object with following keys:
411  *  * size (optional) - the size of the grid
412  *  * cellSize (optional) - the cell size of map
413  *  * lineWidth (optional) - the width of the lines in the grid
414  */
415  ROS2D.Grid = function(options) {
416     var that = this;
417     options = options || {};
418     var size = options.size || 10;
419     var cellSize = options.cellSize || 0.1;
420     var lineWidth = options.lineWidth || 0.001;
421     // draw the arrow
422     var graphics = new createjs.Graphics();
423     // line width
424     graphics.setStrokeStyle(lineWidth*5);
425     graphics.beginStroke(createjs.Graphics.getRGB(0, 0, 0));
426     graphics.beginFill(createjs.Graphics.getRGB(255, 0, 0));
427     graphics.moveTo(-size*cellSize, 0);
428     graphics.lineTo(size*cellSize, 0);
429     graphics.moveTo(0, -size*cellSize);
430     graphics.lineTo(0, size*cellSize);
431     graphics.endFill();
432     graphics.endStroke();
433
434     graphics.setStrokeStyle(lineWidth);
435     graphics.beginStroke(createjs.Graphics.getRGB(0, 0, 0));
436     graphics.beginFill(createjs.Graphics.getRGB(255, 0, 0));
437     for (var i = -size; i <= size; i++) {
438         graphics.moveTo(-size*cellSize, i * cellSize);
439         graphics.lineTo(size*cellSize, i * cellSize);
440         graphics.moveTo(i * cellSize, -size*cellSize);
441         graphics.lineTo(i * cellSize, size*cellSize);
442     }
443     graphics.endFill();
444     graphics.endStroke();
445     // create the shape
446     createjs.Shape.call(this, graphics);
447
448 };
449 ROS2D.Grid.prototype.__proto__ = createjs.Shape.prototype;
450
451 /**
452  * @author Russell Toris - rctoris@wpi.edu
453  */
454
455 /**
456  * A navigation arrow is a directed triangle that can be used to display orientation.
457  *
458  * @constructor
459  * @param options - object with following keys:
460  *   * size (optional) - the size of the marker
461  *   * strokeSize (optional) - the size of the outline
462  *   * strokeColor (optional) - the createjs color for the stroke
463  *   * fillColor (optional) - the createjs color for the fill
464  *   * pulse (optional) - if the marker should "pulse" over time
465  */
466 ROS2D.NavigationArrow = function(options) {
467   var that = this;
468   options = options || {};
469   var size = options.size || 10;
470   var strokeSize = options.strokeSize || 3;
471   var strokeColor = options.strokeColor || createjs.Graphics.getRGB(0, 0, 0);
472   var fillColor = options.fillColor || createjs.Graphics.getRGB(255, 0, 0);
473   var pulse = options.pulse;
474
475   // draw the arrow
476   var graphics = new createjs.Graphics();
477   // line width
478   graphics.setStrokeStyle(strokeSize);
479   graphics.moveTo(-size / 2.0, -size / 2.0);
480   graphics.beginStroke(strokeColor);
481   graphics.beginFill(fillColor);
482   graphics.lineTo(size, 0);
483   graphics.lineTo(-size / 2.0, size / 2.0);
484   graphics.closePath();
485   graphics.endFill();
486   graphics.endStroke();
487
488   // create the shape
489   createjs.Shape.call(this, graphics);
490   
491   // check if we are pulsing
492   if (pulse) {
493     // have the model "pulse"
494     var growCount = 0;
495     var growing = true;
496     createjs.Ticker.addEventListener('tick', function() {
497       if (growing) {
498         that.scaleX *= 1.035;
499         that.scaleY *= 1.035;
500         growing = (++growCount < 10);
501       } else {
502         that.scaleX /= 1.035;
503         that.scaleY /= 1.035;
504         growing = (--growCount < 0);
505       }
506     });
507   }
508 };
509 ROS2D.NavigationArrow.prototype.__proto__ = createjs.Shape.prototype;
510
511 /**
512  * @author Inigo Gonzalez - ingonza85@gmail.com
513  */
514
515 /**
516  * A navigation image that can be used to display orientation.
517  *
518  * @constructor
519  * @param options - object with following keys:
520  *   * size (optional) - the size of the marker
521  *   * image - the image to use as a marker
522  *   * pulse (optional) - if the marker should "pulse" over time
523  */
524 ROS2D.NavigationImage = function(options) {
525   var that = this;
526   options = options || {};
527   var size = options.size || 10;
528   var image_url = options.image;
529   var pulse = options.pulse;
530   var alpha = options.alpha || 1;
531
532   var originals = {};
533
534   var paintImage = function(){
535     createjs.Bitmap.call(that, image);
536     var scale = calculateScale(size);
537     that.alpha = alpha;
538     that.scaleX = scale;
539     that.scaleY = scale;
540     that.regY = that.image.height/2;
541     that.regX = that.image.width/2;
542     originals['rotation'] = that.rotation;
543     Object.defineProperty( that, 'rotation', {
544       get: function(){ return originals['rotation'] + 90; },
545       set: function(value){ originals['rotation'] = value; }
546     });
547     if (pulse) {
548       // have the model "pulse"
549       var growCount = 0;
550       var growing = true;
551       var SCALE_SIZE = 1.020;
552       createjs.Ticker.addEventListener('tick', function() {
553         if (growing) {
554           that.scaleX *= SCALE_SIZE;
555           that.scaleY *= SCALE_SIZE;
556           growing = (++growCount < 10);
557         } else {
558           that.scaleX /= SCALE_SIZE;
559           that.scaleY /= SCALE_SIZE;
560           growing = (--growCount < 0);
561         }
562       });
563     }
564   };
565
566    var calculateScale = function(_size){
567       return _size / image.width;
568   };
569
570   var image = new Image();
571   image.onload = paintImage;
572   image.src = image_url;
573
574 };
575
576 ROS2D.NavigationImage.prototype.__proto__ = createjs.Bitmap.prototype;
577
578 /**
579  * @author Bart van Vliet - bart@dobots.nl
580  */
581
582 /**
583  * A shape to draw a nav_msgs/Path msg
584  *
585  * @constructor
586  * @param options - object with following keys:
587  *   * path (optional) - the initial path to draw
588  *   * strokeSize (optional) - the size of the outline
589  *   * strokeColor (optional) - the createjs color for the stroke
590  */
591 ROS2D.PathShape = function(options) {
592         options = options || {};
593         var path = options.path;
594         this.strokeSize = options.strokeSize || 3;
595         this.strokeColor = options.strokeColor || createjs.Graphics.getRGB(0, 0, 0);
596         
597         // draw the line
598         this.graphics = new createjs.Graphics();
599         
600         if (path !== null && typeof path !== 'undefined') {
601                 this.graphics.setStrokeStyle(this.strokeSize);
602                 this.graphics.beginStroke(this.strokeColor);
603                 this.graphics.moveTo(path.poses[0].pose.position.x / this.scaleX, path.poses[0].pose.position.y / -this.scaleY);
604                 for (var i=1; i<path.poses.length; ++i) {
605                         this.graphics.lineTo(path.poses[i].pose.position.x / this.scaleX, path.poses[i].pose.position.y / -this.scaleY);
606                 }
607                 this.graphics.endStroke();
608         }
609         
610         // create the shape
611         createjs.Shape.call(this, this.graphics);
612 };
613
614 /**
615  * Set the path to draw
616  *
617  * @param path of type nav_msgs/Path
618  */
619 ROS2D.PathShape.prototype.setPath = function(path) {
620         this.graphics.clear();
621         if (path !== null && typeof path !== 'undefined') {
622                 this.graphics.setStrokeStyle(this.strokeSize);
623                 this.graphics.beginStroke(this.strokeColor);
624                 this.graphics.moveTo(path.poses[0].pose.position.x / this.scaleX, path.poses[0].pose.position.y / -this.scaleY);
625                 for (var i=1; i<path.poses.length; ++i) {
626                         this.graphics.lineTo(path.poses[i].pose.position.x / this.scaleX, path.poses[i].pose.position.y / -this.scaleY);
627                 }
628                 this.graphics.endStroke();
629         }
630 };
631
632 ROS2D.PathShape.prototype.__proto__ = createjs.Shape.prototype;
633
634 /**
635  * @author Bart van Vliet - bart@dobots.nl
636  */
637
638 /**
639  * A polygon that can be edited by an end user
640  *
641  * @constructor
642  * @param options - object with following keys:
643  *   * pose (optional) - the first pose of the trace
644  *   * lineSize (optional) - the width of the lines
645  *   * lineColor (optional) - the createjs color of the lines
646  *   * pointSize (optional) - the size of the points
647  *   * pointColor (optional) - the createjs color of the points
648  *   * fillColor (optional) - the createjs color to fill the polygon
649  *   * lineCallBack (optional) - callback function for mouse interaction with a line
650  *   * pointCallBack (optional) - callback function for mouse interaction with a point
651  */
652 ROS2D.PolygonMarker = function(options) {
653 //      var that = this;
654         options = options || {};
655         this.lineSize = options.lineSize || 3;
656         this.lineColor = options.lineColor || createjs.Graphics.getRGB(0, 0, 255, 0.66);
657         this.pointSize = options.pointSize || 10;
658         this.pointColor = options.pointColor || createjs.Graphics.getRGB(255, 0, 0, 0.66);
659         this.fillColor = options.pointColor || createjs.Graphics.getRGB(0, 255, 0, 0.33);
660         this.lineCallBack = options.lineCallBack;
661         this.pointCallBack = options.pointCallBack;
662         
663         // Array of point shapes
664 //      this.points = [];
665         this.pointContainer = new createjs.Container();
666         
667         // Array of line shapes
668 //      this.lines = [];
669         this.lineContainer = new createjs.Container();
670         
671         this.fillShape = new createjs.Shape();
672         
673         // Container with all the lines and points
674         createjs.Container.call(this);
675         
676         this.addChild(this.fillShape);
677         this.addChild(this.lineContainer);
678         this.addChild(this.pointContainer);
679 };
680
681 /**
682  * Internal use only
683  */
684 ROS2D.PolygonMarker.prototype.createLineShape = function(startPoint, endPoint) {
685         var line = new createjs.Shape();
686 //      line.graphics.setStrokeStyle(this.strokeSize);
687 //      line.graphics.beginStroke(this.strokeColor);
688 //      line.graphics.moveTo(startPoint.x, startPoint.y);
689 //      line.graphics.lineTo(endPoint.x, endPoint.y);
690         this.editLineShape(line, startPoint, endPoint);
691         
692         var that = this;
693         line.addEventListener('mousedown', function(event) {
694                 if (that.lineCallBack !== null && typeof that.lineCallBack !== 'undefined') {
695                         that.lineCallBack('mousedown', event, that.lineContainer.getChildIndex(event.target));
696                 }
697         });
698         
699         return line;
700 };
701
702 /**
703  * Internal use only
704  */
705 ROS2D.PolygonMarker.prototype.editLineShape = function(line, startPoint, endPoint) {
706         line.graphics.clear();
707         line.graphics.setStrokeStyle(this.lineSize);
708         line.graphics.beginStroke(this.lineColor);
709         line.graphics.moveTo(startPoint.x, startPoint.y);
710         line.graphics.lineTo(endPoint.x, endPoint.y);
711 };
712
713 /**
714  * Internal use only
715  */
716 ROS2D.PolygonMarker.prototype.createPointShape = function(pos) {
717         var point = new createjs.Shape();
718         point.graphics.beginFill(this.pointColor);
719         point.graphics.drawCircle(0, 0, this.pointSize);
720         point.x = pos.x;
721         point.y = -pos.y;
722         
723         var that = this;
724         point.addEventListener('mousedown', function(event) {
725                 if (that.pointCallBack !== null && typeof that.pointCallBack !== 'undefined') {
726                         that.pointCallBack('mousedown', event, that.pointContainer.getChildIndex(event.target));
727                 }
728         });
729         
730         return point;
731 };
732
733 /**
734  * Adds a point to the polygon
735  *
736  * @param position of type ROSLIB.Vector3
737  */
738 ROS2D.PolygonMarker.prototype.addPoint = function(pos) {
739         var point = this.createPointShape(pos);
740         this.pointContainer.addChild(point);
741         var numPoints = this.pointContainer.getNumChildren();
742         
743         // 0 points -> 1 point, 0 lines
744         // 1 point  -> 2 points, lines: add line between previous and new point, add line between new point and first point
745         // 2 points -> 3 points, 3 lines: change last line, add line between new point and first point
746         // 3 points -> 4 points, 4 lines: change last line, add line between new point and first point
747         // etc
748         
749         if (numPoints < 2) {
750                 // Now 1 point
751         }
752         else if (numPoints < 3) {
753                 // Now 2 points: add line between previous and new point
754                 var line = this.createLineShape(this.pointContainer.getChildAt(numPoints-2), point);
755                 this.lineContainer.addChild(line);
756         }
757         if (numPoints > 2) {
758                 // Now 3 or more points: change last line
759                 this.editLineShape(this.lineContainer.getChildAt(numPoints-2), this.pointContainer.getChildAt(numPoints-2), point);
760         }
761         if (numPoints > 1) {
762                 // Now 2 or more points: add line between new point and first point
763                 var lineEnd = this.createLineShape(point, this.pointContainer.getChildAt(0));
764                 this.lineContainer.addChild(lineEnd);
765         }
766         
767         this.drawFill();
768 };
769
770 /**
771  * Removes a point from the polygon
772  *
773  * @param obj either an index (integer) or a point shape of the polygon
774  */
775 ROS2D.PolygonMarker.prototype.remPoint = function(obj) {
776         var index;
777 //      var point;
778         if (obj instanceof createjs.Shape) {
779                 index = this.pointContainer.getChildIndex(obj);
780 //              point = obj;
781         }
782         else {
783                 index = obj;
784 //              point = this.pointContainer.getChildAt(index);
785         }
786         
787         // 0 points -> 0 points, 0 lines
788         // 1 point  -> 0 points, 0 lines
789         // 2 points -> 1 point,  0 lines: remove all lines
790         // 3 points -> 2 points, 2 lines: change line before point to remove, remove line after point to remove
791         // 4 points -> 3 points, 3 lines: change line before point to remove, remove line after point to remove
792         // etc
793         
794         var numPoints = this.pointContainer.getNumChildren();
795         
796         if (numPoints < 2) {
797                 
798         }
799         else if (numPoints < 3) {
800                 // 2 points: remove all lines
801                 this.lineContainer.removeAllChildren();
802         }
803         else {
804                 // 3 or more points: change line before point to remove, remove line after point to remove
805                 this.editLineShape(
806                         this.lineContainer.getChildAt((index-1+numPoints)%numPoints),
807                         this.pointContainer.getChildAt((index-1+numPoints)%numPoints),
808                         this.pointContainer.getChildAt((index+1)%numPoints)
809                 );
810                 this.lineContainer.removeChildAt(index);
811         }
812         this.pointContainer.removeChildAt(index);
813 //      this.points.splice(index, 1);
814         
815         this.drawFill();
816 };
817
818 /**
819  * Moves a point of the polygon
820  *
821  * @param obj either an index (integer) or a point shape of the polygon
822  * @param position of type ROSLIB.Vector3
823  */
824 ROS2D.PolygonMarker.prototype.movePoint = function(obj, newPos) {
825         var index;
826         var point;
827         if (obj instanceof createjs.Shape) {
828                 index = this.pointContainer.getChildIndex(obj);
829                 point = obj;
830         }
831         else {
832                 index = obj;
833                 point = this.pointContainer.getChildAt(index);
834         }
835         point.x = newPos.x;
836         point.y = -newPos.y;
837         
838         var numPoints = this.pointContainer.getNumChildren();
839         if (numPoints > 1) {
840                 // line before moved point
841                 var line1 = this.lineContainer.getChildAt((index-1+numPoints)%numPoints);
842                 this.editLineShape(line1, this.pointContainer.getChildAt((index-1+numPoints)%numPoints), point);
843                 
844                 // line after moved point
845                 var line2 = this.lineContainer.getChildAt(index);
846                 this.editLineShape(line2, point, this.pointContainer.getChildAt((index+1)%numPoints));
847         }
848         
849         this.drawFill();
850 };
851
852 /**
853  * Splits a line of the polygon: inserts a point at the center of the line
854  *
855  * @param obj either an index (integer) or a line shape of the polygon
856  */
857 ROS2D.PolygonMarker.prototype.splitLine = function(obj) {
858         var index;
859         var line;
860         if (obj instanceof createjs.Shape) {
861                 index = this.lineContainer.getChildIndex(obj);
862                 line = obj;
863         }
864         else {
865                 index = obj;
866                 line = this.lineContainer.getChildAt(index);
867         }
868         var numPoints = this.pointContainer.getNumChildren();
869         var xs = this.pointContainer.getChildAt(index).x;
870         var ys = this.pointContainer.getChildAt(index).y;
871         var xe = this.pointContainer.getChildAt((index+1)%numPoints).x;
872         var ye = this.pointContainer.getChildAt((index+1)%numPoints).y;
873         var xh = (xs+xe)/2.0;
874         var yh = (ys+ye)/2.0;
875         var pos = new ROSLIB.Vector3({ x:xh, y:-yh });
876         
877         // Add a point in the center of the line to split
878         var point = this.createPointShape(pos);
879         this.pointContainer.addChildAt(point, index+1);
880         ++numPoints;
881         
882         // Add a line between the new point and the end of the line to split
883         var lineNew = this.createLineShape(point, this.pointContainer.getChildAt((index+2)%numPoints));
884         this.lineContainer.addChildAt(lineNew, index+1);
885
886         // Set the endpoint of the line to split to the new point
887         this.editLineShape(line, this.pointContainer.getChildAt(index), point);
888         
889         this.drawFill();
890 };
891
892 /**
893  * Internal use only
894  */
895 ROS2D.PolygonMarker.prototype.drawFill = function() {
896         var numPoints = this.pointContainer.getNumChildren();
897         if (numPoints > 2) {
898                 var g = this.fillShape.graphics;
899                 g.clear();
900                 g.setStrokeStyle(0);
901                 g.moveTo(this.pointContainer.getChildAt(0).x, this.pointContainer.getChildAt(0).y);
902                 g.beginStroke();
903                 g.beginFill(this.fillColor);
904                 for (var i=1; i<numPoints; ++i) {
905                         g.lineTo(this.pointContainer.getChildAt(i).x, this.pointContainer.getChildAt(i).y);
906                 }
907                 g.closePath();
908                 g.endFill();
909                 g.endStroke();
910         }
911         else {
912                 this.fillShape.graphics.clear();
913         }
914 };
915
916
917 ROS2D.PolygonMarker.prototype.__proto__ = createjs.Container.prototype;
918
919 /**
920  * @author Bart van Vliet - bart@dobots.nl
921  */
922
923 /**
924  * A trace of poses, handy to see where a robot has been
925  *
926  * @constructor
927  * @param options - object with following keys:
928  *   * pose (optional) - the first pose of the trace
929  *   * strokeSize (optional) - the size of the outline
930  *   * strokeColor (optional) - the createjs color for the stroke
931  *   * maxPoses (optional) - the maximum number of poses to keep, 0 for infinite
932  *   * minDist (optional) - the minimal distance between poses to use the pose for drawing (default 0.05)
933  */
934 ROS2D.TraceShape = function(options) {
935 //      var that = this;
936         options = options || {};
937         var pose = options.pose;
938         this.strokeSize = options.strokeSize || 3;
939         this.strokeColor = options.strokeColor || createjs.Graphics.getRGB(0, 0, 0);
940         this.maxPoses = options.maxPoses || 100;
941         this.minDist = options.minDist || 0.05;
942         
943         // Store minDist as the square of it
944         this.minDist = this.minDist*this.minDist;
945         
946         // Array of the poses
947         // TODO: do we need this?
948         this.poses = [];
949         
950         // Create the graphics
951         this.graphics = new createjs.Graphics();
952         this.graphics.setStrokeStyle(this.strokeSize);
953         this.graphics.beginStroke(this.strokeColor);
954         
955         // Add first pose if given
956         if (pose !== null && typeof pose !== 'undefined') {
957                 this.poses.push(pose);
958         }
959         
960         // Create the shape
961         createjs.Shape.call(this, this.graphics);
962 };
963
964 /**
965  * Adds a pose to the trace and updates the graphics
966  *
967  * @param pose of type ROSLIB.Pose
968  */
969 ROS2D.TraceShape.prototype.addPose = function(pose) {
970         var last = this.poses.length-1;
971         if (last < 0) {
972                 this.poses.push(pose);
973                 this.graphics.moveTo(pose.position.x / this.scaleX, pose.position.y / -this.scaleY);
974         }
975         else {
976                 var prevX = this.poses[last].position.x;
977                 var prevY = this.poses[last].position.y;
978                 var dx = (pose.position.x - prevX);
979                 var dy = (pose.position.y - prevY);
980                 if (dx*dx + dy*dy > this.minDist) {
981                         this.graphics.lineTo(pose.position.x / this.scaleX, pose.position.y / -this.scaleY);
982                         this.poses.push(pose);
983                 }
984         }
985         if (this.maxPoses > 0 && this.maxPoses < this.poses.length) {
986                 this.popFront();
987         }
988 };
989
990 /**
991  * Removes front pose and updates the graphics
992  */
993 ROS2D.TraceShape.prototype.popFront = function() {
994         if (this.poses.length > 0) {
995                 this.poses.shift();
996                 // TODO: shift drawing instructions rather than doing it all over
997                 this.graphics.clear();
998                 this.graphics.setStrokeStyle(this.strokeSize);
999                 this.graphics.beginStroke(this.strokeColor);
1000                 this.graphics.lineTo(this.poses[0].position.x / this.scaleX, this.poses[0].position.y / -this.scaleY);
1001                 for (var i=1; i<this.poses.length; ++i) {
1002                         this.graphics.lineTo(this.poses[i].position.x / this.scaleX, this.poses[i].position.y / -this.scaleY);
1003                 }
1004         }
1005 };
1006
1007 ROS2D.TraceShape.prototype.__proto__ = createjs.Shape.prototype;
1008
1009 /**
1010  * @author Bart van Vliet - bart@dobots.nl
1011  */
1012
1013 /**
1014  * Adds panning to a view
1015  *
1016  * @constructor
1017  * @param options - object with following keys:
1018  *   * rootObject (optional) - the root object to apply panning to
1019  */
1020 ROS2D.PanView = function(options) {
1021         options = options || {};
1022         this.rootObject = options.rootObject;
1023         
1024         // get a handle to the stage
1025         if (this.rootObject instanceof createjs.Stage) {
1026                 this.stage = this.rootObject;
1027         }
1028         else {
1029                 this.stage = this.rootObject.getStage();
1030         }
1031         
1032         this.startPos = new ROSLIB.Vector3();
1033 };
1034
1035
1036 ROS2D.PanView.prototype.startPan = function(startX, startY) {
1037         this.startPos.x = startX;
1038         this.startPos.y = startY;
1039 };
1040
1041 ROS2D.PanView.prototype.pan = function(curX, curY) {
1042         this.stage.x += curX - this.startPos.x;
1043         this.startPos.x = curX;
1044         this.stage.y += curY - this.startPos.y;
1045         this.startPos.y = curY;
1046 };
1047
1048 /**
1049  * @author Russell Toris - rctoris@wpi.edu
1050  */
1051
1052 /**
1053  * A Viewer can be used to render an interactive 2D scene to a HTML5 canvas.
1054  *
1055  * @constructor
1056  * @param options - object with following keys:
1057  *   * divID - the ID of the div to place the viewer in
1058  *   * width - the initial width, in pixels, of the canvas
1059  *   * height - the initial height, in pixels, of the canvas
1060  *   * background (optional) - the color to render the background, like '#efefef'
1061  */
1062 ROS2D.Viewer = function(options) {
1063   var that = this;
1064   options = options || {};
1065   var divID = options.divID;
1066   this.width = options.width;
1067   this.height = options.height;
1068   var background = options.background || '#111111';
1069
1070   // create the canvas to render to
1071   var canvas = document.createElement('canvas');
1072   canvas.width = this.width;
1073   canvas.height = this.height;
1074   canvas.style.background = background;
1075   document.getElementById(divID).appendChild(canvas);
1076   // create the easel to use
1077   this.scene = new createjs.Stage(canvas);
1078
1079   // change Y axis center
1080   this.scene.y = this.height;
1081
1082   // add the renderer to the page
1083   document.getElementById(divID).appendChild(canvas);
1084
1085   // update at 30fps
1086   createjs.Ticker.setFPS(30);
1087   createjs.Ticker.addEventListener('tick', this.scene);
1088 };
1089
1090 /**
1091  * Add the given createjs object to the global scene in the viewer.
1092  *
1093  * @param object - the object to add
1094  */
1095 ROS2D.Viewer.prototype.addObject = function(object) {
1096   this.scene.addChild(object);
1097 };
1098
1099 /**
1100  * Scale the scene to fit the given width and height into the current canvas.
1101  *
1102  * @param width - the width to scale to in meters
1103  * @param height - the height to scale to in meters
1104  */
1105 ROS2D.Viewer.prototype.scaleToDimensions = function(width, height) {
1106   // restore to values before shifting, if ocurred
1107   this.scene.x = typeof this.scene.x_prev_shift !== 'undefined' ? this.scene.x_prev_shift : this.scene.x;
1108   this.scene.y = typeof this.scene.y_prev_shift !== 'undefined' ? this.scene.y_prev_shift : this.scene.y;
1109   
1110   // save scene scaling
1111   this.scene.scaleX = this.width / width;
1112   this.scene.scaleY = this.height / height;
1113 };
1114
1115 /**
1116  * Shift the main view of the canvas by the given amount. This is based on the
1117  * ROS coordinate system. That is, Y is opposite that of a traditional canvas.
1118  *
1119  * @param x - the amount to shift by in the x direction in meters
1120  * @param y - the amount to shift by in the y direction in meters
1121  */
1122 ROS2D.Viewer.prototype.shift = function(x, y) {
1123   // save current offset
1124   this.scene.x_prev_shift = this.scene.x;
1125   this.scene.y_prev_shift = this.scene.y;
1126
1127   // shift scene by scaling the desired offset
1128   this.scene.x -= (x * this.scene.scaleX);
1129   this.scene.y += (y * this.scene.scaleY);
1130 };
1131
1132 /**
1133  * @author Bart van Vliet - bart@dobots.nl
1134  */
1135
1136 /**
1137  * Adds zooming to a view
1138  *
1139  * @constructor
1140  * @param options - object with following keys:
1141  *   * rootObject (optional) - the root object to apply zoom to
1142  *   * minScale (optional) - minimum scale to set to preserve precision
1143  */
1144 ROS2D.ZoomView = function(options) {
1145         options = options || {};
1146         this.rootObject = options.rootObject;
1147         this.minScale = options.minScale || 0.001;
1148         
1149         // get a handle to the stage
1150         if (this.rootObject instanceof createjs.Stage) {
1151                 this.stage = this.rootObject;
1152         }
1153         else {
1154                 this.stage = this.rootObject.getStage();
1155         }
1156         
1157         this.center = new ROSLIB.Vector3();
1158         this.startShift = new ROSLIB.Vector3();
1159         this.startScale = new ROSLIB.Vector3();
1160 };
1161
1162
1163 ROS2D.ZoomView.prototype.startZoom = function(centerX, centerY) {
1164         this.center.x = centerX;
1165         this.center.y = centerY;
1166         this.startShift.x = this.stage.x;
1167         this.startShift.y = this.stage.y;
1168         this.startScale.x = this.stage.scaleX;
1169         this.startScale.y = this.stage.scaleY;
1170 };
1171
1172 ROS2D.ZoomView.prototype.zoom = function(zoom) {
1173         // Make sure scale doesn't become too small
1174         if (this.startScale.x*zoom < this.minScale) {
1175                 zoom = this.minScale/this.startScale.x;
1176         }
1177         if (this.startScale.y*zoom < this.minScale) {
1178                 zoom = this.minScale/this.startScale.y;
1179         }
1180         
1181         this.stage.scaleX = this.startScale.x*zoom;
1182         this.stage.scaleY = this.startScale.y*zoom;
1183         
1184         this.stage.x = this.startShift.x - (this.center.x-this.startShift.x) * (this.stage.scaleX/this.startScale.x - 1);
1185         this.stage.y = this.startShift.y - (this.center.y-this.startShift.y) * (this.stage.scaleY/this.startScale.y - 1);
1186 };