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" 55 TEST(SpawnTest, checkBoxStackDrift)
61 gazebo::SpawnModel spawn_model;
64 std::string urdf_filename = std::string(
g_argv[1]);
65 ROS_DEBUG_NAMED(
"contact_tolerance",
"loading file: %s",urdf_filename.c_str());
67 TiXmlDocument xml_in(urdf_filename);
69 std::ostringstream stream;
71 spawn_model.request.model_xml = stream.str();
72 ROS_DEBUG_NAMED(
"contact_tolerance",
"XML string: %s",stream.str().c_str());
74 spawn_model.request.robot_namespace =
"";
75 spawn_model.request.reference_frame =
"";
78 for (
int i=0;i<10;i++)
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));
94 gazebo::GetModelState model_state;
95 model_state.request.model_name =
"box_9";
96 double test_duration = 10.0;
97 double LINEAR_DRIFT_TOLERANCE = 0.0105;
101 ASSERT_TRUE(get_model_state_client.
call(model_state));
102 geometry_msgs::Pose initial_pose = model_state.response.pose;
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);
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);
117 geometry_msgs::Quaternion current_q = current_pose.orientation;
118 geometry_msgs::Quaternion initial_q = initial_pose.orientation;
128 int main(
int argc,
char** argv)
131 testing::InitGoogleTest(&argc, argv);
134 return RUN_ALL_TESTS();
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)
#define ROS_DEBUG_NAMED(name,...)
ROSCPP_DECL bool waitForService(const std::string &service_name, int32_t timeout)