added panning, zooming, extra models, fixed rotations

src/Ros2D.js

@@ -10,28 +10,30 @@ var ROS2D = ROS2D || {
 createjs.Stage.prototype.globalToRos = function(x, y) {
   var rosX = (x - this.x) / this.scaleX;
   var rosY = (this.y - y) / this.scaleY;
-  return {
+  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) {
-  // convert to radians
+  // 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;
-  var theta = Math.atan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (Math.pow(q2, 2) + Math.pow(q3, 2)));
-
-  // convert to degrees
-  var deg = theta * (180.0 / Math.PI);
-  if (deg >= 0 && deg <= 180) {
-    deg += 270;
-  } else {
-    deg -= 90;
-  }
-
-  return -deg;
+  // 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;
 };

src/maps/OccupancyGridClient.js

@@ -24,7 +24,9 @@ ROS2D.OccupancyGridClient = function(options) {
   this.rootObject = options.rootObject || new createjs.Container();
 
   // current grid that is displayed
-  this.currentGrid = null;
+  // create an empty shape to start with, so that the order remains correct.
+  this.currentGrid = new createjs.Shape();
+  this.rootObject.addChild(this.currentGrid);
 
   // subscribe to the topic
   var rosTopic = new ROSLIB.Topic({
@@ -33,16 +35,24 @@ ROS2D.OccupancyGridClient = function(options) {
     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
     });
-    that.rootObject.addChild(that.currentGrid);
+    if (index !== null) {
+      that.rootObject.addChildAt(that.currentGrid, index);
+    }
+    else {
+      that.rootObject.addChild(that.currentGrid);
+    }
 
     that.emit('change');
 

src/models/ArrowShape.js

@@ -0,0 +1,66 @@
+/**
+ * @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;

src/maps/Grid.js → src/models/Grid.js

@@ -16,7 +16,7 @@
     options = options || {};
     var size = options.size || 10;
     var cellSize = options.cellSize || 0.1;
-    var lineWidth = options.lineWidth || 0.001; 
+    var lineWidth = options.lineWidth || 0.001;
     // draw the arrow
     var graphics = new createjs.Graphics();
     // line width

src/models/NavigationArrow.js

@@ -26,11 +26,10 @@ ROS2D.NavigationArrow = function(options) {
   var graphics = new createjs.Graphics();
   // line width
   graphics.setStrokeStyle(strokeSize);
-  graphics.moveTo(-size / 2.0, size / 2.0);
+  graphics.moveTo(-size / 2.0, -size / 2.0);
   graphics.beginStroke(strokeColor);
   graphics.beginFill(fillColor);
-  graphics.lineTo(0, -size);
-  graphics.lineTo(size / 2.0, size / 2.0);
+  graphics.lineTo(size, 0);
   graphics.lineTo(-size / 2.0, size / 2.0);
   graphics.closePath();
   graphics.endFill();

src/models/PathShape.js

@@ -0,0 +1,50 @@
+/**
+ * @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);
+};
+
+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;

src/models/TraceShape.js

@@ -0,0 +1,82 @@
+/**
+ * @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);
+};
+
+
+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();
+	}
+};
+
+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;

src/visualization/PanView.js

@@ -0,0 +1,38 @@
+/**
+ * @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;
+};

src/visualization/ZoomView.js

@@ -0,0 +1,55 @@
+/**
+ * @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);
+};