2 * @author Russell Toris - rctoris@wpi.edu
3 * @author Lars Kunze - l.kunze@cs.bham.ac.uk
11 * @author Russell Toris - rctoris@wpi.edu
15 * A OccupancyGridClientNav uses an OccupancyGridClient to create a map for use with a Navigator.
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
28 NAV2D.ImageMapClientNav = function(options) {
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;
40 this.navigator = null;
42 // setup a client to get the map
43 var client = new ROS2D.ImageMapClient({
45 rootObject : this.rootObject,
49 client.on('change', function() {
50 that.navigator = new NAV2D.Navigator({
52 serverName : that.serverName,
53 actionName : that.actionName,
54 rootObject : that.rootObject,
55 withOrientation : that.withOrientation
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);
65 * @author Russell Toris - rctoris@wpi.edu
66 * @author Lars Kunze - l.kunze@cs.bham.ac.uk
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).
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)
83 NAV2D.Navigator = function(options) {
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();
92 // setup the actionlib client
93 var actionClient = new ROSLIB.ActionClient({
95 actionName : actionName,
96 serverName : serverName
100 * Send a goal to the navigation stack with the given pose.
102 * @param pose - the goal pose
104 function sendGoal(pose) {
106 var goal = new ROSLIB.Goal({
107 actionClient : actionClient,
119 // create a marker for the goal
120 var goalMarker = new ROS2D.NavigationArrow({
123 fillColor : createjs.Graphics.getRGB(255, 64, 128, 0.66),
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);
133 goal.on('result', function() {
134 that.rootObject.removeChild(goalMarker);
138 // get a handle to the stage
140 if (that.rootObject instanceof createjs.Stage) {
141 stage = that.rootObject;
143 stage = that.rootObject.getStage();
146 // marker for the robot
147 var robotMarker = new ROS2D.NavigationArrow({
150 fillColor : createjs.Graphics.getRGB(255, 128, 0, 0.66),
153 // wait for a pose to come in first
154 robotMarker.visible = false;
155 this.rootObject.addChild(robotMarker);
156 var initScaleSet = false;
158 // setup a listener for the robot pose
159 var poseListener = new ROSLIB.Topic({
161 name : '/robot_pose',
162 messageType : 'geometry_msgs/Pose',
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;
170 robotMarker.scaleX = 1.0 / stage.scaleX;
171 robotMarker.scaleY = 1.0 / stage.scaleY;
176 robotMarker.rotation = stage.rosQuaternionToGlobalTheta(pose.orientation);
178 robotMarker.visible = true;
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)
192 } else { // withOrientation === true
193 // setup a click-and-point listener (with orientation)
195 var positionVec3 = null;
196 var thetaRadians = 0;
197 var thetaDegrees = 0;
198 var orientationMarker = null;
199 var mouseDown = false;
203 var mouseEventHandler = function(event, mouseState) {
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);
211 else if (mouseState === 'move'){
212 // remove obsolete orientation marker
213 that.rootObject.removeChild(orientationMarker);
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);
223 orientationMarker = new ROS2D.NavigationArrow({
226 fillColor : createjs.Graphics.getRGB(0, 255, 0, 0.66),
230 xDelta = currentPosVec3.x - positionVec3.x;
231 yDelta = currentPosVec3.y - positionVec3.y;
233 thetaRadians = Math.atan2(xDelta,yDelta);
235 thetaDegrees = thetaRadians * (180.0 / Math.PI);
237 if (thetaDegrees >= 0 && thetaDegrees <= 180) {
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;
249 that.rootObject.addChild(orientationMarker);
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
259 var goalPos = stage.globalToRos(event.stageX, event.stageY);
261 var goalPosVec3 = new ROSLIB.Vector3(goalPos);
263 xDelta = goalPosVec3.x - positionVec3.x;
264 yDelta = goalPosVec3.y - positionVec3.y;
266 thetaRadians = Math.atan2(xDelta,yDelta);
268 if (thetaRadians >= 0 && thetaRadians <= Math.PI) {
269 thetaRadians += (3 * Math.PI / 2);
271 thetaRadians -= (Math.PI/2);
274 var qz = Math.sin(-thetaRadians/2.0);
275 var qw = Math.cos(-thetaRadians/2.0);
277 var orientation = new ROSLIB.Quaternion({x:0, y:0, z:qz, w:qw});
279 var pose = new ROSLIB.Pose({
280 position : positionVec3,
281 orientation : orientation
288 this.rootObject.addEventListener('stagemousedown', function(event) {
289 mouseEventHandler(event,'down');
292 this.rootObject.addEventListener('stagemousemove', function(event) {
293 mouseEventHandler(event,'move');
296 this.rootObject.addEventListener('stagemouseup', function(event) {
297 mouseEventHandler(event,'up');
303 * @author Russell Toris - rctoris@wpi.edu
307 * A OccupancyGridClientNav uses an OccupancyGridClient to create a map for use with a Navigator.
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
321 NAV2D.OccupancyGridClientNav = function(options) {
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;
333 this.navigator = null;
335 // setup a client to get the map
336 var client = new ROS2D.OccupancyGridClient({
338 rootObject : this.rootObject,
339 continuous : continuous,
342 client.on('change', function() {
343 that.navigator = new NAV2D.Navigator({
345 serverName : that.serverName,
346 actionName : that.actionName,
347 rootObject : that.rootObject,
348 withOrientation : that.withOrientation
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);