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 <ros/ros.h>
00047 #include <gtest/gtest.h>
00048
00049 void spinThread()
00050 {
00051 ros::spin();
00052 }
00053
00054 TEST(MoveArm, goToJointGoal)
00055 {
00056 ros::NodeHandle nh;
00057 ros::NodeHandle private_handle("~");
00058 actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm(nh, "move_right_arm");
00059 boost::thread spin_thread(&spinThread);
00060
00061 move_arm.waitForServer();
00062 ROS_INFO("Connected to server");
00063
00064 arm_navigation_msgs::MoveArmGoal goalB;
00065 std::vector<std::string> names(7);
00066 names[0] = "r_shoulder_pan_joint";
00067 names[1] = "r_shoulder_lift_joint";
00068 names[2] = "r_upper_arm_roll_joint";
00069 names[3] = "r_elbow_flex_joint";
00070 names[4] = "r_forearm_roll_joint";
00071 names[5] = "r_wrist_flex_joint";
00072 names[6] = "r_wrist_roll_joint";
00073
00074 goalB.motion_plan_request.group_name = "right_arm";
00075 private_handle.param<std::string>("planner_id",goalB.motion_plan_request.planner_id,std::string("chomp_planner_longrange"));
00076 private_handle.param<std::string>("planner_service_name",goalB.planner_service_name,std::string("/chomp_planner_longrange/plan_path"));
00077
00078 goalB.motion_plan_request.goal_constraints.joint_constraints.resize(names.size());
00079 for (unsigned int i = 0 ; i < goalB.motion_plan_request.goal_constraints.joint_constraints.size(); ++i)
00080 {
00081
00082
00083 goalB.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = names[i];
00084 goalB.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0;
00085 goalB.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.1;
00086 goalB.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.1;
00087 }
00088
00089 goalB.motion_plan_request.goal_constraints.joint_constraints[0].position = -2.0;
00090 goalB.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2;
00091 goalB.motion_plan_request.goal_constraints.joint_constraints[5].position = 0.15;
00092
00093 if (nh.ok())
00094 {
00095 bool finished_within_time = false;
00096 move_arm.sendGoal(goalB);
00097 finished_within_time = move_arm.waitForResult(ros::Duration(10.0));
00098 actionlib::SimpleClientGoalState state = move_arm.getState();
00099 bool success = (state == actionlib::SimpleClientGoalState::ABORTED);
00100 EXPECT_TRUE(success);
00101 ROS_INFO("Action finished: %s",state.toString().c_str());
00102 }
00103 ros::shutdown();
00104 spin_thread.join();
00105 }
00106
00107 int main(int argc, char **argv){
00108 testing::InitGoogleTest(&argc, argv);
00109 ros::init (argc, argv, "move_arm_regression_test");
00110 return RUN_ALL_TESTS();
00111 }