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
00037 #include <planning_models/robot_model.h>
00038 #include <planning_models/kinematic_state.h>
00039 #include <gtest/gtest.h>
00040 #include <sstream>
00041 #include <algorithm>
00042 #include <ctype.h>
00043 #include <planning_models/robot_model.h>
00044 #include <planning_models/kinematic_state.h>
00045 #include <planning_models/transforms.h>
00046 #include <geometric_shapes/shape_operations.h>
00047 #include <collision_distance_field/collision_distance_field_types.h>
00048 #include <collision_distance_field/collision_robot_distance_field.h>
00049 #include <collision_distance_field/collision_world_distance_field.h>
00050 #include <collision_detection_fcl/collision_world_fcl.h>
00051 #include <collision_detection_fcl/collision_robot_fcl.h>
00052 #include <rdf_loader/rdf_loader.h>
00053
00054 #include <boost/filesystem.hpp>
00055 #include <ros/console.h>
00056
00057 static const unsigned int TRIALS = 10000;
00058
00059 class Pr2DistanceFieldCollisionDetectionTester : public testing::Test{
00060
00061 protected:
00062
00063 virtual void SetUp()
00064 {
00065
00066 rml_.reset(new rdf_loader::RDFLoader("robot_description"));
00067 kmodel_.reset(new planning_models::RobotModel(rml_->getURDF(), rml_->getSRDF()));
00068
00069 acm_.reset(new collision_detection::AllowedCollisionMatrix());
00070
00071 ros::NodeHandle nh;
00072
00073 XmlRpc::XmlRpcValue coll_ops;
00074 nh.getParam("robot_description_planning/default_collision_operations", coll_ops);
00075
00076 if (coll_ops.getType() != XmlRpc::XmlRpcValue::TypeArray)
00077 {
00078 ROS_WARN("default_collision_operations is not an array");
00079 return;
00080 }
00081
00082 if (coll_ops.size() == 0)
00083 {
00084 ROS_WARN("No collision operations in default collision operations");
00085 return;
00086 }
00087
00088 for (int i = 0 ; i < coll_ops.size() ; ++i)
00089 {
00090 if (!coll_ops[i].hasMember("object1") || !coll_ops[i].hasMember("object2") || !coll_ops[i].hasMember("operation"))
00091 {
00092 ROS_WARN("All collision operations must have two objects and an operation");
00093 continue;
00094 }
00095 acm_->setEntry(std::string(coll_ops[i]["object1"]), std::string(coll_ops[i]["object2"]), std::string(coll_ops[i]["operation"]) == "disable");
00096 }
00097
00098 crobot_df_.reset(new collision_distance_field::CollisionRobotDistanceField(kmodel_));
00099 cworld_df_.reset(new collision_distance_field::CollisionWorldDistanceField());
00100
00101 crobot_fcl_.reset(new collision_detection::CollisionRobotFCL(kmodel_));
00102 cworld_fcl_.reset(new collision_detection::CollisionWorldFCL());
00103 }
00104
00105 virtual void TearDown()
00106 {
00107
00108 }
00109
00110 protected:
00111
00112 boost::shared_ptr<rdf_loader::RDFLoader> rml_;
00113
00114 planning_models::RobotModelPtr kmodel_;
00115
00116 planning_models::TransformsPtr ftf_;
00117 planning_models::TransformsConstPtr ftf_const_;
00118
00119 boost::shared_ptr<collision_detection::CollisionRobot> crobot_df_;
00120 boost::shared_ptr<collision_detection::CollisionWorld> cworld_df_;
00121
00122 boost::shared_ptr<collision_detection::CollisionRobot> crobot_fcl_;
00123 boost::shared_ptr<collision_detection::CollisionWorld> cworld_fcl_;
00124
00125
00126 collision_detection::AllowedCollisionMatrixPtr acm_;
00127
00128 };
00129
00130 TEST_F(Pr2DistanceFieldCollisionDetectionTester, SpeedTest)
00131 {
00132 planning_models::RobotState *kstate(kmodel_);
00133 kstate.setToDefaultValues();
00134
00135 planning_models::RobotState *::JointStateGroup* jsg = kstate.getJointStateGroup("right_arm");
00136
00137 ros::WallDuration total_speed_df;
00138 ros::WallDuration total_speed_fcl;
00139
00140 unsigned int in_collision_df = 0;
00141 unsigned int in_collision_fcl = 0;
00142 unsigned int fcl_in_coll_df_not = 0;
00143
00144 collision_detection::CollisionRequest req;
00145 req.group_name = "right_arm";
00146 req.contacts = true;
00147
00148 collision_detection::CollisionResult res1;
00149
00150 crobot_df_->checkSelfCollision(req, res1, kstate, *acm_);
00151 crobot_fcl_->checkSelfCollision(req, res1, kstate, *acm_);
00152
00153 for(unsigned int i = 0; i < TRIALS; i++) {
00154 jsg->setToRandomValues();
00155 collision_detection::CollisionResult res;
00156 ros::WallTime before = ros::WallTime::now();
00157 crobot_df_->checkSelfCollision(req, res, kstate);
00158 total_speed_df += (ros::WallTime::now()-before);
00159 bool df_in_collision = false;
00160 if(res.collision) {
00161 in_collision_df++;
00162 df_in_collision = true;
00163 }
00164 res = collision_detection::CollisionResult();
00165 before = ros::WallTime::now();
00166 crobot_fcl_->checkSelfCollision(req, res, kstate, *acm_);
00167 total_speed_fcl += (ros::WallTime::now()-before);
00168 if(res.collision) {
00169 in_collision_fcl++;
00170 if(!df_in_collision) {
00171 fcl_in_coll_df_not++;
00172 }
00173 }
00174 }
00175 std::cerr << "Average time df " << total_speed_df.toSec()/(1.0*TRIALS) << " hz " << (1.0/(total_speed_df.toSec()/(1.0*TRIALS))) << std::endl;
00176 std::cerr << "In collision df " << in_collision_df << std::endl;
00177 std::cerr << "Average time fcl " << total_speed_fcl.toSec()/(1.0*TRIALS) << " hz " << (1.0/(total_speed_fcl.toSec()/(1.0*TRIALS))) << std::endl;
00178 std::cerr << "In collision fcl " << in_collision_fcl << std::endl;
00179 std::cerr << "Fcl in collision df not " << fcl_in_coll_df_not << std::endl;
00180 }
00181
00182 int main(int argc, char **argv)
00183 {
00184 ros::init(argc, argv, "pr2_moveit_cd_speed_test");
00185 testing::InitGoogleTest(&argc, argv);
00186 return RUN_ALL_TESTS();
00187 }