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 #include <arm_control_msgs/TrajectoryStart.h>
00043
00044 #include <stdio.h>
00045 #include <stdlib.h>
00046 #include <time.h>
00047 #include <boost/thread.hpp>
00048 #include <ros/ros.h>
00049 #include <gtest/gtest.h>
00050
00051 #include <arm_navigation_tests/arm_navigation_utils.h>
00052
00053 typedef actionlib::SimpleActionClient<move_arm_msgs::MoveArmAction> MoveArmClient;
00054
00055 void spinThread()
00056 {
00057 ros::spin();
00058 }
00059
00060 bool sendTuckArm()
00061 {
00062 arm_control_msgs::TrajectoryStart::Request req;
00063 arm_control_msgs::TrajectoryStart::Response res;
00064
00065 ros::NodeHandle nh;
00066 ros::ServiceClient trajectory_start_client = nh.serviceClient<arm_control_msgs::TrajectoryStart>("/r_arm_joint_waypoint_controller/TrajectoryStart");
00067
00068 std::vector<std::string> names(7);
00069 names[0] = "r_shoulder_pan_joint";
00070 names[1] = "r_shoulder_lift_joint";
00071 names[2] = "r_upper_arm_roll_joint";
00072 names[3] = "r_elbow_flex_joint";
00073 names[4] = "r_forearm_roll_joint";
00074 names[5] = "r_wrist_flex_joint";
00075 names[6] = "r_wrist_roll_joint";
00076
00077 req.trajectory.header.frame_id = "base_link";
00078 req.trajectory.header.stamp = ros::Time();
00079 req.trajectory.joint_names = names;
00080 req.trajectory.points.resize(2);
00081 req.request_timing = 0;
00082 req.trajectory.points[0].positions.resize(names.size());
00083 req.trajectory.points[0].positions[0] = -0.4;
00084 req.trajectory.points[0].positions[1] = 0.0;
00085 req.trajectory.points[0].positions[2] = 0.0;
00086 req.trajectory.points[0].positions[3] = -2.25;
00087 req.trajectory.points[0].positions[4] = 0;
00088 req.trajectory.points[0].positions[5] = 0;
00089 req.trajectory.points[0].positions[6] = 0;
00090 req.trajectory.points[1].positions.resize(names.size());
00091 req.trajectory.points[1].positions[0] = -0.01;
00092 req.trajectory.points[1].positions[1] = 0.8;
00093 req.trajectory.points[1].positions[2] = -1.2;
00094 req.trajectory.points[1].positions[3] = -1.4;
00095 req.trajectory.points[1].positions[4] = 1.35;
00096 req.trajectory.points[1].positions[5] = -0.18;
00097 req.trajectory.points[1].positions[6] = 0.31;
00098
00099 if (trajectory_start_client.call(req,res))
00100 {
00101 if (res.trajectory_id < 0)
00102 {
00103 ROS_ERROR("Invalid trajectory id: %d", res.trajectory_id);
00104 return false;
00105 }
00106 ROS_INFO("Sent trajectory %d to controller", res.trajectory_id);
00107 return true;
00108 }
00109 else
00110 {
00111 ROS_ERROR("Unable to start trajectory controller");
00112 return false;
00113 }
00114 }
00115
00116
00117 TEST(MoveArm, goToPoseGoal)
00118 {
00119 ros::NodeHandle nh;
00120 ros::NodeHandle private_handle("~");
00121 actionlib::SimpleActionClient<move_arm_msgs::MoveArmAction> move_arm(nh, "move_right_arm");
00122 boost::thread spin_thread(&spinThread);
00123
00124 move_arm.waitForServer();
00125 ROS_INFO("Connected to server");
00126 move_arm_msgs::MoveArmGoal goalA;
00127
00128 arm_navigation_utils::ArmNavigationUtils arm_nav_utils;
00129 EXPECT_TRUE(arm_nav_utils.takeStaticMap());
00130
00131 goalA.group_name = "right_arm";
00132 goalA.num_planning_attempts = 1;
00133 private_handle.param<std::string>("planner_id",goalA.planner_id,std::string("chomp_planner_longrange"));
00134 private_handle.param<std::string>("planner_service_name",goalA.planner_service_name,std::string("/chomp_planner_longrange/plan_path"));
00135
00136 goalA.allowed_planning_time = 10.0;
00137
00138 goalA.goal_constraints.position_constraints.resize(1);
00139 goalA.goal_constraints.position_constraints[0].header.stamp = ros::Time::now();
00140 goalA.goal_constraints.position_constraints[0].header.frame_id = "torso_lift_link";
00141
00142 goalA.goal_constraints.position_constraints[0].link_name = "r_wrist_roll_link";
00143 goalA.goal_constraints.position_constraints[0].position.x = 0.15;
00144 goalA.goal_constraints.position_constraints[0].position.y = -0.95;
00145 goalA.goal_constraints.position_constraints[0].position.z = 0;
00146
00147 goalA.goal_constraints.position_constraints[0].constraint_region_shape.type = geometry_primitives::Object::BOX;
00148 goalA.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
00149 goalA.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
00150 goalA.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
00151
00152 goalA.goal_constraints.position_constraints[0].constraint_region_pose.position.x = 0.15;
00153 goalA.goal_constraints.position_constraints[0].constraint_region_pose.position.y = -0.95;
00154 goalA.goal_constraints.position_constraints[0].constraint_region_pose.position.z = 0.0;
00155
00156 goalA.goal_constraints.position_constraints[0].constraint_region_pose.orientation.x = 0.0;
00157 goalA.goal_constraints.position_constraints[0].constraint_region_pose.orientation.y = 0.0;
00158 goalA.goal_constraints.position_constraints[0].constraint_region_pose.orientation.z = 0.0;
00159 goalA.goal_constraints.position_constraints[0].constraint_region_pose.orientation.w = 1.0;
00160
00161 goalA.goal_constraints.position_constraints[0].weight = 1.0;
00162
00163 goalA.goal_constraints.orientation_constraints.resize(1);
00164 goalA.goal_constraints.orientation_constraints[0].header.stamp = ros::Time::now();
00165 goalA.goal_constraints.orientation_constraints[0].header.frame_id = "torso_lift_link";
00166 goalA.goal_constraints.orientation_constraints[0].link_name = "r_wrist_roll_link";
00167 goalA.goal_constraints.orientation_constraints[0].orientation.x = 0.0;
00168 goalA.goal_constraints.orientation_constraints[0].orientation.y = 0.0;
00169 goalA.goal_constraints.orientation_constraints[0].orientation.z = -0.7071;
00170 goalA.goal_constraints.orientation_constraints[0].orientation.w = 0.7071;
00171
00172 goalA.goal_constraints.orientation_constraints[0].constraint_region_shape.type = geometry_primitives::Object::BOX;
00173 goalA.goal_constraints.orientation_constraints[0].constraint_region_shape.dimensions.push_back(0.2);
00174 goalA.goal_constraints.orientation_constraints[0].constraint_region_shape.dimensions.push_back(0.2);
00175 goalA.goal_constraints.orientation_constraints[0].constraint_region_shape.dimensions.push_back(0.2);
00176
00177
00178 goalA.goal_constraints.orientation_constraints[0].constraint_region_pose.position.x = 0.0;
00179 goalA.goal_constraints.orientation_constraints[0].constraint_region_pose.position.y = 0.0;
00180 goalA.goal_constraints.orientation_constraints[0].constraint_region_pose.position.z = 0.0;
00181
00182 goalA.goal_constraints.orientation_constraints[0].constraint_region_pose.orientation.x = 0.0;
00183 goalA.goal_constraints.orientation_constraints[0].constraint_region_pose.orientation.y = 0.0;
00184 goalA.goal_constraints.orientation_constraints[0].constraint_region_pose.orientation.z = -0.7071;
00185 goalA.goal_constraints.orientation_constraints[0].constraint_region_pose.orientation.w = 0.7071;
00186
00187 goalA.goal_constraints.orientation_constraints[0].weight = 1.0;
00188
00189 std::vector<std::string> names(7);
00190 names[0] = "r_shoulder_pan_joint";
00191 names[1] = "r_shoulder_lift_joint";
00192 names[2] = "r_upper_arm_roll_joint";
00193 names[3] = "r_elbow_flex_joint";
00194 names[4] = "r_forearm_roll_joint";
00195 names[5] = "r_wrist_flex_joint";
00196 names[6] = "r_wrist_roll_joint";
00197
00198
00199 int num_test_attempts = 0;
00200 int max_attempts = 5;
00201 bool success = false;
00202
00203
00204 while (nh.ok())
00205 {
00206 bool finished_within_time = false;
00207 sleep(5);
00208 move_arm.sendGoal(goalA);
00209 finished_within_time = move_arm.waitForResult(ros::Duration(100.0));
00210 actionlib::SimpleClientGoalState state = move_arm.getState();
00211 success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
00212 if (!finished_within_time && num_test_attempts < max_attempts)
00213 {
00214 move_arm.cancelAllGoals();
00215 ROS_INFO("Timed out achieving goal A, trying again. Trying again, attempt: %d",num_test_attempts);
00216 num_test_attempts++;
00217 }
00218 else
00219 {
00220 if(!success)
00221 {
00222 ROS_INFO("Action unsuccessful");
00223 move_arm.cancelAllGoals();
00224 }
00225 ROS_INFO("Action finished: %s",state.toString().c_str());
00226 break;
00227 }
00228 }
00229 EXPECT_TRUE(success);
00230 ros::shutdown();
00231 spin_thread.join();
00232 }
00233
00234 int main(int argc, char **argv){
00235 testing::InitGoogleTest(&argc, argv);
00236 ros::init (argc, argv, "move_arm_regression_test");
00237
00238 ros::service::waitForService("/r_arm_joint_waypoint_controller/TrajectoryStart");
00239 if(!sendTuckArm())
00240 printf("TUCKARM ain't happening! why?\n");
00241
00242 return RUN_ALL_TESTS();
00243 }
00244