contact_tolerance.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2008, 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 
00035 /* Author: John Hsu */
00036 
00037 #include <string>
00038 #include <gtest/gtest.h>
00039 #include "gazebo/SpawnModel.h"
00040 #include "geometry_msgs/Pose.h"
00041 #include "geometry_msgs/Quaternion.h"
00042 #include "std_srvs/Empty.h"
00043 #include "gazebo/GetModelState.h"
00044 
00045 #include <sstream>
00046 #include <tinyxml.h>
00047 
00048 // Including ros, just to be able to call ros::init(), to remove unwanted
00049 // args from command-line.
00050 #include <ros/ros.h>
00051 
00052 int g_argc;
00053 char** g_argv;
00054 
00055 TEST(SpawnTest, checkBoxStackDrift)
00056 {
00057   ros::NodeHandle nh("");
00058   // make the service call to spawn model
00059   ros::service::waitForService("gazebo/spawn_urdf_model");
00060   ros::ServiceClient spawn_model_client = nh.serviceClient<gazebo::SpawnModel>("gazebo/spawn_urdf_model");
00061   gazebo::SpawnModel spawn_model;
00062 
00063   // load urdf file
00064   std::string urdf_filename = std::string(g_argv[1]);
00065   ROS_DEBUG("loading file: %s",urdf_filename.c_str());
00066   // read urdf / gazebo model xml from file
00067   TiXmlDocument xml_in(urdf_filename);
00068   xml_in.LoadFile();
00069   std::ostringstream stream;
00070   stream << xml_in;
00071   spawn_model.request.model_xml = stream.str(); // load xml file
00072   ROS_DEBUG("XML string: %s",stream.str().c_str());
00073 
00074   spawn_model.request.robot_namespace = "";
00075   spawn_model.request.reference_frame = "";
00076 
00077   // spawn 10 boxes
00078   for (int i=0;i<10;i++)
00079   {
00080     std::ostringstream mn_stream;
00081     mn_stream << "box_" << i;
00082     spawn_model.request.model_name = mn_stream.str();
00083     geometry_msgs::Pose pose;
00084     pose.position.x = pose.position.y = 0; pose.position.z = 1 + 2.0*(i);
00085     pose.orientation.w = 1.0; pose.orientation.x = pose.orientation.y = pose.orientation.z = 0;
00086     spawn_model.request.initial_pose = pose;
00087     ASSERT_TRUE(spawn_model_client.call(spawn_model));
00088   }
00089 
00090   // get pose of top box, check for drift
00091   // setup the service call to get model state
00092   ros::service::waitForService("gazebo/get_model_state");
00093   ros::ServiceClient get_model_state_client = nh.serviceClient<gazebo::GetModelState>("gazebo/get_model_state");
00094   gazebo::GetModelState model_state;
00095   model_state.request.model_name = "box_9";
00096   double test_duration = 10.0; // check drift over this period of sim time
00097   double LINEAR_DRIFT_TOLERANCE = 0.0105; // linear drift tolerance of top block
00098                                           // given contact layer is 1mm, 10 blocks leads to 10*1mm and .5mm of numerical error
00099   ros::Time timeout = ros::Time::now() + ros::Duration(test_duration);
00100   // get intial pose
00101   ASSERT_TRUE(get_model_state_client.call(model_state));
00102   geometry_msgs::Pose initial_pose = model_state.response.pose;
00103   while (ros::Time::now() < timeout)
00104   {
00105     ASSERT_TRUE(get_model_state_client.call(model_state));
00106     geometry_msgs::Pose current_pose = model_state.response.pose;
00107     double error_x = fabs(current_pose.position.x - initial_pose.position.x);
00108     double error_y = fabs(current_pose.position.y - initial_pose.position.y);
00109     double error_z = fabs(current_pose.position.z - initial_pose.position.z);
00110     double error_linear = sqrt(error_x*error_x+error_y*error_y+error_z*error_z);
00111     ROS_INFO("error: %f",error_linear);
00112     if (error_linear > LINEAR_DRIFT_TOLERANCE);
00113       ROS_INFO("box stack teset failed with this linear error (%f):  this means the top box in the box stack is moving too much.  Check to see if surface contact layer has changed from 1mm, bounce parameter has changed, world time step, solver has changed.  You can reproduce this test by:\n\nroslaunch gazebo_tests empty.launch\nrosrun gazebo_tests contact_tolerance `rospack find gazebo_tests`/test/urdf/box.urdf\n",error_linear);
00114     ASSERT_TRUE(error_linear <= LINEAR_DRIFT_TOLERANCE);
00115 
00116     // FIXME: do something about orientation drift too
00117     geometry_msgs::Quaternion current_q = current_pose.orientation;
00118     geometry_msgs::Quaternion initial_q = initial_pose.orientation;
00119   }
00120 
00121 
00122   SUCCEED();
00123 }
00124 
00125 
00126 
00127 
00128 int main(int argc, char** argv)
00129 {
00130   ros::init(argc, argv, "test", ros::init_options::AnonymousName);
00131   testing::InitGoogleTest(&argc, argv);
00132   g_argc = argc;
00133   g_argv = argv;
00134   return RUN_ALL_TESTS();
00135 }


gazebo_plugins
Author(s): John Hsu
autogenerated on Thu Jun 6 2019 18:41:09