diff --git a/CHANGELOG.md b/CHANGELOG.md index 78ed8ce..278ea4f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,4 +1,6 @@ DEVEL - **r2** + * Shift added to grid client to center on map [(rctoris)](https://github.com/rctoris/) + * OccupancyGridClientNav now correctly uses the topic parameter [(rctoris)](https://github.com/rctoris/) 2013-04-15 - **r1** * Initial development of NAV2D [(rctoris)](https://github.com/rctoris/) diff --git a/README.md b/README.md index 51de5f2..60b78a1 100644 --- a/README.md +++ b/README.md @@ -22,7 +22,7 @@ nav2djs depends on: [EaselJS](https://github.com/CreateJS/EaselJS/). The current supported version is 0.6.0. The current supported version can be found [in this project](include/EaselJS/easeljs.js) or on the Robot Web Tools CDN: ([full](http://cdn.robotwebtools.org/EaselJS/0.6.0/easeljs.js)) | ([min](http://cdn.robotwebtools.org/EaselJS/0.6.0/easeljs.min.js)) -[ros2djs](https://github.com/RobotWebTools/ros2djs). The current supported version is r1. The current supported version can be found [in this project](include/ros2djs/ros2d.js) or on the Robot Web Tools CDN: ([full](http://cdn.robotwebtools.org/ros2djs/r1/ros2d.js)) | ([min](http://cdn.robotwebtools.org/ros2djs/r1/ros2d.min.js)) +[ros2djs](https://github.com/RobotWebTools/ros2djs). The current supported version is r2. The current supported version can be found [in this project](include/ros2djs/ros2d.js) or on the Robot Web Tools CDN: ([full](http://cdn.robotwebtools.org/ros2djs/r2/ros2d.js)) | ([min](http://cdn.robotwebtools.org/ros2djs/r2/ros2d.min.js)) ### Build Checkout [utils/README.md](utils/README.md) for details on building. diff --git a/build/nav2d.js b/build/nav2d.js index 1d2ce50..605f29f 100644 --- a/build/nav2d.js +++ b/build/nav2d.js @@ -164,7 +164,8 @@ NAV2D.OccupancyGridClientNav = function(options) { var client = new ROS2D.OccupancyGridClient({ ros : this.ros, rootObject : this.rootObject, - continuous : continuous + continuous : continuous, + topic : topic }); client.on('change', function() { that.navigator = new NAV2D.Navigator({ @@ -175,6 +176,7 @@ NAV2D.OccupancyGridClientNav = function(options) { }); // scale the viewer to fit the map + that.viewer.shift(client.currentGrid.pose.position.x, client.currentGrid.pose.position.y); that.viewer.scaleToDimensions(client.currentGrid.width, client.currentGrid.height); }); }; diff --git a/build/nav2d.min.js b/build/nav2d.min.js index a0ec33b..89cc61b 100644 --- a/build/nav2d.min.js +++ b/build/nav2d.min.js @@ -1 +1 @@ -var NAV2D=NAV2D||{REVISION:"2-devel"};NAV2D.Navigator=function(e){function o(e){var o=new ROSLIB.Goal({actionClient:n,goalMessage:{target_pose:{header:{frame_id:"/map"},pose:e}}});o.send();var a=new ROS2D.NavigationArrow({size:8,strokeSize:1,fillColor:createjs.Graphics.getRGB(255,64,128,.66),pulse:!0});a.x=e.position.x,a.y=-e.position.y,a.rotation=s.rosQuaternionToGlobalTheta(e.orientation),a.scaleX=1/s.scaleX,a.scaleY=1/s.scaleY,t.rootObject.addChild(a),o.on("result",function(){t.rootObject.removeChild(a)})}var t=this;e=e||{};var a=e.ros,r=e.serverName||"/move_base",i=e.actionName||"move_base_msgs/MoveBaseAction";this.rootObject=e.rootObject||new createjs.Container;var s,n=new ROSLIB.ActionClient({ros:a,actionName:i,serverName:r});s=t.rootObject instanceof createjs.Stage?t.rootObject:t.rootObject.getStage();var c=new ROS2D.NavigationArrow({size:12,strokeSize:1,fillColor:createjs.Graphics.getRGB(255,128,0,.66),pulse:!0});c.visible=!1,this.rootObject.addChild(c);var l=!1,v=new ROSLIB.Topic({ros:a,name:"/robot_pose",messageType:"geometry_msgs/Pose",throttle_rate:100});v.subscribe(function(e){c.x=e.position.x,c.y=-e.position.y,l||(c.scaleX=1/s.scaleX,c.scaleY=1/s.scaleY,l=!0),c.rotation=s.rosQuaternionToGlobalTheta(e.orientation),c.visible=!0}),this.rootObject.addEventListener("dblclick",function(e){var t=s.globalToRos(e.stageX,e.stageY),a=new ROSLIB.Pose({position:new ROSLIB.Vector3(t)});o(a)})},NAV2D.OccupancyGridClientNav=function(e){var o=this;e=e||{},this.ros=e.ros,e.topic||"/map";var t=e.continuous;this.serverName=e.serverName||"/move_base",this.actionName=e.actionName||"move_base_msgs/MoveBaseAction",this.rootObject=e.rootObject||new createjs.Container,this.viewer=e.viewer,this.navigator=null;var a=new ROS2D.OccupancyGridClient({ros:this.ros,rootObject:this.rootObject,continuous:t});a.on("change",function(){o.navigator=new NAV2D.Navigator({ros:o.ros,serverName:o.serverName,actionName:o.actionName,rootObject:o.rootObject}),o.viewer.scaleToDimensions(a.currentGrid.width,a.currentGrid.height)})}; \ No newline at end of file +var NAV2D=NAV2D||{REVISION:"2-devel"};NAV2D.Navigator=function(e){function o(e){var o=new ROSLIB.Goal({actionClient:n,goalMessage:{target_pose:{header:{frame_id:"/map"},pose:e}}});o.send();var r=new ROS2D.NavigationArrow({size:8,strokeSize:1,fillColor:createjs.Graphics.getRGB(255,64,128,.66),pulse:!0});r.x=e.position.x,r.y=-e.position.y,r.rotation=s.rosQuaternionToGlobalTheta(e.orientation),r.scaleX=1/s.scaleX,r.scaleY=1/s.scaleY,t.rootObject.addChild(r),o.on("result",function(){t.rootObject.removeChild(r)})}var t=this;e=e||{};var r=e.ros,i=e.serverName||"/move_base",a=e.actionName||"move_base_msgs/MoveBaseAction";this.rootObject=e.rootObject||new createjs.Container;var s,n=new ROSLIB.ActionClient({ros:r,actionName:a,serverName:i});s=t.rootObject instanceof createjs.Stage?t.rootObject:t.rootObject.getStage();var c=new ROS2D.NavigationArrow({size:12,strokeSize:1,fillColor:createjs.Graphics.getRGB(255,128,0,.66),pulse:!0});c.visible=!1,this.rootObject.addChild(c);var l=!1,v=new ROSLIB.Topic({ros:r,name:"/robot_pose",messageType:"geometry_msgs/Pose",throttle_rate:100});v.subscribe(function(e){c.x=e.position.x,c.y=-e.position.y,l||(c.scaleX=1/s.scaleX,c.scaleY=1/s.scaleY,l=!0),c.rotation=s.rosQuaternionToGlobalTheta(e.orientation),c.visible=!0}),this.rootObject.addEventListener("dblclick",function(e){var t=s.globalToRos(e.stageX,e.stageY),r=new ROSLIB.Pose({position:new ROSLIB.Vector3(t)});o(r)})},NAV2D.OccupancyGridClientNav=function(e){var o=this;e=e||{},this.ros=e.ros;var t=e.topic||"/map",r=e.continuous;this.serverName=e.serverName||"/move_base",this.actionName=e.actionName||"move_base_msgs/MoveBaseAction",this.rootObject=e.rootObject||new createjs.Container,this.viewer=e.viewer,this.navigator=null;var i=new ROS2D.OccupancyGridClient({ros:this.ros,rootObject:this.rootObject,continuous:r,topic:t});i.on("change",function(){o.navigator=new NAV2D.Navigator({ros:o.ros,serverName:o.serverName,actionName:o.actionName,rootObject:o.rootObject}),o.viewer.shift(i.currentGrid.pose.position.x,i.currentGrid.pose.position.y),o.viewer.scaleToDimensions(i.currentGrid.width,i.currentGrid.height)})}; \ No newline at end of file diff --git a/include/ros2djs/ros2d.js b/include/ros2djs/ros2d.js index b0fdab3..8f95ec0 100644 --- a/include/ros2djs/ros2d.js +++ b/include/ros2djs/ros2d.js @@ -3,13 +3,12 @@ */ var ROS2D = ROS2D || { - REVISION : '1' + REVISION : '2-devel' }; // convert the given global Stage coordinates to ROS coordinates createjs.Stage.prototype.globalToRos = function(x, y) { - var rosX = x / this.scaleX; - // change Y direction + var rosX = (x - this.x) / this.scaleX; var rosY = (this.y - y) / this.scaleY; return { x : rosX, @@ -102,10 +101,16 @@ ROS2D.OccupancyGrid = function(options) { 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; @@ -165,6 +170,57 @@ ROS2D.OccupancyGridClient = function(options) { }; 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 Russell Toris - rctoris@wpi.edu */ @@ -286,6 +342,23 @@ ROS2D.Viewer.prototype.addObject = function(object) { * @param height - the height to scale to in meters */ ROS2D.Viewer.prototype.scaleToDimensions = function(width, height) { + // store the actual offset in the ROS coordinate system + var tmpY = this.height - (this.scene.y * this.scene.scaleY); this.scene.scaleX = this.width / width; this.scene.scaleY = this.height / height; + // reset the offset + this.scene.x = (this.scene.x * this.scene.scaleX); + this.scene.y -= (tmpY * this.scene.scaleY) - tmpY; +}; + +/** + * 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) { + this.scene.x -= (x * this.scene.scaleX); + this.scene.y += (y * this.scene.scaleY); }; diff --git a/src/navigator/OccupancyGridClientNav.js b/src/navigator/OccupancyGridClientNav.js index d4df216..9372224 100644 --- a/src/navigator/OccupancyGridClientNav.js +++ b/src/navigator/OccupancyGridClientNav.js @@ -33,7 +33,8 @@ NAV2D.OccupancyGridClientNav = function(options) { var client = new ROS2D.OccupancyGridClient({ ros : this.ros, rootObject : this.rootObject, - continuous : continuous + continuous : continuous, + topic : topic }); client.on('change', function() { that.navigator = new NAV2D.Navigator({ @@ -44,6 +45,7 @@ NAV2D.OccupancyGridClientNav = function(options) { }); // scale the viewer to fit the map + that.viewer.shift(client.currentGrid.pose.position.x, client.currentGrid.pose.position.y); that.viewer.scaleToDimensions(client.currentGrid.width, client.currentGrid.height); }); };