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 <move_arm_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 #include <arm_navigation_tests/arm_navigation_utils.h>
00051
00052 typedef actionlib::SimpleActionClient<move_arm_msgs::MoveArmAction> MoveArmClient;
00053
00054 void spinThread()
00055 {
00056 ros::spin();
00057 }
00058
00059 TEST(MoveArm, goToPoseGoal)
00060 {
00061 ros::NodeHandle nh;
00062 ros::NodeHandle private_handle("~");
00063 actionlib::SimpleActionClient<move_arm_msgs::MoveArmAction> move_arm(nh, "move_right_arm");
00064 boost::thread spin_thread(&spinThread);
00065
00066 move_arm.waitForServer();
00067
00068 arm_navigation_utils::ArmNavigationUtils arm_nav_utils;
00069 EXPECT_TRUE(arm_nav_utils.takeStaticMap());
00070
00071 ROS_INFO("Connected to server");
00072 move_arm_msgs::MoveArmGoal goalA;
00073
00074 goalA.motion_plan_request.group_name = "right_arm";
00075 goalA.motion_plan_request.num_planning_attempts = 1;
00076 private_handle.param<std::string>("planner_id",goalA.motion_plan_request.planner_id,std::string("chomp_planner_longrange"));
00077 private_handle.param<std::string>("planner_service_name",goalA.planner_service_name,std::string("/chomp_planner_longrange/plan_path"));
00078
00079 goalA.motion_plan_request.allowed_planning_time = ros::Duration(10.0);
00080
00081 goalA.motion_plan_request.goal_constraints.position_constraints.resize(1);
00082 goalA.motion_plan_request.goal_constraints.position_constraints[0].header.stamp = ros::Time::now();
00083 goalA.motion_plan_request.goal_constraints.position_constraints[0].header.frame_id = "torso_lift_link";
00084
00085 goalA.motion_plan_request.goal_constraints.position_constraints[0].link_name = "r_wrist_roll_link";
00086 goalA.motion_plan_request.goal_constraints.position_constraints[0].position.x = 0.15;
00087 goalA.motion_plan_request.goal_constraints.position_constraints[0].position.y = -0.95;
00088 goalA.motion_plan_request.goal_constraints.position_constraints[0].position.z = 0;
00089
00090 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.type = geometric_shapes_msgs::Shape::BOX;
00091 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
00092 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
00093 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
00094
00095 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.vertices.resize(1);
00096 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.vertices[0].x = 0.15;
00097 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.vertices[0].y = -0.95;
00098 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.vertices[0].z = 0.0;
00099
00100 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_orientation.x = 0.0;
00101 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_orientation.y = 0.0;
00102 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_orientation.z = 0.0;
00103 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_orientation.w = 1.0;
00104
00105 goalA.motion_plan_request.goal_constraints.position_constraints[0].weight = 1.0;
00106
00107 goalA.motion_plan_request.goal_constraints.orientation_constraints.resize(1);
00108 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].header.stamp = ros::Time::now();
00109 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].header.frame_id = "torso_lift_link";
00110 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].link_name = "r_wrist_roll_link";
00111 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.x = 0.0;
00112 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.y = 0.0;
00113 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.z = -0.7071;
00114 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.w = 0.7071;
00115
00116 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_roll_tolerance = 0.2;
00117 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_pitch_tolerance = 0.2;
00118 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_yaw_tolerance = 0.2;
00119
00120 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].weight = 1.0;
00121
00122
00123 goalA.disable_ik = true;
00124
00125 std::vector<std::string> names(7);
00126 names[0] = "r_shoulder_pan_joint";
00127 names[1] = "r_shoulder_lift_joint";
00128 names[2] = "r_upper_arm_roll_joint";
00129 names[3] = "r_elbow_flex_joint";
00130 names[4] = "r_forearm_roll_joint";
00131 names[5] = "r_wrist_flex_joint";
00132 names[6] = "r_wrist_roll_joint";
00133
00134
00135 int num_test_attempts = 0;
00136 int max_attempts = 5;
00137 bool success = false;
00138
00139
00140 while (nh.ok())
00141 {
00142 bool finished_within_time = false;
00143 move_arm.sendGoal(goalA);
00144 finished_within_time = move_arm.waitForResult(ros::Duration(200.0));
00145 actionlib::SimpleClientGoalState state = move_arm.getState();
00146 success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
00147 if ((!finished_within_time || !success) && num_test_attempts < max_attempts)
00148 {
00149 move_arm.cancelAllGoals();
00150 ROS_INFO("Timed out achieving goal A, trying again. Trying again, attempt: %d",num_test_attempts);
00151 num_test_attempts++;
00152 }
00153 else
00154 {
00155 if(!success)
00156 {
00157 ROS_INFO("Action unsuccessful");
00158 move_arm.cancelAllGoals();
00159 }
00160 ROS_INFO("Action finished: %s",state.toString().c_str());
00161 break;
00162 }
00163 }
00164 EXPECT_TRUE(success);
00165 ros::shutdown();
00166 spin_thread.join();
00167 }
00168
00169 int main(int argc, char **argv){
00170 testing::InitGoogleTest(&argc, argv);
00171 ros::init (argc, argv, "move_arm_regression_test");
00172
00173 return RUN_ALL_TESTS();
00174 }
00175