source: trunk/webmap/html/js/webmap/robot.js @ 15

webmap.modules.robot = {} /// Namespace for robot modules.


/// Create an SVG element to represent a robot from an image.
/**
 * \param iri         The IRI of the image.
 * \param width       The width of the robot.
 * \param height      The height of the robot.
 * \param origin_left The distance from the left edge of the robot to it's origin.
 * \param origin_top  The distance from the top edge of the robot to it's origin.
 * 
 * Note that the image used must have it's origin at the top left corner. This is always true for
 * raster images, but it is your own responsibility with SVG images. SVG images should always
 * have a viewBox attribute on the root SVG element, or scaling will not work properly.
 * The viewBox attribute should also ensure that the origin lies at the top left corner of the
 * SVG image.
 */
webmap.createImageModel = function(iri, width, height, origin_left, origin_top) {
        if (origin_left == undefined) origin_left = width  * 0.5;
        if (origin_top  == undefined) origin_top  = height * 0.5;
        
        /// The image element.
        var image = document.createElementNS(webmap.svgns, "image");
        image.setAttributeNS(webmap.xlinkns, "href", iri);
        image.setAttribute("x",      0);
        image.setAttribute("y",      0);
        image.setAttribute("width",  width);
        image.setAttribute("height", height);
        image.style.overflow = "visible";
        
        var transform  = webmap.svg.createSVGMatrix()
        // Translate the image so that the origin is at the specifified location.
        .translate(-origin_left, -origin_top)
        // Flip the Y axis of the image, since we're placing it in a Cartesian coordinate grid.
        .translate(width * 0.5, height * 0.5)
        .scaleNonUniform(1, -1)
        .translate(-width * 0.5, -height * 0.5);
        
        image.transform.baseVal.initialize(image.transform.baseVal.createSVGTransformFromMatrix(transform));
        return image;
}



/// Create a new robot.
/**
 * \param name        The name of the robot.
 * \param model       The SVG element to render.
 */
webmap.Robot = function(name, model) {
        
        // Call parent constructor.
        webmap.Extendable.call(this, webmap.modules.robot);
        
        /// Locally unique ID.
        this.id = webmap.generateId();
        
        /// The name of the robot.
        this.name = name;
        
        /// The map the robot is currently associated with.
        this.map = null;
        
        /// True if the robot is selected.
        this.selected = false;
        
        /// Position in 3D of the robot.
        this.position = {x: 0, y: 0, z: 0};
        
        /// Orientation of the robot as quaternion.
        this.orientation = {x: 0, y: 0, z: 0, w: 0};
        
        /// The orientation flattened to the rotation of the robot around the Z axis.
        this.angle = 0;
        
        /// The odometry topic.
        this.odometry_topic = null;
        
        /// The twist topic.
        this.twist_topic = null
        
        /// The move topic.
        this.move_topic = null
        
        /// The name of the base link.
        this.base_link = "base_link";
        
        /// Indicates if the cached odometry information is valid.
        /**
         * The cached odometry information is valid when the first odometry
         * message has arrived.
         */
        this.valid = false;
        
        /// SVG model content representing the robot.
        this.svg = document.createElementNS(webmap.svgns, "g");
        
        // Add the model to the SVG rendering.
        this.svg.appendChild(model.cloneNode(true));
        
        // Add a reference back to the robot from the SVG element.
        this.svg.robot = this;
        
        // Add event listeners.
        this.mouseClickListener = this.mouseClick.bind(this);
        this.svg.addEventListener("click", this.mouseClickListener, false);
}

webmap.extend(webmap.Extendable, webmap.Robot);

/// Clean up the robot.
/**
 * All subscriptions and advertisements are removed, and the robot is removed from it's map.
 */
webmap.Robot.prototype.destroy = function() {
        webmap.Robot.prototype.unsubscribeOdometryTopic();
        webmap.Robot.prototype.unadvertiseTwistTopic();
        webmap.Robot.prototype.unadvertiseMoveTopic();
        if (this.map) this.map.removeRobot(this);
}


/// Set the odometry topic used by the robot.
/**
 * \param connection  The ROS connection for this topic.
 * \param topic       The name of the topic.
 * \param throttle    (Optional) The minimum time in milliseconds between receiving two updates. Defaults to 100.
 * 
 * The topic has to be a Pose, PoseStamped, PoseWithCovarianceStamped
 * or Pose2D message from the geometry_msgs package.
 */
webmap.Robot.prototype.setOdometryTopic = function(connection, topic, throttle) {
        if (throttle === undefined || throttle === null) throttle = 100;
        this.unsubscribeOdometryTopic();
        connection.subscribe(this.handleOdometry.bind(this), topic, null, throttle, null, null, null, this.id);
        this.odometry_topic = {connection: connection, name: topic};
}

/// Unsubscribe the robot from the odometry topic.
/**
 * This method doesn nothing if the robot wasn't subscribed to an odometry topic.
 */
webmap.Robot.prototype.unsubscribeOdometryTopic = function() {
        if (this.odometry_topic) {
                this.odometry_topic.connection.unsubscribe(this.odometry_topic.name, this.id);
                this.odometry_topic = null;
        }
}

/// Set the twist topic used to control the robot.
/**
 * \param connection  The ROS connection for this topic.
 * \param topic       The name of the topic.
 */
