X-Git-Url: https://defiant.homedns.org/gitweb/?p=ros_wild_thumper.git;a=blobdiff_plain;f=www%2Fassets%2Fjavascripts%2Fnav2d.js;fp=www%2Fassets%2Fjavascripts%2Fnav2d.js;h=770eae7349405e3413d7a5f5a071ecae89a891f1;hp=0000000000000000000000000000000000000000;hb=01848a9a84b296519ca4b4f41906159fd4158213;hpb=e1f7a5148b9999b57a91c1ddb244397bf841de7e diff --git a/www/assets/javascripts/nav2d.js b/www/assets/javascripts/nav2d.js new file mode 100644 index 0000000..770eae7 --- /dev/null +++ b/www/assets/javascripts/nav2d.js @@ -0,0 +1,355 @@ +/** + * @author Russell Toris - rctoris@wpi.edu + * @author Lars Kunze - l.kunze@cs.bham.ac.uk + */ + +var NAV2D = NAV2D || { + REVISION : '0.3.0' +}; + +/** + * @author Russell Toris - rctoris@wpi.edu + */ + +/** + * A OccupancyGridClientNav uses an OccupancyGridClient to create a map for use with a Navigator. + * + * @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 URL of the image to render + * * serverName (optional) - the action server name to use for navigation, like '/move_base' + * * actionName (optional) - the navigation action name, like 'move_base_msgs/MoveBaseAction' + * * rootObject (optional) - the root object to add the click listeners to and render robot markers to + * * withOrientation (optional) - if the Navigator should consider the robot orientation (default: false) + * * viewer - the main viewer to render to + */ +NAV2D.ImageMapClientNav = function(options) { + var that = this; + options = options || {}; + this.ros = options.ros; + var topic = options.topic || '/map_metadata'; + var image = options.image; + this.serverName = options.serverName || '/move_base'; + this.actionName = options.actionName || 'move_base_msgs/MoveBaseAction'; + this.rootObject = options.rootObject || new createjs.Container(); + this.viewer = options.viewer; + this.withOrientation = options.withOrientation || false; + + this.navigator = null; + + // setup a client to get the map + var client = new ROS2D.ImageMapClient({ + ros : this.ros, + rootObject : this.rootObject, + topic : topic, + image : image + }); + client.on('change', function() { + that.navigator = new NAV2D.Navigator({ + ros : that.ros, + serverName : that.serverName, + actionName : that.actionName, + rootObject : that.rootObject, + withOrientation : that.withOrientation + }); + + // scale the viewer to fit the map + that.viewer.scaleToDimensions(client.currentImage.width, client.currentImage.height); + that.viewer.shift(client.currentImage.pose.position.x, client.currentImage.pose.position.y); + }); +}; + +/** + * @author Russell Toris - rctoris@wpi.edu + * @author Lars Kunze - l.kunze@cs.bham.ac.uk + */ + +/** + * A navigator can be used to add click-to-navigate options to an object. If + * withOrientation is set to true, the user can also specify the orientation of + * the robot by clicking at the goal position and pointing into the desired + * direction (while holding the button pressed). + * + * @constructor + * @param options - object with following keys: + * * ros - the ROSLIB.Ros connection handle + * * serverName (optional) - the action server name to use for navigation, like '/move_base' + * * actionName (optional) - the navigation action name, like 'move_base_msgs/MoveBaseAction' + * * rootObject (optional) - the root object to add the click listeners to and render robot markers to + * * withOrientation (optional) - if the Navigator should consider the robot orientation (default: false) + */ +NAV2D.Navigator = function(options) { + var that = this; + options = options || {}; + var ros = options.ros; + var serverName = options.serverName || '/move_base'; + var actionName = options.actionName || 'move_base_msgs/MoveBaseAction'; + var withOrientation = options.withOrientation || false; + this.rootObject = options.rootObject || new createjs.Container(); + + // setup the actionlib client + var actionClient = new ROSLIB.ActionClient({ + ros : ros, + actionName : actionName, + serverName : serverName + }); + + /** + * Send a goal to the navigation stack with the given pose. + * + * @param pose - the goal pose + */ + function sendGoal(pose) { + // create a goal + var goal = new ROSLIB.Goal({ + actionClient : actionClient, + goalMessage : { + target_pose : { + header : { + frame_id : '/map' + }, + pose : pose + } + } + }); + goal.send(); + + // create a marker for the goal + var goalMarker = new ROS2D.NavigationArrow({ + size : 15, + strokeSize : 1, + fillColor : createjs.Graphics.getRGB(255, 64, 128, 0.66), + pulse : true + }); + goalMarker.x = pose.position.x; + goalMarker.y = -pose.position.y; + goalMarker.rotation = stage.rosQuaternionToGlobalTheta(pose.orientation); + goalMarker.scaleX = 1.0 / stage.scaleX; + goalMarker.scaleY = 1.0 / stage.scaleY; + that.rootObject.addChild(goalMarker); + + goal.on('result', function() { + that.rootObject.removeChild(goalMarker); + }); + } + + // get a handle to the stage + var stage; + if (that.rootObject instanceof createjs.Stage) { + stage = that.rootObject; + } else { + stage = that.rootObject.getStage(); + } + + // marker for the robot + var robotMarker = new ROS2D.NavigationArrow({ + size : 25, + strokeSize : 1, + fillColor : createjs.Graphics.getRGB(255, 128, 0, 0.66), + pulse : true + }); + // wait for a pose to come in first + robotMarker.visible = false; + this.rootObject.addChild(robotMarker); + var initScaleSet = false; + + // setup a listener for the robot pose + var poseListener = new ROSLIB.Topic({ + ros : ros, + name : '/robot_pose', + messageType : 'geometry_msgs/Pose', + throttle_rate : 100 + }); + poseListener.subscribe(function(pose) { + // update the robots position on the map + robotMarker.x = pose.position.x; + robotMarker.y = -pose.position.y; + if (!initScaleSet) { + robotMarker.scaleX = 1.0 / stage.scaleX; + robotMarker.scaleY = 1.0 / stage.scaleY; + initScaleSet = true; + } + + // change the angle + robotMarker.rotation = stage.rosQuaternionToGlobalTheta(pose.orientation); + + robotMarker.visible = true; + }); + + if (withOrientation === false){ + // setup a double click listener (no orientation) + this.rootObject.addEventListener('dblclick', function(event) { + // convert to ROS coordinates + var coords = stage.globalToRos(event.stageX, event.stageY); + var pose = new ROSLIB.Pose({ + position : new ROSLIB.Vector3(coords) + }); + // send the goal + sendGoal(pose); + }); + } else { // withOrientation === true + // setup a click-and-point listener (with orientation) + var position = null; + var positionVec3 = null; + var thetaRadians = 0; + var thetaDegrees = 0; + var orientationMarker = null; + var mouseDown = false; + var xDelta = 0; + var yDelta = 0; + + var mouseEventHandler = function(event, mouseState) { + + if (mouseState === 'down'){ + // get position when mouse button is pressed down + position = stage.globalToRos(event.stageX, event.stageY); + positionVec3 = new ROSLIB.Vector3(position); + mouseDown = true; + } + else if (mouseState === 'move'){ + // remove obsolete orientation marker + that.rootObject.removeChild(orientationMarker); + + if ( mouseDown === true) { + // if mouse button is held down: + // - get current mouse position + // - calulate direction between stored and current position + // - place orientation marker + var currentPos = stage.globalToRos(event.stageX, event.stageY); + var currentPosVec3 = new ROSLIB.Vector3(currentPos); + + orientationMarker = new ROS2D.NavigationArrow({ + size : 25, + strokeSize : 1, + fillColor : createjs.Graphics.getRGB(0, 255, 0, 0.66), + pulse : false + }); + + xDelta = currentPosVec3.x - positionVec3.x; + yDelta = currentPosVec3.y - positionVec3.y; + + thetaRadians = Math.atan2(xDelta,yDelta); + + thetaDegrees = thetaRadians * (180.0 / Math.PI); + + if (thetaDegrees >= 0 && thetaDegrees <= 180) { + thetaDegrees += 270; + } else { + thetaDegrees -= 90; + } + + orientationMarker.x = positionVec3.x; + orientationMarker.y = -positionVec3.y; + orientationMarker.rotation = thetaDegrees; + orientationMarker.scaleX = 1.0 / stage.scaleX; + orientationMarker.scaleY = 1.0 / stage.scaleY; + + that.rootObject.addChild(orientationMarker); + } + } else if (mouseDown) { // mouseState === 'up' + // if mouse button is released + // - get current mouse position (goalPos) + // - calulate direction between stored and goal position + // - set pose with orientation + // - send goal + mouseDown = false; + + var goalPos = stage.globalToRos(event.stageX, event.stageY); + + var goalPosVec3 = new ROSLIB.Vector3(goalPos); + + xDelta = goalPosVec3.x - positionVec3.x; + yDelta = goalPosVec3.y - positionVec3.y; + + thetaRadians = Math.atan2(xDelta,yDelta); + + if (thetaRadians >= 0 && thetaRadians <= Math.PI) { + thetaRadians += (3 * Math.PI / 2); + } else { + thetaRadians -= (Math.PI/2); + } + + var qz = Math.sin(-thetaRadians/2.0); + var qw = Math.cos(-thetaRadians/2.0); + + var orientation = new ROSLIB.Quaternion({x:0, y:0, z:qz, w:qw}); + + var pose = new ROSLIB.Pose({ + position : positionVec3, + orientation : orientation + }); + // send the goal + sendGoal(pose); + } + }; + + this.rootObject.addEventListener('stagemousedown', function(event) { + mouseEventHandler(event,'down'); + }); + + this.rootObject.addEventListener('stagemousemove', function(event) { + mouseEventHandler(event,'move'); + }); + + this.rootObject.addEventListener('stagemouseup', function(event) { + mouseEventHandler(event,'up'); + }); + } +}; + +/** + * @author Russell Toris - rctoris@wpi.edu + */ + +/** + * A OccupancyGridClientNav uses an OccupancyGridClient to create a map for use with a Navigator. + * + * @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) + * * serverName (optional) - the action server name to use for navigation, like '/move_base' + * * actionName (optional) - the navigation action name, like 'move_base_msgs/MoveBaseAction' + * * rootObject (optional) - the root object to add the click listeners to and render robot markers to + * * withOrientation (optional) - if the Navigator should consider the robot orientation (default: false) + * * viewer - the main viewer to render to + */ +NAV2D.OccupancyGridClientNav = function(options) { + var that = this; + options = options || {}; + this.ros = options.ros; + var topic = options.topic || '/map'; + var continuous = options.continuous; + this.serverName = options.serverName || '/move_base'; + this.actionName = options.actionName || 'move_base_msgs/MoveBaseAction'; + this.rootObject = options.rootObject || new createjs.Container(); + this.viewer = options.viewer; + this.withOrientation = options.withOrientation || false; + + this.navigator = null; + + // setup a client to get the map + var client = new ROS2D.OccupancyGridClient({ + ros : this.ros, + rootObject : this.rootObject, + continuous : continuous, + topic : topic + }); + client.on('change', function() { + that.navigator = new NAV2D.Navigator({ + ros : that.ros, + serverName : that.serverName, + actionName : that.actionName, + rootObject : that.rootObject, + withOrientation : that.withOrientation + }); + + // scale the viewer to fit the map + that.viewer.scaleToDimensions(client.currentGrid.width, client.currentGrid.height); + that.viewer.shift(client.currentGrid.pose.position.x, client.currentGrid.pose.position.y); + }); +};