Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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
00049
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
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
00064 std::string urdf_filename = std::string(g_argv[1]);
00065 ROS_DEBUG("loading file: %s",urdf_filename.c_str());
00066
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();
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
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
00091
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;
00097 double LINEAR_DRIFT_TOLERANCE = 0.0105;
00098
00099 ros::Time timeout = ros::Time::now() + ros::Duration(test_duration);
00100
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
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 }