[15] | 1 | webmap.modules.robot = {} /// Namespace for robot modules. |
---|
| 2 | |
---|
| 3 | |
---|
| 4 | /// Create an SVG element to represent a robot from an image. |
---|
| 5 | /** |
---|
| 6 | * \param iri The IRI of the image. |
---|
| 7 | * \param width The width of the robot. |
---|
| 8 | * \param height The height of the robot. |
---|
| 9 | * \param origin_left The distance from the left edge of the robot to it's origin. |
---|
| 10 | * \param origin_top The distance from the top edge of the robot to it's origin. |
---|
| 11 | * |
---|
| 12 | * Note that the image used must have it's origin at the top left corner. This is always true for |
---|
| 13 | * raster images, but it is your own responsibility with SVG images. SVG images should always |
---|
| 14 | * have a viewBox attribute on the root SVG element, or scaling will not work properly. |
---|
| 15 | * The viewBox attribute should also ensure that the origin lies at the top left corner of the |
---|
| 16 | * SVG image. |
---|
| 17 | */ |
---|
| 18 | webmap.createImageModel = function(iri, width, height, origin_left, origin_top) { |
---|
| 19 | if (origin_left == undefined) origin_left = width * 0.5; |
---|
| 20 | if (origin_top == undefined) origin_top = height * 0.5; |
---|
| 21 | |
---|
| 22 | /// The image element. |
---|
| 23 | var image = document.createElementNS(webmap.svgns, "image"); |
---|
| 24 | image.setAttributeNS(webmap.xlinkns, "href", iri); |
---|
| 25 | image.setAttribute("x", 0); |
---|
| 26 | image.setAttribute("y", 0); |
---|
| 27 | image.setAttribute("width", width); |
---|
| 28 | image.setAttribute("height", height); |
---|
| 29 | image.style.overflow = "visible"; |
---|
| 30 | |
---|
| 31 | var transform = webmap.svg.createSVGMatrix() |
---|
| 32 | // Translate the image so that the origin is at the specifified location. |
---|
| 33 | .translate(-origin_left, -origin_top) |
---|
| 34 | // Flip the Y axis of the image, since we're placing it in a Cartesian coordinate grid. |
---|
| 35 | .translate(width * 0.5, height * 0.5) |
---|
| 36 | .scaleNonUniform(1, -1) |
---|
| 37 | .translate(-width * 0.5, -height * 0.5); |
---|
| 38 | |
---|
| 39 | image.transform.baseVal.initialize(image.transform.baseVal.createSVGTransformFromMatrix(transform)); |
---|
| 40 | return image; |
---|
| 41 | } |
---|
| 42 | |
---|
| 43 | |
---|
| 44 | |
---|
| 45 | /// Create a new robot. |
---|
| 46 | /** |
---|
| 47 | * \param name The name of the robot. |
---|
| 48 | * \param model The SVG element to render. |
---|
| 49 | */ |
---|
| 50 | webmap.Robot = function(name, model) { |
---|
| 51 | |
---|
| 52 | // Call parent constructor. |
---|
| 53 | webmap.Extendable.call(this, webmap.modules.robot); |
---|
| 54 | |
---|
| 55 | /// Locally unique ID. |
---|
| 56 | this.id = webmap.generateId(); |
---|
| 57 | |
---|
| 58 | /// The name of the robot. |
---|
| 59 | this.name = name; |
---|
| 60 | |
---|
| 61 | /// The map the robot is currently associated with. |
---|
| 62 | this.map = null; |
---|
| 63 | |
---|
| 64 | /// True if the robot is selected. |
---|
| 65 | this.selected = false; |
---|
| 66 | |
---|
| 67 | /// Position in 3D of the robot. |
---|
| 68 | this.position = {x: 0, y: 0, z: 0}; |
---|
| 69 | |
---|
| 70 | /// Orientation of the robot as quaternion. |
---|
| 71 | this.orientation = {x: 0, y: 0, z: 0, w: 0}; |
---|
| 72 | |
---|
| 73 | /// The orientation flattened to the rotation of the robot around the Z axis. |
---|
| 74 | this.angle = 0; |
---|
| 75 | |
---|
| 76 | /// The odometry topic. |
---|
| 77 | this.odometry_topic = null; |
---|
| 78 | |
---|
| 79 | /// The twist topic. |
---|
| 80 | this.twist_topic = null |
---|
| 81 | |
---|
| 82 | /// The move topic. |
---|
| 83 | this.move_topic = null |
---|
| 84 | |
---|
| 85 | /// The name of the base link. |
---|
| 86 | this.base_link = "base_link"; |
---|
| 87 | |
---|
| 88 | /// Indicates if the cached odometry information is valid. |
---|
| 89 | /** |
---|
| 90 | * The cached odometry information is valid when the first odometry |
---|
| 91 | * message has arrived. |
---|
| 92 | */ |
---|
| 93 | this.valid = false; |
---|
| 94 | |
---|
| 95 | /// SVG model content representing the robot. |
---|
| 96 | this.svg = document.createElementNS(webmap.svgns, "g"); |
---|
| 97 | |
---|
| 98 | // Add the model to the SVG rendering. |
---|
| 99 | this.svg.appendChild(model.cloneNode(true)); |
---|
| 100 | |
---|
| 101 | // Add a reference back to the robot from the SVG element. |
---|
| 102 | this.svg.robot = this; |
---|
| 103 | |
---|
| 104 | // Add event listeners. |
---|
| 105 | this.mouseClickListener = this.mouseClick.bind(this); |
---|
| 106 | this.svg.addEventListener("click", this.mouseClickListener, false); |
---|
| 107 | } |
---|
| 108 | |
---|
| 109 | webmap.extend(webmap.Extendable, webmap.Robot); |
---|
| 110 | |
---|
| 111 | /// Clean up the robot. |
---|
| 112 | /** |
---|
| 113 | * All subscriptions and advertisements are removed, and the robot is removed from it's map. |
---|
| 114 | */ |
---|
| 115 | webmap.Robot.prototype.destroy = function() { |
---|
| 116 | webmap.Robot.prototype.unsubscribeOdometryTopic(); |
---|
| 117 | webmap.Robot.prototype.unadvertiseTwistTopic(); |
---|
| 118 | webmap.Robot.prototype.unadvertiseMoveTopic(); |
---|
| 119 | if (this.map) this.map.removeRobot(this); |
---|
| 120 | } |
---|
| 121 | |
---|
| 122 | |
---|
| 123 | /// Set the odometry topic used by the robot. |
---|
| 124 | /** |
---|
| 125 | * \param connection The ROS connection for this topic. |
---|
| 126 | * \param topic The name of the topic. |
---|
| 127 | * \param throttle (Optional) The minimum time in milliseconds between receiving two updates. Defaults to 100. |
---|
| 128 | * |
---|
| 129 | * The topic has to be a Pose, PoseStamped, PoseWithCovarianceStamped |
---|
| 130 | * or Pose2D message from the geometry_msgs package. |
---|
| 131 | */ |
---|
| 132 | webmap.Robot.prototype.setOdometryTopic = function(connection, topic, throttle) { |
---|
| 133 | if (throttle === undefined || throttle === null) throttle = 100; |
---|
| 134 | this.unsubscribeOdometryTopic(); |
---|
| 135 | connection.subscribe(this.handleOdometry.bind(this), topic, null, throttle, null, null, null, this.id); |
---|
| 136 | this.odometry_topic = {connection: connection, name: topic}; |
---|
| 137 | } |
---|
| 138 | |
---|
| 139 | /// Unsubscribe the robot from the odometry topic. |
---|
| 140 | /** |
---|
| 141 | * This method doesn nothing if the robot wasn't subscribed to an odometry topic. |
---|
| 142 | */ |
---|
| 143 | webmap.Robot.prototype.unsubscribeOdometryTopic = function() { |
---|
| 144 | if (this.odometry_topic) { |
---|
| 145 | this.odometry_topic.connection.unsubscribe(this.odometry_topic.name, this.id); |
---|
| 146 | this.odometry_topic = null; |
---|
| 147 | } |
---|
| 148 | } |
---|
| 149 | |
---|
| 150 | /// Set the twist topic used to control the robot. |
---|
| 151 | /** |
---|
| 152 | * \param connection The ROS connection for this topic. |
---|
| 153 | * \param topic The name of the topic. |
---|
| 154 | */ |
---|
| 155 | webmap.Robot.prototype.setTwistTopic = function(connection, topic) { |
---|
| 156 | connection.advertise(topic, "geometry_msgs/Twist", this.id); |
---|
| 157 | this.twist_topic = {connection: connection, name: topic}; |
---|
| 158 | } |
---|
| 159 | |
---|
| 160 | /// Unadvertise the twist topic |
---|
| 161 | /** |
---|
| 162 | * This method does nothing if the robot hasn't advertised a twist topic yet. |
---|
| 163 | */ |
---|
| 164 | webmap.Robot.prototype.unadvertiseTwistTopic = function() { |
---|
| 165 | if (this.twist_topic) { |
---|
| 166 | this.twist_topic.connection.unadvertise(this.twist_topic.name, this.id); |
---|
| 167 | this.twist_topic = null; |
---|
| 168 | } |
---|
| 169 | } |
---|
| 170 | |
---|
| 171 | /// Set the move topic used to control the robot. |
---|
| 172 | /** |
---|
| 173 | * The move topic is used to send 2D position goals for the robot. |
---|
| 174 | * |
---|
| 175 | * \param connection The ROS connection for this robot. |
---|
| 176 | * \param topic The name of the move topic. |
---|
| 177 | */ |
---|
| 178 | webmap.Robot.prototype.setMoveTopic = function(connection, topic) { |
---|
| 179 | connection.advertise(topic, "move_base_simple/goal", this.id); |
---|
| 180 | this.move_topic = {connection: connection, name: topic}; |
---|
| 181 | } |
---|
| 182 | |
---|
| 183 | /// Unadvertise the move topic |
---|
| 184 | /** |
---|
| 185 | * If there was no move topic yet, nothing happens. |
---|
| 186 | */ |
---|
| 187 | webmap.Robot.prototype.unadvertiseMoveTopic = function() { |
---|
| 188 | if (this.move_topic) { |
---|
| 189 | this.move_topic.connection.unadvertise(this.move_topic.name, this.id); |
---|
| 190 | this.move_topic = null; |
---|
| 191 | } |
---|
| 192 | } |
---|
| 193 | |
---|
| 194 | /// Send a twist message to the robot. |
---|
| 195 | /** |
---|
| 196 | * \param x The linear X component. |
---|
| 197 | * \param y The linear Y component. |
---|
| 198 | * \param z The linear Z component. |
---|
| 199 | * \param rx The angular X component. |
---|
| 200 | * \param ry The angular Y component. |
---|
| 201 | * \param rz The angular Z component. |
---|
| 202 | */ |
---|
| 203 | webmap.Robot.prototype.twist = function(x, y, z, rx, ry, rz) { |
---|
| 204 | if (this.twist_topic) { |
---|
| 205 | var msg = { |
---|
| 206 | linear: {x: x, y: y, z: z}, |
---|
| 207 | angular: {x: rx, y: ry, z: rz} |
---|
| 208 | }; |
---|
| 209 | this.twist_topic.connection.publish(this.twist_topic.name, msg); |
---|
| 210 | this.notifyModules("onSendTwist", msg); |
---|
| 211 | } |
---|
| 212 | } |
---|
| 213 | |
---|
| 214 | /// Send a position goal to the robot. |
---|
| 215 | /** |
---|
| 216 | * \param position A 3D vector object with x, y and z attributes representing the desired position of the base link. |
---|
| 217 | * \param orientation A 4D quaternion object with w, x, y and z attribute representing the desired orientation of the base link. |
---|
| 218 | */ |
---|
| 219 | webmap.Robot.prototype.move = function(position, orientation) { |
---|
| 220 | if (this.move_topic) { |
---|
| 221 | msg = { |
---|
| 222 | frame_id: this.base_link, |
---|
| 223 | positions: { |
---|
| 224 | x: position.x, |
---|
| 225 | y: position.y, |
---|
| 226 | z: position.z |
---|
| 227 | }, |
---|
| 228 | orientation: { |
---|
| 229 | x: orientation.x, |
---|
| 230 | y: orientation.y, |
---|
| 231 | z: orientation.z, |
---|
| 232 | w: orientation.w |
---|
| 233 | } |
---|
| 234 | }; |
---|
| 235 | this.move_topic.connection.publish(this.move_topic.name, msg); |
---|
| 236 | this.notifyModules("onSendMove", msg); |
---|
| 237 | } |
---|
| 238 | } |
---|
| 239 | |
---|
| 240 | /// Set the selected state of the robot. |
---|
| 241 | /** |
---|
| 242 | * Adds or removes the "selected" class to/from the SVG content. |
---|
| 243 | * \param selected True to make the robot selected, false to make the robot deselected. |
---|
| 244 | */ |
---|
| 245 | webmap.Robot.prototype.setSelected = function(selected) { |
---|
| 246 | this.selected = selected; |
---|
| 247 | if (selected) { |
---|
| 248 | this.svg.classList.add("selected"); |
---|
| 249 | } else { |
---|
| 250 | this.svg.classList.remove("selected"); |
---|
| 251 | } |
---|
| 252 | this.notifyModules("onSelect", selected); |
---|
| 253 | } |
---|
| 254 | |
---|
| 255 | /// Process an odometry message. |
---|
| 256 | /** |
---|
| 257 | * \param msg The odometry message. |
---|
| 258 | */ |
---|
| 259 | webmap.Robot.prototype.handleOdometry = function(msg) { |
---|
| 260 | var position = null; |
---|
| 261 | var orientation = null; |
---|
| 262 | var angle = null; |
---|
| 263 | |
---|
| 264 | // It's a Pose message. |
---|
| 265 | if (msg.position && msg.orientation) { |
---|
| 266 | position = msg.position; |
---|
| 267 | orientation = msg.orientation; |
---|
| 268 | angle = webmap.quaternionZ(orientation); |
---|
| 269 | // It's a PoseStamped message. |
---|
| 270 | } else if (msg.pose && msg.pose.position && msg.pose.orientation) { |
---|
| 271 | position = msg.pose.position; |
---|
| 272 | orientation = msg.pose.orientation; |
---|
| 273 | angle = webmap.quaternionZ(orientation); |
---|
| 274 | // It's a PoseWithCovarianceStamped message. |
---|
| 275 | } else if (msg.pose && msg.pose.pose && msg.pose.pose.position && msg.pose.pose.orientation) { |
---|
| 276 | position = msg.pose.pose.position; |
---|
| 277 | orientation = msg.pose.pose.orientation; |
---|
| 278 | angle = webmap.quaternionZ(orientation); |
---|
| 279 | // It's a Pose2D message. |
---|
| 280 | } else if (msg.x && msg.y && msg.theta) { |
---|
| 281 | position = {x: msg.x, y: msg.y, z: 0}; |
---|
| 282 | orientation = null; |
---|
| 283 | angle = msg.theta; |
---|
| 284 | } |
---|
| 285 | |
---|
| 286 | // Check if anything actually changed. |
---|
| 287 | // If the old data was invalid, any data is a change. |
---|
| 288 | var dirty = !this.valid; |
---|
| 289 | dirty |= !webmap.vectorEqual(this.position, position); |
---|
| 290 | dirty |= this.angle !== angle; |
---|
| 291 | |
---|
| 292 | if (dirty) { |
---|
| 293 | this.position = position; |
---|
| 294 | this.orientation = orientation; |
---|
| 295 | this.angle = angle; |
---|
| 296 | this.valid = true; |
---|
| 297 | |
---|
| 298 | // Update the SVG rendering. |
---|
| 299 | var transform = webmap.svg.createSVGMatrix().translate(this.position.x, this.position.y).rotate(this.angle); |
---|
| 300 | this.svg.transform.baseVal.initialize(this.svg.transform.baseVal.createSVGTransformFromMatrix(transform)); |
---|
| 301 | this.svg.style.display = this.valid ? "block" : "none"; |
---|
| 302 | |
---|
| 303 | this.notifyModules("onOdometryUpdate"); |
---|
| 304 | } |
---|
| 305 | } |
---|
| 306 | |
---|
| 307 | /// Handle mouse click events. |
---|
| 308 | webmap.Robot.prototype.mouseClick = function(event) { |
---|
| 309 | event = event || window.event; |
---|
| 310 | event.stopPropagation(); |
---|
| 311 | event.preventDefault(); |
---|
| 312 | if (this.map) this.map.setSelected(this); |
---|
| 313 | } |
---|