00001 // 00002 // Created by tom on 20/04/16. 00003 // 00004 00005 #include <ros/ros.h> 00006 00007 #include <moveit/move_group_interface/move_group.h> 00008 #include <moveit/planning_scene_interface/planning_scene_interface.h> 00009 #include <moveit_msgs/DisplayRobotState.h> 00010 #include <moveit_msgs/DisplayTrajectory.h> 00011 00012 using namespace std; 00013 00014 int main(int argc, char **argv) { 00015 ros::init(argc, argv,"moveit_group_goal"); 00016 ros::AsyncSpinner spinner(1); 00017 spinner.start(); 00018 ros::NodeHandle nodeHandle; 00019 00020 moveit::planning_interface::MoveGroup group("arm"); 00021 00022 group.setMaxVelocityScalingFactor(0.1); 00023 group.setMaxAccelerationScalingFactor(0.5); 00024 group.setPlanningTime(10.0); 00025 group.setNumPlanningAttempts(500); 00026 group.setPlannerId("RRTConnectkConfigDefault"); 00027 group.setPoseReferenceFrame("base_footprint"); 00028 group.setGoalTolerance(0.1); 00029 00030 group.setStartStateToCurrentState(); 00031 00032 // ROS_INFO("End effector reference frame: %s", group.getEndEffectorLink().c_str()); 00033 00034 geometry_msgs::PoseStamped target_pose; 00035 target_pose.header.frame_id="base_footprint"; 00036 target_pose.header.stamp=ros::Time::now()+ros::Duration(10.0); 00037 target_pose.pose.position.x = 0.54; 00038 target_pose.pose.position.y = 0.0; 00039 target_pose.pose.position.z = 0.84; 00040 target_pose.pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(M_PI,0.0,0.0); //horizontal 00041 group.setPoseTarget(target_pose); 00042 moveit::planning_interface::MoveGroup::Plan my_plan; 00043 bool success = group.plan(my_plan); 00044 ROS_INFO("plan: %s",success?"SUCCESS":"FAILED"); 00045 if(success) { 00046 ROS_INFO("Moving..."); 00047 group.move(); 00048 } 00049 sleep(5); 00050 00051 00052 00053 return 0; 00054 }