BaseControllerNode.java

Go to the documentation of this file.

/*
 * Copyright (C) 2013 Creativa77.
 *
 * 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 com.github.c77.base_controller;

import com.github.c77.base_driver.BaseDevice;

import org.apache.commons.logging.Log;
import org.apache.commons.logging.LogFactory;
import org.ros.message.MessageListener;
import org.ros.namespace.GraphName;
import org.ros.node.AbstractNodeMain;
import org.ros.node.ConnectedNode;
import org.ros.node.Node;
import org.ros.node.topic.Subscriber;

import geometry_msgs.Twist;

public class BaseControllerNode extends AbstractNodeMain implements MessageListener<Twist> {
    private final BaseDevice baseDevice;
    private double linearVelX = 0.0;
    private double angVelZ = 0.0;
    private String CMD_VEL_TOPIC;

    private static final Log log = LogFactory.getLog(BaseControllerNode.class);
    Thread baseControllerThread;

    @Override
    public GraphName getDefaultNodeName() {
        return GraphName.of("android/move_base");
    }

    public BaseControllerNode(BaseDevice baseDevice, String vel_topic) {
        // TODO: Use ROS params to configure topic names
        CMD_VEL_TOPIC = vel_topic;
        this.baseDevice = baseDevice;
    }

    @Override
    public void onStart(ConnectedNode connectedNode) {
        log.info("Base controller starting.");

        baseControllerThread = new Thread() {
            @Override
            public void run() {
                // thread to constantly send commands to the base
                try {
                    while(true) {
                        baseDevice.move(linearVelX, angVelZ);
                        Thread.sleep(250);
                    }
                } catch (Throwable t) {
                    log.error("Exception occurred during move loop", t);
                    // Whenever we get interrupted out of the loop, for any reason
                    // we try to stop the base, just in case
                    try {
                        setTwistValues(0.0, 0.0);
                        baseDevice.move(0.0, 0.0);
                    } catch(Throwable t0) {
                    }
                }
            }
        };

        // Initialize base.
        baseDevice.initialize();
        baseControllerThread.start();

        // Start base_controller subscriber
        Subscriber<Twist> vel_listener = connectedNode.newSubscriber(CMD_VEL_TOPIC, Twist._TYPE);
        vel_listener.addMessageListener(this);

        log.info("Base controller initialized.");
    }

    @Override
    public void onShutdown(Node node) {
        // TODO: Make sure shutdown is working properly. i.e.: shutdown controller thread
        super.onShutdown(node);
    }

    @Override
    public void onShutdownComplete(Node node) {
        // TODO: Verify shutdown is working properly
        super.onShutdownComplete(node);
    }

    private synchronized void setTwistValues(double linearVelX, double angVelZ) {
        this.linearVelX = linearVelX;
        this.angVelZ = angVelZ;
        log.info("synchronized setting: (" + this.linearVelX + "," + this.angVelZ + ")");
    }

    @Override
    public void onNewMessage(Twist twist) {
        log.info("Current Twist msg: " + twist);
        setTwistValues(twist.getLinear().getX(), twist.getAngular().getZ());
    }
}