test_linear_spline_velocity_scaler.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2009, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00037 #include <gtest/gtest.h>
00038 #include <filters/filter_chain.h>
00039 #include <arm_navigation_msgs/FilterJointTrajectory.h>
00040 
00041 using namespace filters;
00042 
00043 TEST(TestLinearSplineVelocityScaler, TestLinearSplineVelocityScaler1)
00044 {
00045   // make the filter chain:
00046   FilterChain<arm_navigation_msgs::FilterJointTrajectory> chain("arm_navigation_msgs::FilterJointTrajectory");
00047   ASSERT_TRUE(chain.configure("TestLinearSplineVelocityScaler"));
00048 
00049   // create the input:
00050   int length = 4;
00051   int joints = 2;
00052 
00053   arm_navigation_msgs::FilterJointTrajectory wpt;
00054   arm_navigation_msgs::FilterJointTrajectory wpt_out;
00055   wpt.request.trajectory.points.resize(length);
00056   wpt.request.trajectory.joint_names.resize(joints);
00057   wpt.request.trajectory.joint_names[0] = std::string("test0");
00058   wpt.request.trajectory.joint_names[1] = std::string("test1");
00059 
00060   wpt.request.limits.resize(joints);
00061   wpt.request.limits[0].max_velocity = 0.5;
00062   wpt.request.limits[0].has_velocity_limits = 1;
00063 
00064   wpt.request.limits[1].max_velocity = 0.25;
00065   wpt.request.limits[1].has_velocity_limits = 1;
00066 
00067   for (int i=0; i<length; i++)
00068   {
00069     wpt.request.trajectory.points[i].positions.resize(joints);
00070     wpt.request.trajectory.points[i].velocities.resize(joints);
00071     wpt.request.trajectory.points[i].accelerations.resize(joints);
00072     for(int j=0; j<joints; j++)
00073     {
00074       wpt.request.trajectory.points[i].positions[j] = i+j;
00075       wpt.request.trajectory.points[i].velocities[j] = 0.0;
00076       wpt.request.trajectory.points[i].accelerations[j] = 0.0;
00077     }
00078     wpt.request.trajectory.points[i].time_from_start = ros::Duration(0.0);
00079   }
00080   chain.update(wpt, wpt_out);
00081   // verify that velocities are 0:
00082 
00083   EXPECT_NEAR(wpt_out.request.trajectory.points[0].time_from_start.toSec(), 0.0, 1e-8);
00084   EXPECT_NEAR(wpt_out.request.trajectory.points[1].time_from_start.toSec(), 4.0, 1e-8);
00085   EXPECT_NEAR(wpt_out.request.trajectory.points[2].time_from_start.toSec(), 8.0, 1e-8);
00086   EXPECT_NEAR(wpt_out.request.trajectory.points[3].time_from_start.toSec(), 12.0, 1e-8);
00087 
00088 }
00089 
00090 int main(int argc, char** argv)
00091 {
00092   testing::InitGoogleTest(&argc, argv);
00093   ros::init(argc, argv, "test_linear_spline_velocity_scaler");
00094   return RUN_ALL_TESTS();
00095 }


spline_smoother
Author(s): Mrinal Kalakrishnan / mail@mrinal.net
autogenerated on Mon Dec 2 2013 12:35:44