test_buffer_client.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 *
00003 * Software License Agreement (BSD License)
00004 *
00005 *  Copyright (c) 2009, Willow Garage, Inc.
00006 *  All rights reserved.
00007 *
00008 *  Redistribution and use in source and binary forms, with or without
00009 *  modification, are permitted provided that the following conditions
00010 *  are met:
00011 *
00012 *   * Redistributions of source code must retain the above copyright
00013 *     notice, this list of conditions and the following disclaimer.
00014 *   * Redistributions in binary form must reproduce the above
00015 *     copyright notice, this list of conditions and the following
00016 *     disclaimer in the documentation and/or other materials provided
00017 *     with the distribution.
00018 *   * Neither the name of Willow Garage, Inc. nor the names of its
00019 *     contributors may be used to endorse or promote products derived
00020 *     from this software without specific prior written permission.
00021 *
00022 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 *  POSSIBILITY OF SUCH DAMAGE.
00034 *
00035 * Author: Eitan Marder-Eppstein
00036 *********************************************************************/
00037 #include <gtest/gtest.h>
00038 #include <tf2_ros/buffer_client.h>
00039 #include <ros/ros.h>
00040 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
00041 #include <tf2_kdl/tf2_kdl.h>
00042 #include <tf2_bullet/tf2_bullet.h>
00043 
00044 static const double EPS = 1e-3;
00045 
00046 TEST(tf2_ros, buffer_client)
00047 {
00048   tf2::BufferClient client("tf_action");
00049 
00050   //make sure that things are set up
00051   client.waitForServer();
00052 
00053   geometry_msgs::PointStamped p1;
00054   p1.header.frame_id = "a";
00055   p1.header.stamp = ros::Time();
00056   p1.point.x = 0.0;
00057   p1.point.y = 0.0;
00058   p1.point.z = 0.0;
00059 
00060   try
00061   {
00062     geometry_msgs::PointStamped p2 = client.transform(p1, "b");
00063     ROS_INFO("p1: (%.2f, %.2f, %.2f), p2: (%.2f, %.2f, %.2f)", p1.point.x,
00064         p1.point.y, p1.point.z, p2.point.x, p2.point.y, p2.point.z);
00065 
00066     EXPECT_NEAR(p2.point.x, -5.0, EPS);
00067     EXPECT_NEAR(p2.point.y, -6.0, EPS);
00068     EXPECT_NEAR(p2.point.z, -7.0, EPS);
00069   }
00070   catch(tf2::TransformException& ex)
00071   {
00072     ROS_ERROR("Failed to transform: %s", ex.what());
00073   }
00074 } 
00075 
00076 TEST(tf2_ros, buffer_client_different_types)
00077 {
00078   tf2::BufferClient client("tf_action");
00079 
00080   //make sure that things are set up
00081   client.waitForServer();
00082 
00083   tf2::Stamped<KDL::Vector> k1(KDL::Vector(0, 0, 0), ros::Time(), "a");
00084 
00085   try
00086   {
00087     tf2::Stamped<btVector3> b1;
00088     client.transform(k1, b1, "b");
00089     ROS_INFO_STREAM("Bullet: (" << b1[0] << ", " << b1[1] << ", " << b1[2] << ")");
00090     ROS_INFO_STREAM("KDL: (" << k1[0] << ", " << k1[1] << ", " << k1[2] << ")");
00091     EXPECT_NEAR(b1[0], -5.0, EPS);
00092     EXPECT_NEAR(b1[1], -6.0, EPS);
00093     EXPECT_NEAR(b1[2], -7.0, EPS);
00094     EXPECT_EQ(b1.frame_id_, "b");
00095     EXPECT_EQ(k1.frame_id_, "a");
00096   }
00097   catch(tf2::TransformException& ex)
00098   {
00099     ROS_ERROR("Failed to transform: %s", ex.what());
00100   }
00101 } 
00102 
00103 int main(int argc, char** argv)
00104 {
00105   testing::InitGoogleTest(&argc, argv);
00106   ros::init(argc, argv, "buffer_client_test");
00107   return RUN_ALL_TESTS();
00108 }
00109 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends Defines


test_tf2
Author(s): Tully Foote, Eitan Marder-Eppstein
autogenerated on Mon Feb 11 2013 02:14:30