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