ompl_ros_interface: ompl_ros_interface: test_ompl

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 <arm_navigation_msgs/SetPlanningSceneDiff.h>
00040 #include <arm_navigation_msgs/GetMotionPlan.h>
00041 #include <planning_environment/models/model_utils.h>
00042 #include <planning_environment/models/collision_models_interface.h>
00043 #include <actionlib/client/simple_action_client.h>
00044 
00045 static const std::string SET_PLANNING_SCENE_DIFF_SERVICE="/environment_server/set_planning_scene_diff";
00046 static const std::string PLANNER_SERVICE_NAME="/ompl_planning/plan_kinematic_path";
00047 
00048 class OmplPlanningTest : public testing::Test {
00049 protected:
00050 
00051   virtual void SetUp() {
00052 
00053     ready_ = false;
00054     done_ = false;
00055 
00056     cm_ = new planning_environment::CollisionModels("robot_description");
00057 
00058     ros::service::waitForService(SET_PLANNING_SCENE_DIFF_SERVICE);
00059     ros::service::waitForService(PLANNER_SERVICE_NAME);
00060 
00061     set_planning_scene_diff_client_ = nh_.serviceClient<arm_navigation_msgs::SetPlanningSceneDiff>(SET_PLANNING_SCENE_DIFF_SERVICE);
00062     planning_service_client_ = nh_.serviceClient<arm_navigation_msgs::GetMotionPlan>(PLANNER_SERVICE_NAME);
00063 
00064     mplan_req.motion_plan_request.group_name = "right_arm";
00065     mplan_req.motion_plan_request.num_planning_attempts = 1;
00066     mplan_req.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
00067     const std::vector<std::string>& joint_names = cm_->getKinematicModel()->getModelGroup("right_arm")->getJointModelNames();
00068     mplan_req.motion_plan_request.goal_constraints.joint_constraints.resize(joint_names.size());
00069     for(unsigned int i = 0; i < joint_names.size(); i++) {
00070       mplan_req.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = joint_names[i];
00071       mplan_req.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0;
00072       mplan_req.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.001;
00073       mplan_req.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.001;
00074     }      
00075   }
00076 
00077   virtual void TearDown() {
00078     delete cm_;
00079   }
00080 
00081   void GetAndSetPlanningScene() {
00082     ASSERT_TRUE(set_planning_scene_diff_client_.call(get_req, get_res));
00083   }
00084       
00085 protected:
00086 
00087   ros::NodeHandle nh_;
00088 
00089   bool ready_, done_;
00090 
00091   planning_environment::CollisionModels* cm_;
00092 
00093   arm_navigation_msgs::SetPlanningSceneDiff::Request get_req;
00094   arm_navigation_msgs::SetPlanningSceneDiff::Response get_res;
00095   arm_navigation_msgs::GetMotionPlan::Request mplan_req;
00096 
00097   ros::ServiceClient set_planning_scene_diff_client_;
00098   ros::ServiceClient planning_service_client_;
00099 };
00100 
00101 TEST_F(OmplPlanningTest, TestPole)
00102 {
00103   arm_navigation_msgs::CollisionObject pole;
00104   
00105   pole.header.stamp = ros::Time::now();
00106   pole.header.frame_id = "odom_combined";
00107   pole.id = "pole";
00108   pole.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD;
00109   pole.shapes.resize(1);
00110   pole.shapes[0].type = arm_navigation_msgs::Shape::CYLINDER;
00111   pole.shapes[0].dimensions.resize(2);
00112   pole.shapes[0].dimensions[0] = 0.1;
00113   pole.shapes[0].dimensions[1] = 1.5;
00114   pole.poses.resize(1);
00115   pole.poses[0].position.x = .6;
00116   pole.poses[0].position.y = -.6;
00117   pole.poses[0].position.z = .75;
00118   pole.poses[0].orientation.w = 1.0;
00119 
00120   get_req.planning_scene_diff.collision_objects.push_back(pole);
00121 
00122   GetAndSetPlanningScene();
00123 
00124   mplan_req.motion_plan_request.goal_constraints.joint_constraints[0].position = -2.0;
00125   mplan_req.motion_plan_request.goal_constraints.joint_constraints[3].position = -.2;
00126   mplan_req.motion_plan_request.goal_constraints.joint_constraints[5].position = -.2;
00127 
00128   for(unsigned int i = 0; i < 10; i++) {
00129     arm_navigation_msgs::GetMotionPlan::Response mplan_res;
00130     ASSERT_TRUE(planning_service_client_.call(mplan_req, mplan_res));
00131     
00132     ASSERT_EQ(mplan_res.error_code.val,mplan_res.error_code.SUCCESS);
00133     
00134     EXPECT_GT(mplan_res.trajectory.joint_trajectory.points.size(), 0);
00135     
00136     arm_navigation_msgs::ArmNavigationErrorCodes error_code;
00137     std::vector<arm_navigation_msgs::ArmNavigationErrorCodes> trajectory_error_codes;
00138     
00139     EXPECT_TRUE(cm_->isJointTrajectoryValid(get_res.planning_scene,
00140                                             mplan_res.trajectory.joint_trajectory,
00141                                             mplan_req.motion_plan_request.goal_constraints,                              
00142                                             mplan_req.motion_plan_request.path_constraints,
00143                                             error_code,
00144                                             trajectory_error_codes, false));
00145   }
00146 }
00147 
00148 int main(int argc, char **argv)
00149 {
00150   testing::InitGoogleTest(&argc, argv);
00151   ros::init(argc, argv, "test_ompl_planning");
00152     
00153   return RUN_ALL_TESTS();
00154 }
test_ompl_planning.cpp
