Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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, PathConstrainedSimplePlan)
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, NULL);
00062 planning_scene::PlanningScene &scene = *psm.getPlanningScene();
00063 EXPECT_TRUE(scene.isConfigured());
00064
00065 mplan_req.motion_plan_request.planner_id = "RRTConnectkConfigDefault";
00066 mplan_req.motion_plan_request.group_name = "arms";
00067 mplan_req.motion_plan_request.num_planning_attempts = 1;
00068 mplan_req.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
00069
00070
00071
00072
00073 moveit_msgs::PositionConstraint pcm;
00074 pcm.link_name = "l_wrist_roll_link";
00075 pcm.target_point_offset.x = 0;
00076 pcm.target_point_offset.y = 0;
00077 pcm.target_point_offset.z = 0;
00078 pcm.constraint_region_shape.type = shape_msgs::Shape::BOX;
00079 pcm.constraint_region_shape.dimensions.push_back(0.001);
00080 pcm.constraint_region_shape.dimensions.push_back(0.001);
00081 pcm.constraint_region_shape.dimensions.push_back(0.001);
00082
00083 pcm.constraint_region_pose.header.frame_id = scene.getRobotModel()->getModelFrame();
00084 pcm.constraint_region_pose.pose.position.x = 0.5;
00085 pcm.constraint_region_pose.pose.position.y = 0.4;
00086 pcm.constraint_region_pose.pose.position.z = 0.7;
00087 pcm.constraint_region_pose.pose.orientation.x = 0.0;
00088 pcm.constraint_region_pose.pose.orientation.y = 0.0;
00089 pcm.constraint_region_pose.pose.orientation.z = 0.0;
00090 pcm.constraint_region_pose.pose.orientation.w = 1.0;
00091 pcm.weight = 1.0;
00092
00093 mplan_req.motion_plan_request.goal_constraints.resize(1);
00094 mplan_req.motion_plan_request.goal_constraints[0].position_constraints.push_back(pcm);
00095
00096
00097
00098 moveit_msgs::Constraints &c = mplan_req.motion_plan_request.path_constraints;
00099
00100 moveit_msgs::PositionConstraint pcm2;
00101 pcm2.link_name = "r_wrist_roll_link";
00102 pcm2.target_point_offset.x = 0.7;
00103 pcm2.target_point_offset.y = 0;
00104 pcm2.target_point_offset.z = 0;
00105 pcm2.constraint_region_shape.type = shape_msgs::Shape::BOX;
00106 pcm2.constraint_region_shape.dimensions.push_back(0.1);
00107 pcm2.constraint_region_shape.dimensions.push_back(0.1);
00108 pcm2.constraint_region_shape.dimensions.push_back(0.1);
00109
00110 pcm2.constraint_region_pose.header.frame_id = "l_wrist_roll_link";
00111 pcm2.constraint_region_pose.pose.position.x = 0.0;
00112 pcm2.constraint_region_pose.pose.position.y = 0.0;
00113 pcm2.constraint_region_pose.pose.position.z = 0.0;
00114 pcm2.constraint_region_pose.pose.orientation.x = 0.0;
00115 pcm2.constraint_region_pose.pose.orientation.y = 0.0;
00116 pcm2.constraint_region_pose.pose.orientation.z = 0.0;
00117 pcm2.constraint_region_pose.pose.orientation.w = 1.0;
00118 pcm2.weight = 1.0;
00119 c.position_constraints.push_back(pcm2);
00120
00121 moveit_msgs::OrientationConstraint ocm;
00122 ocm.link_name = "l_wrist_roll_link";
00123 ocm.orientation.header.frame_id = scene.getRobotModel()->getModelFrame();
00124 ocm.orientation.quaternion.x = 0.5;
00125 ocm.orientation.quaternion.y = 0.5;
00126 ocm.orientation.quaternion.z = 0.5;
00127 ocm.orientation.quaternion.w = 0.5;
00128 ocm.absolute_x_axis_tolerance = 0.1;
00129 ocm.absolute_y_axis_tolerance = M_PI;
00130 ocm.absolute_z_axis_tolerance = 0.1;
00131 ocm.weight = 1.0;
00132 c.orientation_constraints.push_back(ocm);
00133
00134 ocm.link_name = "r_wrist_roll_link";
00135 ocm.orientation.header.frame_id = "l_wrist_roll_link";
00136 ocm.orientation.quaternion.x = 0.0;
00137 ocm.orientation.quaternion.y = 0.0;
00138 ocm.orientation.quaternion.z = 1.0;
00139 ocm.orientation.quaternion.w = 0.0;
00140 ocm.absolute_x_axis_tolerance = 0.1;
00141 ocm.absolute_y_axis_tolerance = 0.1;
00142 ocm.absolute_z_axis_tolerance = 0.1;
00143 ocm.weight = 1.0;
00144 c.orientation_constraints.push_back(ocm);
00145
00146
00147
00148
00149
00150 kinematics_plugin_loader::KinematicsPluginLoader kinematics_loader;
00151 kinematics_plugin_loader::KinematicsLoaderFn kinematics_allocator = kinematics_loader.getLoaderFunction();
00152 kinematic_constraints::KinematicsSubgroupAllocator sa;
00153 sa[scene.getRobotModel()->getJointModelGroup("left_arm")] = kinematics_allocator;
00154 sa[scene.getRobotModel()->getJointModelGroup("right_arm")] = kinematics_allocator;
00155
00156 kinematic_constraints::ConstraintSamplerPtr s = kinematic_constraints::ConstraintSampler::constructFromMessage
00157 (scene.getRobotModel()->getJointModelGroup("arms"), c, scene.getRobotModel(), scene.getTransforms(), kinematic_constraints::KinematicsAllocator(), sa);
00158
00159 EXPECT_TRUE(s.get() != NULL);
00160
00161 kinematic_constraints::KinematicConstraintSet kset(scene.getRobotModel(), scene.getTransforms());
00162 kset.add(c);
00163
00164 bool found = false;
00165 planning_models::RobotState *ks(scene.getRobotModel());
00166 ks.setToDefaultValues();
00167 for (int i = 0 ; i < 100 ; ++i)
00168 {
00169 std::vector<double> values;
00170 if (s->sample(values, ks, 10))
00171 {
00172 ks.getJointStateGroup("arms")->setStateValues(values);
00173 planning_models::robotStateToRobotStateMsg(ks, mplan_req.motion_plan_request.start_state);
00174 moveit_msgs::DisplayTrajectory d;
00175 d.model_id = scene.getRobotModel()->getName();
00176 d.trajectory_start = mplan_req.motion_plan_request.start_state;
00177 pub.publish(d);
00178 ros::Duration(0.5).sleep();
00179 found = true;
00180 break;
00181 }
00182 }
00183 EXPECT_TRUE(found);
00184
00185
00186
00187 ASSERT_TRUE(planning_service_client.call(mplan_req, mplan_res));
00188 ASSERT_EQ(mplan_res.error_code.val, mplan_res.error_code.SUCCESS);
00189 EXPECT_GT(mplan_res.trajectory.joint_trajectory.points.size(), 0);
00190
00191
00192 moveit_msgs::DisplayTrajectory d;
00193 d.model_id = scene.getRobotModel()->getName();
00194 d.trajectory_start = mplan_res.trajectory_start;
00195 d.trajectory = mplan_res.trajectory;
00196 pub.publish(d);
00197 ros::Duration(0.5).sleep();
00198 }
00199
00200 int main(int argc, char **argv)
00201 {
00202 testing::InitGoogleTest(&argc, argv);
00203
00204 ros::init(argc, argv, "test_ompl_planning");
00205 ros::AsyncSpinner spinner(1);
00206 spinner.start();
00207
00208 return RUN_ALL_TESTS();
00209 }