move_arm_utils_test.cpp
Go to the documentation of this file.
00001 #include <move_arm_warehouse/move_arm_utils.h>
00002 #include <gtest/gtest.h>
00003 static const std::string VIS_TOPIC_NAME = "planning_scene_visualizer_markers";
00004 
00005 //in 100 hz ticks
00006 static const unsigned int CONTROL_SPEED = 5;
00007 
00008 static const double BASE_TRANS_SPEED = .3;
00009 static const double BASE_ROT_SPEED = .15;
00010 
00011 static const double HAND_TRANS_SPEED = .05;
00012 static const double HAND_ROT_SPEED = .15;
00013 
00014 static const std::string EXECUTE_RIGHT_TRAJECTORY = "/r_arm_controller/follow_joint_trajectory";
00015 static const std::string EXECUTE_LEFT_TRAJECTORY = "/l_arm_controller/follow_joint_trajectory";
00016 static const std::string LEFT_IK_NAME = "/pr2_left_arm_kinematics/get_constraint_aware_ik";
00017 static const std::string RIGHT_IK_NAME = "/pr2_right_arm_kinematics/get_constraint_aware_ik";
00018 
00019 static const std::string NON_COLL_LEFT_IK_NAME = "/pr2_left_arm_kinematics/get_ik";
00020 static const std::string NON_COLL_RIGHT_IK_NAME = "/pr2_right_arm_kinematics/get_ik";
00021 
00022 static const std::string RIGHT_ARM_GROUP = "right_arm";
00023 static const std::string LEFT_ARM_GROUP = "left_arm";
00024 
00025 static const std::string RIGHT_ARM_REDUNDANCY = "r_upper_arm_roll_joint";
00026 static const std::string LEFT_ARM_REDUNDANCY = "l_upper_arm_roll_joint";
00027 
00028 static const std::string LEFT_IK_LINK = "l_wrist_roll_link";
00029 static const std::string RIGHT_IK_LINK = "r_wrist_roll_link";
00030 
00031 static const std::string PLANNER_1_SERVICE_NAME = "/ompl_planning/plan_kinematic_path";
00032 static const std::string PLANNER_2_SERVICE_NAME = "/chomp_planner_longrange/plan_path";
00033 static const std::string LEFT_INTERPOLATE_SERVICE_NAME = "/l_interpolated_ik_motion_plan";
00034 static const std::string RIGHT_INTERPOLATE_SERVICE_NAME = "/r_interpolated_ik_motion_plan";
00035 static const std::string TRAJECTORY_FILTER_SERVICE_NAME = "/trajectory_filter/filter_trajectory_with_constraints";
00036 static const std::string PROXIMITY_SPACE_SERVICE_NAME = "/collision_proximity_server_test/get_distance_aware_plan";
00037 static const std::string PROXIMITY_SPACE_VALIDITY_NAME = "/collision_proximity_server_test/get_state_validity";
00038 static const std::string PROXIMITY_SPACE_PLANNER_NAME = "/collision_proximity_planner/plan";
00039 static const std::string SET_PLANNING_SCENE_DIFF_NAME = "environment_server/set_planning_scene_diff";
00040 
00041 using namespace collision_space;
00042 using namespace kinematics_msgs;
00043 using namespace arm_navigation_msgs;
00044 using namespace head_monitor_msgs;
00045 using namespace move_arm_warehouse;
00046 using namespace planning_environment;
00047 using namespace planning_models;
00048 using namespace std;
00049 using namespace trajectory_msgs;
00050 using namespace planning_scene_utils;
00051 using namespace ros::param;
00052 
00053 class PlanningSceneEditorTest : public planning_scene_utils::PlanningSceneEditor
00054 {
00055 public:
00056 
00057   PlanningSceneEditorTest(PlanningSceneParameters& params) : PlanningSceneEditor(params)
00058   {
00059   }
00060 
00061   virtual void planCallback(arm_navigation_msgs::ArmNavigationErrorCodes& errorCode)
00062   {
00063   }
00064   virtual void filterCallback(arm_navigation_msgs::ArmNavigationErrorCodes& errorCode)
00065   {
00066   }
00067 
00068   virtual void attachObjectCallback(const std::string& name)
00069   {
00070   }
00071 
00072 };
00073 
00074 PlanningSceneEditorTest* editor = NULL;
00075 planning_scene_utils::PlanningSceneParameters params;
00076 
00077 bool killThread = false;
00078 bool inited = false;
00079 
00083 TEST(TestSuite, connectTest)
00084 {
00085   editor = new PlanningSceneEditorTest(params);
00086 }
00087 
00091 TEST(TestSuite, createPlanningSceneTest)
00092 {
00093   for(int i = 0; i < 10; i++)
00094   {
00095     std::string name = editor->createNewPlanningScene();
00096     inited = true;
00097     ROS_INFO("Created scene %s", name.c_str());
00098   }
00099 }
00100 
00101 
00105 TEST(TestSuite, createSaveTest)
00106 {
00107   std::string name = editor->createNewPlanningScene();
00108   ROS_INFO("Created scene %s", name.c_str());
00109   ROS_INFO("Saving...");
00110   editor->savePlanningScene(editor->planning_scene_map_[name]);
00111 }
00112 
00116 TEST(TestSuite, createDeleteMotionPlanTest)
00117 {
00118   ROS_INFO("Creating planning scene ...");
00119   std::string planningSceneID = editor->createNewPlanningScene();
00120   ROS_INFO("Created planning scene %s", planningSceneID.c_str());
00121 
00122   for(int i = 0; i < 100; i++)
00123   {
00124     unsigned int id;
00125     std::string rightarm = RIGHT_ARM_GROUP;
00126     std::string rightik = RIGHT_IK_LINK;
00127 
00128     ROS_INFO("Creating motion plan request ...");
00129     editor->createMotionPlanRequest(*(editor->getRobotState()),
00130                                     *(editor->getRobotState()),
00131                                     rightarm, rightik,
00132                                     getPlanningSceneIdFromName(planningSceneID), false, id);
00133     ROS_INFO_STREAM("Deleting motion plan request " << id);
00134     std::vector<unsigned int> traj;
00135     editor->deleteMotionPlanRequest(id, traj);
00136 
00137   }
00138 }
00139 
00143 TEST(TestSuite, planTrajectoryTest)
00144 {
00145   ROS_INFO("Creating planning scene ...");
00146   std::string planningSceneID = editor->createNewPlanningScene();
00147   ROS_INFO("Created planning scene %s", planningSceneID.c_str());
00148 
00149   unsigned int id;
00150   std::string rightarm = RIGHT_ARM_GROUP;
00151   std::string rightik = RIGHT_IK_LINK;
00152 
00153   ROS_INFO("Creating motion plan request ...");
00154   editor->createMotionPlanRequest(*(editor->getRobotState()),
00155                                   *(editor->getRobotState()),
00156                                   rightarm, rightik,
00157                                   getPlanningSceneIdFromName(planningSceneID), 
00158                                   false, id);
00159 
00160 
00161   MotionPlanRequestData& data = editor->motion_plan_map_[getMotionPlanRequestNameFromId(id)];
00162   for(int i = 0; i < 100; i++)
00163   {
00164     ROS_INFO("Randomly Perturbing");
00165     editor->randomlyPerturb(data, GoalPosition);
00166     unsigned int traj_id;
00167     ROS_INFO("Planning...");
00168     editor->planToRequest(data, traj_id);
00169     ROS_INFO_STREAM("Got plan " << traj_id);
00170     editor->playTrajectory(editor->motion_plan_map_[getMotionPlanRequestNameFromId(id)],
00171                            editor->trajectory_map_[getMotionPlanRequestNameFromId(id)][getTrajectoryNameFromId(traj_id)]);
00172     ROS_INFO("Deleting...");
00173     editor->deleteTrajectory(data.getId(), traj_id);
00174   }
00175   std::vector<unsigned int> traj;
00176   editor->deleteMotionPlanRequest(id, traj);
00177 
00178 }
00179 
00183 TEST(TestSuite, filterTrajectoryTest)
00184 {
00185   ROS_INFO("Creating planning scene ...");
00186   std::string planningSceneID = editor->createNewPlanningScene();
00187   ROS_INFO("Created planning scene %s", planningSceneID.c_str());
00188 
00189   unsigned int id;
00190   std::string rightarm = RIGHT_ARM_GROUP;
00191   std::string rightik = RIGHT_IK_LINK;
00192 
00193   ROS_INFO("Creating motion plan request ...");
00194   editor->createMotionPlanRequest(*(editor->getRobotState()),
00195                                   *(editor->getRobotState()),
00196                                   rightarm, rightik,
00197                                   getPlanningSceneIdFromName(planningSceneID), 
00198                                   false, id);
00199 
00200 
00201   MotionPlanRequestData& data = editor->motion_plan_map_[getMotionPlanRequestNameFromId(id)];
00202   for(int i = 0; i < 20; i++)
00203   {
00204     ROS_INFO("Randomly Perturbing");
00205     editor->randomlyPerturb(data, GoalPosition);
00206     unsigned int traj_id;
00207     ROS_INFO("Planning...");
00208     editor->planToRequest(data, traj_id);
00209     ROS_INFO_STREAM("Got plan  " << traj_id);
00210     editor->playTrajectory(editor->motion_plan_map_[getMotionPlanRequestNameFromId(id)],
00211                            editor->trajectory_map_[getMotionPlanRequestNameFromId(id)][getTrajectoryNameFromId(traj_id)]);
00212     unsigned int filter_id;
00213     editor->filterTrajectory(editor->motion_plan_map_[getMotionPlanRequestNameFromId(id)],
00214                              editor->trajectory_map_[getMotionPlanRequestNameFromId(id)][getTrajectoryNameFromId(traj_id)],
00215                              filter_id);
00216     editor->playTrajectory(editor->motion_plan_map_[getMotionPlanRequestNameFromId(id)],
00217                            editor->trajectory_map_[getMotionPlanRequestNameFromId(id)][getTrajectoryNameFromId(filter_id)]);
00218     ROS_INFO("Deleting...");
00219     editor->deleteTrajectory(data.getId(), traj_id);
00220     editor->deleteTrajectory(data.getId(), filter_id);
00221   }
00222 }
00223 
00224 float randFloat(float minValue, float maxValue)
00225 {
00226   return ((float)random() / (float)RAND_MAX) * (maxValue - minValue) + minValue;
00227 }
00228 
00229 TEST(TestSuite, randomGeneratorTest)
00230 {
00231   for(int i = 0; i < 100; i++)
00232   {
00233     ASSERT_TRUE(randFloat(-100, 100) >= -100);
00234     ASSERT_TRUE(randFloat(-100, 100) <= 100);
00235   }
00236   ASSERT_FLOAT_EQ(5, randFloat(5, 5));
00237 }
00238 
00242 TEST(TestSuite, collisionObjectTest)
00243 {
00244   geometry_msgs::Pose pose;
00245   PlanningSceneEditor::GeneratedShape shape;
00246   float scaleX;
00247   float scaleY;
00248   float scaleZ;
00249 
00250   std::vector<std::string> objs;
00251   for(int i = 0; i < 100; i++)
00252   {
00253     pose.position.x = randFloat(-1.5f, 1.5f);
00254     pose.position.y = randFloat(-1.5f, 1.5f);
00255     pose.position.z = randFloat(-1.5f, 1.5f);
00256     pose.orientation.x = randFloat(0.0f, 1.0f);
00257     pose.orientation.y = randFloat(0.0f, 1.0f);
00258     pose.orientation.z = randFloat(0.0f, 1.0f);
00259     pose.orientation.w = 1.0f;
00260 
00261     float rand = randFloat(0.0f, 1.0f);
00262     if(rand < 0.333f)
00263     {
00264       shape = PlanningSceneEditor::Box;
00265     }
00266     else if(rand < 0.666f)
00267     {
00268       shape = PlanningSceneEditor::Cylinder;
00269     }
00270     else
00271     {
00272       shape = PlanningSceneEditor::Sphere;
00273     }
00274 
00275     scaleX = randFloat(0.01f, 0.2f);
00276     scaleY = randFloat(0.01f, 0.2f);
00277     scaleZ = randFloat(0.01f, 0.2f);
00278     std_msgs::ColorRGBA color;
00279     color.r = 0.5;
00280     color.g = 0.5;
00281     color.b = 0.5;
00282     color.a = 1.0;
00283     ROS_INFO("Creating object with size %f %f %f and position %f %f %f",scaleX, scaleY, scaleZ, pose.position.x, pose.position.y, pose.position.z);
00284     std::string obj = editor->createCollisionObject("", pose, shape, scaleX, scaleY, scaleZ,color);
00285     objs.push_back(obj);
00286   }
00287 
00288   for(size_t i = 0; i < objs.size(); i ++)
00289   {
00290     ROS_INFO("Deleting...");
00291     editor->deleteCollisionObject(objs[i]);
00292   }
00293 }
00294 
00295 
00296 void spinThread()
00297 {
00298   while(!inited)
00299   {
00300     usleep(10000);
00301   }
00302   while(!killThread && ros::ok())
00303   {
00304     ros::spinOnce();
00305     editor->sendMarkers();
00306     editor->sendTransformsAndClock();
00307     usleep(10000);
00308   }
00309 }
00310 
00311 int main(int argc, char **argv)
00312 {
00313   testing::InitGoogleTest(&argc, argv);
00314   ros::init(argc, argv, "planning_scene_warehouse_viewer", ros::init_options::NoSigintHandler);
00315 
00316 
00317   param<string>("set_planning_scene_diff_name", params.set_planning_scene_diff_name_, SET_PLANNING_SCENE_DIFF_NAME);
00318   param<string>("left_ik_name", params.left_ik_name_, LEFT_IK_NAME);
00319   param<string>("left_interpolate_service_name", params.left_interpolate_service_name_, LEFT_INTERPOLATE_SERVICE_NAME);
00320   param<string>("non_coll_left_ik_name", params.non_coll_left_ik_name_, NON_COLL_LEFT_IK_NAME);
00321   param<string>("non_coll_right_ik_name", params.non_coll_right_ik_name_, NON_COLL_RIGHT_IK_NAME);
00322   param<string>("planner_1_service_name", params.planner_1_service_name_, PLANNER_1_SERVICE_NAME);
00323   param<string>("planner_2_service_name", params.planner_2_service_name_, PLANNER_2_SERVICE_NAME);
00324   param<string>("proximity_space_planner_name", params.proximity_space_planner_name_, PROXIMITY_SPACE_PLANNER_NAME);
00325   param<string>("proximity_space_service_name",  params.proximity_space_service_name_, PROXIMITY_SPACE_SERVICE_NAME);
00326   param<string>("proximity_space_validity_name",  params.proximity_space_validity_name_,  PROXIMITY_SPACE_VALIDITY_NAME);
00327   param<string>("right_ik_name", params.right_ik_name_, RIGHT_IK_NAME);
00328   param<string>("right_interpolate_service_name", params.right_interpolate_service_name_, RIGHT_INTERPOLATE_SERVICE_NAME);
00329   param<string>("trajectory_filter_1_service_name", params.trajectory_filter_1_service_name_, TRAJECTORY_FILTER_SERVICE_NAME);
00330   param<string>("vis_topic_name", params.vis_topic_name_ , VIS_TOPIC_NAME);
00331   param<string>("right_ik_link", params.right_ik_link_ , RIGHT_IK_LINK);
00332   param<string>("left_ik_link", params.left_ik_link_ , LEFT_IK_LINK);
00333   param<string>("right_arm_group", params.right_arm_group_ , RIGHT_ARM_GROUP);
00334   param<string>("left_arm_group", params.left_arm_group_ , LEFT_ARM_GROUP);
00335   param<string>("right_redundancy", params.right_redundancy_ , RIGHT_ARM_REDUNDANCY);
00336   param<string>("left_redundancy", params.left_redundancy_ , LEFT_ARM_REDUNDANCY);
00337   param<string>("execute_left_trajectory", params.execute_left_trajectory_ , EXECUTE_LEFT_TRAJECTORY);
00338   param<string>("execute_right_trajectory", params.execute_right_trajectory_ , EXECUTE_RIGHT_TRAJECTORY);
00339 
00340 
00341   boost::thread spin_thread(boost::bind(&spinThread));
00342 
00343   int ret = RUN_ALL_TESTS();
00344 
00345   killThread = true;
00346 
00347   spin_thread.join();
00348 
00349 
00350   return ret;
00351 }


move_arm_warehouse
Author(s): Ioan Sucan, Sachin Chitta(sachinc@willowgarage.com)
autogenerated on Fri Dec 6 2013 21:12:34