test_common.h
Go to the documentation of this file.
00001 
00002 // Copyright (C) 2013, PAL Robotics S.L.
00003 //
00004 // Redistribution and use in source and binary forms, with or without
00005 // modification, are permitted provided that the following conditions are met:
00006 //   * Redistributions of source code must retain the above copyright notice,
00007 //     this list of conditions and the following disclaimer.
00008 //   * Redistributions in binary form must reproduce the above copyright
00009 //     notice, this list of conditions and the following disclaimer in the
00010 //     documentation and/or other materials provided with the distribution.
00011 //   * Neither the name of PAL Robotics, Inc. nor the names of its
00012 //     contributors may be used to endorse or promote products derived from
00013 //     this software without specific prior written permission.
00014 //
00015 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00016 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00017 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00018 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00019 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00020 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00021 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00022 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00023 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00024 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00025 // POSSIBILITY OF SUCH DAMAGE.
00027 
00029 
00030 #include <cmath>
00031 
00032 #include <gtest/gtest.h>
00033 
00034 #include <ros/ros.h>
00035 
00036 #include <geometry_msgs/TwistStamped.h>
00037 #include <nav_msgs/Odometry.h>
00038 #include <tf/tf.h>
00039 
00040 #include <std_srvs/Empty.h>
00041 
00042 // Floating-point value comparison threshold
00043 const double EPS = 0.01;
00044 const double POSITION_TOLERANCE = 0.01; // 1 cm-s precision
00045 const double VELOCITY_TOLERANCE = 0.02; // 2 cm-s-1 precision
00046 const double JERK_LINEAR_VELOCITY_TOLERANCE = 0.10; // 10 cm-s-1 precision
00047 const double JERK_ANGULAR_VELOCITY_TOLERANCE = 0.05; // 3 deg-s-1 precision
00048 const double ORIENTATION_TOLERANCE = 0.03; // 0.57 degree precision
00049 
00050 class DiffDriveControllerTest : public ::testing::Test
00051 {
00052 public:
00053 
00054   DiffDriveControllerTest()
00055   : cmd_pub(nh.advertise<geometry_msgs::Twist>("cmd_vel", 100))
00056   , odom_sub(nh.subscribe("odom", 100, &DiffDriveControllerTest::odomCallback, this))
00057   , vel_out_sub(nh.subscribe("cmd_vel_out", 100, &DiffDriveControllerTest::cmdVelOutCallback, this))
00058   , start_srv(nh.serviceClient<std_srvs::Empty>("start"))
00059   , stop_srv(nh.serviceClient<std_srvs::Empty>("stop"))
00060   {
00061   }
00062 
00063   ~DiffDriveControllerTest()
00064   {
00065     odom_sub.shutdown();
00066   }
00067 
00068   nav_msgs::Odometry getLastOdom(){ return last_odom; }
00069   geometry_msgs::TwistStamped getLastCmdVelOut(){ return last_cmd_vel_out; }
00070   void publish(geometry_msgs::Twist cmd_vel){ cmd_pub.publish(cmd_vel); }
00071   bool isControllerAlive(){ return (odom_sub.getNumPublishers() > 0) && (cmd_pub.getNumSubscribers() > 0); }
00072   bool isPublishingCmdVelOut(){ return (vel_out_sub.getNumPublishers() > 0); }
00073 
00074   void start(){ std_srvs::Empty srv; start_srv.call(srv); }
00075   void stop(){ std_srvs::Empty srv; stop_srv.call(srv); }
00076 
00077 private:
00078   ros::NodeHandle nh;
00079   ros::Publisher cmd_pub;
00080   ros::Subscriber odom_sub;
00081   ros::Subscriber vel_out_sub;
00082   nav_msgs::Odometry last_odom;
00083   geometry_msgs::TwistStamped last_cmd_vel_out;
00084 
00085   ros::ServiceClient start_srv;
00086   ros::ServiceClient stop_srv;
00087 
00088   void odomCallback(const nav_msgs::Odometry& odom)
00089   {
00090     ROS_INFO_STREAM("Callback received: pos.x: " << odom.pose.pose.position.x
00091                      << ", orient.z: " << odom.pose.pose.orientation.z
00092                      << ", lin_est: " << odom.twist.twist.linear.x
00093                      << ", ang_est: " << odom.twist.twist.angular.z);
00094     last_odom = odom;
00095   }
00096 
00097   void cmdVelOutCallback(const geometry_msgs::TwistStamped& cmd_vel_out)
00098   {
00099     ROS_INFO_STREAM("Callback received: lin: " << cmd_vel_out.twist.linear.x
00100                      << ", ang: " << cmd_vel_out.twist.angular.z);
00101     last_cmd_vel_out = cmd_vel_out;
00102   }
00103 };
00104 
00105 inline tf::Quaternion tfQuatFromGeomQuat(const geometry_msgs::Quaternion& quat)
00106 {
00107   return tf::Quaternion(quat.x, quat.y, quat.z, quat.w);
00108 }
00109 


diff_drive_controller
Author(s): Bence Magyar
autogenerated on Thu Jun 6 2019 18:58:48