au.com.rsutton.rosjava.differentialDrive.DiffTf.java Source code

Java tutorial

Introduction

Here is the source code for au.com.rsutton.rosjava.differentialDrive.DiffTf.java

Source

package au.com.rsutton.rosjava.differentialDrive;

import geometry_msgs.Transform;
import geometry_msgs.TransformStamped;
import nav_msgs.Odometry;

import org.apache.commons.lang.NotImplementedException;
import org.ros.concurrent.CancellableLoop;
import org.ros.message.MessageListener;
import org.ros.message.Time;
import org.ros.namespace.GraphName;
import org.ros.node.AbstractNodeMain;
import org.ros.node.ConnectedNode;
import org.ros.node.topic.Publisher;
import org.ros.node.topic.Subscriber;

import std_msgs.Int16;

/*
 * 
 * Ported to java by Robert Sutton 2014
 * 
 diff_tf.py - follows the output of a wheel encoder and
 creates tf and odometry messages.
 some code borrowed from the arbotix diff_controller script
 A good reference: http://rossum.sourceforge.net/papers/DiffSteer/
    
 Copyright (C) 2012 Jon Stephan. 
    
 This program is free software: you can redistribute it and/or modify
 it under the terms of the GNU General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.
    
 This program is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU General Public License for more details.
    
 You should have received a copy of the GNU General Public License
 along with this program.  If not, see <http://www.gnu.org/licenses/>.
    
 ----------------------------------
 Portions of this code borrowed from the arbotix_python diff_controller.
    
 diff_controller.py - controller for a differential drive
 Copyright (c) 2010-2011 Vanadium Labs LLC.  All right reserved.
    
 Redistribution and use in source and binary forms, with or without
 modification, are permitted provided that the following conditions are met:
 * Redistributions of source code must retain the above copyright
 notice, this list of conditions and the following disclaimer.
 * Redistributions in binary form must reproduce the above copyright
 notice, this list of conditions and the following disclaimer in the
 documentation and/or other materials provided with the distribution.
 * Neither the name of Vanadium Labs LLC nor the names of its 
 contributors may be used to endorse or promote products derived 
 from this software without specific prior written permission.
    
 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS BE LIABLE FOR ANY DIRECT, INDIRECT,
 INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
 OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
 OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
 ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

public class DiffTf extends AbstractNodeMain {

    private int rate;
    private int ticks_meter;
    private double base_width;
    private String base_frame_id;
    private String odom_frame_id;
    private int encoder_min;
    private int encoder_max;
    private double encoder_low_wrap;
    private double encoder_high_wrap;
    private double t_delta;
    private double t_next;
    private Double enc_left;

    private double left;
    private Double right;
    private int lmult;
    private int prev_lencoder;
    private int rmult;
    private int prev_rencoder;
    private double x;
    private double y;
    private double th;
    private double dx;
    private double dr;
    private long then;
    private Double enc_right;
    private String nameSpace;

    public DiffTf(String nameSpace) {
        this.nameSpace = nameSpace;
        // initialize with sensible defaults
        rate = 10;
        ticks_meter = 50;
        base_width = 0.245; // The wheel base width in meters

        base_frame_id = "base_link";// # the name of the base frame of the robot
        odom_frame_id = "odom";// # the name of the odometry reference frame

        encoder_min = -32768;
        encoder_max = 32768;
        encoder_low_wrap = (encoder_max - encoder_min) * 0.3 + encoder_min;
        encoder_high_wrap = (encoder_max - encoder_min) * 0.7 + encoder_min;

        t_delta = 1.0 / rate;
        t_next = System.currentTimeMillis() + t_delta;

        // internal data
        enc_left = null;// # wheel encoder readings
        enc_right = null;//
        left = 0d; // # actual values coming back from robot
        right = 0d;
        lmult = 0;
        rmult = 0;
        prev_lencoder = 0;
        prev_rencoder = 0;
        x = 0;// # position in xy plane
        y = 0;
        th = 0;
        dx = 0;// # speeds in x/rotation
        dr = 0;
        then = System.currentTimeMillis();
    }

    @Override
    public void onStart(ConnectedNode connectedNode) {
        // log = connectedNode.getLog();

        final Publisher<Odometry> odomPub = Topic.ODOM.newPublisher(connectedNode, nameSpace);

        final Publisher<TransformStamped> odomBroadcaster = Topic.TF.newPublisher(connectedNode, nameSpace);

        // This CancellableLoop will be canceled automatically when the node
        // shuts
        // down.
        connectedNode.executeCancellableLoop(new CancellableLoop() {

            @Override
            protected void setup() {
            }

            @Override
            protected void loop() throws InterruptedException {
                update();
                Thread.sleep(rate);
            }

            private void update() {
                long now = System.currentTimeMillis();
                if (now > t_next) {
                    long elapsed = now - then;
                    then = now;
                    elapsed /= 1000;

                    // calculate odometry

                    double d_left = 0;
                    double d_right = 0;
                    if (enc_left != null)

                    {
                        d_left = (left - enc_left) / ticks_meter;
                        d_right = (right - enc_right) / ticks_meter;
                    }
                    enc_left = left;
                    enc_right = right;

                    // distance traveled is the average of the two wheels
                    double d = (d_left + d_right) / 2;
                    // this approximation works (in radians) for small angles
                    th = (d_right - d_left) / base_width;
                    // calculate velocities
                    dx = d / elapsed;
                    dr = th / elapsed;

                    if (d != 0) {
                        // calculate distance traveled in x and y
                        x = Math.cos(th) * d;
                        y = -Math.sin(th) * d;
                        // calculate the final position of the robot
                        x = x + (Math.cos(th) * x - Math.sin(th) * y);
                        y = y + (Math.sin(th) * x + Math.cos(th) * y);
                    }
                    if (th != 0) {
                        th = th + th;
                    }

                    // publish the odom information
                    TransformStamped transform = odomBroadcaster.newMessage();
                    transform.getTransform().getRotation().setX(0);
                    transform.getTransform().getRotation().setY(0);
                    transform.getTransform().getRotation().setZ(Math.sin(th / 2));
                    transform.getTransform().getRotation().setW(Math.cos(th / 2));
                    transform.getTransform().getTranslation().setX(x);
                    transform.getTransform().getTranslation().setY(y);
                    transform.getTransform().getTranslation().setZ(0);
                    transform.getHeader().setStamp(new Time());
                    transform.setChildFrameId(odom_frame_id);
                    transform.getHeader().setFrameId(base_frame_id);
                    odomBroadcaster.publish(transform);

                    Odometry odom = odomPub.newMessage();

                    odom.getHeader().setStamp(new Time());
                    odom.getHeader().setFrameId(odom_frame_id);
                    odom.getPose().getPose().getPosition().setX(x);
                    odom.getPose().getPose().getPosition().setY(y);
                    odom.getPose().getPose().getPosition().setZ(0);
                    odom.getPose().getPose().setOrientation(transform.getTransform().getRotation());
                    odom.setChildFrameId(base_frame_id);
                    odom.getTwist().getTwist().getLinear().setX(dx);
                    odom.getTwist().getTwist().getLinear().setY(0);
                    odom.getTwist().getTwist().getAngular().setZ(dr);
                    odomPub.publish(odom);

                }
            }
        });

        Subscriber<Int16> sub_lwheel = Topic.LWHEEL.newSubscriber(connectedNode, nameSpace);
        sub_lwheel.addMessageListener(new MessageListener<Int16>() {
            @Override
            public void onNewMessage(Int16 message) {
                short enc = message.getData();
                if (enc < encoder_low_wrap && prev_lencoder > encoder_high_wrap) {
                    lmult = lmult + 1;
                }

                if (enc > encoder_high_wrap && prev_lencoder < encoder_low_wrap) {
                    lmult = lmult - 1;
                }

                left = 1.0 * (enc + lmult * (encoder_max - encoder_min));
                prev_lencoder = enc;
            }
        });
        Subscriber<Int16> sub_rwheel = Topic.RWHEEL.newSubscriber(connectedNode, nameSpace);
        sub_rwheel.addMessageListener(new MessageListener<Int16>() {
            @Override
            public void onNewMessage(Int16 message) {
                short enc = message.getData();
                if (enc < encoder_low_wrap && prev_rencoder > encoder_high_wrap) {
                    rmult = rmult + 1;
                }

                if (enc > encoder_high_wrap && prev_rencoder < encoder_low_wrap) {
                    rmult = rmult - 1;
                }

                right = 1.0 * (enc + rmult * (encoder_max - encoder_min));
                prev_rencoder = enc;
            }
        });

    }

    @Override
    public GraphName getDefaultNodeName() {
        throw new NotImplementedException();
    }
}