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