+/**
+ * @author Russell Toris - rctoris@wpi.edu
+ */
+
+var ROS2D = ROS2D || {
+ REVISION : '0.9.0'
+};
+
+// convert the given global Stage coordinates to ROS coordinates
+createjs.Stage.prototype.globalToRos = function(x, y) {
+ var rosX = (x - this.x) / this.scaleX;
+ var rosY = (this.y - y) / this.scaleY;
+ return new ROSLIB.Vector3({
+ x : rosX,
+ y : rosY
+ });
+};
+
+// convert the given ROS coordinates to global Stage coordinates
+createjs.Stage.prototype.rosToGlobal = function(pos) {
+ var x = pos.x * this.scaleX + this.x;
+ var y = pos.y * this.scaleY + this.y;
+ return {
+ x : x,
+ y : y
+ };
+};
+
+// convert a ROS quaternion to theta in degrees
+createjs.Stage.prototype.rosQuaternionToGlobalTheta = function(orientation) {
+ // See https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Rotation_matrices
+ // here we use [x y z] = R * [1 0 0]
+ var q0 = orientation.w;
+ var q1 = orientation.x;
+ var q2 = orientation.y;
+ var q3 = orientation.z;
+ // Canvas rotation is clock wise and in degrees
+ return -Math.atan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2 * q2 + q3 * q3)) * 180.0 / Math.PI;
+};
+
+/**
+ * @author Russell Toris - rctoris@wpi.edu
+ */
+
+/**
+ * An image map is a PNG image scaled to fit to the dimensions of a OccupancyGrid.
+ *
+ * @constructor
+ * @param options - object with following keys:
+ * * message - the occupancy grid map meta data message
+ * * image - the image URL to load
+ */
+ROS2D.ImageMap = function(options) {
+ options = options || {};
+ var message = options.message;
+ var image = options.image;
+
+ // save the metadata we need
+ this.pose = new ROSLIB.Pose({
+ position : message.origin.position,
+ orientation : message.origin.orientation
+ });
+
+ // set the size
+ this.width = message.width;
+ this.height = message.height;
+
+ // create the bitmap
+ createjs.Bitmap.call(this, image);
+ // change Y direction
+ this.y = -this.height * message.resolution;
+
+ // scale the image
+ this.scaleX = message.resolution;
+ this.scaleY = message.resolution;
+ this.width *= this.scaleX;
+ this.height *= this.scaleY;
+
+ // set the pose
+ this.x += this.pose.position.x;
+ this.y -= this.pose.position.y;
+};
+ROS2D.ImageMap.prototype.__proto__ = createjs.Bitmap.prototype;
+
+/**
+ * @author Russell Toris - rctoris@wpi.edu
+ */
+
+/**
+ * A image map is a PNG image scaled to fit to the dimensions of a OccupancyGrid.
+ *
+ * Emits the following events:
+ * * 'change' - there was an update or change in the map
+ *
+ * @constructor
+ * @param options - object with following keys:
+ * * ros - the ROSLIB.Ros connection handle
+ * * topic (optional) - the map meta data topic to listen to
+ * * image - the image URL to load
+ * * rootObject (optional) - the root object to add this marker to
+ */
+ROS2D.ImageMapClient = function(options) {
+ var that = this;
+ options = options || {};
+ var ros = options.ros;
+ var topic = options.topic || '/map_metadata';
+ this.image = options.image;
+ this.rootObject = options.rootObject || new createjs.Container();
+
+ // create an empty shape to start with
+ this.currentImage = new createjs.Shape();
+
+ // subscribe to the topic
+ var rosTopic = new ROSLIB.Topic({
+ ros : ros,
+ name : topic,
+ messageType : 'nav_msgs/MapMetaData'
+ });
+
+ rosTopic.subscribe(function(message) {
+ // we only need this once
+ rosTopic.unsubscribe();
+
+ // create the image
+ that.currentImage = new ROS2D.ImageMap({
+ message : message,
+ image : that.image
+ });
+ that.rootObject.addChild(that.currentImage);
+ // work-around for a bug in easeljs -- needs a second object to render correctly
+ that.rootObject.addChild(new ROS2D.Grid({size:1}));
+
+ that.emit('change');
+ });
+};
+ROS2D.ImageMapClient.prototype.__proto__ = EventEmitter2.prototype;
+
+/**
+ * @author Russell Toris - rctoris@wpi.edu
+ */
+
+/**
+ * An OccupancyGrid can convert a ROS occupancy grid message into a createjs Bitmap object.
+ *
+ * @constructor
+ * @param options - object with following keys:
+ * * message - the occupancy grid message
+ */
+ROS2D.OccupancyGrid = function(options) {
+ options = options || {};
+ var message = options.message;
+
+ // internal drawing canvas
+ var canvas = document.createElement('canvas');
+ var context = canvas.getContext('2d');
+
+ // save the metadata we need
+ this.pose = new ROSLIB.Pose({
+ position : message.info.origin.position,
+ orientation : message.info.origin.orientation
+ });
+
+ // set the size
+ this.width = message.info.width;
+ this.height = message.info.height;
+ canvas.width = this.width;
+ canvas.height = this.height;
+
+ var imageData = context.createImageData(this.width, this.height);
+ for ( var row = 0; row < this.height; row++) {
+ for ( var col = 0; col < this.width; col++) {
+ // determine the index into the map data
+ var mapI = col + ((this.height - row - 1) * this.width);
+ // determine the value
+ var data = message.data[mapI];
+ var val;
+ if (data === 100) {
+ val = 0;
+ } else if (data === 0) {
+ val = 255;
+ } else {
+ val = 127;
+ }
+
+ // determine the index into the image data array
+ var i = (col + (row * this.width)) * 4;
+ // r
+ imageData.data[i] = val;
+ // g
+ imageData.data[++i] = val;
+ // b
+ imageData.data[++i] = val;
+ // a
+ imageData.data[++i] = 255;
+ }
+ }
+ context.putImageData(imageData, 0, 0);
+
+ // create the bitmap
+ createjs.Bitmap.call(this, canvas);
+ // change Y direction
+ this.y = -this.height * message.info.resolution;
+
+ // scale the image
+ this.scaleX = message.info.resolution;
+ this.scaleY = message.info.resolution;
+ this.width *= this.scaleX;
+ this.height *= this.scaleY;
+
+ // set the pose
+ this.x += this.pose.position.x;
+ this.y -= this.pose.position.y;
+};
+ROS2D.OccupancyGrid.prototype.__proto__ = createjs.Bitmap.prototype;
+
+/**
+ * @author Russell Toris - rctoris@wpi.edu
+ */
+
+/**
+ * A map that listens to a given occupancy grid topic.
+ *
+ * Emits the following events:
+ * * 'change' - there was an update or change in the map
+ *
+ * @constructor
+ * @param options - object with following keys:
+ * * ros - the ROSLIB.Ros connection handle
+ * * topic (optional) - the map topic to listen to
+ * * rootObject (optional) - the root object to add this marker to
+ * * continuous (optional) - if the map should be continuously loaded (e.g., for SLAM)
+ */
+ROS2D.OccupancyGridClient = function(options) {
+ var that = this;
+ options = options || {};
+ var ros = options.ros;
+ var topic = options.topic || '/map';
+ this.continuous = options.continuous;
+ this.rootObject = options.rootObject || new createjs.Container();
+
+ // current grid that is displayed
+ // create an empty shape to start with, so that the order remains correct.
+ this.currentGrid = new createjs.Shape();
+ this.rootObject.addChild(this.currentGrid);
+ // work-around for a bug in easeljs -- needs a second object to render correctly
+ this.rootObject.addChild(new ROS2D.Grid({size:1}));
+
+ // subscribe to the topic
+ var rosTopic = new ROSLIB.Topic({
+ ros : ros,
+ name : topic,
+ messageType : 'nav_msgs/OccupancyGrid',
+ compression : 'png'
+ });
+
+ rosTopic.subscribe(function(message) {
+ // check for an old map
+ var index = null;
+ if (that.currentGrid) {
+ index = that.rootObject.getChildIndex(that.currentGrid);
+ that.rootObject.removeChild(that.currentGrid);
+ }
+
+ that.currentGrid = new ROS2D.OccupancyGrid({
+ message : message
+ });
+ if (index !== null) {
+ that.rootObject.addChildAt(that.currentGrid, index);
+ }
+ else {
+ that.rootObject.addChild(that.currentGrid);
+ }
+
+ that.emit('change');
+
+ // check if we should unsubscribe
+ if (!that.continuous) {
+ rosTopic.unsubscribe();
+ }
+ });
+};
+ROS2D.OccupancyGridClient.prototype.__proto__ = EventEmitter2.prototype;
+
+/**
+ * @author Jihoon Lee- jihoonlee.in@gmail.com
+ * @author Russell Toris - rctoris@wpi.edu
+ */
+
+/**
+ * A static map that receives from map_server.
+ *
+ * Emits the following events:
+ * * 'change' - there was an update or change in the map
+ *
+ * @constructor
+ * @param options - object with following keys:
+ * * ros - the ROSLIB.Ros connection handle
+ * * service (optional) - the map topic to listen to, like '/static_map'
+ * * rootObject (optional) - the root object to add this marker to
+ */
+ROS2D.OccupancyGridSrvClient = function(options) {
+ var that = this;
+ options = options || {};
+ var ros = options.ros;
+ var service = options.service || '/static_map';
+ this.rootObject = options.rootObject || new createjs.Container();
+
+ // current grid that is displayed
+ this.currentGrid = null;
+
+ // Setting up to the service
+ var rosService = new ROSLIB.Service({
+ ros : ros,
+ name : service,
+ serviceType : 'nav_msgs/GetMap',
+ compression : 'png'
+ });
+
+ rosService.callService(new ROSLIB.ServiceRequest(),function(response) {
+ // check for an old map
+ if (that.currentGrid) {
+ that.rootObject.removeChild(that.currentGrid);
+ }
+
+ that.currentGrid = new ROS2D.OccupancyGrid({
+ message : response.map
+ });
+ that.rootObject.addChild(that.currentGrid);
+
+ that.emit('change', that.currentGrid);
+ });
+};
+ROS2D.OccupancyGridSrvClient.prototype.__proto__ = EventEmitter2.prototype;
+
+/**
+ * @author Bart van Vliet - bart@dobots.nl
+ */
+
+/**
+ * An arrow with line and triangular head, based on the navigation arrow.
+ * Aims to the left at 0 rotation, as would be expected.
+ *
+ * @constructor
+ * @param options - object with following keys:
+ * * size (optional) - the size of the marker
+ * * strokeSize (optional) - the size of the outline
+ * * strokeColor (optional) - the createjs color for the stroke
+ * * fillColor (optional) - the createjs color for the fill
+ * * pulse (optional) - if the marker should "pulse" over time
+ */
+ROS2D.ArrowShape = function(options) {
+ var that = this;
+ options = options || {};
+ var size = options.size || 10;
+ var strokeSize = options.strokeSize || 3;
+ var strokeColor = options.strokeColor || createjs.Graphics.getRGB(0, 0, 0);
+ var fillColor = options.fillColor || createjs.Graphics.getRGB(255, 0, 0);
+ var pulse = options.pulse;
+
+ // draw the arrow
+ var graphics = new createjs.Graphics();
+
+ var headLen = size / 3.0;
+ var headWidth = headLen * 2.0 / 3.0;
+
+ graphics.setStrokeStyle(strokeSize);
+ graphics.beginStroke(strokeColor);
+ graphics.moveTo(0, 0);
+ graphics.lineTo(size-headLen, 0);
+
+ graphics.beginFill(fillColor);
+ graphics.moveTo(size, 0);
+ graphics.lineTo(size-headLen, headWidth / 2.0);
+ graphics.lineTo(size-headLen, -headWidth / 2.0);
+ graphics.closePath();
+ graphics.endFill();
+ graphics.endStroke();
+
+ // create the shape
+ createjs.Shape.call(this, graphics);
+
+ // check if we are pulsing
+ if (pulse) {
+ // have the model "pulse"
+ var growCount = 0;
+ var growing = true;
+ createjs.Ticker.addEventListener('tick', function() {
+ if (growing) {
+ that.scaleX *= 1.035;
+ that.scaleY *= 1.035;
+ growing = (++growCount < 10);
+ } else {
+ that.scaleX /= 1.035;
+ that.scaleY /= 1.035;
+ growing = (--growCount < 0);
+ }
+ });
+ }
+};
+ROS2D.ArrowShape.prototype.__proto__ = createjs.Shape.prototype;
+
+/**
+ * @author Raffaello Bonghi - raffaello.bonghi@officinerobotiche.it
+ */
+
+/**
+ * A Grid object draw in map.
+ *
+ * @constructor
+ * @param options - object with following keys:
+ * * size (optional) - the size of the grid
+ * * cellSize (optional) - the cell size of map
+ * * lineWidth (optional) - the width of the lines in the grid
+ */
+ ROS2D.Grid = function(options) {
+ var that = this;
+ options = options || {};
+ var size = options.size || 10;
+ var cellSize = options.cellSize || 0.1;
+ var lineWidth = options.lineWidth || 0.001;
+ // draw the arrow
+ var graphics = new createjs.Graphics();
+ // line width
+ graphics.setStrokeStyle(lineWidth*5);
+ graphics.beginStroke(createjs.Graphics.getRGB(0, 0, 0));
+ graphics.beginFill(createjs.Graphics.getRGB(255, 0, 0));
+ graphics.moveTo(-size*cellSize, 0);
+ graphics.lineTo(size*cellSize, 0);
+ graphics.moveTo(0, -size*cellSize);
+ graphics.lineTo(0, size*cellSize);
+ graphics.endFill();
+ graphics.endStroke();
+
+ graphics.setStrokeStyle(lineWidth);
+ graphics.beginStroke(createjs.Graphics.getRGB(0, 0, 0));
+ graphics.beginFill(createjs.Graphics.getRGB(255, 0, 0));
+ for (var i = -size; i <= size; i++) {
+ graphics.moveTo(-size*cellSize, i * cellSize);
+ graphics.lineTo(size*cellSize, i * cellSize);
+ graphics.moveTo(i * cellSize, -size*cellSize);
+ graphics.lineTo(i * cellSize, size*cellSize);
+ }
+ graphics.endFill();
+ graphics.endStroke();
+ // create the shape
+ createjs.Shape.call(this, graphics);
+
+};
+ROS2D.Grid.prototype.__proto__ = createjs.Shape.prototype;
+
+/**
+ * @author Russell Toris - rctoris@wpi.edu
+ */
+
+/**
+ * A navigation arrow is a directed triangle that can be used to display orientation.
+ *
+ * @constructor
+ * @param options - object with following keys:
+ * * size (optional) - the size of the marker
+ * * strokeSize (optional) - the size of the outline
+ * * strokeColor (optional) - the createjs color for the stroke
+ * * fillColor (optional) - the createjs color for the fill
+ * * pulse (optional) - if the marker should "pulse" over time
+ */
+ROS2D.NavigationArrow = function(options) {
+ var that = this;
+ options = options || {};
+ var size = options.size || 10;
+ var strokeSize = options.strokeSize || 3;
+ var strokeColor = options.strokeColor || createjs.Graphics.getRGB(0, 0, 0);
+ var fillColor = options.fillColor || createjs.Graphics.getRGB(255, 0, 0);
+ var pulse = options.pulse;
+
+ // draw the arrow
+ var graphics = new createjs.Graphics();
+ // line width
+ graphics.setStrokeStyle(strokeSize);
+ graphics.moveTo(-size / 2.0, -size / 2.0);
+ graphics.beginStroke(strokeColor);
+ graphics.beginFill(fillColor);
+ graphics.lineTo(size, 0);
+ graphics.lineTo(-size / 2.0, size / 2.0);
+ graphics.closePath();
+ graphics.endFill();
+ graphics.endStroke();
+
+ // create the shape
+ createjs.Shape.call(this, graphics);
+
+ // check if we are pulsing
+ if (pulse) {
+ // have the model "pulse"
+ var growCount = 0;
+ var growing = true;
+ createjs.Ticker.addEventListener('tick', function() {
+ if (growing) {
+ that.scaleX *= 1.035;
+ that.scaleY *= 1.035;
+ growing = (++growCount < 10);
+ } else {
+ that.scaleX /= 1.035;
+ that.scaleY /= 1.035;
+ growing = (--growCount < 0);
+ }
+ });
+ }
+};
+ROS2D.NavigationArrow.prototype.__proto__ = createjs.Shape.prototype;
+
+/**
+ * @author Inigo Gonzalez - ingonza85@gmail.com
+ */
+
+/**
+ * A navigation image that can be used to display orientation.
+ *
+ * @constructor
+ * @param options - object with following keys:
+ * * size (optional) - the size of the marker
+ * * image - the image to use as a marker
+ * * pulse (optional) - if the marker should "pulse" over time
+ */
+ROS2D.NavigationImage = function(options) {
+ var that = this;
+ options = options || {};
+ var size = options.size || 10;
+ var image_url = options.image;
+ var pulse = options.pulse;
+ var alpha = options.alpha || 1;
+
+ var originals = {};
+
+ var paintImage = function(){
+ createjs.Bitmap.call(that, image);
+ var scale = calculateScale(size);
+ that.alpha = alpha;
+ that.scaleX = scale;
+ that.scaleY = scale;
+ that.regY = that.image.height/2;
+ that.regX = that.image.width/2;
+ originals['rotation'] = that.rotation;
+ Object.defineProperty( that, 'rotation', {
+ get: function(){ return originals['rotation'] + 90; },
+ set: function(value){ originals['rotation'] = value; }
+ });
+ if (pulse) {
+ // have the model "pulse"
+ var growCount = 0;
+ var growing = true;
+ var SCALE_SIZE = 1.020;
+ createjs.Ticker.addEventListener('tick', function() {
+ if (growing) {
+ that.scaleX *= SCALE_SIZE;
+ that.scaleY *= SCALE_SIZE;
+ growing = (++growCount < 10);
+ } else {
+ that.scaleX /= SCALE_SIZE;
+ that.scaleY /= SCALE_SIZE;
+ growing = (--growCount < 0);
+ }
+ });
+ }
+ };
+
+ var calculateScale = function(_size){
+ return _size / image.width;
+ };
+
+ var image = new Image();
+ image.onload = paintImage;
+ image.src = image_url;
+
+};
+
+ROS2D.NavigationImage.prototype.__proto__ = createjs.Bitmap.prototype;
+
+/**
+ * @author Bart van Vliet - bart@dobots.nl
+ */
+
+/**
+ * A shape to draw a nav_msgs/Path msg
+ *
+ * @constructor
+ * @param options - object with following keys:
+ * * path (optional) - the initial path to draw
+ * * strokeSize (optional) - the size of the outline
+ * * strokeColor (optional) - the createjs color for the stroke
+ */
+ROS2D.PathShape = function(options) {
+ options = options || {};
+ var path = options.path;
+ this.strokeSize = options.strokeSize || 3;
+ this.strokeColor = options.strokeColor || createjs.Graphics.getRGB(0, 0, 0);
+
+ // draw the line
+ this.graphics = new createjs.Graphics();
+
+ if (path !== null && typeof path !== 'undefined') {
+ this.graphics.setStrokeStyle(this.strokeSize);
+ this.graphics.beginStroke(this.strokeColor);
+ this.graphics.moveTo(path.poses[0].pose.position.x / this.scaleX, path.poses[0].pose.position.y / -this.scaleY);
+ for (var i=1; i<path.poses.length; ++i) {
+ this.graphics.lineTo(path.poses[i].pose.position.x / this.scaleX, path.poses[i].pose.position.y / -this.scaleY);
+ }
+ this.graphics.endStroke();
+ }
+
+ // create the shape
+ createjs.Shape.call(this, this.graphics);
+};
+
+/**
+ * Set the path to draw
+ *
+ * @param path of type nav_msgs/Path
+ */
+ROS2D.PathShape.prototype.setPath = function(path) {
+ this.graphics.clear();
+ if (path !== null && typeof path !== 'undefined') {
+ this.graphics.setStrokeStyle(this.strokeSize);
+ this.graphics.beginStroke(this.strokeColor);
+ this.graphics.moveTo(path.poses[0].pose.position.x / this.scaleX, path.poses[0].pose.position.y / -this.scaleY);
+ for (var i=1; i<path.poses.length; ++i) {
+ this.graphics.lineTo(path.poses[i].pose.position.x / this.scaleX, path.poses[i].pose.position.y / -this.scaleY);
+ }
+ this.graphics.endStroke();
+ }
+};
+
+ROS2D.PathShape.prototype.__proto__ = createjs.Shape.prototype;
+
+/**
+ * @author Bart van Vliet - bart@dobots.nl
+ */
+
+/**
+ * A polygon that can be edited by an end user
+ *
+ * @constructor
+ * @param options - object with following keys:
+ * * pose (optional) - the first pose of the trace
+ * * lineSize (optional) - the width of the lines
+ * * lineColor (optional) - the createjs color of the lines
+ * * pointSize (optional) - the size of the points
+ * * pointColor (optional) - the createjs color of the points
+ * * fillColor (optional) - the createjs color to fill the polygon
+ * * lineCallBack (optional) - callback function for mouse interaction with a line
+ * * pointCallBack (optional) - callback function for mouse interaction with a point
+ */
+ROS2D.PolygonMarker = function(options) {
+// var that = this;
+ options = options || {};
+ this.lineSize = options.lineSize || 3;
+ this.lineColor = options.lineColor || createjs.Graphics.getRGB(0, 0, 255, 0.66);
+ this.pointSize = options.pointSize || 10;
+ this.pointColor = options.pointColor || createjs.Graphics.getRGB(255, 0, 0, 0.66);
+ this.fillColor = options.pointColor || createjs.Graphics.getRGB(0, 255, 0, 0.33);
+ this.lineCallBack = options.lineCallBack;
+ this.pointCallBack = options.pointCallBack;
+
+ // Array of point shapes
+// this.points = [];
+ this.pointContainer = new createjs.Container();
+
+ // Array of line shapes
+// this.lines = [];
+ this.lineContainer = new createjs.Container();
+
+ this.fillShape = new createjs.Shape();
+
+ // Container with all the lines and points
+ createjs.Container.call(this);
+
+ this.addChild(this.fillShape);
+ this.addChild(this.lineContainer);
+ this.addChild(this.pointContainer);
+};
+
+/**
+ * Internal use only
+ */
+ROS2D.PolygonMarker.prototype.createLineShape = function(startPoint, endPoint) {
+ var line = new createjs.Shape();
+// line.graphics.setStrokeStyle(this.strokeSize);
+// line.graphics.beginStroke(this.strokeColor);
+// line.graphics.moveTo(startPoint.x, startPoint.y);
+// line.graphics.lineTo(endPoint.x, endPoint.y);
+ this.editLineShape(line, startPoint, endPoint);
+
+ var that = this;
+ line.addEventListener('mousedown', function(event) {
+ if (that.lineCallBack !== null && typeof that.lineCallBack !== 'undefined') {
+ that.lineCallBack('mousedown', event, that.lineContainer.getChildIndex(event.target));
+ }
+ });
+
+ return line;
+};
+
+/**
+ * Internal use only
+ */
+ROS2D.PolygonMarker.prototype.editLineShape = function(line, startPoint, endPoint) {
+ line.graphics.clear();
+ line.graphics.setStrokeStyle(this.lineSize);
+ line.graphics.beginStroke(this.lineColor);
+ line.graphics.moveTo(startPoint.x, startPoint.y);
+ line.graphics.lineTo(endPoint.x, endPoint.y);
+};
+
+/**
+ * Internal use only
+ */
+ROS2D.PolygonMarker.prototype.createPointShape = function(pos) {
+ var point = new createjs.Shape();
+ point.graphics.beginFill(this.pointColor);
+ point.graphics.drawCircle(0, 0, this.pointSize);
+ point.x = pos.x;
+ point.y = -pos.y;
+
+ var that = this;
+ point.addEventListener('mousedown', function(event) {
+ if (that.pointCallBack !== null && typeof that.pointCallBack !== 'undefined') {
+ that.pointCallBack('mousedown', event, that.pointContainer.getChildIndex(event.target));
+ }
+ });
+
+ return point;
+};
+
+/**
+ * Adds a point to the polygon
+ *
+ * @param position of type ROSLIB.Vector3
+ */
+ROS2D.PolygonMarker.prototype.addPoint = function(pos) {
+ var point = this.createPointShape(pos);
+ this.pointContainer.addChild(point);
+ var numPoints = this.pointContainer.getNumChildren();
+
+ // 0 points -> 1 point, 0 lines
+ // 1 point -> 2 points, lines: add line between previous and new point, add line between new point and first point
+ // 2 points -> 3 points, 3 lines: change last line, add line between new point and first point
+ // 3 points -> 4 points, 4 lines: change last line, add line between new point and first point
+ // etc
+
+ if (numPoints < 2) {
+ // Now 1 point
+ }
+ else if (numPoints < 3) {
+ // Now 2 points: add line between previous and new point
+ var line = this.createLineShape(this.pointContainer.getChildAt(numPoints-2), point);
+ this.lineContainer.addChild(line);
+ }
+ if (numPoints > 2) {
+ // Now 3 or more points: change last line
+ this.editLineShape(this.lineContainer.getChildAt(numPoints-2), this.pointContainer.getChildAt(numPoints-2), point);
+ }
+ if (numPoints > 1) {
+ // Now 2 or more points: add line between new point and first point
+ var lineEnd = this.createLineShape(point, this.pointContainer.getChildAt(0));
+ this.lineContainer.addChild(lineEnd);
+ }
+
+ this.drawFill();
+};
+
+/**
+ * Removes a point from the polygon
+ *
+ * @param obj either an index (integer) or a point shape of the polygon
+ */
+ROS2D.PolygonMarker.prototype.remPoint = function(obj) {
+ var index;
+// var point;
+ if (obj instanceof createjs.Shape) {
+ index = this.pointContainer.getChildIndex(obj);
+// point = obj;
+ }
+ else {
+ index = obj;
+// point = this.pointContainer.getChildAt(index);
+ }
+
+ // 0 points -> 0 points, 0 lines
+ // 1 point -> 0 points, 0 lines
+ // 2 points -> 1 point, 0 lines: remove all lines
+ // 3 points -> 2 points, 2 lines: change line before point to remove, remove line after point to remove
+ // 4 points -> 3 points, 3 lines: change line before point to remove, remove line after point to remove
+ // etc
+
+ var numPoints = this.pointContainer.getNumChildren();
+
+ if (numPoints < 2) {
+
+ }
+ else if (numPoints < 3) {
+ // 2 points: remove all lines
+ this.lineContainer.removeAllChildren();
+ }
+ else {
+ // 3 or more points: change line before point to remove, remove line after point to remove
+ this.editLineShape(
+ this.lineContainer.getChildAt((index-1+numPoints)%numPoints),
+ this.pointContainer.getChildAt((index-1+numPoints)%numPoints),
+ this.pointContainer.getChildAt((index+1)%numPoints)
+ );
+ this.lineContainer.removeChildAt(index);
+ }
+ this.pointContainer.removeChildAt(index);
+// this.points.splice(index, 1);
+
+ this.drawFill();
+};
+
+/**
+ * Moves a point of the polygon
+ *
+ * @param obj either an index (integer) or a point shape of the polygon
+ * @param position of type ROSLIB.Vector3
+ */
+ROS2D.PolygonMarker.prototype.movePoint = function(obj, newPos) {
+ var index;
+ var point;
+ if (obj instanceof createjs.Shape) {
+ index = this.pointContainer.getChildIndex(obj);
+ point = obj;
+ }
+ else {
+ index = obj;
+ point = this.pointContainer.getChildAt(index);
+ }
+ point.x = newPos.x;
+ point.y = -newPos.y;
+
+ var numPoints = this.pointContainer.getNumChildren();
+ if (numPoints > 1) {
+ // line before moved point
+ var line1 = this.lineContainer.getChildAt((index-1+numPoints)%numPoints);
+ this.editLineShape(line1, this.pointContainer.getChildAt((index-1+numPoints)%numPoints), point);
+
+ // line after moved point
+ var line2 = this.lineContainer.getChildAt(index);
+ this.editLineShape(line2, point, this.pointContainer.getChildAt((index+1)%numPoints));
+ }
+
+ this.drawFill();
+};
+
+/**
+ * Splits a line of the polygon: inserts a point at the center of the line
+ *
+ * @param obj either an index (integer) or a line shape of the polygon
+ */
+ROS2D.PolygonMarker.prototype.splitLine = function(obj) {
+ var index;
+ var line;
+ if (obj instanceof createjs.Shape) {
+ index = this.lineContainer.getChildIndex(obj);
+ line = obj;
+ }
+ else {
+ index = obj;
+ line = this.lineContainer.getChildAt(index);
+ }
+ var numPoints = this.pointContainer.getNumChildren();
+ var xs = this.pointContainer.getChildAt(index).x;
+ var ys = this.pointContainer.getChildAt(index).y;
+ var xe = this.pointContainer.getChildAt((index+1)%numPoints).x;
+ var ye = this.pointContainer.getChildAt((index+1)%numPoints).y;
+ var xh = (xs+xe)/2.0;
+ var yh = (ys+ye)/2.0;
+ var pos = new ROSLIB.Vector3({ x:xh, y:-yh });
+
+ // Add a point in the center of the line to split
+ var point = this.createPointShape(pos);
+ this.pointContainer.addChildAt(point, index+1);
+ ++numPoints;
+
+ // Add a line between the new point and the end of the line to split
+ var lineNew = this.createLineShape(point, this.pointContainer.getChildAt((index+2)%numPoints));
+ this.lineContainer.addChildAt(lineNew, index+1);
+
+ // Set the endpoint of the line to split to the new point
+ this.editLineShape(line, this.pointContainer.getChildAt(index), point);
+
+ this.drawFill();
+};
+
+/**
+ * Internal use only
+ */
+ROS2D.PolygonMarker.prototype.drawFill = function() {
+ var numPoints = this.pointContainer.getNumChildren();
+ if (numPoints > 2) {
+ var g = this.fillShape.graphics;
+ g.clear();
+ g.setStrokeStyle(0);
+ g.moveTo(this.pointContainer.getChildAt(0).x, this.pointContainer.getChildAt(0).y);
+ g.beginStroke();
+ g.beginFill(this.fillColor);
+ for (var i=1; i<numPoints; ++i) {
+ g.lineTo(this.pointContainer.getChildAt(i).x, this.pointContainer.getChildAt(i).y);
+ }
+ g.closePath();
+ g.endFill();
+ g.endStroke();
+ }
+ else {
+ this.fillShape.graphics.clear();
+ }
+};
+
+
+ROS2D.PolygonMarker.prototype.__proto__ = createjs.Container.prototype;
+
+/**
+ * @author Bart van Vliet - bart@dobots.nl
+ */
+
+/**
+ * A trace of poses, handy to see where a robot has been
+ *
+ * @constructor
+ * @param options - object with following keys:
+ * * pose (optional) - the first pose of the trace
+ * * strokeSize (optional) - the size of the outline
+ * * strokeColor (optional) - the createjs color for the stroke
+ * * maxPoses (optional) - the maximum number of poses to keep, 0 for infinite
+ * * minDist (optional) - the minimal distance between poses to use the pose for drawing (default 0.05)
+ */
+ROS2D.TraceShape = function(options) {
+// var that = this;
+ options = options || {};
+ var pose = options.pose;
+ this.strokeSize = options.strokeSize || 3;
+ this.strokeColor = options.strokeColor || createjs.Graphics.getRGB(0, 0, 0);
+ this.maxPoses = options.maxPoses || 100;
+ this.minDist = options.minDist || 0.05;
+
+ // Store minDist as the square of it
+ this.minDist = this.minDist*this.minDist;
+
+ // Array of the poses
+ // TODO: do we need this?
+ this.poses = [];
+
+ // Create the graphics
+ this.graphics = new createjs.Graphics();
+ this.graphics.setStrokeStyle(this.strokeSize);
+ this.graphics.beginStroke(this.strokeColor);
+
+ // Add first pose if given
+ if (pose !== null && typeof pose !== 'undefined') {
+ this.poses.push(pose);
+ }
+
+ // Create the shape
+ createjs.Shape.call(this, this.graphics);
+};
+
+/**
+ * Adds a pose to the trace and updates the graphics
+ *
+ * @param pose of type ROSLIB.Pose
+ */
+ROS2D.TraceShape.prototype.addPose = function(pose) {
+ var last = this.poses.length-1;
+ if (last < 0) {
+ this.poses.push(pose);
+ this.graphics.moveTo(pose.position.x / this.scaleX, pose.position.y / -this.scaleY);
+ }
+ else {
+ var prevX = this.poses[last].position.x;
+ var prevY = this.poses[last].position.y;
+ var dx = (pose.position.x - prevX);
+ var dy = (pose.position.y - prevY);
+ if (dx*dx + dy*dy > this.minDist) {
+ this.graphics.lineTo(pose.position.x / this.scaleX, pose.position.y / -this.scaleY);
+ this.poses.push(pose);
+ }
+ }
+ if (this.maxPoses > 0 && this.maxPoses < this.poses.length) {
+ this.popFront();
+ }
+};
+
+/**
+ * Removes front pose and updates the graphics
+ */
+ROS2D.TraceShape.prototype.popFront = function() {
+ if (this.poses.length > 0) {
+ this.poses.shift();
+ // TODO: shift drawing instructions rather than doing it all over
+ this.graphics.clear();
+ this.graphics.setStrokeStyle(this.strokeSize);
+ this.graphics.beginStroke(this.strokeColor);
+ this.graphics.lineTo(this.poses[0].position.x / this.scaleX, this.poses[0].position.y / -this.scaleY);
+ for (var i=1; i<this.poses.length; ++i) {
+ this.graphics.lineTo(this.poses[i].position.x / this.scaleX, this.poses[i].position.y / -this.scaleY);
+ }
+ }
+};
+
+ROS2D.TraceShape.prototype.__proto__ = createjs.Shape.prototype;
+
+/**
+ * @author Bart van Vliet - bart@dobots.nl
+ */
+
+/**
+ * Adds panning to a view
+ *
+ * @constructor
+ * @param options - object with following keys:
+ * * rootObject (optional) - the root object to apply panning to
+ */
+ROS2D.PanView = function(options) {
+ options = options || {};
+ this.rootObject = options.rootObject;
+
+ // get a handle to the stage
+ if (this.rootObject instanceof createjs.Stage) {
+ this.stage = this.rootObject;
+ }
+ else {
+ this.stage = this.rootObject.getStage();
+ }
+
+ this.startPos = new ROSLIB.Vector3();
+};
+
+
+ROS2D.PanView.prototype.startPan = function(startX, startY) {
+ this.startPos.x = startX;
+ this.startPos.y = startY;
+};
+
+ROS2D.PanView.prototype.pan = function(curX, curY) {
+ this.stage.x += curX - this.startPos.x;
+ this.startPos.x = curX;
+ this.stage.y += curY - this.startPos.y;
+ this.startPos.y = curY;
+};
+
+/**
+ * @author Russell Toris - rctoris@wpi.edu
+ */
+
+/**
+ * A Viewer can be used to render an interactive 2D scene to a HTML5 canvas.
+ *
+ * @constructor
+ * @param options - object with following keys:
+ * * divID - the ID of the div to place the viewer in
+ * * width - the initial width, in pixels, of the canvas
+ * * height - the initial height, in pixels, of the canvas
+ * * background (optional) - the color to render the background, like '#efefef'
+ */
+ROS2D.Viewer = function(options) {
+ var that = this;
+ options = options || {};
+ var divID = options.divID;
+ this.width = options.width;
+ this.height = options.height;
+ var background = options.background || '#111111';
+
+ // create the canvas to render to
+ var canvas = document.createElement('canvas');
+ canvas.width = this.width;
+ canvas.height = this.height;
+ canvas.style.background = background;
+ document.getElementById(divID).appendChild(canvas);
+ // create the easel to use
+ this.scene = new createjs.Stage(canvas);
+
+ // change Y axis center
+ this.scene.y = this.height;
+
+ // add the renderer to the page
+ document.getElementById(divID).appendChild(canvas);
+
+ // update at 30fps
+ createjs.Ticker.setFPS(30);
+ createjs.Ticker.addEventListener('tick', this.scene);
+};
+
+/**
+ * Add the given createjs object to the global scene in the viewer.
+ *
+ * @param object - the object to add
+ */
+ROS2D.Viewer.prototype.addObject = function(object) {
+ this.scene.addChild(object);
+};
+
+/**
+ * Scale the scene to fit the given width and height into the current canvas.
+ *
+ * @param width - the width to scale to in meters
+ * @param height - the height to scale to in meters
+ */
+ROS2D.Viewer.prototype.scaleToDimensions = function(width, height) {
+ // restore to values before shifting, if ocurred
+ this.scene.x = typeof this.scene.x_prev_shift !== 'undefined' ? this.scene.x_prev_shift : this.scene.x;
+ this.scene.y = typeof this.scene.y_prev_shift !== 'undefined' ? this.scene.y_prev_shift : this.scene.y;
+
+ // save scene scaling
+ this.scene.scaleX = this.width / width;
+ this.scene.scaleY = this.height / height;
+};
+
+/**
+ * Shift the main view of the canvas by the given amount. This is based on the
+ * ROS coordinate system. That is, Y is opposite that of a traditional canvas.
+ *
+ * @param x - the amount to shift by in the x direction in meters
+ * @param y - the amount to shift by in the y direction in meters
+ */
+ROS2D.Viewer.prototype.shift = function(x, y) {
+ // save current offset
+ this.scene.x_prev_shift = this.scene.x;
+ this.scene.y_prev_shift = this.scene.y;
+
+ // shift scene by scaling the desired offset
+ this.scene.x -= (x * this.scene.scaleX);
+ this.scene.y += (y * this.scene.scaleY);
+};
+
+/**
+ * @author Bart van Vliet - bart@dobots.nl
+ */
+
+/**
+ * Adds zooming to a view
+ *
+ * @constructor
+ * @param options - object with following keys:
+ * * rootObject (optional) - the root object to apply zoom to
+ * * minScale (optional) - minimum scale to set to preserve precision
+ */
+ROS2D.ZoomView = function(options) {
+ options = options || {};
+ this.rootObject = options.rootObject;
+ this.minScale = options.minScale || 0.001;
+
+ // get a handle to the stage
+ if (this.rootObject instanceof createjs.Stage) {
+ this.stage = this.rootObject;
+ }
+ else {
+ this.stage = this.rootObject.getStage();
+ }
+
+ this.center = new ROSLIB.Vector3();
+ this.startShift = new ROSLIB.Vector3();
+ this.startScale = new ROSLIB.Vector3();
+};
+
+
+ROS2D.ZoomView.prototype.startZoom = function(centerX, centerY) {
+ this.center.x = centerX;
+ this.center.y = centerY;
+ this.startShift.x = this.stage.x;
+ this.startShift.y = this.stage.y;
+ this.startScale.x = this.stage.scaleX;
+ this.startScale.y = this.stage.scaleY;
+};
+
+ROS2D.ZoomView.prototype.zoom = function(zoom) {
+ // Make sure scale doesn't become too small
+ if (this.startScale.x*zoom < this.minScale) {
+ zoom = this.minScale/this.startScale.x;
+ }
+ if (this.startScale.y*zoom < this.minScale) {
+ zoom = this.minScale/this.startScale.y;
+ }
+
+ this.stage.scaleX = this.startScale.x*zoom;
+ this.stage.scaleY = this.startScale.y*zoom;
+
+ this.stage.x = this.startShift.x - (this.center.x-this.startShift.x) * (this.stage.scaleX/this.startScale.x - 1);
+ this.stage.y = this.startShift.y - (this.center.y-this.startShift.y) * (this.stage.scaleY/this.startScale.y - 1);
+};
projects / ros_wild_thumper.git / blobdiff