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 <geometric_shapes_msgs/Shape.h>
00051
00052 #include <arm_navigation_tests/arm_navigation_utils.h>
00053
00054
00055 typedef actionlib::SimpleActionClient<move_arm_msgs::MoveArmAction> MoveArmClient;
00056
00057 void spinThread()
00058 {
00059 ros::spin();
00060 }
00061
00062 TEST(MoveArm, goToPoseGoal)
00063 {
00064 ros::NodeHandle nh;
00065 ros::NodeHandle private_handle("~");
00066 actionlib::SimpleActionClient<move_arm_msgs::MoveArmAction> move_right_arm(nh, "move_right_arm");
00067 actionlib::SimpleActionClient<move_arm_msgs::MoveArmAction> move_left_arm(nh, "move_left_arm");
00068 boost::thread spin_thread(&spinThread);
00069
00070 move_right_arm.waitForServer();
00071 move_left_arm.waitForServer();
00072 ROS_INFO("Connected to servers");
00073
00074 arm_navigation_utils::ArmNavigationUtils arm_nav_utils;
00075 EXPECT_TRUE(arm_nav_utils.takeStaticMap());
00076
00077 move_arm_msgs::MoveArmGoal goalLeft, goalRight;
00078 goalRight.motion_plan_request.group_name = "right_arm";
00079 goalRight.motion_plan_request.num_planning_attempts = 1;
00080 private_handle.param<std::string>("planner_id",goalRight.motion_plan_request.planner_id,std::string("chomp_planner_longrange"));
00081 private_handle.param<std::string>("planner_service_name",goalRight.planner_service_name,std::string("/chomp_planner_longrange/plan_path"));
00082
00083 goalRight.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
00084
00085 goalRight.motion_plan_request.goal_constraints.position_constraints.resize(1);
00086 goalRight.motion_plan_request.goal_constraints.position_constraints[0].header.stamp = ros::Time::now();
00087 goalRight.motion_plan_request.goal_constraints.position_constraints[0].header.frame_id = "torso_lift_link";
00088
00089 goalRight.motion_plan_request.goal_constraints.position_constraints[0].link_name = "r_wrist_roll_link";
00090 goalRight.motion_plan_request.goal_constraints.position_constraints[0].position.x = 0.75;
00091 goalRight.motion_plan_request.goal_constraints.position_constraints[0].position.y = -0.188;
00092 goalRight.motion_plan_request.goal_constraints.position_constraints[0].position.z = 0;
00093
00094 goalRight.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.type = geometric_shapes_msgs::Shape::BOX;
00095 goalRight.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
00096 goalRight.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
00097 goalRight.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
00098
00099 goalRight.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_orientation.w = 1.0;
00100 goalRight.motion_plan_request.goal_constraints.position_constraints[0].weight = 1.0;
00101
00102 goalRight.motion_plan_request.goal_constraints.orientation_constraints.resize(1);
00103 goalRight.motion_plan_request.goal_constraints.orientation_constraints[0].header.stamp = ros::Time::now();
00104 goalRight.motion_plan_request.goal_constraints.orientation_constraints[0].header.frame_id = "torso_lift_link";
00105 goalRight.motion_plan_request.goal_constraints.orientation_constraints[0].link_name = "r_wrist_roll_link";
00106 goalRight.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.x = 0.0;
00107 goalRight.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.y = 0.0;
00108 goalRight.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.z = 0.0;
00109 goalRight.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.w = 1.0;
00110
00111 goalRight.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_roll_tolerance = 0.04;
00112 goalRight.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_pitch_tolerance = 0.04;
00113 goalRight.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_yaw_tolerance = 0.04;
00114
00115 goalRight.motion_plan_request.goal_constraints.orientation_constraints[0].weight = 1.0;
00116
00117
00118
00119
00120 goalLeft.motion_plan_request.group_name = "left_arm";
00121 goalLeft.motion_plan_request.num_planning_attempts = 1;
00122 private_handle.param<std::string>("planner_id",goalLeft.motion_plan_request.planner_id,std::string("chomp_planner_longrange"));
00123 private_handle.param<std::string>("planner_service_name",goalLeft.planner_service_name,std::string("/chomp_planner_longrange/plan_path"));
00124
00125 goalLeft.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
00126
00127 goalLeft.motion_plan_request.goal_constraints.position_constraints.resize(1);
00128 goalLeft.motion_plan_request.goal_constraints.position_constraints[0].header.stamp = ros::Time::now();
00129 goalLeft.motion_plan_request.goal_constraints.position_constraints[0].header.frame_id = "torso_lift_link";
00130
00131 goalLeft.motion_plan_request.goal_constraints.position_constraints[0].link_name = "l_wrist_roll_link";
00132 goalLeft.motion_plan_request.goal_constraints.position_constraints[0].position.x = 0.75;
00133 goalLeft.motion_plan_request.goal_constraints.position_constraints[0].position.y = 0.188;
00134 goalLeft.motion_plan_request.goal_constraints.position_constraints[0].position.z = 0;
00135
00136 goalLeft.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.type = geometric_shapes_msgs::Shape::BOX;
00137 goalLeft.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
00138 goalLeft.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
00139 goalLeft.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
00140
00141 goalLeft.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_orientation.w = 1.0;
00142 goalLeft.motion_plan_request.goal_constraints.position_constraints[0].weight = 1.0;
00143
00144 goalLeft.motion_plan_request.goal_constraints.orientation_constraints.resize(1);
00145 goalLeft.motion_plan_request.goal_constraints.orientation_constraints[0].header.stamp = ros::Time::now();
00146 goalLeft.motion_plan_request.goal_constraints.orientation_constraints[0].header.frame_id = "torso_lift_link";
00147 goalLeft.motion_plan_request.goal_constraints.orientation_constraints[0].link_name = "l_wrist_roll_link";
00148 goalLeft.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.x = 0.0;
00149 goalLeft.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.y = 0.0;
00150 goalLeft.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.z = 0.0;
00151 goalLeft.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.w = 1.0;
00152
00153 goalLeft.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_roll_tolerance = 0.04;
00154 goalLeft.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_pitch_tolerance = 0.04;
00155 goalLeft.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_yaw_tolerance = 0.04;
00156
00157 goalLeft.motion_plan_request.goal_constraints.orientation_constraints[0].weight = 1.0;
00158
00159
00160
00161
00162
00163
00164
00165
00166
00167
00168
00169
00170
00171 int num_test_attempts = 0;
00172 int max_attempts = 5;
00173 bool success = false;
00174 while (nh.ok())
00175 {
00176 bool right_finished_within_time = false;
00177 bool left_finished_within_time = false;
00178 move_right_arm.sendGoal(goalRight);
00179 move_left_arm.sendGoal(goalLeft);
00180
00181 right_finished_within_time = move_right_arm.waitForResult(ros::Duration(200.0));
00182 left_finished_within_time = move_left_arm.waitForResult(ros::Duration(10.0));
00183
00184 actionlib::SimpleClientGoalState right_state = move_right_arm.getState();
00185 actionlib::SimpleClientGoalState left_state = move_left_arm.getState();
00186 success = (right_state == actionlib::SimpleClientGoalState::SUCCEEDED && left_state == actionlib::SimpleClientGoalState::SUCCEEDED);
00187 if(!right_finished_within_time || right_state != actionlib::SimpleClientGoalState::SUCCEEDED) {
00188 move_right_arm.cancelGoal();
00189 ROS_INFO("Right arm timed out achieving goal");
00190 }
00191 if(!left_finished_within_time || left_state != actionlib::SimpleClientGoalState::SUCCEEDED) {
00192 move_left_arm.cancelGoal();
00193 ROS_INFO("Left arm timed out achieving goal");
00194 }
00195 if(!success) {
00196 num_test_attempts++;
00197 if(num_test_attempts > max_attempts) {
00198 break;
00199 }
00200 } else {
00201 break;
00202 }
00203 }
00204 EXPECT_TRUE(success);
00205 ros::shutdown();
00206 spin_thread.join();
00207 }
00208
00209 int main(int argc, char **argv){
00210 testing::InitGoogleTest(&argc, argv);
00211 ros::init (argc, argv, "move_arm_regression_test");
00212 return RUN_ALL_TESTS();
00213 }