$search
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 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.allowed_planning_time = ros::Duration(5.0); 00018 00019 nh.param<std::string>("planner_id",goalA.motion_plan_request.planner_id,std::string("")); 00020 nh.param<std::string>("planner_service_name",goalA.planner_service_name,std::string("ompl_planning/plan_kinematic_path")); 00021 goalA.motion_plan_request.goal_constraints.set_position_constraints_size(1); 00022 goalA.motion_plan_request.goal_constraints.position_constraints[0].header.stamp = ros::Time::now(); 00023 goalA.motion_plan_request.goal_constraints.position_constraints[0].header.frame_id = "torso_lift_link"; 00024 00025 goalA.motion_plan_request.goal_constraints.position_constraints[0].link_name = "r_wrist_roll_link"; 00026 goalA.motion_plan_request.goal_constraints.position_constraints[0].position.x = 0.75; 00027 goalA.motion_plan_request.goal_constraints.position_constraints[0].position.y = -0.188; 00028 goalA.motion_plan_request.goal_constraints.position_constraints[0].position.z = 0; 00029 00030 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.type = arm_navigation_msgs::Shape::BOX; 00031 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02); 00032 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02); 00033 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02); 00034 00035 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_orientation.w = 1.0; 00036 goalA.motion_plan_request.goal_constraints.position_constraints[0].weight = 1.0; 00037 00038 goalA.motion_plan_request.goal_constraints.set_orientation_constraints_size(1); 00039 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].header.stamp = ros::Time::now(); 00040 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].header.frame_id = "torso_lift_link"; 00041 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].link_name = "r_wrist_roll_link"; 00042 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.x = 0.0; 00043 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.y = 0.0; 00044 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.z = 0.0; 00045 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.w = 1.0; 00046 00047 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_roll_tolerance = 0.04; 00048 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_pitch_tolerance = 0.04; 00049 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_yaw_tolerance = 0.04; 00050 00051 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].weight = 1.0; 00052 00053 if (nh.ok()) 00054 { 00055 bool finished_within_time = false; 00056 move_arm.sendGoal(goalA); 00057 finished_within_time = move_arm.waitForResult(ros::Duration(200.0)); 00058 if (!finished_within_time) 00059 { 00060 move_arm.cancelGoal(); 00061 ROS_INFO("Timed out achieving goal A"); 00062 } 00063 else 00064 { 00065 actionlib::SimpleClientGoalState state = move_arm.getState(); 00066 bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED); 00067 if(success) 00068 ROS_INFO("Action finished: %s",state.toString().c_str()); 00069 else 00070 ROS_INFO("Action failed: %s",state.toString().c_str()); 00071 } 00072 } 00073 ros::shutdown(); 00074 }