TFBroadcaster.java
Go to the documentation of this file.
00001 /* 
00002  * Copyright (c) 2011, Sjoerd van den Dries
00003  * All rights reserved.
00004  * 
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  * 
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Technische Universiteit Eindhoven nor the
00014  *       names of its contributors may be used to endorse or promote products
00015  *       derived from this software without specific prior written permission.
00016  * 
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 package tfjava;
00031 
00032 import ros.*;
00033 import ros.communication.*;
00034 
00035 import javax.vecmath.Vector3d;
00036 import javax.vecmath.Quat4d;
00037 
00038 import ros.pkg.tf.msg.tfMessage;
00039 
00040 import ros.pkg.geometry_msgs.msg.TransformStamped;
00041 import ros.pkg.geometry_msgs.msg.Vector3;
00042 import ros.pkg.geometry_msgs.msg.Quaternion;
00043 
00044 import java.util.ArrayList;
00045 
00054 public class TFBroadcaster {
00055 
00059     public static void sendTransform(Vector3d transl, Quat4d rot, Time time, String parentFrame, String childFrame) {
00060         // get ROS instance and create node handle
00061         Ros ros = Ros.getInstance();
00062         NodeHandle node = ros.createNodeHandle();
00063         
00064         try {
00065             // convert translation vector and quaternion to geometry messages
00066             Vector3 tMsg = new Vector3();
00067             Quaternion rMsg = new Quaternion();
00068             tMsg.x = transl.x; tMsg.y = transl.y; tMsg.z = transl.z;
00069             rMsg.x = rot.x; rMsg.y = rot.y; rMsg.z = rot.z; rMsg.w = rot.w;
00070                 
00071             // create TransformStamped message (is a geometry msg, do NOT confuse with StampedTransform class)
00072             TransformStamped tfMsg = new TransformStamped();        
00073             tfMsg.header.frame_id = parentFrame;
00074             tfMsg.header.stamp = time;
00075             tfMsg.child_frame_id = childFrame;
00076             tfMsg.transform.translation = tMsg;
00077             tfMsg.transform.rotation = rMsg;
00078             
00079             // create tfMessage and add TransformStamped message to it
00080             tfMessage msg = new tfMessage();
00081             msg.transforms = new ArrayList();
00082             msg.transforms.add(tfMsg);
00083             
00084             // publish the message
00085             Publisher<tfMessage> pub = node.advertise("/tf", new tfMessage(), 10);
00086             pub.publish(msg);
00087         } catch (Exception e) {
00088             ros.logError("TFBroadcaster: " + e.toString());
00089         }
00090     }
00091     
00092     public static void sendTransform(StampedTransform t) {
00093         sendTransform(t.getTranslation(), t.getRotation(), t.timeStamp, t.frameID, t.childFrameID);
00094     }
00095 
00096 }


tfjava
Author(s): Sjoerd van den Dries
autogenerated on Sun Oct 5 2014 22:54:19