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 <geometry_msgs/Quaternion.h>
00044 #include <tf/tf.h>
00045
00046 #include <stdio.h>
00047 #include <stdlib.h>
00048 #include <time.h>
00049 #include <boost/thread.hpp>
00050 #include <ros/ros.h>
00051 #include <gtest/gtest.h>
00052
00053 typedef actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> MoveArmClient;
00054
00055 void spinThread()
00056 {
00057 ros::spin();
00058 }
00059
00060 TEST(MoveArm, goToPoseGoal)
00061 {
00062 ros::NodeHandle nh;
00063 ros::NodeHandle private_handle("~");
00064 actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm(nh, "move_right_arm");
00065 boost::thread spin_thread(&spinThread);
00066
00067 move_arm.waitForServer();
00068 ROS_INFO("Connected to server");
00069 arm_navigation_msgs::MoveArmGoal goalA;
00070
00071 goalA.motion_plan_request.group_name = "right_arm";
00072 goalA.motion_plan_request.num_planning_attempts = 1;
00073 private_handle.param<std::string>("planner_id",goalA.motion_plan_request.planner_id,std::string("chomp_planner_longrange"));
00074 private_handle.param<std::string>("planner_service_name",goalA.planner_service_name,std::string("/chomp_planner_longrange/plan_path"));
00075
00076 double goal_x, goal_y, goal_z, goal_roll, goal_pitch, goal_yaw;
00077 private_handle.param<double>("goal_x",goal_x,0.75);
00078 private_handle.param<double>("goal_y",goal_y,-0.188);
00079 private_handle.param<double>("goal_z",goal_z,0.0);
00080
00081 private_handle.param<double>("goal_roll",goal_roll,0.0);
00082 private_handle.param<double>("goal_pitch",goal_pitch,0.0);
00083 private_handle.param<double>("goal_yaw",goal_yaw,0.0);
00084
00085 tf::Quaternion gripper_goal;
00086 geometry_msgs::Quaternion gripper_goal_msg;
00087 gripper_goal.setRPY(goal_roll,goal_pitch,goal_yaw);
00088 tf::quaternionTFToMsg(gripper_goal,gripper_goal_msg);
00089
00090 goalA.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
00091
00092 goalA.motion_plan_request.goal_constraints.set_position_constraints_size(1);
00093 goalA.motion_plan_request.goal_constraints.position_constraints[0].header.stamp = ros::Time::now();
00094 goalA.motion_plan_request.goal_constraints.position_constraints[0].header.frame_id = "torso_lift_link";
00095
00096 goalA.motion_plan_request.goal_constraints.position_constraints[0].link_name = "r_wrist_roll_link";
00097 goalA.motion_plan_request.goal_constraints.position_constraints[0].position.x = goal_x;
00098 goalA.motion_plan_request.goal_constraints.position_constraints[0].position.y = goal_y;
00099 goalA.motion_plan_request.goal_constraints.position_constraints[0].position.z = goal_z;
00100
00101 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.type = arm_navigation_msgs::Shape::BOX;
00102 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
00103 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
00104 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
00105
00106 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_orientation.w = 1.0;
00107
00108 goalA.motion_plan_request.goal_constraints.position_constraints[0].weight = 1.0;
00109
00110 goalA.motion_plan_request.goal_constraints.set_orientation_constraints_size(1);
00111 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].header.stamp = ros::Time::now();
00112 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].header.frame_id = "torso_lift_link";
00113 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].link_name = "r_wrist_roll_link";
00114 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.x = gripper_goal_msg.x;
00115 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.y = gripper_goal_msg.y;
00116 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.z = gripper_goal_msg.z;
00117 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.w = gripper_goal_msg.w;
00118
00119 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_roll_tolerance = 0.04;
00120 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_pitch_tolerance = 0.04;
00121 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_yaw_tolerance = 0.04;
00122
00123 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].weight = 1.0;
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137 int num_test_attempts = 0;
00138 int max_attempts = 5;
00139 bool success = false;
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(100.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.cancelGoal();
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 finished %s",state.toString().c_str());
00158 move_arm.cancelGoal();
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 return RUN_ALL_TESTS();
00173 }