]> defiant.homedns.org Git - ros_wild_thumper.git/blob - www/assets/javascripts/keyboardteleop.js
remove ir sensors
[ros_wild_thumper.git] / www / assets / javascripts / keyboardteleop.js
1 /**
2  * @author Russell Toris - rctoris@wpi.edu
3  */
4
5 var KEYBOARDTELEOP = KEYBOARDTELEOP || {
6   REVISION : '0.3.0'
7 };
8
9 /**
10  * @author Russell Toris - rctoris@wpi.edu
11  */
12
13 /**
14  * Manages connection to the server and all interactions with ROS.
15  *
16  * Emits the following events:
17  *   * 'change' - emitted with a change in speed occurs
18  *
19  * @constructor
20  * @param options - possible keys include:
21  *   * ros - the ROSLIB.Ros connection handle
22  *   * topic (optional) - the Twist topic to publish to, like '/cmd_vel'
23  *   * throttle (optional) - a constant throttle for the speed
24  */
25 KEYBOARDTELEOP.Teleop = function(options) {
26   var that = this;
27   options = options || {};
28   var ros = options.ros;
29   var topic = options.topic || '/cmd_vel';
30   // permanent throttle
31   var throttle = options.throttle || 1.0;
32
33   // used to externally throttle the speed (e.g., from a slider)
34   this.scale = 1.0;
35
36   // linear x and y movement and angular z movement
37   var x = 0;
38   var y = 0;
39   var z = 0;
40
41   var cmdVel = new ROSLIB.Topic({
42     ros : ros,
43     name : topic,
44     messageType : 'geometry_msgs/Twist'
45   });
46
47   // sets up a key listener on the page used for keyboard teleoperation
48   var handleKey = function(keyCode, keyDown) {
49     // used to check for changes in speed
50     var oldX = x;
51     var oldY = y;
52     var oldZ = z;
53     
54     var pub = true;
55
56     var speed = 0;
57     // throttle the speed by the slider and throttle constant
58     if (keyDown === true) {
59       speed = throttle * that.scale;
60     }
61     // check which key was pressed
62     switch (keyCode) {
63       case 65:
64         // turn left
65         z = 1 * speed;
66         break;
67       case 87:
68         // up
69         x = 0.5 * speed;
70         break;
71       case 68:
72         // turn right
73         z = -1 * speed;
74         break;
75       case 83:
76         // down
77         x = -0.5 * speed;
78         break;
79       case 69:
80         // strafe right
81         y = -0.5 * speed;
82         break;
83       case 81:
84         // strafe left
85         y = 0.5 * speed;
86         break;
87       default:
88         pub = false;
89     }
90
91     // publish the command
92     if (pub === true) {
93       var twist = new ROSLIB.Message({
94         angular : {
95           x : 0,
96           y : 0,
97           z : z
98         },
99         linear : {
100           x : x,
101           y : y,
102           z : z
103         }
104       });
105       cmdVel.publish(twist);
106
107       // check for changes
108       if (oldX !== x || oldY !== y || oldZ !== z) {
109         that.emit('change', twist);
110       }
111     }
112   };
113
114   // handle the key
115   var body = document.getElementsByTagName('body')[0];
116   body.addEventListener('keydown', function(e) {
117     handleKey(e.keyCode, true);
118   }, false);
119   body.addEventListener('keyup', function(e) {
120     handleKey(e.keyCode, false);
121   }, false);
122 };
123 KEYBOARDTELEOP.Teleop.prototype.__proto__ = EventEmitter2.prototype;