webmap.Robot.prototype.setTwistTopic = function(connection, topic) {
        connection.advertise(topic, "geometry_msgs/Twist", this.id);
        this.twist_topic = {connection: connection, name: topic};
}

/// Unadvertise the twist topic
/**
 * This method does nothing if the robot hasn't advertised a twist topic yet.
 */
webmap.Robot.prototype.unadvertiseTwistTopic = function() {
        if (this.twist_topic) {
                this.twist_topic.connection.unadvertise(this.twist_topic.name, this.id);
                this.twist_topic = null;
        }
}

/// Set the move topic used to control the robot.
/**
 * The move topic is used to send 2D position goals for the robot.
 * 
 * \param connection  The ROS connection for this robot.
 * \param topic       The name of the move topic.
 */
webmap.Robot.prototype.setMoveTopic = function(connection, topic) {
        connection.advertise(topic, "move_base_simple/goal", this.id);
        this.move_topic = {connection: connection, name: topic};
}

/// Unadvertise the move topic
/**
 * If there was no move topic yet, nothing happens.
 */
webmap.Robot.prototype.unadvertiseMoveTopic = function() {
        if (this.move_topic) {
                this.move_topic.connection.unadvertise(this.move_topic.name, this.id);
                this.move_topic = null;
        }
}

/// Send a twist message to the robot.
/**
 * \param x  The linear X component.
 * \param y  The linear Y component.
 * \param z  The linear Z component.
 * \param rx The angular X component.
 * \param ry The angular Y component.
 * \param rz The angular Z component.
 */
webmap.Robot.prototype.twist = function(x, y, z, rx, ry, rz) {
        if (this.twist_topic) {
                var msg = {
                        linear:  {x:  x, y:  y, z:  z},
                        angular: {x: rx, y: ry, z: rz}
                };
                this.twist_topic.connection.publish(this.twist_topic.name, msg);
                this.notifyModules("onSendTwist", msg);
        }
}

/// Send a position goal to the robot.
/**
 * \param position    A 3D vector object with x, y and z attributes representing the desired position of the base link.
 * \param orientation A 4D quaternion object with w, x, y and z attribute representing the desired orientation of the base link.
 */
webmap.Robot.prototype.move = function(position, orientation) {
        if (this.move_topic) {
                msg = {
                        frame_id: this.base_link,
                        positions: {
                                x: position.x,
                                y: position.y,
                                z: position.z
                        },
                        orientation: {
                                x: orientation.x,
                                y: orientation.y,
                                z: orientation.z,
                                w: orientation.w
                        }
                };
                this.move_topic.connection.publish(this.move_topic.name, msg);
                this.notifyModules("onSendMove", msg);
        }
}

/// Set the selected state of the robot.
/**
 * Adds or removes the "selected" class to/from the SVG content.
 * \param selected True to make the robot selected, false to make the robot deselected.
 */
webmap.Robot.prototype.setSelected = function(selected) {
        this.selected = selected;
        if (selected) {
                this.svg.classList.add("selected");
        } else {
                this.svg.classList.remove("selected");
        }
        this.notifyModules("onSelect", selected);
}

/// Process an odometry message.
/**
 * \param msg The odometry message.
 */
webmap.Robot.prototype.handleOdometry = function(msg) {
        var position    = null;
        var orientation = null;
        var angle       = null;
        
        // It's a Pose message.
        if (msg.position && msg.orientation) {
                position    = msg.position;
                orientation = msg.orientation;
                angle       = webmap.quaternionZ(orientation);
        // It's a PoseStamped message.
        } else if (msg.pose && msg.pose.position && msg.pose.orientation) {
                position    = msg.pose.position;
                orientation = msg.pose.orientation;
                angle       = webmap.quaternionZ(orientation);
        // It's a PoseWithCovarianceStamped message.
        } else if (msg.pose && msg.pose.pose && msg.pose.pose.position && msg.pose.pose.orientation) {
                position    = msg.pose.pose.position;
                orientation = msg.pose.pose.orientation;
                angle       = webmap.quaternionZ(orientation);
        // It's a Pose2D message.
        } else if (msg.x && msg.y && msg.theta) {
                position    = {x: msg.x, y: msg.y, z: 0};
                orientation = null;
                angle       = msg.theta;
        }
        
        // Check if anything actually changed.
        // If the old data was invalid, any data is a change.
        var dirty = !this.valid;
        dirty |= !webmap.vectorEqual(this.position, position);
        dirty |= this.angle !== angle;
        
        if (dirty) {
                this.position    = position;
                this.orientation = orientation;
                this.angle       = angle;
                this.valid       = true;
                
                // Update the SVG rendering.
                var transform = webmap.svg.createSVGMatrix().translate(this.position.x, this.position.y).rotate(this.angle);
                this.svg.transform.baseVal.initialize(this.svg.transform.baseVal.createSVGTransformFromMatrix(transform));
                this.svg.style.display = this.valid ? "block" : "none";
                
                this.notifyModules("onOdometryUpdate");
        }
}

/// Handle mouse click events.
webmap.Robot.prototype.mouseClick = function(event) {
        event = event || window.event;
        event.stopPropagation();
        event.preventDefault();
        if (this.map) this.map.setSelected(this);
}