planning_environment: planning_environment: test_planning

00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 * 
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 * 
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 * 
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 * 
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00037 #include <ros/time.h>
00038 #include <gtest/gtest.h>
00039 #include <iostream>
00040 #include <sstream>
00041 #include <ros/package.h>
00042 
00043 #include <planning_environment/monitors/planning_monitor.h>
00044 #include <planning_environment/models/collision_models_interface.h>
00045 #include <actionlib/client/simple_action_client.h>
00046 
00047 static const double VERY_SMALL = .0001;
00048 
00049 class PlanningMonitorTest : public testing::Test {
00050 public:
00051 
00052   // void actionFeedbackCallback(const arm_navigation_msgs::SetPlanningSceneFeedbackConstPtr& feedback) {
00053   //   ready_ = true;  
00054   // }
00055 
00056   // void actionDoneCallback(const actionlib::SimpleClientGoalState& state,
00057   //                         const arm_navigation_msgs::SetPlanningSceneResultConstPtr& result)
00058   // {
00059   //   EXPECT_TRUE(state == actionlib::SimpleClientGoalState::PREEMPTED);
00060   //   ROS_INFO("Got preempted");
00061   // }
00062 
00063   void setPlanningSceneCallback(const arm_navigation_msgs::PlanningScene& scene) {
00064     got_set_callback_ = true;
00065   }
00066 
00067   void revertPlanningSceneCallback() {
00068     got_revert_callback_ = true;
00069   }
00070 
00071 protected:
00072 
00073   virtual void SetUp() {
00074     
00075     got_set_callback_ = false;
00076     got_revert_callback_ = false;
00077     ready_ = false;
00078 
00079     group_name_ = "right_arm";
00080 
00081     std::string robot_description_name = nh_.resolveName("robot_description", true);
00082 
00083     ros::WallRate h(10.0);
00084     
00085     while(nh_.ok() && !nh_.hasParam(robot_description_name)) {
00086       ros::spinOnce();
00087       h.sleep();
00088     }
00089     
00090     collision_models_ = new planning_environment::CollisionModels(robot_description_name);
00091     planning_monitor_ = new planning_environment::PlanningMonitor(collision_models_, &tf_);
00092     planning_monitor_->setUseCollisionMap(false);
00093     planning_monitor_->waitForState();
00094     planning_monitor_->startEnvironmentMonitor();
00095   }
00096 
00097   virtual void TearDown()
00098   {
00099     delete collision_models_;
00100     delete planning_monitor_;
00101   }
00102 
00103   bool CallPlanningScene() {
00104     return planning_monitor_->getCompletePlanningScene(planning_scene_diff_,
00105                                                        operations_,
00106                                                        planning_scene_);
00107   }
00108 
00109 protected:
00110 
00111   bool ready_;
00112   bool got_set_callback_;
00113   bool got_revert_callback_;
00114 
00115   std::string group_name_;
00116   ros::NodeHandle nh_;
00117   tf::TransformListener tf_;
00118   planning_environment::CollisionModels *collision_models_;
00119   planning_environment::PlanningMonitor *planning_monitor_;
00120 
00121   arm_navigation_msgs::PlanningScene planning_scene_diff_;
00122   arm_navigation_msgs::PlanningScene planning_scene_;
00123 
00124   arm_navigation_msgs::OrderedCollisionOperations operations_;
00125   
00126 };
00127 
00128 TEST_F(PlanningMonitorTest, ChangingObjects)
00129 {
00130   planning_environment::CollisionModels test_collision_models("robot_description");
00131 
00132   planning_scene_diff_.robot_state.multi_dof_joint_state.stamp = ros::Time::now();
00133   planning_scene_diff_.robot_state.multi_dof_joint_state.joint_names.push_back("base_joint");
00134   planning_scene_diff_.robot_state.multi_dof_joint_state.frame_ids.push_back("odom_combined");
00135   planning_scene_diff_.robot_state.multi_dof_joint_state.child_frame_ids.push_back("base_footprint");
00136   planning_scene_diff_.robot_state.multi_dof_joint_state.poses.resize(1);
00137   planning_scene_diff_.robot_state.multi_dof_joint_state.poses[0].position.x = 4.0;
00138   planning_scene_diff_.robot_state.multi_dof_joint_state.poses[0].orientation.w = 1.0;
00139 
00140   arm_navigation_msgs::CollisionObject obj1;
00141   obj1.header.stamp = ros::Time::now();
00142   obj1.header.frame_id = "map_to_stapler";
00143   obj1.id = "obj1";
00144   obj1.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD;
00145   obj1.shapes.resize(1);
00146   obj1.shapes[0].type = arm_navigation_msgs::Shape::BOX;
00147   obj1.shapes[0].dimensions.resize(3);
00148   obj1.shapes[0].dimensions[0] = .1;
00149   obj1.shapes[0].dimensions[1] = .1;
00150   obj1.shapes[0].dimensions[2] = .75;
00151   obj1.poses.resize(1);
00152   obj1.poses[0].position.x = .5;
00153   obj1.poses[0].position.y = .5;
00154   obj1.poses[0].position.z = 0;
00155   obj1.poses[0].orientation.w = 1.0;
00156 
00157   arm_navigation_msgs::AttachedCollisionObject att_obj;
00158   att_obj.object = obj1;
00159   att_obj.object.header.stamp = ros::Time::now();
00160   att_obj.object.header.frame_id = "odom_combined";
00161   att_obj.link_name = "r_gripper_palm_link";
00162   att_obj.touch_links.push_back("r_gripper_palm_link");
00163   att_obj.touch_links.push_back("r_gripper_r_finger_link");
00164   att_obj.touch_links.push_back("r_gripper_l_finger_link");
00165   att_obj.touch_links.push_back("r_gripper_r_finger_tip_link");
00166   att_obj.touch_links.push_back("r_gripper_l_finger_tip_link");
00167   att_obj.touch_links.push_back("r_wrist_roll_link");
00168   att_obj.touch_links.push_back("r_wrist_flex_link");
00169   att_obj.touch_links.push_back("r_forearm_link");
00170   att_obj.touch_links.push_back("r_gripper_motor_accelerometer_link");
00171   att_obj.object.id = "obj2";
00172   att_obj.object.shapes[0].type = arm_navigation_msgs::Shape::CYLINDER;
00173   att_obj.object.shapes[0].dimensions.resize(2);
00174   att_obj.object.shapes[0].dimensions[0] = .025;
00175   att_obj.object.shapes[0].dimensions[1] = .5;
00176   att_obj.object.poses.resize(1);
00177   att_obj.object.poses[0].position.x = 0.0;
00178   att_obj.object.poses[0].position.y = 0.0;
00179   att_obj.object.poses[0].position.z = 0.0;
00180   att_obj.object.poses[0].orientation.w = 1.0;
00181 
00182   planning_scene_diff_.collision_objects.push_back(obj1);
00183   planning_scene_diff_.attached_collision_objects.push_back(att_obj);
00184 
00185   att_obj.object.header.frame_id = "map_to_stapler";
00186   planning_scene_diff_.attached_collision_objects.push_back(att_obj);
00187   
00188   CallPlanningScene();
00189 
00190   ASSERT_EQ(planning_scene_.collision_objects.size(), 1);
00191   //extra object with same id should have overwritten first
00192   ASSERT_EQ(planning_scene_.attached_collision_objects.size(), 1);
00193   EXPECT_EQ(planning_scene_.attached_collision_objects[0].object.header.frame_id,std::string("map_to_stapler"));
00194 
00195   planning_models::KinematicState* state = test_collision_models.setPlanningScene(planning_scene_);
00196   ASSERT_TRUE(state != NULL);
00197 
00198   EXPECT_TRUE(test_collision_models.convertAttachedCollisionObjectToNewWorldFrame(*state, att_obj));  
00199   EXPECT_EQ(att_obj.object.header.frame_id, "r_gripper_palm_link");
00200 
00201   att_obj.object.header.frame_id = "stapler_to_monkey";
00202   EXPECT_FALSE(test_collision_models.convertAttachedCollisionObjectToNewWorldFrame(*state, att_obj));  
00203   
00204   EXPECT_TRUE(test_collision_models.convertCollisionObjectToNewWorldFrame(*state, obj1));
00205   EXPECT_EQ(obj1.header.frame_id, test_collision_models.getWorldFrameId());
00206   //.5 in x in map_to_stapler to map,-.5 to odom_combined = -3.5
00207   EXPECT_LE(fabs(obj1.poses[0].position.x+3.5), VERY_SMALL);
00208   //.5 in y in map_to_stapler to map, 3.5 to odom_combined = 3.5
00209   EXPECT_LE(fabs(obj1.poses[0].position.y-3.5), VERY_SMALL);
00210     
00211   att_obj.link_name = "base_footprint";
00212   att_obj.object.header.frame_id = "base_footprint";
00213   att_obj.object.id = "obj3";
00214   att_obj.object.poses[0].position.x = 0.12;
00215   att_obj.object.poses[0].position.y = 0.0;
00216   att_obj.object.poses[0].position.z = 0.0;
00217   att_obj.object.poses[0].orientation.w = 1.0;
00218 
00219   planning_scene_diff_.attached_collision_objects.pop_back();
00220   planning_scene_diff_.attached_collision_objects.push_back(att_obj);
00221   
00222   CallPlanningScene();
00223 
00224   //now they should be different
00225   EXPECT_EQ(planning_scene_.attached_collision_objects[0].object.header.frame_id, "odom_combined");
00226   EXPECT_EQ(planning_scene_.attached_collision_objects[1].object.header.frame_id, "base_footprint");
00227 
00228   test_collision_models.revertPlanningScene(state);
00229 
00230   //now we mess with ordered collision operations
00231   arm_navigation_msgs::CollisionOperation cop;
00232   cop.object1 = "l_end_effector";
00233   cop.object2 = "obj3";
00234   cop.operation = cop.DISABLE;
00235   operations_.collision_operations.push_back(cop);
00236 
00237   cop.object1 = "up_end_effector";
00238   cop.object2 = "obj3";
00239   cop.operation = cop.DISABLE;
00240   operations_.collision_operations.push_back(cop);
00241 
00242   CallPlanningScene();
00243 
00244   state = test_collision_models.setPlanningScene(planning_scene_);
00245   ASSERT_TRUE(state != NULL);
00246 
00247   const collision_space::EnvironmentModel::AllowedCollisionMatrix& acm = test_collision_models.getCollisionSpace()->getCurrentAllowedCollisionMatrix();
00248   
00249   bool allowed;
00250   EXPECT_TRUE(acm.getAllowedCollision("l_gripper_l_finger_tip_link", "obj3", allowed));
00251   EXPECT_TRUE(allowed);
00252 
00253   EXPECT_TRUE(acm.getAllowedCollision("l_gripper_l_finger_tip_link", "obj2", allowed));
00254   EXPECT_FALSE(allowed);
00255 
00256   //defaults shouldn't have changed
00257   const collision_space::EnvironmentModel::AllowedCollisionMatrix& acm2 = test_collision_models.getCollisionSpace()->getDefaultAllowedCollisionMatrix();
00258   
00259   EXPECT_TRUE(acm2.getAllowedCollision("l_gripper_l_finger_tip_link", "obj3", allowed));
00260   EXPECT_FALSE(allowed);
00261 
00262   EXPECT_TRUE(acm2.getAllowedCollision("l_gripper_l_finger_tip_link", "obj2", allowed));
00263   EXPECT_FALSE(allowed);
00264 
00265 
00266   test_collision_models.revertPlanningScene(state);
00267 }
00268 
00269 TEST_F(PlanningMonitorTest, ChangingRobotState)
00270 {
00271 
00272   planning_environment::CollisionModels test_collision_models("robot_description");
00273 
00274   arm_navigation_msgs::CollisionObject obj1;
00275   obj1.header.stamp = ros::Time::now();
00276   obj1.header.frame_id = "odom_combined";
00277   obj1.id = "table";
00278   obj1.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD;
00279   obj1.shapes.resize(1);
00280   obj1.shapes[0].type = arm_navigation_msgs::Shape::BOX;
00281   obj1.shapes[0].dimensions.resize(3);
00282   obj1.shapes[0].dimensions[0] = 1.0;
00283   obj1.shapes[0].dimensions[1] = 1.0;
00284   obj1.shapes[0].dimensions[2] = .2;
00285   obj1.poses.resize(1);
00286   obj1.poses[0].position.x = 4.25;
00287   obj1.poses[0].position.y = 0.0;
00288   obj1.poses[0].position.z = .8;
00289   obj1.poses[0].orientation.w = 1.0;
00290 
00291   planning_scene_diff_.collision_objects.push_back(obj1);
00292 
00293   CallPlanningScene();
00294 
00295   planning_models::KinematicState* state = test_collision_models.setPlanningScene(planning_scene_);
00296 
00297   //without transforming state nowhere near the table
00298   ASSERT_TRUE(state != NULL);
00299   EXPECT_FALSE(test_collision_models.isKinematicStateInCollision(*state));
00300 
00301   test_collision_models.revertPlanningScene(state);
00302 
00303   planning_scene_diff_.robot_state.multi_dof_joint_state.stamp = ros::Time::now();
00304   planning_scene_diff_.robot_state.multi_dof_joint_state.joint_names.push_back("base_joint");
00305   planning_scene_diff_.robot_state.multi_dof_joint_state.frame_ids.push_back("odom_combined");
00306   planning_scene_diff_.robot_state.multi_dof_joint_state.child_frame_ids.push_back("base_footprint");
00307   planning_scene_diff_.robot_state.multi_dof_joint_state.poses.resize(1);
00308   planning_scene_diff_.robot_state.multi_dof_joint_state.poses[0].position.x = 4.0;
00309   planning_scene_diff_.robot_state.multi_dof_joint_state.poses[0].orientation.w = 1.0;
00310 
00311   CallPlanningScene();
00312 
00313   state = test_collision_models.setPlanningScene(planning_scene_);
00314 
00315   //expect collisions with table
00316   ASSERT_TRUE(state != NULL);
00317   std::map<std::string, double> joint_state_values;
00318   state->getKinematicStateValues(joint_state_values);
00319   EXPECT_EQ(joint_state_values["floating_trans_x"], 4.0);
00320   EXPECT_TRUE(test_collision_models.isKinematicStateInCollision(*state));
00321   EXPECT_FALSE(test_collision_models.isKinematicStateInSelfCollision(*state));
00322   EXPECT_TRUE(test_collision_models.isKinematicStateInEnvironmentCollision(*state));
00323 
00324   test_collision_models.revertPlanningScene(state);
00325 
00326   //bad multi-dof
00327   planning_scene_diff_.robot_state.multi_dof_joint_state.stamp = ros::Time::now();
00328   planning_scene_diff_.robot_state.multi_dof_joint_state.joint_names[0] = "base_joint";
00329   planning_scene_diff_.robot_state.multi_dof_joint_state.frame_ids[0] = "";
00330   planning_scene_diff_.robot_state.multi_dof_joint_state.child_frame_ids[0] = "base_footprint";
00331   planning_scene_diff_.robot_state.multi_dof_joint_state.poses.resize(1);
00332   planning_scene_diff_.robot_state.multi_dof_joint_state.poses[0].position.x = 4.0;
00333   planning_scene_diff_.robot_state.multi_dof_joint_state.poses[0].orientation.w = 1.0;
00334 
00335   CallPlanningScene();
00336 
00337   //but now we shouldn't be in collision
00338   state = test_collision_models.setPlanningScene(planning_scene_);
00339 
00340   ASSERT_TRUE(state != NULL);
00341   //expect collisions with table
00342   state->getKinematicStateValues(joint_state_values);
00343   EXPECT_EQ(joint_state_values["floating_trans_x"], 0.0);
00344   EXPECT_FALSE(test_collision_models.isKinematicStateInCollision(*state));
00345 
00346   test_collision_models.revertPlanningScene(state);
00347 
00348   planning_scene_diff_.robot_state.multi_dof_joint_state.joint_names.clear();
00349   planning_scene_diff_.robot_state.multi_dof_joint_state.poses.clear();
00350   planning_scene_diff_.robot_state.multi_dof_joint_state.frame_ids.clear();
00351   planning_scene_diff_.robot_state.multi_dof_joint_state.child_frame_ids.clear();
00352   
00353   planning_scene_diff_.robot_state.joint_state.name.push_back("floating_trans_x");
00354   planning_scene_diff_.robot_state.joint_state.position.push_back(3.3);
00355 
00356   CallPlanningScene();
00357 
00358   //back in collision
00359   state = test_collision_models.setPlanningScene(planning_scene_);
00360 
00361   ASSERT_TRUE(state != NULL);
00362   state->getKinematicStateValues(joint_state_values);
00363   EXPECT_EQ(joint_state_values["floating_trans_x"], 3.3);
00364   EXPECT_TRUE(test_collision_models.isKinematicStateInCollision(*state));
00365   EXPECT_FALSE(test_collision_models.isKinematicStateInSelfCollision(*state));
00366   EXPECT_TRUE(test_collision_models.isKinematicStateInEnvironmentCollision(*state));
00367 
00368   // test_collision_models.writePlanningSceneBag(ros::package::getPath("planning_environment")+"/test.bag",
00369   //                          complete_robot_state,
00370   //                          allowed_collision_matrix,
00371   //                          transformed_allowed_contacts,
00372   //                          all_link_paddings,
00373   //                          all_collision_objects,
00374   //                          all_attached_collision_objects,
00375   //                          unmasked_collision_map);
00376 
00377   test_collision_models.revertPlanningScene(state);
00378 
00379   //now we turn out of collision
00380   planning_scene_diff_.robot_state.joint_state.name.push_back("floating_rot_z");
00381   planning_scene_diff_.robot_state.joint_state.name.push_back("floating_rot_w");
00382   planning_scene_diff_.robot_state.joint_state.position.push_back(.7071);
00383   planning_scene_diff_.robot_state.joint_state.position.push_back(.7071);
00384 
00385   CallPlanningScene();
00386 
00387   state = test_collision_models.setPlanningScene(planning_scene_);
00388 
00389   ASSERT_TRUE(state != NULL);
00390   EXPECT_FALSE(test_collision_models.isKinematicStateInCollision(*state));
00391 
00392   test_collision_models.revertPlanningScene(state);
00393 }
00394 
00395 TEST_F(PlanningMonitorTest, PlanningMonitorWithCollisionInterface)
00396 {
00397 
00398   ros::AsyncSpinner async(2);
00399   async.start();
00400 
00401   std::string robot_description_name = nh_.resolveName("robot_description", true);
00402   planning_environment::CollisionModelsInterface test_collision_models(robot_description_name, false);
00403 
00404   //this test is important because calling the service calls collision checking from a different thread
00405   test_collision_models.addSetPlanningSceneCallback(boost::bind(&PlanningMonitorTest::setPlanningSceneCallback, this, _1));
00406   test_collision_models.addRevertPlanningSceneCallback(boost::bind(&PlanningMonitorTest::revertPlanningSceneCallback, this));
00407 
00408   CallPlanningScene();
00409 
00410   ros::NodeHandle priv_nh("~");
00411 
00412   actionlib::SimpleActionClient<arm_navigation_msgs::SyncPlanningSceneAction> ac(priv_nh, "sync_planning_scene", true);
00413 
00414   arm_navigation_msgs::SyncPlanningSceneGoal goal;
00415   
00416   goal.planning_scene = planning_scene_;
00417 
00418   ASSERT_TRUE(ac.waitForServer());
00419 
00420   //ac.sendGoal(goal, boost::bind(&PlanningMonitorTest::actionDoneCallback, this, _1, _2), NULL, boost::bind(&PlanningMonitorTest::actionFeedbackCallback, this, _1));
00421 
00422   actionlib::SimpleClientGoalState gs = ac.sendGoalAndWait(goal);
00423 
00424   EXPECT_TRUE(gs == actionlib::SimpleClientGoalState::SUCCEEDED);
00425 
00426   EXPECT_TRUE(got_set_callback_);
00427   EXPECT_FALSE(test_collision_models.isKinematicStateInCollision(*(test_collision_models.getPlanningSceneState())));
00428 
00429   gs = ac.sendGoalAndWait(goal);
00430 
00431   EXPECT_TRUE(gs == actionlib::SimpleClientGoalState::SUCCEEDED);
00432   
00433   EXPECT_TRUE(got_revert_callback_);
00434 
00435   EXPECT_FALSE(test_collision_models.isKinematicStateInCollision(*(test_collision_models.getPlanningSceneState())));
00436 
00437   EXPECT_TRUE(test_collision_models.getPlanningSceneState() != NULL);
00438   
00439 }
00440 
00441 int main(int argc, char** argv)
00442 {
00443   testing::InitGoogleTest(&argc, argv);
00444   ros::init(argc, argv, "test_planning_monitor");
00445   return RUN_ALL_TESTS();
00446 }
test_planning_monitor.cpp
