move_arm: move_arm: regression_test_pose_goal

00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  * 
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  All rights reserved.
00006  * 
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  * 
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  * 
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  *
00035  *********************************************************************/
00036 
00037 /* \author: Ioan Sucan, Sachin Chitta */
00038 
00039 #include <ros/ros.h>
00040 #include <actionlib/client/simple_action_client.h>
00041 #include <arm_navigation_msgs/MoveArmAction.h>
00042 #include <arm_control_msgs/TrajectoryStart.h>
00043 
00044 #include <stdio.h>
00045 #include <stdlib.h>
00046 #include <time.h>
00047 #include <boost/thread.hpp>
00048 #include <ros/ros.h>
00049 #include <gtest/gtest.h>
00050 
00051 typedef actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> MoveArmClient;
00052 
00053 void spinThread()
00054 {
00055   ros::spin();
00056 }
00057 
00058 bool sendTuckArm()
00059 {
00060   arm_control_msgs::TrajectoryStart::Request  req;
00061   arm_control_msgs::TrajectoryStart::Response res;
00062 
00063   ros::NodeHandle nh;
00064   ros::ServiceClient trajectory_start_client  = nh.serviceClient<arm_control_msgs::TrajectoryStart>("/r_arm_joint_waypoint_controller/TrajectoryStart");
00065 
00066   std::vector<std::string> names(7);
00067   names[0] = "r_shoulder_pan_joint";
00068   names[1] = "r_shoulder_lift_joint";
00069   names[2] = "r_upper_arm_roll_joint";
00070   names[3] = "r_elbow_flex_joint";
00071   names[4] = "r_forearm_roll_joint";
00072   names[5] = "r_wrist_flex_joint";
00073   names[6] = "r_wrist_roll_joint";
00074 
00075   req.trajectory.header.frame_id = "base_link";
00076   req.trajectory.header.stamp = ros::Time();
00077   req.trajectory.joint_names = names;
00078   req.trajectory.set_points_size(2);
00079   req.request_timing = 0;
00080   req.trajectory.points[0].set_positions_size(names.size());
00081   req.trajectory.points[0].positions[0] = -0.4;
00082   req.trajectory.points[0].positions[1] = 0.0;
00083   req.trajectory.points[0].positions[2] = 0.0;
00084   req.trajectory.points[0].positions[3] = -2.25;
00085   req.trajectory.points[0].positions[4] = 0;
00086   req.trajectory.points[0].positions[5] = 0;
00087   req.trajectory.points[0].positions[6] = 0;
00088   req.trajectory.points[1].set_positions_size(names.size());
00089   req.trajectory.points[1].positions[0] = -0.01;
00090   req.trajectory.points[1].positions[1] = 0.8;
00091   req.trajectory.points[1].positions[2] = -1.2;
00092   req.trajectory.points[1].positions[3] = -1.4;
00093   req.trajectory.points[1].positions[4] = 1.35;
00094   req.trajectory.points[1].positions[5] = -0.18;
00095   req.trajectory.points[1].positions[6] = 0.31;
00096 
00097   if (trajectory_start_client.call(req,res))
00098   {
00099     if (res.trajectory_id < 0)
00100     {
00101       ROS_ERROR("Invalid trajectory id: %d", res.trajectory_id);
00102       return false;
00103     }
00104     ROS_INFO("Sent trajectory %d to controller", res.trajectory_id);
00105     return true;
00106   }
00107   else
00108   {
00109     ROS_ERROR("Unable to start trajectory controller");
00110     return false;
00111   }
00112 }
00113 
00114 
00115 TEST(MoveArm, goToPoseGoal)
00116 {
00117   ros::NodeHandle nh;
00118   ros::NodeHandle private_handle("~");
00119   actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm(nh, "move_right_arm");
00120   boost::thread spin_thread(&spinThread);
00121 
00122   move_arm.waitForServer();
00123   ROS_INFO("Connected to server");
00124   arm_navigation_msgs::MoveArmGoal goalA;
00125 
00126   goalA.group_name = "right_arm";
00127   goalA.num_planning_attempts = 1;
00128   private_handle.param<std::string>("planner_id",goalA.planner_id,std::string("chomp_planner_longrange"));
00129   private_handle.param<std::string>("planner_service_name",goalA.planner_service_name,std::string("/chomp_planner_longrange/plan_path"));
00130 
00131   goalA.allowed_planning_time = 10.0;
00132     
00133   goalA.goal_constraints.set_position_constraints_size(1);
00134   goalA.goal_constraints.position_constraints[0].header.stamp = ros::Time::now();
00135   goalA.goal_constraints.position_constraints[0].header.frame_id = "torso_lift_link";
00136     
00137   goalA.goal_constraints.position_constraints[0].link_name = "r_wrist_roll_link";
00138   goalA.goal_constraints.position_constraints[0].position.x = 0.15;
00139   goalA.goal_constraints.position_constraints[0].position.y = -0.95;
00140   goalA.goal_constraints.position_constraints[0].position.z = 0;
00141     
00142   goalA.goal_constraints.position_constraints[0].constraint_region_shape.type = geometry_primitives::Object::BOX;
00143   goalA.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
00144   goalA.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
00145   goalA.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
00146 
00147   goalA.goal_constraints.position_constraints[0].constraint_region_pose.position.x = 0.15;
00148   goalA.goal_constraints.position_constraints[0].constraint_region_pose.position.y = -0.95;
00149   goalA.goal_constraints.position_constraints[0].constraint_region_pose.position.z = 0.0;
00150 
00151   goalA.goal_constraints.position_constraints[0].constraint_region_pose.orientation.x = 0.0;
00152   goalA.goal_constraints.position_constraints[0].constraint_region_pose.orientation.y = 0.0;
00153   goalA.goal_constraints.position_constraints[0].constraint_region_pose.orientation.z = 0.0;
00154   goalA.goal_constraints.position_constraints[0].constraint_region_pose.orientation.w = 1.0;
00155 
00156   goalA.goal_constraints.position_constraints[0].weight = 1.0;
00157 
00158   goalA.goal_constraints.set_orientation_constraints_size(1);
00159   goalA.goal_constraints.orientation_constraints[0].header.stamp = ros::Time::now();
00160   goalA.goal_constraints.orientation_constraints[0].header.frame_id = "torso_lift_link";    
00161   goalA.goal_constraints.orientation_constraints[0].link_name = "r_wrist_roll_link";
00162   goalA.goal_constraints.orientation_constraints[0].orientation.x = 0.0;
00163   goalA.goal_constraints.orientation_constraints[0].orientation.y = 0.0;
00164   goalA.goal_constraints.orientation_constraints[0].orientation.z = -0.7071;
00165   goalA.goal_constraints.orientation_constraints[0].orientation.w = 0.7071;
00166     
00167   goalA.goal_constraints.orientation_constraints[0].constraint_region_shape.type = geometry_primitives::Object::BOX;
00168   goalA.goal_constraints.orientation_constraints[0].constraint_region_shape.dimensions.push_back(0.2);
00169   goalA.goal_constraints.orientation_constraints[0].constraint_region_shape.dimensions.push_back(0.2);
00170   goalA.goal_constraints.orientation_constraints[0].constraint_region_shape.dimensions.push_back(0.2);
00171 
00172 
00173   goalA.goal_constraints.orientation_constraints[0].constraint_region_pose.position.x = 0.0;
00174   goalA.goal_constraints.orientation_constraints[0].constraint_region_pose.position.y = 0.0;
00175   goalA.goal_constraints.orientation_constraints[0].constraint_region_pose.position.z = 0.0;
00176 
00177   goalA.goal_constraints.orientation_constraints[0].constraint_region_pose.orientation.x = 0.0;
00178   goalA.goal_constraints.orientation_constraints[0].constraint_region_pose.orientation.y = 0.0;
00179   goalA.goal_constraints.orientation_constraints[0].constraint_region_pose.orientation.z = -0.7071;
00180   goalA.goal_constraints.orientation_constraints[0].constraint_region_pose.orientation.w = 0.7071;
00181 
00182   goalA.goal_constraints.orientation_constraints[0].weight = 1.0;
00183    
00184   std::vector<std::string> names(7);
00185   names[0] = "r_shoulder_pan_joint";
00186   names[1] = "r_shoulder_lift_joint";
00187   names[2] = "r_upper_arm_roll_joint";
00188   names[3] = "r_elbow_flex_joint";
00189   names[4] = "r_forearm_roll_joint";
00190   names[5] = "r_wrist_flex_joint";
00191   names[6] = "r_wrist_roll_joint";
00192 
00193 
00194   int num_test_attempts = 0;
00195   int max_attempts = 5;
00196   bool success = false;
00197 
00198 
00199   while (nh.ok())
00200   {
00201     bool finished_within_time = false;
00202     sleep(5);
00203     move_arm.sendGoal(goalA);
00204     finished_within_time = move_arm.waitForResult(ros::Duration(100.0));
00205     actionlib::SimpleClientGoalState state = move_arm.getState();
00206     success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
00207     if (!finished_within_time && num_test_attempts < max_attempts)
00208     {
00209       move_arm.cancelAllGoals();
00210       ROS_INFO("Timed out achieving goal A, trying again. Trying again, attempt: %d",num_test_attempts);
00211       num_test_attempts++;
00212     }
00213     else
00214     {
00215       if(!success)
00216       {
00217         ROS_INFO("Action unsuccessful");
00218         move_arm.cancelAllGoals();
00219       }
00220       ROS_INFO("Action finished: %s",state.toString().c_str());
00221       break;
00222     }
00223   }
00224   EXPECT_TRUE(success);
00225   ros::shutdown();
00226   spin_thread.join();
00227 }
00228 
00229 int main(int argc, char **argv){
00230   testing::InitGoogleTest(&argc, argv);
00231   ros::init (argc, argv, "move_arm_regression_test");
00232 
00233   ros::service::waitForService("/r_arm_joint_waypoint_controller/TrajectoryStart");
00234   if(!sendTuckArm())
00235     printf("TUCKARM ain't happening! why?\n");
00236 
00237   return RUN_ALL_TESTS();
00238 }
00239
regression_test_pose_goal_floorobj.cpp
