contact_tolerance.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: John Hsu */
36 
37 #include <string>
38 #include <gtest/gtest.h>
39 #include "gazebo/SpawnModel.h"
40 #include "geometry_msgs/Pose.h"
41 #include "geometry_msgs/Quaternion.h"
42 #include "std_srvs/Empty.h"
43 #include "gazebo/GetModelState.h"
44 
45 #include <sstream>
46 #include <tinyxml.h>
47 
48 // Including ros, just to be able to call ros::init(), to remove unwanted
49 // args from command-line.
50 #include <ros/ros.h>
51 
52 int g_argc;
53 char** g_argv;
54 
55 TEST(SpawnTest, checkBoxStackDrift)
56 {
57  ros::NodeHandle nh("");
58  // make the service call to spawn model
59  ros::service::waitForService("gazebo/spawn_urdf_model");
60  ros::ServiceClient spawn_model_client = nh.serviceClient<gazebo::SpawnModel>("gazebo/spawn_urdf_model");
61  gazebo::SpawnModel spawn_model;
62 
63  // load urdf file
64  std::string urdf_filename = std::string(g_argv[1]);
65  ROS_DEBUG_NAMED("contact_tolerance", "loading file: %s",urdf_filename.c_str());
66  // read urdf / gazebo model xml from file
67  TiXmlDocument xml_in(urdf_filename);
68  xml_in.LoadFile();
69  std::ostringstream stream;
70  stream << xml_in;
71  spawn_model.request.model_xml = stream.str(); // load xml file
72  ROS_DEBUG_NAMED("contact_tolerance", "XML string: %s",stream.str().c_str());
73 
74  spawn_model.request.robot_namespace = "";
75  spawn_model.request.reference_frame = "";
76 
77  // spawn 10 boxes
78  for (int i=0;i<10;i++)
79  {
80  std::ostringstream mn_stream;
81  mn_stream << "box_" << i;
82  spawn_model.request.model_name = mn_stream.str();
83  geometry_msgs::Pose pose;
84  pose.position.x = pose.position.y = 0; pose.position.z = 1 + 2.0*(i);
85  pose.orientation.w = 1.0; pose.orientation.x = pose.orientation.y = pose.orientation.z = 0;
86  spawn_model.request.initial_pose = pose;
87  ASSERT_TRUE(spawn_model_client.call(spawn_model));
88  }
89 
90  // get pose of top box, check for drift
91  // setup the service call to get model state
92  ros::service::waitForService("gazebo/get_model_state");
93  ros::ServiceClient get_model_state_client = nh.serviceClient<gazebo::GetModelState>("gazebo/get_model_state");
94  gazebo::GetModelState model_state;
95  model_state.request.model_name = "box_9";
96  double test_duration = 10.0; // check drift over this period of sim time
97  double LINEAR_DRIFT_TOLERANCE = 0.0105; // linear drift tolerance of top block
98  // given contact layer is 1mm, 10 blocks leads to 10*1mm and .5mm of numerical error
99  ros::Time timeout = ros::Time::now() + ros::Duration(test_duration);
100  // get intial pose
101  ASSERT_TRUE(get_model_state_client.call(model_state));
102  geometry_msgs::Pose initial_pose = model_state.response.pose;
103  while (ros::Time::now() < timeout)
104  {
105  ASSERT_TRUE(get_model_state_client.call(model_state));
106  geometry_msgs::Pose current_pose = model_state.response.pose;
107  double error_x = fabs(current_pose.position.x - initial_pose.position.x);
108  double error_y = fabs(current_pose.position.y - initial_pose.position.y);
109  double error_z = fabs(current_pose.position.z - initial_pose.position.z);
110  double error_linear = sqrt(error_x*error_x+error_y*error_y+error_z*error_z);
111  ROS_INFO_NAMED("contact_tolerance", "error: %f",error_linear);
112  if (error_linear > LINEAR_DRIFT_TOLERANCE);
113  ROS_INFO_NAMED("contact_tolerance", "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);
114  ASSERT_TRUE(error_linear <= LINEAR_DRIFT_TOLERANCE);
115 
116  // FIXME: do something about orientation drift too
117  geometry_msgs::Quaternion current_q = current_pose.orientation;
118  geometry_msgs::Quaternion initial_q = initial_pose.orientation;
119  }
120 
121 
122  SUCCEED();
123 }
124 
125 
126 
127 
128 int main(int argc, char** argv)
129 {
130  ros::init(argc, argv, "test", ros::init_options::AnonymousName);
131  testing::InitGoogleTest(&argc, argv);
132  g_argc = argc;
133  g_argv = argv;
134  return RUN_ALL_TESTS();
135 }
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
#define ROS_INFO_NAMED(name,...)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
bool call(MReq &req, MRes &res)
int main(int argc, char **argv)
TEST(SpawnTest, checkBoxStackDrift)
#define ROS_DEBUG_NAMED(name,...)
int g_argc
int SUCCEED
char ** g_argv
static Time now()
ROSCPP_DECL bool waitForService(const std::string &service_name, int32_t timeout)


gazebo_plugins
Author(s): John Hsu
autogenerated on Tue Apr 6 2021 02:19:39