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