00001 #include <ros/ros.h> 00002 #include <actionlib/client/simple_action_client.h> 00003 00004 #include <arm_navigation_msgs/MoveArmAction.h> 00005 #include <arm_navigation_msgs/utils.h> 00006 00007 int main(int argc, char **argv){ 00008 ros::init (argc, argv, "move_arm_pose_goal_test"); 00009 ros::NodeHandle nh; 00010 actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm("move_right_arm",true); 00011 move_arm.waitForServer(); 00012 ROS_INFO("Connected to server"); 00013 arm_navigation_msgs::MoveArmGoal goalA; 00014 00015 goalA.motion_plan_request.group_name = "right_arm"; 00016 goalA.motion_plan_request.num_planning_attempts = 1; 00017 goalA.motion_plan_request.planner_id = std::string(""); 00018 goalA.planner_service_name = std::string("ompl_planning/plan_kinematic_path"); 00019 goalA.motion_plan_request.allowed_planning_time = ros::Duration(5.0); 00020 00021 arm_navigation_msgs::SimplePoseConstraint desired_pose; 00022 desired_pose.header.frame_id = "torso_lift_link"; 00023 desired_pose.link_name = "r_wrist_roll_link"; 00024 desired_pose.pose.position.x = 0.75; 00025 desired_pose.pose.position.y = -0.188; 00026 desired_pose.pose.position.z = 0; 00027 00028 desired_pose.pose.orientation.x = 0.0; 00029 desired_pose.pose.orientation.y = 0.0; 00030 desired_pose.pose.orientation.z = 0.0; 00031 desired_pose.pose.orientation.w = 1.0; 00032 00033 desired_pose.absolute_position_tolerance.x = 0.02; 00034 desired_pose.absolute_position_tolerance.y = 0.02; 00035 desired_pose.absolute_position_tolerance.z = 0.02; 00036 00037 desired_pose.absolute_roll_tolerance = 0.04; 00038 desired_pose.absolute_pitch_tolerance = 0.04; 00039 desired_pose.absolute_yaw_tolerance = 0.04; 00040 00041 arm_navigation_msgs::addGoalConstraintToMoveArmGoal(desired_pose,goalA); 00042 00043 if (nh.ok()) 00044 { 00045 bool finished_within_time = false; 00046 move_arm.sendGoal(goalA); 00047 finished_within_time = move_arm.waitForResult(ros::Duration(200.0)); 00048 if (!finished_within_time) 00049 { 00050 move_arm.cancelGoal(); 00051 ROS_INFO("Timed out achieving goal A"); 00052 } 00053 else 00054 { 00055 actionlib::SimpleClientGoalState state = move_arm.getState(); 00056 bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED); 00057 if(success) 00058 ROS_INFO("Action finished: %s",state.toString().c_str()); 00059 else 00060 ROS_INFO("Action failed: %s",state.toString().c_str()); 00061 } 00062 } 00063 ros::shutdown(); 00064 }