www/assets/javascripts/nav2d.js

   1 /**
   2  * @author Russell Toris - rctoris@wpi.edu
   3  * @author Lars Kunze - l.kunze@cs.bham.ac.uk
   4  */
   5
   6 var NAV2D = NAV2D || {
   7   REVISION : '0.3.0'
   8 };
   9
  10 /**
  11  * @author Russell Toris - rctoris@wpi.edu
  12  */
  13
  14 /**
  15  * A OccupancyGridClientNav uses an OccupancyGridClient to create a map for use with a Navigator.
  16  *
  17  * @constructor
  18  * @param options - object with following keys:
  19  *   * ros - the ROSLIB.Ros connection handle
  20  *   * topic (optional) - the map meta data topic to listen to
  21  *   * image - the URL of the image to render
  22  *   * serverName (optional) - the action server name to use for navigation, like '/move_base'
  23  *   * actionName (optional) - the navigation action name, like 'move_base_msgs/MoveBaseAction'
  24  *   * rootObject (optional) - the root object to add the click listeners to and render robot markers to
  25  *   * withOrientation (optional) - if the Navigator should consider the robot orientation (default: false)
  26  *   * viewer - the main viewer to render to
  27  */
  28 NAV2D.ImageMapClientNav = function(options) {
  29   var that = this;
  30   options = options || {};
  31   this.ros = options.ros;
  32   var topic = options.topic || '/map_metadata';
  33   var image = options.image;
  34   this.serverName = options.serverName || '/move_base';
  35   this.actionName = options.actionName || 'move_base_msgs/MoveBaseAction';
  36   this.rootObject = options.rootObject || new createjs.Container();
  37   this.viewer = options.viewer;
  38   this.withOrientation = options.withOrientation || false;
  39
  40   this.navigator = null;
  41
  42   // setup a client to get the map
  43   var client = new ROS2D.ImageMapClient({
  44     ros : this.ros,
  45     rootObject : this.rootObject,
  46     topic : topic,
  47     image : image
  48   });
  49   client.on('change', function() {
  50     that.navigator = new NAV2D.Navigator({
  51       ros : that.ros,
  52       serverName : that.serverName,
  53       actionName : that.actionName,
  54       rootObject : that.rootObject,
  55       withOrientation : that.withOrientation
  56     });
  57
  58     // scale the viewer to fit the map
  59     that.viewer.scaleToDimensions(client.currentImage.width, client.currentImage.height);
  60     that.viewer.shift(client.currentImage.pose.position.x, client.currentImage.pose.position.y);
  61   });
  62 };
  63
  64 /**
  65  * @author Russell Toris - rctoris@wpi.edu
  66  * @author Lars Kunze - l.kunze@cs.bham.ac.uk
  67  */
  68
  69 /**
  70  * A navigator can be used to add click-to-navigate options to an object. If
  71  * withOrientation is set to true, the user can also specify the orientation of
  72  * the robot by clicking at the goal position and pointing into the desired
  73  * direction (while holding the button pressed).
  74  *
  75  * @constructor
  76  * @param options - object with following keys:
  77  *   * ros - the ROSLIB.Ros connection handle
  78  *   * serverName (optional) - the action server name to use for navigation, like '/move_base'
  79  *   * actionName (optional) - the navigation action name, like 'move_base_msgs/MoveBaseAction'
  80  *   * rootObject (optional) - the root object to add the click listeners to and render robot markers to
  81  *   * withOrientation (optional) - if the Navigator should consider the robot orientation (default: false)
  82  */
  83 NAV2D.Navigator = function(options) {
  84   var that = this;
  85   options = options || {};
  86   var ros = options.ros;
  87   var serverName = options.serverName || '/move_base';
  88   var actionName = options.actionName || 'move_base_msgs/MoveBaseAction';
  89   var withOrientation = options.withOrientation || false;
  90   this.rootObject = options.rootObject || new createjs.Container();
  91
  92   // setup the actionlib client
  93   var actionClient = new ROSLIB.ActionClient({
  94     ros : ros,
  95     actionName : actionName,
  96     serverName : serverName
  97   });
  98
  99   /**
 100    * Send a goal to the navigation stack with the given pose.
 101    *
 102    * @param pose - the goal pose
 103    */
 104   function sendGoal(pose) {
 105     // create a goal
 106     var goal = new ROSLIB.Goal({
 107       actionClient : actionClient,
 108       goalMessage : {
 109         target_pose : {
 110           header : {
 111             frame_id : '/map'
 112           },
 113           pose : pose
 114         }
 115       }
 116     });
 117     goal.send();
 118
 119     // create a marker for the goal
 120     var goalMarker = new ROS2D.NavigationArrow({
 121       size : 15,
 122       strokeSize : 1,
 123       fillColor : createjs.Graphics.getRGB(255, 64, 128, 0.66),
 124       pulse : true
 125     });
 126     goalMarker.x = pose.position.x;
 127     goalMarker.y = -pose.position.y;
 128     goalMarker.rotation = stage.rosQuaternionToGlobalTheta(pose.orientation);
 129     goalMarker.scaleX = 1.0 / stage.scaleX;
 130     goalMarker.scaleY = 1.0 / stage.scaleY;
 131     that.rootObject.addChild(goalMarker);
 132
 133     goal.on('result', function() {
 134       that.rootObject.removeChild(goalMarker);
 135     });
 136   }
 137
 138   // get a handle to the stage
 139   var stage;
 140   if (that.rootObject instanceof createjs.Stage) {
 141     stage = that.rootObject;
 142   } else {
 143     stage = that.rootObject.getStage();
 144   }
 145
 146   // marker for the robot
 147   var robotMarker = new ROS2D.NavigationArrow({
 148     size : 25,
 149     strokeSize : 1,
 150     fillColor : createjs.Graphics.getRGB(255, 128, 0, 0.66),
 151     pulse : true
 152   });
 153   // wait for a pose to come in first
 154   robotMarker.visible = false;
 155   this.rootObject.addChild(robotMarker);
 156   var initScaleSet = false;
 157
 158   // setup a listener for the robot pose
 159   var poseListener = new ROSLIB.Topic({
 160     ros : ros,
 161     name : '/robot_pose',
 162     messageType : 'geometry_msgs/Pose',
 163     throttle_rate : 100
 164   });
 165   poseListener.subscribe(function(pose) {
 166     // update the robots position on the map
 167     robotMarker.x = pose.position.x;
 168     robotMarker.y = -pose.position.y;
 169     if (!initScaleSet) {
 170       robotMarker.scaleX = 1.0 / stage.scaleX;
 171       robotMarker.scaleY = 1.0 / stage.scaleY;
 172       initScaleSet = true;
 173     }
 174
 175     // change the angle
 176     robotMarker.rotation = stage.rosQuaternionToGlobalTheta(pose.orientation);
 177
 178     robotMarker.visible = true;
 179   });
 180
 181   if (withOrientation === false){
 182     // setup a double click listener (no orientation)
 183     this.rootObject.addEventListener('dblclick', function(event) {
 184       // convert to ROS coordinates
 185       var coords = stage.globalToRos(event.stageX, event.stageY);
 186       var pose = new ROSLIB.Pose({
 187         position : new ROSLIB.Vector3(coords)
 188       });
 189       // send the goal
 190       sendGoal(pose);
 191     });
 192   } else { // withOrientation === true
 193     // setup a click-and-point listener (with orientation)
 194     var position = null;
 195     var positionVec3 = null;
 196     var thetaRadians = 0;
 197     var thetaDegrees = 0;
 198     var orientationMarker = null;
 199     var mouseDown = false;
 200     var xDelta = 0;
 201     var yDelta = 0;
 202
 203     var mouseEventHandler = function(event, mouseState) {
 204
 205       if (mouseState === 'down'){
 206         // get position when mouse button is pressed down
 207         position = stage.globalToRos(event.stageX, event.stageY);
 208         positionVec3 = new ROSLIB.Vector3(position);
 209         mouseDown = true;
 210       }
 211       else if (mouseState === 'move'){
 212         // remove obsolete orientation marker
 213         that.rootObject.removeChild(orientationMarker);
 214
 215         if ( mouseDown === true) {
 216           // if mouse button is held down:
 217           // - get current mouse position
 218           // - calulate direction between stored <position> and current position
 219           // - place orientation marker
 220           var currentPos = stage.globalToRos(event.stageX, event.stageY);
 221           var currentPosVec3 = new ROSLIB.Vector3(currentPos);
 222
 223           orientationMarker = new ROS2D.NavigationArrow({
 224             size : 25,
 225             strokeSize : 1,
 226             fillColor : createjs.Graphics.getRGB(0, 255, 0, 0.66),
 227             pulse : false
 228           });
 229
 230           xDelta =  currentPosVec3.x - positionVec3.x;
 231           yDelta =  currentPosVec3.y - positionVec3.y;
 232
 233           thetaRadians  = Math.atan2(xDelta,yDelta);
 234
 235           thetaDegrees = thetaRadians * (180.0 / Math.PI);
 236
 237           if (thetaDegrees >= 0 && thetaDegrees <= 180) {
 238             thetaDegrees += 270;
 239           } else {
 240             thetaDegrees -= 90;
 241           }
 242
 243           orientationMarker.x =  positionVec3.x;
 244           orientationMarker.y = -positionVec3.y;
 245           orientationMarker.rotation = thetaDegrees;
 246           orientationMarker.scaleX = 1.0 / stage.scaleX;
 247           orientationMarker.scaleY = 1.0 / stage.scaleY;
 248
 249           that.rootObject.addChild(orientationMarker);
 250         }
 251       } else if (mouseDown) { // mouseState === 'up'
 252         // if mouse button is released
 253         // - get current mouse position (goalPos)
 254         // - calulate direction between stored <position> and goal position
 255         // - set pose with orientation
 256         // - send goal
 257         mouseDown = false;
 258
 259         var goalPos = stage.globalToRos(event.stageX, event.stageY);
 260
 261         var goalPosVec3 = new ROSLIB.Vector3(goalPos);
 262
 263         xDelta =  goalPosVec3.x - positionVec3.x;
 264         yDelta =  goalPosVec3.y - positionVec3.y;
 265
 266         thetaRadians  = Math.atan2(xDelta,yDelta);
 267
 268         if (thetaRadians >= 0 && thetaRadians <= Math.PI) {
 269           thetaRadians += (3 * Math.PI / 2);
 270         } else {
 271           thetaRadians -= (Math.PI/2);
 272         }
 273
 274         var qz =  Math.sin(-thetaRadians/2.0);
 275         var qw =  Math.cos(-thetaRadians/2.0);
 276
 277         var orientation = new ROSLIB.Quaternion({x:0, y:0, z:qz, w:qw});
 278
 279         var pose = new ROSLIB.Pose({
 280           position :    positionVec3,
 281           orientation : orientation
 282         });
 283         // send the goal
 284         sendGoal(pose);
 285       }
 286     };
 287
 288     this.rootObject.addEventListener('stagemousedown', function(event) {
 289       mouseEventHandler(event,'down');
 290     });
 291
 292     this.rootObject.addEventListener('stagemousemove', function(event) {
 293       mouseEventHandler(event,'move');
 294     });
 295
 296     this.rootObject.addEventListener('stagemouseup', function(event) {
 297       mouseEventHandler(event,'up');
 298     });
 299   }
 300 };
 301
 302 /**
 303  * @author Russell Toris - rctoris@wpi.edu
 304  */
 305
 306 /**
 307  * A OccupancyGridClientNav uses an OccupancyGridClient to create a map for use with a Navigator.
 308  *
 309  * @constructor
 310  * @param options - object with following keys:
 311  *   * ros - the ROSLIB.Ros connection handle
 312  *   * topic (optional) - the map topic to listen to
 313  *   * rootObject (optional) - the root object to add this marker to
 314  *   * continuous (optional) - if the map should be continuously loaded (e.g., for SLAM)
 315  *   * serverName (optional) - the action server name to use for navigation, like '/move_base'
 316  *   * actionName (optional) - the navigation action name, like 'move_base_msgs/MoveBaseAction'
 317  *   * rootObject (optional) - the root object to add the click listeners to and render robot markers to
 318  *   * withOrientation (optional) - if the Navigator should consider the robot orientation (default: false)
 319  *   * viewer - the main viewer to render to
 320  */
 321 NAV2D.OccupancyGridClientNav = function(options) {
 322   var that = this;
 323   options = options || {};
 324   this.ros = options.ros;
 325   var topic = options.topic || '/map';
 326   var continuous = options.continuous;
 327   this.serverName = options.serverName || '/move_base';
 328   this.actionName = options.actionName || 'move_base_msgs/MoveBaseAction';
 329   this.rootObject = options.rootObject || new createjs.Container();
 330   this.viewer = options.viewer;
 331   this.withOrientation = options.withOrientation || false;
 332
 333   this.navigator = null;
 334
 335   // setup a client to get the map
 336   var client = new ROS2D.OccupancyGridClient({
 337     ros : this.ros,
 338     rootObject : this.rootObject,
 339     continuous : continuous,
 340     topic : topic
 341   });
 342   client.on('change', function() {
 343     that.navigator = new NAV2D.Navigator({
 344       ros : that.ros,
 345       serverName : that.serverName,
 346       actionName : that.actionName,
 347       rootObject : that.rootObject,
 348       withOrientation : that.withOrientation
 349     });
 350
 351     // scale the viewer to fit the map
 352     that.viewer.scaleToDimensions(client.currentGrid.width, client.currentGrid.height);
 353     that.viewer.shift(client.currentGrid.pose.position.x, client.currentGrid.pose.position.y);
 354   });
 355 };