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