org.robokind.api.motion.messaging.RemoteRobot Maven / Gradle / Ivy

Go to download
/*
 * Copyright 2011 Hanson Robokind LLC.
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */
package org.robokind.api.motion.messaging;

import java.util.Map.Entry;
import java.util.logging.Logger;
import org.robokind.api.common.position.NormalizedDouble;
import org.robokind.api.common.utils.TimeUtils;
import org.robokind.api.motion.AbstractRobot;
import org.robokind.api.motion.Robot.RobotPositionMap;
import org.robokind.api.motion.protocol.RobotDefinitionResponse;
import org.robokind.api.motion.protocol.RobotDefinitionResponse.JointDefinition;
import org.robokind.api.motion.protocol.DefaultMotionFrame;
import org.robokind.api.motion.protocol.MotionFrame;

/**
 * RemoteRobot is a facade for controlling a remotely connected Robot.
 * The RemoteRobot controls the Robot using a RemoteRobotClient.
 * 
 * @author Matthew Stevenson 
 */
public class RemoteRobot extends AbstractRobot {
    private final static Logger theLogger = 
            Logger.getLogger(RemoteRobot.class.getName());
    private RemoteRobotClient myRobotClient;
    private RobotPositionMap myPreviousPositions;
    
    /**
     * Creates a RemoteRobot which uses the given RemoteRobotClient.
     * @param client client for the remote Robot
     */
    public RemoteRobot(RemoteRobotClient client){
        super(client.getRobotId());
        myRobotClient = client;
        updateRobotDefinition();
    }
    
    private void updateRobotDefinition(){
        RobotDefinitionResponse robotDef = myRobotClient.requestRobotDefinition();
        if(robotDef == null){
            theLogger.warning("RobotRequest timed out.  "
                    + "Unable to update definition.");
            throw new NullPointerException();
        }
        for(JointDefinition def : robotDef.getJointDefinitions()){
            RemoteJoint rj = new RemoteJoint(this, def);
            addJoint(rj);
        }
    }
    /**
     * Updates all cached values.
     * @return true if successful
     */
    public boolean updateRobot(){
        try{
            updateRobotDefinition();
        }catch(NullPointerException ex){
            return false;
        }
        return true;
    }
    /**
     * Sends a command for the remote robot to connect.
     * @return true if successful
     */
    @Override
    public boolean connect() {
        Boolean ret = myRobotClient.sendConnect();
        if(ret == null){
            theLogger.warning("RobotRequest timed out.  Unable to connect.");
            return false;
        }
        return ret;
    }
    /**
     * Sends a command for the remote robot to disconnect.
     */
    @Override
    public void disconnect() {
        Boolean ret = myRobotClient.sendDisconnect();
        if(ret == null){
            theLogger.warning("RobotRequest timed out.  Unable to disconnect.");
        }
    }
    /**
     * Returns the remote robot's connection status.
     * @return true if successful
     */
    @Override
    public boolean isConnected() {
        Boolean ret = myRobotClient.getConnected();
        if(ret == null){
            theLogger.warning("RobotRequest timed out.  "
                    + "Unable to get connection status.");
            return false;
        }
        return ret;
    }

    @Override
    public void setEnabled(boolean val) {
        Boolean ret = 
                val ? myRobotClient.sendEnable() : myRobotClient.sendDisable();
        if(ret == null){
            theLogger.warning("RobotRequest timed out.  "
                    + "Unable to set enabled value.");
        }
    }

    @Override
    public boolean isEnabled() {
        Boolean ret = myRobotClient.getEnabled();
        if(ret == null){
            theLogger.warning("RobotRequest timed out.  "
                    + "Unable to get enabled status.");
            return false;
        }
        return ret;
    }
    
    boolean setJointEnabled(JointId jId, boolean val){
        Boolean ret = val ?
                myRobotClient.sendJointEnable(jId) :
                myRobotClient.sendJointDisable(jId);
        if(ret == null){
            theLogger.warning("RobotRequest timed out.  "
                    + "Unable to get joint enabled confirmation.");
            return false;
        }
        return ret;
    }
    
    boolean getJointEnabled(JointId jId){
        Boolean ret = myRobotClient.getJointEnabled(jId);
        if(ret == null){
            theLogger.warning("RobotRequest timed out.  "
                    + "Unable to get joint enabled status.");
        }
        return ret;
    }

    @Override
    public void move(RobotPositionMap positions, long lenMillisec) {
        MotionFrame frame = new DefaultMotionFrame();
        frame.setFrameLengthMillisec(lenMillisec);
        frame.setGoalPositions(positions);
        frame.setPreviousPositions(myPreviousPositions);
        frame.setTimestampMillisecUTC(TimeUtils.now());
        myRobotClient.sendMovement(frame);
        myPreviousPositions = positions;
        setGoals(myPreviousPositions);
    }
    
    private void setGoals(RobotPositionMap goals){
        for(Entry e : goals.entrySet()){
            JointId jId = e.getKey();
            NormalizedDouble val = e.getValue();
            RemoteJoint j = getJoint(jId);
            if(j == null){
                continue;
            }
            j.setGoalPosition(val);
        }
    }
}