Go to the documentation of this file.00001
00002 #include <ros/ros.h>
00003 #include <actionlib/client/simple_action_client.h>
00004 #include <move_arm_msgs/MoveArmAction.h>
00005
00006 int main(int argc, char **argv){
00007 ros::init (argc, argv, "move_arm_joint_goal_test");
00008 ros::NodeHandle nh;
00009 actionlib::SimpleActionClient<move_arm_msgs::MoveArmAction> move_arm("move_right_arm",true);
00010
00011 move_arm.waitForServer();
00012 ROS_INFO("Connected to server");
00013
00014 move_arm_msgs::MoveArmGoal goalB;
00015 std::vector<std::string> names(7);
00016 names[0] = "r_shoulder_pan_joint";
00017 names[1] = "r_shoulder_lift_joint";
00018 names[2] = "r_upper_arm_roll_joint";
00019 names[3] = "r_elbow_flex_joint";
00020 names[4] = "r_forearm_roll_joint";
00021 names[5] = "r_wrist_flex_joint";
00022 names[6] = "r_wrist_roll_joint";
00023
00024 goalB.motion_plan_request.group_name = "right_arm";
00025 goalB.motion_plan_request.num_planning_attempts = 1;
00026 goalB.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
00027
00028 goalB.motion_plan_request.planner_id= std::string("");
00029 goalB.planner_service_name = std::string("ompl_planning/plan_kinematic_path");
00030 goalB.motion_plan_request.goal_constraints.joint_constraints.resize(names.size());
00031
00032 for (unsigned int i = 0 ; i < goalB.motion_plan_request.goal_constraints.joint_constraints.size(); ++i)
00033 {
00034 goalB.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = names[i];
00035 goalB.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0;
00036 goalB.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.05;
00037 goalB.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.05;
00038 }
00039
00040 goalB.motion_plan_request.goal_constraints.joint_constraints[0].position = -0.082971740453390508;
00041 goalB.motion_plan_request.goal_constraints.joint_constraints[1].position = -0.00024050597696160733;
00042 goalB.motion_plan_request.goal_constraints.joint_constraints[2].position = -1.7613330259531139;
00043 goalB.motion_plan_request.goal_constraints.joint_constraints[3].position = -1.1626271435626965;
00044 goalB.motion_plan_request.goal_constraints.joint_constraints[4].position = -0.032142093106232839;
00045 goalB.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.19405610249061456;
00046 goalB.motion_plan_request.goal_constraints.joint_constraints[6].position = 0.18618502991750852;
00047
00048
00049
00050 if (nh.ok())
00051 {
00052 bool finished_within_time = false;
00053 move_arm.sendGoal(goalB);
00054 finished_within_time = move_arm.waitForResult(ros::Duration(200.0));
00055 if (!finished_within_time)
00056 {
00057 move_arm.cancelGoal();
00058 ROS_INFO("Timed out achieving goal A");
00059 }
00060 else
00061 {
00062 actionlib::SimpleClientGoalState state = move_arm.getState();
00063 bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
00064 if(success)
00065 ROS_INFO("Action finished: %s",state.toString().c_str());
00066 else
00067 ROS_INFO("Action failed: %s",state.toString().c_str());
00068 }
00069 }
00070 ros::shutdown();
00071 }
00072
00073
00074