arm_navigation_tests: arm_navigation_tests: test_simple_trajectory

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/GetPlanningScene.h>
00040 #include <arm_navigation_msgs/GetMotionPlan.h>
00041 #include <arm_navigation_msgs/SetPlanningSceneAction.h>
00042 #include <planning_environment/models/model_utils.h>
00043 #include <planning_environment/models/collision_models_interface.h>
00044 #include <actionlib/client/simple_action_client.h>
00045 #include <arm_navigation_msgs/FilterJointTrajectoryWithConstraints.h>
00046 #include <kinematics_msgs/GetConstraintAwarePositionIK.h>
00047 #include <arm_navigation_msgs/convert_messages.h>
00048 #include <ros/package.h>
00049 
00050 static const std::string GET_PLANNING_SCENE_SERVICE="/environment_server/get_planning_scene";
00051 static const std::string SET_PLANNING_SCENE_NAME_1="/ompl_planning/set_planning_scene";
00052 static const std::string SET_PLANNING_SCENE_NAME_2="/trajectory_filter/set_planning_scene";
00053 static const std::string SET_PLANNING_SCENE_NAME_3="/pr2_right_arm_kinematics/set_planning_scene";
00054 static const std::string PLANNER_SERVICE_NAME="/ompl_planning/plan_kinematic_path";
00055 static const std::string TRAJECTORY_FILTER_SERVICE_NAME="/trajectory_filter/filter_trajectory_with_constraints";
00056 static const std::string IK_NAME="/pr2_right_arm_kinematics/get_constraint_aware_ik";
00057 
00058 class TrajectoryPipelineTest : public testing::Test {
00059 protected:
00060 
00061   virtual void SetUp() {
00062 
00063     cm_ = new planning_environment::CollisionModels("robot_description");
00064     planning_scene_state_ = NULL;
00065 
00066     ros::service::waitForService(GET_PLANNING_SCENE_SERVICE);
00067     ros::service::waitForService(PLANNER_SERVICE_NAME);
00068     ros::service::waitForService(TRAJECTORY_FILTER_SERVICE_NAME);
00069     ros::service::waitForService(IK_NAME);
00070 
00071     get_planning_scene_client_ = nh_.serviceClient<arm_navigation_msgs::GetPlanningScene>(GET_PLANNING_SCENE_SERVICE);
00072     planning_service_client_ = nh_.serviceClient<arm_navigation_msgs::GetMotionPlan>(PLANNER_SERVICE_NAME);
00073     trajectory_filter_client_ = nh_.serviceClient<arm_navigation_msgs::FilterJointTrajectoryWithConstraints>(TRAJECTORY_FILTER_SERVICE_NAME);
00074     ik_service_client_ = nh_.serviceClient<kinematics_msgs::GetConstraintAwarePositionIK>(IK_NAME);
00075 
00076     set_planning_scene_action_1_ = new actionlib::SimpleActionClient<arm_navigation_msgs::SetPlanningSceneAction>(SET_PLANNING_SCENE_NAME_1, true);
00077     set_planning_scene_action_1_->waitForServer();
00078 
00079     set_planning_scene_action_2_ = new actionlib::SimpleActionClient<arm_navigation_msgs::SetPlanningSceneAction>(SET_PLANNING_SCENE_NAME_2, true);
00080     set_planning_scene_action_2_->waitForServer();
00081 
00082     set_planning_scene_action_3_ = new actionlib::SimpleActionClient<arm_navigation_msgs::SetPlanningSceneAction>(SET_PLANNING_SCENE_NAME_3, true);
00083     set_planning_scene_action_3_->waitForServer();
00084 
00085     mplan_req.motion_plan_request.group_name = "right_arm";
00086     mplan_req.motion_plan_request.num_planning_attempts = 1;
00087     mplan_req.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
00088     const std::vector<std::string>& joint_names = cm_->getKinematicModel()->getModelGroup("right_arm")->getJointModelNames();
00089     mplan_req.motion_plan_request.goal_constraints.joint_constraints.resize(joint_names.size());
00090     for(unsigned int i = 0; i < joint_names.size(); i++) {
00091       mplan_req.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = joint_names[i];
00092       mplan_req.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0;
00093       mplan_req.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.001;
00094       mplan_req.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.001;
00095     }      
00096   }
00097 
00098   virtual void TearDown() {
00099     if(planning_scene_state_ != NULL) {
00100       delete planning_scene_state_;
00101     }
00102     delete cm_;
00103     delete set_planning_scene_action_1_;
00104     delete set_planning_scene_action_2_;
00105     delete set_planning_scene_action_3_;
00106   }
00107 
00108   void GetAndSetPlanningScene() {
00109     ASSERT_TRUE(get_planning_scene_client_.call(get_req, get_res));
00110 
00111     planning_scene_state_ = cm_->setPlanningScene(get_res.planning_scene);
00112 
00113     arm_navigation_msgs::SetPlanningSceneGoal planning_scene_goal;
00114     planning_scene_goal.planning_scene = get_res.planning_scene;
00115 
00116     //set_planning_scene_action_->sendGoal(planning_scene_goal, boost::bind(&OmplPlanningTest::actionDoneCallback, this, _1, _2), NULL, boost::bind(&OmplPlanningTest::actionFeedbackCallback, this, _1));
00117     set_planning_scene_action_1_->sendGoal(planning_scene_goal);
00118     set_planning_scene_action_2_->sendGoal(planning_scene_goal);
00119     set_planning_scene_action_3_->sendGoal(planning_scene_goal);
00120 
00121     set_planning_scene_action_1_->waitForResult();
00122     set_planning_scene_action_2_->waitForResult();
00123     set_planning_scene_action_3_->waitForResult();
00124 
00125     actionlib::SimpleClientGoalState gs = set_planning_scene_action_1_->getState();
00126     EXPECT_TRUE(gs == actionlib::SimpleClientGoalState::SUCCEEDED);
00127 
00128     gs = set_planning_scene_action_2_->getState();
00129     EXPECT_TRUE(gs == actionlib::SimpleClientGoalState::SUCCEEDED);
00130 
00131     gs = set_planning_scene_action_3_->getState();
00132     EXPECT_TRUE(gs == actionlib::SimpleClientGoalState::SUCCEEDED);
00133 
00134   }
00135 
00136 protected:
00137 
00138   ros::NodeHandle nh_;
00139 
00140   planning_environment::CollisionModels* cm_;
00141 
00142   planning_models::KinematicState* planning_scene_state_;
00143 
00144   arm_navigation_msgs::GetPlanningScene::Request get_req;
00145   arm_navigation_msgs::GetPlanningScene::Response get_res;
00146   arm_navigation_msgs::GetMotionPlan::Request mplan_req;
00147 
00148   ros::ServiceClient get_planning_scene_client_;
00149   ros::ServiceClient planning_service_client_;
00150   ros::ServiceClient trajectory_filter_client_;
00151   ros::ServiceClient ik_service_client_;
00152 
00153   actionlib::SimpleActionClient<arm_navigation_msgs::SetPlanningSceneAction>* set_planning_scene_action_1_;  
00154   actionlib::SimpleActionClient<arm_navigation_msgs::SetPlanningSceneAction>* set_planning_scene_action_2_;  
00155   actionlib::SimpleActionClient<arm_navigation_msgs::SetPlanningSceneAction>* set_planning_scene_action_3_;  
00156 
00157 };
00158 
00159 TEST_F(TrajectoryPipelineTest, TestPole)
00160 {
00161   arm_navigation_msgs::CollisionObject pole;
00162   
00163   pole.header.stamp = ros::Time::now();
00164   pole.header.frame_id = "odom_combined";
00165   pole.id = "pole";
00166   pole.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD;
00167   pole.shapes.resize(1);
00168   pole.shapes[0].type = arm_navigation_msgs::Shape::CYLINDER;
00169   pole.shapes[0].dimensions.resize(2);
00170   pole.shapes[0].dimensions[0] = 0.1;
00171   pole.shapes[0].dimensions[1] = 1.5;
00172   pole.poses.resize(1);
00173   pole.poses[0].position.x = .6;
00174   pole.poses[0].position.y = -.6;
00175   pole.poses[0].position.z = .75;
00176   pole.poses[0].orientation.w = 1.0;
00177 
00178   get_req.planning_scene_diff.collision_objects.push_back(pole);
00179 
00180   GetAndSetPlanningScene();
00181 
00182   cm_->writePlanningSceneBag(ros::package::getPath("move_arm")+"/test_pole.bag",
00183                              get_res.planning_scene);
00184 
00185   mplan_req.motion_plan_request.goal_constraints.joint_constraints[0].position = -2.0;
00186   mplan_req.motion_plan_request.goal_constraints.joint_constraints[3].position = -.2;
00187   mplan_req.motion_plan_request.goal_constraints.joint_constraints[5].position = -.2;
00188 
00189   for(unsigned int i = 0; i < 1; i++) {
00190     arm_navigation_msgs::GetMotionPlan::Response mplan_res;
00191     ASSERT_TRUE(planning_service_client_.call(mplan_req, mplan_res));
00192     
00193     ASSERT_EQ(mplan_res.error_code.val,mplan_res.error_code.SUCCESS);
00194     
00195     EXPECT_GT(mplan_res.trajectory.joint_trajectory.points.size(), 0);
00196     
00197     arm_navigation_msgs::ArmNavigationErrorCodes error_code;
00198     std::vector<arm_navigation_msgs::ArmNavigationErrorCodes> trajectory_error_codes;
00199     
00200     EXPECT_TRUE(cm_->isJointTrajectoryValid(*planning_scene_state_, 
00201                                        mplan_res.trajectory.joint_trajectory,
00202                                        mplan_req.motion_plan_request.goal_constraints,
00203                                        mplan_req.motion_plan_request.path_constraints,
00204                                        error_code,
00205                                        trajectory_error_codes, false)) << error_code;
00206 
00207     planning_environment::setRobotStateAndComputeTransforms(get_res.planning_scene.robot_state, *planning_scene_state_);
00208     double planner_length = cm_->getTotalTrajectoryJointLength(*planning_scene_state_, mplan_res.trajectory.joint_trajectory);
00209 
00210     arm_navigation_msgs::FilterJointTrajectoryWithConstraints::Request filter_req;
00211     arm_navigation_msgs::FilterJointTrajectoryWithConstraints::Response filter_res;
00212     
00213     filter_req.trajectory = mplan_res.trajectory.joint_trajectory;
00214   
00215     filter_req.goal_constraints = mplan_req.motion_plan_request.goal_constraints;
00216     filter_req.path_constraints = mplan_req.motion_plan_request.path_constraints;
00217     filter_req.allowed_time = ros::Duration(1.0);
00218 
00219     EXPECT_TRUE(trajectory_filter_client_.call(filter_req, filter_res));
00220 
00221     EXPECT_TRUE(cm_->isJointTrajectoryValid(*planning_scene_state_, 
00222                                        filter_res.trajectory,
00223                                        mplan_req.motion_plan_request.goal_constraints,
00224                                        mplan_req.motion_plan_request.path_constraints,
00225                                        error_code,
00226                                        trajectory_error_codes, false)) << error_code;
00227 
00228     planning_environment::setRobotStateAndComputeTransforms(get_res.planning_scene.robot_state, *planning_scene_state_);
00229     double filter_length = cm_->getTotalTrajectoryJointLength(*planning_scene_state_, filter_res.trajectory);
00230   
00231     EXPECT_GE(planner_length, filter_length);
00232 
00233     ROS_INFO_STREAM("Planner points " << mplan_res.trajectory.joint_trajectory.points.size() << " filter points " << filter_res.trajectory.points.size() << " planner length " << planner_length << " filter length " << filter_length);
00234   }
00235   cm_->revertPlanningScene(planning_scene_state_);
00236 }
00237 
00238 TEST_F(TrajectoryPipelineTest, TestObjectTable)
00239 {
00240   arm_navigation_msgs::CollisionObject table;
00241 
00242   table.header.stamp = ros::Time::now();
00243   table.header.frame_id = "odom_combined";
00244   table.id = "table";
00245   table.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD;
00246   table.shapes.resize(1);
00247   table.shapes[0].type = arm_navigation_msgs::Shape::BOX;
00248   table.shapes[0].dimensions.resize(3);
00249   table.shapes[0].dimensions[0] = .5;
00250   table.shapes[0].dimensions[1] = .5;
00251   table.shapes[0].dimensions[2] = .1;
00252   table.poses.resize(1);
00253   table.poses[0].position.x = 4.0;
00254   table.poses[0].position.y = -4.0;
00255   table.poses[0].position.z = .5;
00256   table.poses[0].orientation.w = 1.0;
00257 
00258   geometry_msgs::PoseStamped obj_pose;
00259   obj_pose.header.frame_id = "odom_combined";
00260   obj_pose.pose.position.x = 3.77;
00261   obj_pose.pose.position.y = -4.2;
00262   obj_pose.pose.position.z = .851;
00263   //obj_pose.header.frame_id = "base_link";
00264   //obj_pose.pose.position.x = .52;
00265   //obj_pose.pose.position.y = -.2;
00266   //obj_pose.pose.position.z = .8;
00267   obj_pose.pose.orientation.x = 0.0;
00268   obj_pose.pose.orientation.y = 0.7071;
00269   obj_pose.pose.orientation.z = 0.0;
00270   obj_pose.pose.orientation.w = 0.7071;
00271 
00272   arm_navigation_msgs::Constraints goal_constraints;
00273   goal_constraints.position_constraints.resize(1);
00274   goal_constraints.orientation_constraints.resize(1);
00275 
00276   arm_navigation_msgs::poseStampedToPositionOrientationConstraints(obj_pose,
00277                                                                     "r_wrist_roll_link",
00278                                                                     goal_constraints.position_constraints[0],
00279                                                                     goal_constraints.orientation_constraints[0]);
00280  
00281   get_req.planning_scene_diff.collision_objects.push_back(table);
00282   
00283   GetAndSetPlanningScene();
00284 
00285   arm_navigation_msgs::RobotState rs;
00286   planning_environment::convertKinematicStateToRobotState(*planning_scene_state_, ros::Time::now(), cm_->getWorldFrameId(), rs);
00287 
00288   rs.multi_dof_joint_state.joint_names.resize(1);
00289   rs.multi_dof_joint_state.frame_ids.resize(1);  
00290   rs.multi_dof_joint_state.child_frame_ids.resize(1);
00291   rs.multi_dof_joint_state.stamp = ros::Time::now();
00292   rs.multi_dof_joint_state.joint_names[0] = "base_joint";
00293   rs.multi_dof_joint_state.frame_ids[0] = "odom_combined";
00294   rs.multi_dof_joint_state.child_frame_ids[0] = "base_footprint";
00295   rs.multi_dof_joint_state.poses.resize(1);
00296 
00297   //first pose - straight on
00298 
00299   //X is 0.57 y -0.2 z -0.097925
00300 
00301   rs.multi_dof_joint_state.poses[0].position.x = 3.25;
00302   rs.multi_dof_joint_state.poses[0].position.y = -4.0;
00303   rs.multi_dof_joint_state.poses[0].position.z = 0.0;
00304   rs.multi_dof_joint_state.poses[0].orientation.w = 1.0;
00305   
00306   arm_navigation_msgs::PlanningScene temp_scene = get_res.planning_scene;
00307   temp_scene.robot_state = rs;
00308 
00309   cm_->writePlanningSceneBag(ros::package::getPath("planning_environment")+"/test_table_moved.bag",
00310                              temp_scene);
00311 
00312   kinematics_msgs::PositionIKRequest ik_request;
00313   ik_request.ik_link_name = "r_wrist_roll_link";
00314   ik_request.pose_stamped = obj_pose;
00315   ik_request.robot_state = rs;
00316   ik_request.ik_seed_state = rs;
00317 
00318   kinematics_msgs::GetConstraintAwarePositionIK::Request ik_req;
00319   kinematics_msgs::GetConstraintAwarePositionIK::Response ik_res;
00320   ik_req.ik_request = ik_request;
00321   ik_req.timeout = ros::Duration(1.0);
00322 
00323 
00324   ASSERT_TRUE(ik_service_client_.call(ik_req, ik_res));
00325   ASSERT_TRUE(ik_res.error_code.val == ik_res.error_code.SUCCESS);
00326 
00327   planning_environment::setRobotStateAndComputeTransforms(ik_res.solution, *planning_scene_state_);
00328 
00329   arm_navigation_msgs::ArmNavigationErrorCodes error_code;
00330   arm_navigation_msgs::Constraints path_constraints;
00331   ASSERT_TRUE(cm_->isKinematicStateValid(*planning_scene_state_,
00332                                          ik_res.solution.joint_state.name,
00333                                          error_code,
00334                                          goal_constraints,
00335                                          path_constraints));
00336 
00337   mplan_req.motion_plan_request.goal_constraints.joint_constraints.clear();
00338   mplan_req.motion_plan_request.goal_constraints.position_constraints.clear();
00339 
00340   for(unsigned int i = 0; i < ik_res.solution.joint_state.name.size(); i++) {
00341     arm_navigation_msgs::JointConstraint jc;
00342     jc.joint_name = ik_res.solution.joint_state.name[i];
00343     jc.position = ik_res.solution.joint_state.position[i];
00344     jc.tolerance_below = 0.01;
00345     jc.tolerance_above = 0.01;
00346     mplan_req.motion_plan_request.goal_constraints.joint_constraints.push_back(jc);
00347   }
00348   mplan_req.motion_plan_request.start_state = rs;
00349   
00350   arm_navigation_msgs::GetMotionPlan::Response mplan_res;
00351   ASSERT_TRUE(planning_service_client_.call(mplan_req, mplan_res));
00352   
00353   ASSERT_EQ(mplan_res.error_code.val,mplan_res.error_code.SUCCESS);
00354   
00355   EXPECT_GT(mplan_res.trajectory.joint_trajectory.points.size(), 0);
00356   
00357   std::vector<arm_navigation_msgs::ArmNavigationErrorCodes> trajectory_error_codes;  
00358   EXPECT_TRUE(cm_->isJointTrajectoryValid(*planning_scene_state_, 
00359                                      mplan_res.trajectory.joint_trajectory,
00360                                      mplan_req.motion_plan_request.goal_constraints,
00361                                      mplan_req.motion_plan_request.path_constraints,
00362                                      error_code,
00363                                      trajectory_error_codes, false)) << error_code;
00364   
00365   planning_environment::setRobotStateAndComputeTransforms(get_res.planning_scene.robot_state, *planning_scene_state_);
00366   double planner_length = cm_->getTotalTrajectoryJointLength(*planning_scene_state_, mplan_res.trajectory.joint_trajectory);
00367   
00368   arm_navigation_msgs::FilterJointTrajectoryWithConstraints::Request filter_req;
00369   arm_navigation_msgs::FilterJointTrajectoryWithConstraints::Response filter_res;
00370   
00371   filter_req.trajectory = mplan_res.trajectory.joint_trajectory;
00372   
00373   filter_req.goal_constraints = mplan_req.motion_plan_request.goal_constraints;
00374   filter_req.path_constraints = mplan_req.motion_plan_request.path_constraints;
00375   filter_req.allowed_time = ros::Duration(1.0);
00376   
00377   EXPECT_TRUE(trajectory_filter_client_.call(filter_req, filter_res));
00378   
00379   EXPECT_TRUE(cm_->isJointTrajectoryValid(*planning_scene_state_, 
00380                                      filter_res.trajectory,
00381                                      mplan_req.motion_plan_request.goal_constraints,
00382                                      mplan_req.motion_plan_request.path_constraints,
00383                                      error_code,
00384                                      trajectory_error_codes, false)) << error_code;
00385   
00386   planning_environment::setRobotStateAndComputeTransforms(get_res.planning_scene.robot_state, *planning_scene_state_);
00387   double filter_length = cm_->getTotalTrajectoryJointLength(*planning_scene_state_, filter_res.trajectory);
00388   
00389   EXPECT_GE(planner_length, filter_length);
00390   
00391   ROS_INFO_STREAM("Planner points " << mplan_res.trajectory.joint_trajectory.points.size() << " filter points " << filter_res.trajectory.points.size() << " planner length " << planner_length << " filter length " << filter_length);
00392 }
00393   
00394 int main(int argc, char **argv)
00395 {
00396   testing::InitGoogleTest(&argc, argv);
00397   ros::init(argc, argv, "test_simple_trajectory_pipeline");
00398     
00399   return RUN_ALL_TESTS();
00400 }
test_simple_trajectory_pipeline.cpp
