Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039 #include <ros/ros.h>
00040 #include <actionlib/client/simple_action_client.h>
00041 #include <arm_navigation_msgs/MoveArmAction.h>
00042
00043 #include <stdio.h>
00044 #include <stdlib.h>
00045 #include <time.h>
00046 #include <boost/thread.hpp>
00047 #include <ros/ros.h>
00048 #include <gtest/gtest.h>
00049
00050 typedef actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> MoveArmClient;
00051
00052 void spinThread()
00053 {
00054 ros::spin();
00055 }
00056
00057 TEST(MoveArm, goToPoseGoal)
00058 {
00059 ros::NodeHandle nh;
00060 ros::NodeHandle private_handle("~");
00061 actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm(nh, "move_right_arm");
00062 boost::thread spin_thread(&spinThread);
00063
00064 move_arm.waitForServer();
00065 ROS_INFO("Connected to server");
00066 arm_navigation_msgs::MoveArmGoal goalA;
00067
00068 goalA.motion_plan_request.group_name = "right_arm";
00069 goalA.motion_plan_request.num_planning_attempts = 1;
00070 private_handle.param<std::string>("planner_id",goalA.motion_plan_request.planner_id,std::string("chomp_planner_longrange"));
00071 private_handle.param<std::string>("planner_service_name",goalA.planner_service_name,std::string("/chomp_planner_longrange/plan_path"));
00072
00073 goalA.motion_plan_request.allowed_planning_time = ros::Duration(10.0);
00074
00075 goalA.motion_plan_request.goal_constraints.set_position_constraints_size(1);
00076 goalA.motion_plan_request.goal_constraints.position_constraints[0].header.stamp = ros::Time::now();
00077 goalA.motion_plan_request.goal_constraints.position_constraints[0].header.frame_id = "torso_lift_link";
00078
00079 goalA.motion_plan_request.goal_constraints.position_constraints[0].link_name = "r_wrist_roll_link";
00080 goalA.motion_plan_request.goal_constraints.position_constraints[0].position.x = 0.15;
00081 goalA.motion_plan_request.goal_constraints.position_constraints[0].position.y = -0.95;
00082 goalA.motion_plan_request.goal_constraints.position_constraints[0].position.z = 0;
00083
00084 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.type = arm_navigation_msgs::Shape::BOX;
00085 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
00086 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
00087 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
00088
00089 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.vertices.resize(1);
00090 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.vertices[0].x = 0.15;
00091 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.vertices[0].y = -0.95;
00092 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.vertices[0].z = 0.0;
00093
00094 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_orientation.x = 0.0;
00095 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_orientation.y = 0.0;
00096 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_orientation.z = 0.0;
00097 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_orientation.w = 1.0;
00098
00099 goalA.motion_plan_request.goal_constraints.position_constraints[0].weight = 1.0;
00100
00101 goalA.motion_plan_request.goal_constraints.set_orientation_constraints_size(1);
00102 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].header.stamp = ros::Time::now();
00103 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].header.frame_id = "torso_lift_link";
00104 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].link_name = "r_wrist_roll_link";
00105 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.x = 0.0;
00106 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.y = 0.0;
00107 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.z = -0.7071;
00108 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.w = 0.7071;
00109
00110 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_roll_tolerance = 0.2;
00111 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_pitch_tolerance = 0.2;
00112 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_yaw_tolerance = 0.2;
00113
00114 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].weight = 1.0;
00115
00116
00117 goalA.disable_ik = true;
00118
00119 std::vector<std::string> names(7);
00120 names[0] = "r_shoulder_pan_joint";
00121 names[1] = "r_shoulder_lift_joint";
00122 names[2] = "r_upper_arm_roll_joint";
00123 names[3] = "r_elbow_flex_joint";
00124 names[4] = "r_forearm_roll_joint";
00125 names[5] = "r_wrist_flex_joint";
00126 names[6] = "r_wrist_roll_joint";
00127
00128
00129 int num_test_attempts = 0;
00130 int max_attempts = 5;
00131 bool success = false;
00132
00133
00134 while (nh.ok())
00135 {
00136 bool finished_within_time = false;
00137 move_arm.sendGoal(goalA);
00138 finished_within_time = move_arm.waitForResult(ros::Duration(200.0));
00139 actionlib::SimpleClientGoalState state = move_arm.getState();
00140 success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
00141 if ((!finished_within_time || !success) && num_test_attempts < max_attempts)
00142 {
00143 move_arm.cancelAllGoals();
00144 ROS_INFO("Timed out achieving goal A, trying again. Trying again, attempt: %d",num_test_attempts);
00145 num_test_attempts++;
00146 }
00147 else
00148 {
00149 if(!success)
00150 {
00151 ROS_INFO("Action unsuccessful");
00152 move_arm.cancelAllGoals();
00153 }
00154 ROS_INFO("Action finished: %s",state.toString().c_str());
00155 break;
00156 }
00157 }
00158 EXPECT_TRUE(success);
00159 ros::shutdown();
00160 spin_thread.join();
00161 }
00162
00163 int main(int argc, char **argv){
00164 testing::InitGoogleTest(&argc, argv);
00165 ros::init (argc, argv, "move_arm_regression_test");
00166
00167 return RUN_ALL_TESTS();
00168 }
00169