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 #include <pr2_controllers_msgs/PointHeadAction.h>
00043
00044 #include <stdio.h>
00045 #include <stdlib.h>
00046 #include <time.h>
00047 #include <ros/ros.h>
00048 #include <gtest/gtest.h>
00049
00050 void spinThread()
00051 {
00052 ros::spin();
00053 }
00054
00055 TEST(MoveArm, goToJointGoal)
00056 {
00057 ros::NodeHandle nh;
00058 ros::NodeHandle private_handle("~");
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068 actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm(nh, "move_right_arm");
00069 boost::thread spin_thread(&spinThread);
00070
00071 move_arm.waitForServer();
00072 ROS_INFO("Connected to server");
00073
00074 ros::Publisher point_head_pub = nh.advertise<geometry_msgs::PointStamped>("head_controller/point_head", 1);
00075
00076 sleep(5.0);
00077
00078
00079 geometry_msgs::PointStamped point;
00080 point.header.frame_id = "base_link";
00081 point.point.x = .54;
00082 point.point.y = -.56;
00083 point.point.z = .35;
00084 point.header.stamp = ros::Time::now();
00085 point_head_pub.publish(point);
00086
00087 std::cout << "Should have advertised point\n";
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103
00104 sleep(5.0);
00105
00106 arm_navigation_msgs::MoveArmGoal goalB;
00107 std::vector<std::string> names(7);
00108 names[0] = "r_shoulder_pan_joint";
00109 names[1] = "r_shoulder_lift_joint";
00110 names[2] = "r_upper_arm_roll_joint";
00111 names[3] = "r_elbow_flex_joint";
00112 names[4] = "r_forearm_roll_joint";
00113 names[5] = "r_wrist_flex_joint";
00114 names[6] = "r_wrist_roll_joint";
00115
00116 goalB.motion_plan_request.group_name = "right_arm";
00117 goalB.motion_plan_request.num_planning_attempts = 1;
00118 goalB.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
00119
00120 private_handle.param<std::string>("planner_id",goalB.motion_plan_request.planner_id,std::string("chomp_planner_longrange"));
00121 private_handle.param<std::string>("planner_service_name",goalB.planner_service_name,std::string("/chomp_planner_longrange/plan_path"));
00122
00123 goalB.motion_plan_request.goal_constraints.joint_constraints.resize(names.size());
00124 for (unsigned int i = 0 ; i < goalB.motion_plan_request.goal_constraints.joint_constraints.size(); ++i)
00125 {
00126
00127
00128 goalB.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = names[i];
00129 goalB.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0;
00130 goalB.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.1;
00131 goalB.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.1;
00132 }
00133
00134 goalB.motion_plan_request.goal_constraints.joint_constraints[0].position = -2.0;
00135 goalB.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2;
00136 goalB.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.20;
00137
00138 int num_test_attempts = 0;
00139 int max_attempts = 5;
00140 bool success = false;
00141 while (nh.ok())
00142 {
00143 bool finished_within_time = false;
00144 move_arm.sendGoal(goalB);
00145 finished_within_time = move_arm.waitForResult(ros::Duration(200.0));
00146 actionlib::SimpleClientGoalState state = move_arm.getState();
00147 success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
00148 if ((!finished_within_time || !success) && num_test_attempts < max_attempts)
00149 {
00150 move_arm.cancelGoal();
00151 ROS_INFO("Timed out achieving goal A");
00152 num_test_attempts++;
00153 }
00154 else
00155 {
00156 if(!success)
00157 {
00158 ROS_INFO("Action finished: %s",state.toString().c_str());
00159 move_arm.cancelGoal();
00160 }
00161 ROS_INFO("Action finished: %s",state.toString().c_str());
00162 break;
00163 }
00164 }
00165 EXPECT_TRUE(success);
00166 ros::shutdown();
00167 spin_thread.join();
00168 }
00169
00170 int main(int argc, char **argv){
00171 testing::InitGoogleTest(&argc, argv);
00172 ros::init (argc, argv, "move_arm_regression_test");
00173 return RUN_ALL_TESTS();
00174 }