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 <boost/thread.hpp>
00047 #include <ros/ros.h>
00048 #include <gtest/gtest.h>
00049 #include <arm_navigation_msgs/CollisionObject.h>
00050 #include <arm_navigation_msgs/Shape.h>
00051
00052 typedef actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> MoveArmClient;
00053
00054 unsigned int REPS_TO_TRY = 3;
00055
00056
00057 void spinThread()
00058 {
00059 ros::spin();
00060 }
00061
00062 TEST(MoveArm, goToPoseGoal)
00063 {
00064
00065 ros::NodeHandle nh;
00066 ros::NodeHandle private_handle("~");
00067
00068 ros::Publisher object_in_map_pub_;
00069 object_in_map_pub_ = nh.advertise<arm_navigation_msgs::CollisionObject>("collision_object", 10);
00070
00071 actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm(nh, "move_right_arm");
00072 boost::thread spin_thread(&spinThread);
00073
00074 move_arm.waitForServer();
00075 ROS_INFO("Connected to server");
00076
00077
00078 arm_navigation_msgs::CollisionObject table_object;
00079 table_object.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD;
00080 table_object.header.frame_id = "base_link";
00081 table_object.header.stamp = ros::Time::now();
00082 arm_navigation_msgs::Shape object;
00083 object.type = arm_navigation_msgs::Shape::BOX;
00084 object.dimensions.resize(3);
00085 object.dimensions[0] = 1.0;
00086 object.dimensions[1] = 1.0;
00087 object.dimensions[2] = 0.05;
00088 geometry_msgs::Pose pose;
00089 pose.position.x = 1.0;
00090 pose.position.y = 0;
00091 pose.position.z = .5;
00092 pose.orientation.x = 0;
00093 pose.orientation.y = 0;
00094 pose.orientation.z = 0;
00095 pose.orientation.w = 1;
00096 table_object.shapes.push_back(object);
00097 table_object.poses.push_back(pose);
00098
00099 table_object.id = "table";
00100 object_in_map_pub_.publish(table_object);
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127
00128 std::vector<std::string> names(7);
00129 names[0] = "r_shoulder_pan_joint";
00130 names[1] = "r_shoulder_lift_joint";
00131 names[2] = "r_upper_arm_roll_joint";
00132 names[3] = "r_elbow_flex_joint";
00133 names[4] = "r_forearm_roll_joint";
00134 names[5] = "r_wrist_flex_joint";
00135 names[6] = "r_wrist_roll_joint";
00136
00137 arm_navigation_msgs::MoveArmGoal goalA, goalB;
00138 goalB.motion_plan_request.group_name = "right_arm";
00139 goalB.motion_plan_request.num_planning_attempts = 1;
00140 goalB.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
00141
00142 private_handle.param<std::string>("planner_id",goalB.motion_plan_request.planner_id,std::string(""));
00143 private_handle.param<std::string>("planner_service_name",goalB.planner_service_name,std::string("/ompl_planning/plan_kinematic_path"));
00144
00145 goalB.motion_plan_request.goal_constraints.joint_constraints.resize(names.size());
00146 for (unsigned int i = 0 ; i < goalB.motion_plan_request.goal_constraints.joint_constraints.size(); ++i)
00147 {
00148
00149
00150 goalB.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = names[i];
00151 goalB.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0;
00152 goalB.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.1;
00153 goalB.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.1;
00154 }
00155
00156 goalB.motion_plan_request.goal_constraints.joint_constraints[0].position = -1.5;
00157 goalB.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2;
00158 goalB.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.20;
00159
00160 goalA.motion_plan_request.planner_id = "";
00161 goalA.planner_service_name ="ompl_planning/plan_kinematic_path";
00162
00163 goalA.motion_plan_request.group_name = "right_arm";
00164 goalA.motion_plan_request.num_planning_attempts = 1;
00165 goalA.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
00166 goalA.motion_plan_request.goal_constraints.set_position_constraints_size(1);
00167 goalA.motion_plan_request.goal_constraints.position_constraints[0].header.stamp = ros::Time::now();
00168 goalA.motion_plan_request.goal_constraints.position_constraints[0].header.frame_id = "base_link";
00169
00170 goalA.motion_plan_request.goal_constraints.position_constraints[0].link_name = "r_wrist_roll_link";
00171 goalA.motion_plan_request.goal_constraints.position_constraints[0].position.x = 0.6;
00172 goalA.motion_plan_request.goal_constraints.position_constraints[0].position.y = 0;
00173 goalA.motion_plan_request.goal_constraints.position_constraints[0].position.z = .35;
00174
00175 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.type = arm_navigation_msgs::Shape::BOX;
00176 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
00177 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
00178 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
00179
00180 goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_orientation.w = 1.0;
00181
00182 goalA.motion_plan_request.goal_constraints.position_constraints[0].weight = 1.0;
00183
00184 goalA.motion_plan_request.goal_constraints.set_orientation_constraints_size(1);
00185 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].header.stamp = ros::Time::now();
00186 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].header.frame_id = "base_link";
00187 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].link_name = "r_wrist_roll_link";
00188 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.x = 0.0;
00189 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.y = 0.0;
00190 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.z = 0.0;
00191 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.w = 1.0;
00192
00193 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_roll_tolerance = 0.04;
00194 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_pitch_tolerance = 0.04;
00195 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_yaw_tolerance = 0.04;
00196
00197 goalA.motion_plan_request.goal_constraints.orientation_constraints[0].weight = 1.0;
00198
00199
00200 goalA.disable_ik = true;
00201
00202 for(unsigned int i = 0; i < REPS_TO_TRY; i++) {
00203
00204 if (nh.ok())
00205 {
00206 bool finished_within_time = false;
00207 move_arm.sendGoal(goalA);
00208 finished_within_time = move_arm.waitForResult(ros::Duration(200.0));
00209 EXPECT_TRUE(finished_within_time);
00210 if (!finished_within_time)
00211 {
00212 move_arm.cancelGoal();
00213 ROS_INFO("Timed out achieving goal A");
00214 }
00215 else
00216 {
00217 actionlib::SimpleClientGoalState state = move_arm.getState();
00218 bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
00219 EXPECT_TRUE(success);
00220 ROS_INFO("Action finished: %s",state.toString().c_str());
00221 EXPECT_TRUE(state == actionlib::SimpleClientGoalState::SUCCEEDED);
00222 }
00223 }
00224
00225 if (nh.ok())
00226 {
00227 bool finished_within_time = false;
00228 move_arm.sendGoal(goalB);
00229 finished_within_time = move_arm.waitForResult(ros::Duration(100.0));
00230 EXPECT_TRUE(finished_within_time);
00231 if (!finished_within_time)
00232 {
00233 move_arm.cancelAllGoals();
00234 ROS_INFO("Timed out achieving goal B");
00235 }
00236 else
00237 {
00238 actionlib::SimpleClientGoalState state = move_arm.getState();
00239 bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
00240 EXPECT_TRUE(success);
00241 ROS_INFO("Action finished: %s",state.toString().c_str());
00242 EXPECT_TRUE(state == actionlib::SimpleClientGoalState::SUCCEEDED);
00243 }
00244 }
00245 }
00246 ros::shutdown();
00247 spin_thread.join();
00248 }
00249
00250 int main(int argc, char **argv){
00251 testing::InitGoogleTest(&argc, argv);
00252 ros::init (argc, argv, "move_arm_regression_test");
00253 return RUN_ALL_TESTS();
00254 }