00001 #include "actionlib/client/simple_action_client.h" 00002 #include "geometry_msgs/Pose.h" 00003 #include "moveit_goal_builder/builder.h" 00004 #include "moveit_msgs/MoveGroupAction.h" 00005 #include "moveit_msgs/MoveItErrorCodes.h" 00006 #include "ros/ros.h" 00007 00008 int main(int argc, char** argv) { 00009 ros::init(argc, argv, "moveit_goal_builder_demo"); 00010 00011 actionlib::SimpleActionClient<moveit_msgs::MoveGroupAction> client( 00012 "move_group", true); 00013 while (ros::ok() && !client.waitForServer(ros::Duration(5.0))) { 00014 ROS_INFO("Waiting for move_group server."); 00015 } 00016 00017 moveit_goal_builder::Builder builder("base_link", "arms"); 00018 geometry_msgs::Pose r_pose; 00019 r_pose.position.x = 0.372; 00020 r_pose.position.y = -0.07; 00021 r_pose.position.z = 1.11; 00022 r_pose.orientation.w = 1; 00023 geometry_msgs::Pose l_pose; 00024 l_pose.position.x = 0.372; 00025 l_pose.position.y = 0.07; 00026 l_pose.position.z = 1.11; 00027 l_pose.orientation.w = 1; 00028 builder.AddPoseGoal("l_wrist_roll_link", l_pose); 00029 builder.AddPoseGoal("r_wrist_roll_link", r_pose); 00030 00031 moveit_msgs::MoveGroupGoal goal; 00032 builder.Build(&goal); 00033 client.sendGoal(goal); 00034 00035 while (ros::ok() && !client.getState().isDone()) { 00036 ros::spinOnce(); 00037 } 00038 00039 actionlib::SimpleClientGoalState state = client.getState(); 00040 if (state == actionlib::SimpleClientGoalState::SUCCEEDED) { 00041 moveit_msgs::MoveGroupResultConstPtr result = client.getResult(); 00042 if (result->error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS) { 00043 ROS_INFO("Succeeded!"); 00044 } else { 00045 ROS_ERROR( 00046 "MoveIt error: %d (see " 00047 "http://docs.ros.org/api/moveit_msgs/html/msg/MoveItErrorCodes.html)", 00048 result->error_code.val); 00049 } 00050 } else { 00051 ROS_ERROR("Action error: %s", state.toString().c_str()); 00052 } 00053 00054 client.cancelAllGoals(); 00055 00056 return 0; 00057 }