test_dual_arm_plan.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2011, 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 
00035 /* Author: Ioan Sucan */
00036 
00037 #include <gtest/gtest.h>
00038 #include <planning_scene_monitor/planning_scene_monitor.h>
00039 #include <ompl_interface_ros/ompl_interface_ros.h>
00040 #include <moveit_msgs/GetMotionPlan.h>
00041 #include <moveit_msgs/DisplayTrajectory.h>
00042 #include <kinematic_constraints/utils.h>
00043 
00044 static const std::string PLANNER_SERVICE_NAME="/ompl_planning/plan_kinematic_path";
00045 static const std::string ROBOT_DESCRIPTION="robot_description";
00046 
00047 TEST(OmplPlanning, SimplePlan)
00048 {
00049     ros::NodeHandle nh;
00050     ros::service::waitForService(PLANNER_SERVICE_NAME);
00051     ros::Publisher pub = nh.advertise<moveit_msgs::DisplayTrajectory>("display_motion_plan", 1);
00052 
00053     ros::ServiceClient planning_service_client = nh.serviceClient<moveit_msgs::GetMotionPlan>(PLANNER_SERVICE_NAME);
00054     EXPECT_TRUE(planning_service_client.exists());
00055     EXPECT_TRUE(planning_service_client.isValid());
00056 
00057     moveit_msgs::GetMotionPlan::Request mplan_req;
00058     moveit_msgs::GetMotionPlan::Response mplan_res;
00059 
00060     planning_scene_monitor::PlanningSceneMonitor psm(ROBOT_DESCRIPTION);
00061     planning_scene::PlanningScene &scene = *psm.getPlanningScene();
00062     EXPECT_TRUE(scene.isConfigured());
00063 
00064     mplan_req.motion_plan_request.group_name = "arms";
00065     mplan_req.motion_plan_request.num_planning_attempts = 1;
00066     mplan_req.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
00067 
00068     geometry_msgs::PoseStamped pose;
00069     pose.header.frame_id = scene.getRobotModel()->getModelFrame();
00070     pose.pose.position.x = 0.55;
00071     pose.pose.position.y = 0.2;
00072     pose.pose.position.z = 1.25;
00073     pose.pose.orientation.x = 0.0;
00074     pose.pose.orientation.y = 0.0;
00075     pose.pose.orientation.z = 0.0;
00076     pose.pose.orientation.w = 1.0;
00077     moveit_msgs::Constraints g0 = kinematic_constraints::constructGoalConstraints("l_wrist_roll_link", pose);
00078 
00079 
00080     pose.pose.position.x = 0.35;
00081     pose.pose.position.y = -0.6;
00082     pose.pose.position.z = 1.25;
00083     pose.pose.orientation.x = 0.0;
00084     pose.pose.orientation.y = 0.0;
00085     pose.pose.orientation.z = 0.0;
00086     pose.pose.orientation.w = 1.0;
00087     moveit_msgs::Constraints g1 = kinematic_constraints::constructGoalConstraints("r_wrist_roll_link", pose);
00088 
00089     mplan_req.motion_plan_request.goal_constraints.resize(1);
00090     mplan_req.motion_plan_request.goal_constraints[0] = kinematic_constraints::mergeConstraints(g1, g0);
00091 
00092     ASSERT_TRUE(planning_service_client.call(mplan_req, mplan_res));
00093     ASSERT_EQ(mplan_res.error_code.val, mplan_res.error_code.SUCCESS);
00094     EXPECT_GT(mplan_res.trajectory.joint_trajectory.points.size(), 0);
00095 
00096     moveit_msgs::DisplayTrajectory d;
00097     d.model_id = scene.getRobotModel()->getName();
00098     d.trajectory_start = mplan_res.trajectory_start;
00099     d.trajectory = mplan_res.trajectory;
00100     pub.publish(d);
00101     ros::Duration(0.5).sleep();
00102 }
00103 
00104 int main(int argc, char **argv)
00105 {
00106   testing::InitGoogleTest(&argc, argv);
00107 
00108   ros::init(argc, argv, "test_ompl_planning", ros::init_options::AnonymousName);
00109   ros::AsyncSpinner spinner(1);
00110   spinner.start();
00111 
00112   return RUN_ALL_TESTS();
00113 }


pr2_ompl_planning_tests
Author(s): Ioan Sucan
autogenerated on Mon Oct 6 2014 11:13:17