00001 #include <ros/ros.h> 00002 #include <arm_navigation_msgs/GetMotionPlan.h> 00003 00004 #include <planning_environment/monitors/joint_state_monitor.h> 00005 #include <boost/thread.hpp> 00006 00007 void spinThread() 00008 { 00009 ros::spin(); 00010 } 00011 00012 int main(int argc, char **argv) 00013 { 00014 ros::init (argc, argv, "ompl_pose_goal"); 00015 boost::thread spin_thread(&spinThread); 00016 00017 ros::NodeHandle nh; 00018 00019 arm_navigation_msgs::GetMotionPlan::Request request; 00020 arm_navigation_msgs::GetMotionPlan::Response response; 00021 00022 request.motion_plan_request.group_name = "right_arm"; 00023 request.motion_plan_request.num_planning_attempts = 1; 00024 request.motion_plan_request.planner_id = std::string(""); 00025 request.motion_plan_request.allowed_planning_time = ros::Duration(5.0); 00026 00027 request.motion_plan_request.goal_constraints.position_constraints.resize(1); 00028 request.motion_plan_request.goal_constraints.position_constraints[0].header.stamp = ros::Time::now(); 00029 request.motion_plan_request.goal_constraints.position_constraints[0].header.frame_id = "torso_lift_link"; 00030 00031 request.motion_plan_request.goal_constraints.position_constraints[0].link_name = "r_wrist_roll_link"; 00032 request.motion_plan_request.goal_constraints.position_constraints[0].position.x = 0.75; 00033 request.motion_plan_request.goal_constraints.position_constraints[0].position.y = -0.188; 00034 request.motion_plan_request.goal_constraints.position_constraints[0].position.z = 0; 00035 00036 request.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.type = geometric_shapes_msgs::Shape::BOX; 00037 request.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02); 00038 request.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02); 00039 request.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02); 00040 00041 request.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_orientation.w = 1.0; 00042 request.motion_plan_request.goal_constraints.position_constraints[0].weight = 1.0; 00043 00044 request.motion_plan_request.goal_constraints.orientation_constraints.resize(1); 00045 request.motion_plan_request.goal_constraints.orientation_constraints[0].header.stamp = ros::Time::now(); 00046 request.motion_plan_request.goal_constraints.orientation_constraints[0].header.frame_id = "torso_lift_link"; 00047 request.motion_plan_request.goal_constraints.orientation_constraints[0].link_name = "r_wrist_roll_link"; 00048 request.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.x = 0.0; 00049 request.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.y = 0.0; 00050 request.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.z = 0.0; 00051 request.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.w = 1.0; 00052 00053 request.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_roll_tolerance = 0.04; 00054 request.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_pitch_tolerance = 0.04; 00055 request.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_yaw_tolerance = 0.04; 00056 00057 request.motion_plan_request.goal_constraints.orientation_constraints[0].weight = 1.0; 00058 00059 00060 ros::ServiceClient service_client = nh.serviceClient<motion_planning_msgs::GetMotionPlan>("ompl_planning/plan_kinematic_path"); 00061 service_client.call(request,response); 00062 if(response.error_code.val != response.error_code.SUCCESS) 00063 { 00064 ROS_ERROR("Motion planning failed"); 00065 } 00066 else 00067 { 00068 ROS_INFO("Motion planning succeeded"); 00069 } 00070 00071 00072 planning_environment::JointStateMonitor joint_state_monitor; 00073 ros::Publisher display_trajectory_publisher = nh.advertise<motion_planning_msgs::DisplayTrajectory>("joint_path_display", 1); 00074 while(display_trajectory_publisher.getNumSubscribers() < 1 && nh.ok()) 00075 { 00076 ROS_INFO("Waiting for subscriber"); 00077 ros::Duration(0.1).sleep(); 00078 } 00079 motion_planning_msgs::DisplayTrajectory display_trajectory; 00080 00081 display_trajectory.model_id = "pr2"; 00082 display_trajectory.trajectory.joint_trajectory.header.frame_id = "base_footprint"; 00083 display_trajectory.trajectory.joint_trajectory.header.stamp = ros::Time::now(); 00084 display_trajectory.robot_state.joint_state = joint_state_monitor.getJointStateRealJoints(); 00085 display_trajectory.trajectory = response.trajectory; 00086 ROS_INFO("Publishing path for display"); 00087 display_trajectory_publisher.publish(display_trajectory); 00088 joint_state_monitor.stop(); 00089 ros::shutdown(); 00090 spin_thread.join(); 00091 return(0); 00092 00093 } 00094