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