regression_test_under_table_pose_goal_no_ik.cpp
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 
00052 typedef actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> MoveArmClient;
00053 
00054 unsigned int REPS_TO_TRY = 3;
00055 
00056 
00057 void spinThread()
00058 {
00059   ros::spin();
00060 }
00061 
00062 TEST(MoveArm, goToPoseGoal)
00063 {
00064 
00065   ros::NodeHandle nh;
00066   ros::NodeHandle private_handle("~");
00067 
00068   ros::Publisher object_in_map_pub_;
00069   object_in_map_pub_  = nh.advertise<arm_navigation_msgs::CollisionObject>("collision_object", 10);
00070 
00071   actionlib::SimpleActionClient<arm_navigation_msgs::MoveArmAction> move_arm(nh, "move_right_arm");
00072   boost::thread spin_thread(&spinThread);
00073   
00074   move_arm.waitForServer();
00075   ROS_INFO("Connected to server");
00076 
00077   //push the table and legs into the collision space
00078   arm_navigation_msgs::CollisionObject table_object;
00079   table_object.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD;
00080   table_object.header.frame_id = "base_link";
00081   table_object.header.stamp = ros::Time::now();
00082   arm_navigation_msgs::Shape object;
00083   object.type = arm_navigation_msgs::Shape::BOX;
00084   object.dimensions.resize(3);
00085   object.dimensions[0] = 1.0;
00086   object.dimensions[1] = 1.0;
00087   object.dimensions[2] = 0.05;
00088   geometry_msgs::Pose pose;
00089   pose.position.x = 1.0;
00090   pose.position.y = 0;
00091   pose.position.z = .5;
00092   pose.orientation.x = 0;
00093   pose.orientation.y = 0;
00094   pose.orientation.z = 0;
00095   pose.orientation.w = 1;
00096   table_object.shapes.push_back(object);
00097   table_object.poses.push_back(pose);
00098 
00099   table_object.id = "table";
00100   object_in_map_pub_.publish(table_object);
00101 
00102  //  arm_navigation_msgs::ObjectInMap leg_object;
00103 //   leg_object.action = arm_navigation_msgs::ObjectInMap::ADD;
00104 //   leg_object.header.frame_id = "base_link";
00105 //   leg_object.header.stamp = ros::Time::now();
00106 //   leg_object.object.type = arm_navigation_msgs::Shape::BOX;
00107 //   leg_object.object.dimensions.resize(3);
00108 //   leg_object.object.dimensions[0] = 0.02;
00109 //   leg_object.object.dimensions[1] = 0.02;
00110 //   leg_object.object.dimensions[2] = .5;
00111 //   leg_object.pose.position.x = .5;
00112 //   leg_object.pose.position.y = .5;
00113 //   leg_object.pose.position.z = .25;
00114 //   leg_object.pose.orientation.x = 0;
00115 //   leg_object.pose.orientation.y = 0;
00116 //   leg_object.pose.orientation.z = 0;
00117 //   leg_object.pose.orientation.w = 1;
00118   
00119 //   leg_object.id = "leg1";
00120 //   object_in_map_pub_.publish(leg_object);
00121 
00122 //   leg_object.id = "leg2";
00123 //   leg_object.pose.position.x = .5;
00124 //   leg_object.pose.position.y = -.5;
00125 //   leg_object.pose.position.z = .25;
00126 //   object_in_map_pub_.publish(leg_object);
00127   
00128   std::vector<std::string> names(7);
00129   names[0] = "r_shoulder_pan_joint";
00130   names[1] = "r_shoulder_lift_joint";
00131   names[2] = "r_upper_arm_roll_joint";
00132   names[3] = "r_elbow_flex_joint";
00133   names[4] = "r_forearm_roll_joint";
00134   names[5] = "r_wrist_flex_joint";
00135   names[6] = "r_wrist_roll_joint";
00136 
00137   arm_navigation_msgs::MoveArmGoal goalA, goalB;
00138   goalB.motion_plan_request.group_name = "right_arm";
00139   goalB.motion_plan_request.num_planning_attempts = 1;
00140   goalB.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
00141 
00142   private_handle.param<std::string>("planner_id",goalB.motion_plan_request.planner_id,std::string(""));
00143   private_handle.param<std::string>("planner_service_name",goalB.planner_service_name,std::string("/ompl_planning/plan_kinematic_path"));
00144     
00145   goalB.motion_plan_request.goal_constraints.joint_constraints.resize(names.size());
00146   for (unsigned int i = 0 ; i < goalB.motion_plan_request.goal_constraints.joint_constraints.size(); ++i)
00147   {
00148     //    goalB.motion_plan_request.goal_constraints.joint_constraints[i].header.stamp = ros::Time::now();
00149     //    goalB.motion_plan_request.goal_constraints.joint_constraints[i].header.frame_id = "base_link";
00150     goalB.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = names[i];
00151     goalB.motion_plan_request.goal_constraints.joint_constraints[i].position = 0.0;
00152     goalB.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = 0.1;
00153     goalB.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = 0.1;
00154   }
00155     
00156   goalB.motion_plan_request.goal_constraints.joint_constraints[0].position = -1.5;
00157   goalB.motion_plan_request.goal_constraints.joint_constraints[3].position = -0.2;
00158   goalB.motion_plan_request.goal_constraints.joint_constraints[5].position = -0.20;
00159 
00160   goalA.motion_plan_request.planner_id = "";
00161   goalA.planner_service_name ="ompl_planning/plan_kinematic_path"; 
00162 
00163   goalA.motion_plan_request.group_name = "right_arm";
00164   goalA.motion_plan_request.num_planning_attempts = 1;
00165   goalA.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
00166   goalA.motion_plan_request.goal_constraints.set_position_constraints_size(1);
00167   goalA.motion_plan_request.goal_constraints.position_constraints[0].header.stamp = ros::Time::now();
00168   goalA.motion_plan_request.goal_constraints.position_constraints[0].header.frame_id = "base_link";
00169     
00170   goalA.motion_plan_request.goal_constraints.position_constraints[0].link_name = "r_wrist_roll_link";
00171   goalA.motion_plan_request.goal_constraints.position_constraints[0].position.x = 0.6;
00172   goalA.motion_plan_request.goal_constraints.position_constraints[0].position.y = 0;
00173   goalA.motion_plan_request.goal_constraints.position_constraints[0].position.z = .35;
00174     
00175   goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.type = arm_navigation_msgs::Shape::BOX;
00176   goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
00177   goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
00178   goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.dimensions.push_back(0.02);
00179 
00180   goalA.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_orientation.w = 1.0;
00181 
00182   goalA.motion_plan_request.goal_constraints.position_constraints[0].weight = 1.0;
00183 
00184   goalA.motion_plan_request.goal_constraints.set_orientation_constraints_size(1);
00185   goalA.motion_plan_request.goal_constraints.orientation_constraints[0].header.stamp = ros::Time::now();
00186   goalA.motion_plan_request.goal_constraints.orientation_constraints[0].header.frame_id = "base_link";    
00187   goalA.motion_plan_request.goal_constraints.orientation_constraints[0].link_name = "r_wrist_roll_link";
00188   goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.x = 0.0;
00189   goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.y = 0.0;
00190   goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.z = 0.0;
00191   goalA.motion_plan_request.goal_constraints.orientation_constraints[0].orientation.w = 1.0;
00192 
00193   goalA.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_roll_tolerance = 0.04;
00194   goalA.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_pitch_tolerance = 0.04;
00195   goalA.motion_plan_request.goal_constraints.orientation_constraints[0].absolute_yaw_tolerance = 0.04;
00196     
00197   goalA.motion_plan_request.goal_constraints.orientation_constraints[0].weight = 1.0;
00198 
00199   //move arm should send the pose constraint straight to the planner
00200   goalA.disable_ik = true;
00201 
00202   for(unsigned int i = 0; i < REPS_TO_TRY; i++) {
00203     
00204     if (nh.ok())
00205     {
00206       bool finished_within_time = false;
00207       move_arm.sendGoal(goalA);
00208       finished_within_time = move_arm.waitForResult(ros::Duration(200.0));
00209       EXPECT_TRUE(finished_within_time);
00210       if (!finished_within_time)
00211       {
00212         move_arm.cancelGoal();
00213         ROS_INFO("Timed out achieving goal A");
00214       }
00215       else
00216       {
00217         actionlib::SimpleClientGoalState state = move_arm.getState();
00218         bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
00219         EXPECT_TRUE(success);
00220         ROS_INFO("Action finished: %s",state.toString().c_str());
00221         EXPECT_TRUE(state == actionlib::SimpleClientGoalState::SUCCEEDED);
00222       }
00223     }
00224 
00225     if (nh.ok())
00226     {
00227       bool finished_within_time = false;
00228       move_arm.sendGoal(goalB);
00229       finished_within_time = move_arm.waitForResult(ros::Duration(100.0));
00230       EXPECT_TRUE(finished_within_time);
00231       if (!finished_within_time)
00232       {
00233         move_arm.cancelAllGoals();
00234         ROS_INFO("Timed out achieving goal B");
00235       }
00236       else
00237       {
00238         actionlib::SimpleClientGoalState state = move_arm.getState();
00239         bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
00240         EXPECT_TRUE(success);
00241         ROS_INFO("Action finished: %s",state.toString().c_str());
00242         EXPECT_TRUE(state == actionlib::SimpleClientGoalState::SUCCEEDED);
00243       }
00244     }
00245   }
00246   ros::shutdown();
00247   spin_thread.join();
00248 }
00249 
00250 int main(int argc, char **argv){
00251   testing::InitGoogleTest(&argc, argv);
00252   ros::init (argc, argv, "move_arm_regression_test");
00253   return RUN_ALL_TESTS();
00254 }


move_arm
Author(s): Ioan Sucan, Sachin Chitta(sachinc@willowgarage.com)
autogenerated on Thu Dec 12 2013 11:09:40