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