test_simple_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 #include <planning_models/conversions.h>
00044 
00045 static const std::string PLANNER_SERVICE_NAME="/ompl_planning/plan_kinematic_path";
00046 static const std::string ROBOT_DESCRIPTION="robot_description";
00047 
00048 TEST(OmplPlanning, SimplePlan)
00049 {
00050     ros::NodeHandle nh;
00051     ros::service::waitForService(PLANNER_SERVICE_NAME);
00052     ros::Publisher pub = nh.advertise<moveit_msgs::DisplayTrajectory>("display_motion_plan", 1);
00053 
00054     ros::ServiceClient planning_service_client = nh.serviceClient<moveit_msgs::GetMotionPlan>(PLANNER_SERVICE_NAME);
00055     EXPECT_TRUE(planning_service_client.exists());
00056     EXPECT_TRUE(planning_service_client.isValid());
00057 
00058     moveit_msgs::GetMotionPlan::Request mplan_req;
00059     moveit_msgs::GetMotionPlan::Response mplan_res;
00060 
00061     planning_scene_monitor::PlanningSceneMonitor psm(ROBOT_DESCRIPTION);
00062     planning_scene::PlanningScene &scene = *psm.getPlanningScene();
00063     EXPECT_TRUE(scene.isConfigured());
00064 
00065     mplan_req.motion_plan_request.planner_id = "msRRTConnectkConfigDefault";
00066     mplan_req.motion_plan_request.group_name = "whole_body";
00067     mplan_req.motion_plan_request.num_planning_attempts = 1;
00068     mplan_req.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
00069     planning_models::RobotState *start = scene.getCurrentState();
00070     planning_models::robotStateToRobotStateMsg(start, mplan_req.motion_plan_request.start_state);
00071 
00072     start.getJointStateGroup("right_arm")->setToRandomValues();
00073     mplan_req.motion_plan_request.goal_constraints.resize(1);
00074     mplan_req.motion_plan_request.goal_constraints[0] = kinematic_constraints::constructGoalConstraints(start.getJointStateGroup("right_arm"));
00075     std::cout << mplan_req.motion_plan_request << std::endl;
00076 
00077     ASSERT_TRUE(planning_service_client.call(mplan_req, mplan_res));
00078     ASSERT_EQ(mplan_res.error_code.val, mplan_res.error_code.SUCCESS);
00079     EXPECT_GT(mplan_res.trajectory.joint_trajectory.points.size(), 0);
00080 
00081     moveit_msgs::DisplayTrajectory d;
00082     d.model_id = scene.getRobotModel()->getName();
00083     d.trajectory_start = mplan_res.trajectory_start;
00084     d.trajectory = mplan_res.trajectory;
00085     pub.publish(d);
00086     ros::Duration(0.5).sleep();
00087 }
00088 
00089 int main(int argc, char **argv)
00090 {
00091   testing::InitGoogleTest(&argc, argv);
00092 
00093   ros::init(argc, argv, "test_ompl_planning", ros::init_options::AnonymousName);
00094   ros::AsyncSpinner spinner(1);
00095   spinner.start();
00096 
00097   return RUN_ALL_TESTS();
00098 }


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