move_arm: regression_test_under_table_pose

Go to the documentation of this file.
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 
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 <arm_navigation_msgs/CollisionObject.h>
00050 #include <arm_navigation_msgs/Shape.h>
00051 #include <arm_navigation_msgs/MakeStaticCollisionMapAction.h>
00052 
00053 typedef actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> MoveArmClient;
00054 
00055 unsigned int REPS_TO_TRY = 10;
00056 
00057 
00058 void spinThread()
00059 {
00060   ros::spin();
00061 }
00062 
00063 TEST(MoveArm, goToPoseGoal)
00064 {
00065 
00066   ros::NodeHandle nh;
00067   ros::NodeHandle private_handle("~");
00068 
00069   ros::Publisher object_in_map_pub_;
00070   object_in_map_pub_  = nh.advertise<arm_navigation_msgs::CollisionObject>("collision_object", 10);
00071 
00072   actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm(nh, "move_right_arm");
00073   actionlib::SimpleActionClient<arm_navigation_msgs::MakeStaticCollisionMapAction> make_static_map(nh, "make_static_collision_map");
00074 
00075   boost::thread spin_thread(&spinThread);
00076   
00077   move_arm.waitForServer();
00078   make_static_map.waitForServer();
00079   ROS_INFO("Connected to servers");
00080 
00081   //push the table and legs into the collision space
00082   arm_navigation_msgs::CollisionObject table_object;
00083   table_object.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD;
00084   table_object.header.frame_id = "base_link";
00085   table_object.header.stamp = ros::Time::now();
00086   arm_navigation_msgs::Shape object;
00087   object.type = arm_navigation_msgs::Shape::BOX;
00088   object.dimensions.resize(3);
00089   object.dimensions[0] = 1.0;
00090   object.dimensions[1] = 1.0;
00091   object.dimensions[2] = 0.05;
00092   geometry_msgs::Pose pose;
00093   pose.position.x = 1.0;
00094   pose.position.y = 0;
00095   pose.position.z = .5;
00096   pose.orientation.x = 0;
00097   pose.orientation.y = 0;
00098   pose.orientation.z = 0;
00099   pose.orientation.w = 1;
00100   table_object.shapes.push_back(object);
00101   table_object.poses.push_back(pose);
00102 
00103   table_object.id = "table";
00104   object_in_map_pub_.publish(table_object);
00105 
00106   arm_navigation_msgs::CollisionObject leg_object;
00107   leg_object.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD;
00108   leg_object.header.frame_id = "base_link";
00109   leg_object.header.stamp = ros::Time::now();
00110   arm_navigation_msgs::Shape l_object;
00111   l_object.type = arm_navigation_msgs::Shape::BOX;
00112   l_object.dimensions.resize(3);
00113   l_object.dimensions[0] = 0.05;
00114   l_object.dimensions[1] = 0.05;
00115   l_object.dimensions[2] = 0.5;
00116   geometry_msgs::Pose l_pose;
00117   l_pose.position.x = .5;
00118   l_pose.position.y = .5;
00119   l_pose.position.z = .25;
00120   l_pose.orientation.x = 0;
00121   l_pose.orientation.y = 0;
00122   l_pose.orientation.z = 0;
00123   l_pose.orientation.w = 1;
00124   leg_object.shapes.push_back(l_object);
00125   leg_object.poses.push_back(l_pose);
00126   l_pose.position.x = .5;
00127   l_pose.position.y = -.5;
00128   l_pose.position.z = .25;
00129   leg_object.shapes.push_back(l_object);
00130   leg_object.poses.push_back(l_pose);
00131   leg_object.id = "legs";
00132   object_in_map_pub_.publish(leg_object);
00133 
00134   ros::Duration(10.0).sleep();
00135 
00136   std::vector<std::string> names(7);
00137   names[0] = "r_shoulder_pan_joint";
00138   names[1] = "r_shoulder_lift_joint";
00139   names[2] = "r_upper_arm_roll_joint";
00140   names[3] = "r_elbow_flex_joint";
00141   names[4] = "r_forearm_roll_joint";
00142   names[5] = "r_wrist_flex_joint";
00143   names[6] = "r_wrist_roll_joint";
00144 
00145   arm_navigation_msgs::MoveArmGoal goalA, goalB;
00146   goalB.motion_plan_request.group_name = "right_arm";
00147   goalB.motion_plan_request.num_planning_attempts = 1;
00148   goalB.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
00149 
00150   private_handle.param<std::string>("planner_id",goalB.motion_plan_request.planner_id,std::string(""));
00151   private_handle.param<std::string>("planner_service_name",goalB.planner_service_name,std::string("/ompl_planning/plan_kinematic_path"));
00152     
00153   goalB.motion_plan_request.goal_constraints.joint_constraints.resize(names.size());
00154   for (unsigned int i = 0 ; i < goalB.motion_plan_request.goal_constraints.joint_constraints.size(); ++i)
00155   {
00156     //    goalB.motion_plan_request.goal_constraints.joint_constraints[i].header.stamp = ros::Time::now();
00157     //    goalB.motion_plan_request.goal_constraints.joint_constraints[i].header.frame_id = "base_link";
00158     goalB.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = names[i];
00159     goalB.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0;
00160     goalB.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.1;
00161     goalB.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.1;
00162   }
00163     
00164 // Joint r_shoulder_pan_joint val -0.453899
00165 // [ INFO] [61.024000000]: Joint r_shoulder_lift_joint val 0.31966
00166 // [ INFO] [61.024000000]: Joint r_upper_arm_roll_joint val -0.142258
00167 // [ INFO] [61.024000000]: Joint r_elbow_flex_joint val -0.563305
00168 // [ INFO] [61.024000000]: Joint r_forearm_roll_joint val -0.00941518
00169 // [ INFO] [61.024000000]: Joint r_wrist_flex_joint val -0.744197
00170 // [ INFO] [61.024000000]: Joint r_wrist_roll_joint val 0.410848
00171 
00172 
00173   goalB.motion_plan_request.goal_constraints.joint_constraints[0].position = -1.5;
00174   goalB.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2;
00175   goalB.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.20;
00176 
00177 //   goalB.motion_plan_request.goal_constraints.joint_constraints[0].position = -.453899;
00178 //   goalB.motion_plan_request.goal_constraints.joint_constraints[1].position = .31966;
00179 //   goalB.motion_plan_request.goal_constraints.joint_constraints[2].position = -0.142258;
00180 //   goalB.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.563305;
00181 //   goalB.motion_plan_request.goal_constraints.joint_constraints[4].position = -0.00941518;
00182 //   goalB.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.744197;
00183 //   goalB.motion_plan_request.goal_constraints.joint_constraints[6].position = .410848;
00184 
00185   goalB.motion_plan_request.goal_constraints.joint_constraints[0].position = -1.05023;
00186   goalB.motion_plan_request.goal_constraints.joint_constraints[1].position = -0.0454;
00187   goalB.motion_plan_request.goal_constraints.joint_constraints[2].position = -2.81824;
00188   goalB.motion_plan_request.goal_constraints.joint_constraints[3].position = -1.50497;
00189   goalB.motion_plan_request.goal_constraints.joint_constraints[4].position = 1.40266;
00190   goalB.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.476981;
00191   goalB.motion_plan_request.goal_constraints.joint_constraints[6].position = .477598;
00192 
00193 
00194   goalA.motion_plan_request.planner_id = "";
00195   goalA.planner_service_name ="ompl_planning/plan_kinematic_path"; 
00196 
00197   goalA.motion_plan_request.group_name = "right_arm";
00198   goalA.motion_plan_request.num_planning_attempts = 1;
00199   goalA.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
00200   goalA.motion_plan_request.goal_constraints.position_constraints.resize(1);
00201   goalA.motion_plan_request.goal_constraints.position_constraints[0].header.stamp = ros::Time::now();
00202   goalA.motion_plan_request.goal_constraints.position_constraints[0].header.frame_id = "base_link";
00203     
00204   goalA.motion_plan_request.goal_constraints.position_constraints[0].link_name = "r_wrist_roll_link";
00205   goalA.motion_plan_request.goal_constraints.position_constraints[0].position.x = 0.6;
00206   goalA.motion_plan_request.goal_constraints.position_constraints[0].position.y = 0;
00207   goalA.motion_plan_request.goal_constraints.position_constraints[0].position.z = .35;
00208     
00209   goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.type = arm_navigation_msgs::Shape::BOX;
00210   goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
00211   goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
00212   goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
00213 
00214   goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_orientation.w = 1.0;
00215 
00216   goalA.motion_plan_request.goal_constraints.position_constraints[0].weight = 1.0;
00217 
00218   goalA.motion_plan_request.goal_constraints.orientation_constraints.resize(1);
00219   goalA.motion_plan_request.goal_constraints.orientation_constraints[0].header.stamp = ros::Time::now();
00220   goalA.motion_plan_request.goal_constraints.orientation_constraints[0].header.frame_id = "base_link";    
00221   goalA.motion_plan_request.goal_constraints.orientation_constraints[0].link_name = "r_wrist_roll_link";
00222   goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.x = 0.0;
00223   goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.y = 0.0;
00224   goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.z = 0.0;
00225   goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.w = 1.0;
00226 
00227   goalA.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_roll_tolerance = 3.14;
00228   goalA.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_pitch_tolerance = 3.14;
00229   goalA.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_yaw_tolerance = 3.14;
00230     
00231   goalA.motion_plan_request.goal_constraints.orientation_constraints[0].weight = 1.0;
00232 
00233   for(unsigned int i = 0; i < REPS_TO_TRY; i++) {
00234     
00235     if (nh.ok())
00236     {
00237       bool finished_within_time = false;
00238       move_arm.sendGoal(goalA);
00239       finished_within_time = move_arm.waitForResult(ros::Duration(200.0));
00240       EXPECT_TRUE(finished_within_time);
00241       if (!finished_within_time)
00242       {
00243         move_arm.cancelGoal();
00244         ROS_INFO("Timed out achieving goal A");
00245       }
00246       else
00247       {
00248         actionlib::SimpleClientGoalState state = move_arm.getState();
00249         bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
00250         ASSERT_TRUE(success);
00251         ROS_INFO("Action finished: %s",state.toString().c_str());
00252         ASSERT_TRUE(state == actionlib::SimpleClientGoalState::SUCCEEDED);
00253       }
00254     }
00255 
00256   //   arm_navigation_msgs::MakeStaticCollisionMapGoal static_map_goal;
00257     
00258 //     static_map_goal.cloud_source = "full_cloud_filtered";
00259 //     static_map_goal.number_of_clouds = 2;
00260 
00261 //     make_static_map.sendGoal(static_map_goal);
00262 
00263 //     make_static_map.waitForResult(ros::Duration(10.0));
00264 
00265 //     ROS_INFO("Should have static map");
00266 
00267     if (nh.ok())
00268     {
00269       bool finished_within_time = false;
00270       move_arm.sendGoal(goalB);
00271       finished_within_time = move_arm.waitForResult(ros::Duration(100.0));
00272       EXPECT_TRUE(finished_within_time);
00273       if (!finished_within_time)
00274       {
00275         move_arm.cancelAllGoals();
00276         ROS_INFO("Timed out achieving goal B");
00277       }
00278       else
00279       {
00280         actionlib::SimpleClientGoalState state = move_arm.getState();
00281         bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
00282         ASSERT_TRUE(success);
00283         ROS_INFO("Action finished: %s",state.toString().c_str());
00284         ASSERT_TRUE(state == actionlib::SimpleClientGoalState::SUCCEEDED);
00285       }
00286     }
00287   }
00288   ros::shutdown();
00289   spin_thread.join();
00290 }
00291 
00292 int main(int argc, char **argv){
00293   testing::InitGoogleTest(&argc, argv);
00294   ros::init (argc, argv, "move_arm_regression_test");
00295   return RUN_ALL_TESTS();
00296 }