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 <ros/ros.h>
00047 #include <gtest/gtest.h>
00048
00049 #include <arm_navigation_tests/arm_navigation_utils.h>
00050
00051 void spinThread()
00052 {
00053 ros::spin();
00054 }
00055
00056 TEST(MoveArm, goToJointGoal)
00057 {
00058 ros::NodeHandle nh;
00059 ros::NodeHandle private_handle("~");
00060 actionlib::SimpleActionClient<move_arm_msgs::MoveArmAction> move_right_arm(nh, "move_right_arm");
00061 actionlib::SimpleActionClient<move_arm_msgs::MoveArmAction> move_left_arm(nh, "move_left_arm");
00062 boost::thread spin_thread(&spinThread);
00063
00064 move_right_arm.waitForServer();
00065 move_left_arm.waitForServer();
00066 ROS_INFO("Connected to servers");
00067
00068 arm_navigation_utils::ArmNavigationUtils arm_nav_utils;
00069 EXPECT_TRUE(arm_nav_utils.takeStaticMap());
00070
00071 move_arm_msgs::MoveArmGoal goalRight;
00072 std::vector<std::string> right_names(7);
00073 right_names[0] = "r_shoulder_pan_joint";
00074 right_names[1] = "r_shoulder_lift_joint";
00075 right_names[2] = "r_upper_arm_roll_joint";
00076 right_names[3] = "r_elbow_flex_joint";
00077 right_names[4] = "r_forearm_roll_joint";
00078 right_names[5] = "r_wrist_flex_joint";
00079 right_names[6] = "r_wrist_roll_joint";
00080
00081 move_arm_msgs::MoveArmGoal goalLeft;
00082 std::vector<std::string> left_names(7);
00083 left_names[0] = "l_shoulder_pan_joint";
00084 left_names[1] = "l_shoulder_lift_joint";
00085 left_names[2] = "l_upper_arm_roll_joint";
00086 left_names[3] = "l_elbow_flex_joint";
00087 left_names[4] = "l_forearm_roll_joint";
00088 left_names[5] = "l_wrist_flex_joint";
00089 left_names[6] = "l_wrist_roll_joint";
00090
00091 goalRight.motion_plan_request.group_name = "right_arm";
00092 goalRight.motion_plan_request.num_planning_attempts = 1;
00093 goalRight.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
00094
00095 private_handle.param<std::string>("planner_id",goalRight.motion_plan_request.planner_id,std::string("chomp_planner_longrange"));
00096 private_handle.param<std::string>("planner_service_name",goalRight.planner_service_name,std::string("/chomp_planner_longrange/plan_path"));
00097
00098 goalRight.motion_plan_request.goal_constraints.joint_constraints.resize(right_names.size());
00099 for (unsigned int i = 0 ; i < goalRight.motion_plan_request.goal_constraints.joint_constraints.size(); ++i)
00100 {
00101
00102
00103 goalRight.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = right_names[i];
00104 goalRight.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0;
00105 goalRight.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.1;
00106 goalRight.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.1;
00107 }
00108
00109 goalRight.motion_plan_request.goal_constraints.joint_constraints[0].position = -2.0;
00110 goalRight.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2;
00111 goalRight.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.20;
00112
00113
00114
00115 goalLeft.motion_plan_request.group_name = "left_arm";
00116 goalLeft.motion_plan_request.num_planning_attempts = 1;
00117 goalLeft.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
00118
00119 private_handle.param<std::string>("planner_id",goalLeft.motion_plan_request.planner_id,std::string("chomp_planner_longrange"));
00120 private_handle.param<std::string>("planner_service_name",goalLeft.planner_service_name,std::string("/chomp_planner_longrange/plan_path"));
00121
00122 goalLeft.motion_plan_request.goal_constraints.joint_constraints.resize(left_names.size());
00123 for (unsigned int i = 0 ; i < goalLeft.motion_plan_request.goal_constraints.joint_constraints.size(); ++i)
00124 {
00125
00126
00127 goalLeft.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = left_names[i];
00128 goalLeft.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0;
00129 goalLeft.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.1;
00130 goalLeft.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.1;
00131 }
00132
00133 goalLeft.motion_plan_request.goal_constraints.joint_constraints[0].position = 2.0;
00134 goalLeft.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2;
00135 goalLeft.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.20;
00136
00137 int num_test_attempts = 0;
00138 int max_attempts = 5;
00139 bool success = false;
00140 while (nh.ok())
00141 {
00142 bool right_finished_within_time = false;
00143 bool left_finished_within_time = false;
00144 move_right_arm.sendGoal(goalRight);
00145 move_left_arm.sendGoal(goalLeft);
00146
00147 right_finished_within_time = move_right_arm.waitForResult(ros::Duration(200.0));
00148 left_finished_within_time = move_left_arm.waitForResult(ros::Duration(10.0));
00149
00150 actionlib::SimpleClientGoalState right_state = move_right_arm.getState();
00151 actionlib::SimpleClientGoalState left_state = move_left_arm.getState();
00152 success = (right_state == actionlib::SimpleClientGoalState::SUCCEEDED && left_state == actionlib::SimpleClientGoalState::SUCCEEDED);
00153 if(!right_finished_within_time || right_state != actionlib::SimpleClientGoalState::SUCCEEDED) {
00154 move_right_arm.cancelGoal();
00155 ROS_INFO("Right arm timed out achieving goal");
00156 }
00157 if(!left_finished_within_time || left_state != actionlib::SimpleClientGoalState::SUCCEEDED) {
00158 move_left_arm.cancelGoal();
00159 ROS_INFO("Left arm timed out achieving goal");
00160 }
00161 if(!success) {
00162 num_test_attempts++;
00163 if(num_test_attempts > max_attempts) {
00164 break;
00165 }
00166 } else {
00167 break;
00168 }
00169 }
00170 EXPECT_TRUE(success);
00171 ros::shutdown();
00172 spin_thread.join();
00173 }
00174
00175 int main(int argc, char **argv){
00176 testing::InitGoogleTest(&argc, argv);
00177 ros::init (argc, argv, "move_arm_regression_test");
00178 return RUN_ALL_TESTS();
00179 }