00001 #include <ros/ros.h> 00002 #include <actionlib/client/simple_action_client.h> 00003 #include <arm_navigation_msgs/MoveArmAction.h> 00004 00005 int main(int argc, char **argv){ 00006 ros::init (argc, argv, "move_arm_joint_goal_test"); 00007 ros::NodeHandle nh; 00008 actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm("move_right_arm",true); 00009 00010 move_arm.waitForServer(); 00011 ROS_INFO("Connected to server"); 00012 00013 arm_navigation_msgs::MoveArmGoal goalB; 00014 std::vector<std::string> names(7); 00015 names[0] = "r_shoulder_pan_joint"; 00016 names[1] = "r_shoulder_lift_joint"; 00017 names[2] = "r_upper_arm_roll_joint"; 00018 names[3] = "r_elbow_flex_joint"; 00019 names[4] = "r_forearm_roll_joint"; 00020 names[5] = "r_wrist_flex_joint"; 00021 names[6] = "r_wrist_roll_joint"; 00022 00023 goalB.motion_plan_request.group_name = "right_arm"; 00024 goalB.motion_plan_request.num_planning_attempts = 1; 00025 goalB.motion_plan_request.allowed_planning_time = ros::Duration(5.0); 00026 00027 goalB.motion_plan_request.planner_id= std::string(""); 00028 goalB.planner_service_name = std::string("ompl_planning/plan_kinematic_path"); 00029 goalB.motion_plan_request.goal_constraints.joint_constraints.resize(names.size()); 00030 00031 for (unsigned int i = 0 ; i < goalB.motion_plan_request.goal_constraints.joint_constraints.size(); ++i) 00032 { 00033 goalB.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = names[i]; 00034 goalB.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0; 00035 goalB.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.1; 00036 goalB.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.1; 00037 } 00038 00039 goalB.motion_plan_request.goal_constraints.joint_constraints[0].position = -2.0; 00040 goalB.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2; 00041 goalB.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.15; 00042 00043 if (nh.ok()) 00044 { 00045 bool finished_within_time = false; 00046 move_arm.sendGoal(goalB); 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 }