ias_drawer_executive: ias_drawer_executive: TestClient.cpp Source File

00001 /*
00002  * Copyright (c) 2010, Thomas Ruehr <ruehr@cs.tum.edu>
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include <ros/ros.h>
00031 #include <actionlib/client/simple_action_client.h>
00032 #include <actionlib/client/terminal_state.h>
00033 #include <ias_drawer_executive/OperateHandleAction.h>
00034 #include <ias_drawer_executive/OpenContainerAction.h>
00035 #include <ias_drawer_executive/CloseContainerAction.h>
00036 
00037 #include <signal.h>
00038 
00039 
00040 /*void test_op()
00041 {
00042 
00043     if (!ac_)
00044        ac_ = new actionlib::SimpleActionClient<ias_drawer_executive::OperateHandleAction>("operate_handle_action", true);
00045 
00046     ROS_INFO("Waiting for action server to start.");
00047     // wait for the action server to start
00048     ac_->waitForServer(); //will wait for infinite time
00049 
00050     ROS_INFO("Action server started, sending goal.");
00051 
00052     // send a goal to the action
00053     ias_drawer_executive::OperateHandleGoal goal;
00054     goal.arm = 0;
00055     goal.positionIdx= 8;
00056     goal.heightIdx= 3;
00057     ac_->sendGoal(goal);
00058 
00059     //wait for the action to return
00060     bool finished_before_timeout = ac_->waitForResult(ros::Duration(1000.0));
00061 
00062     if (finished_before_timeout)
00063     {
00064         actionlib::SimpleClientGoalState state = ac_->getState();
00065         ROS_INFO("Action finished: %s",state.toString().c_str());
00066     }
00067     else
00068         ROS_INFO("Action did not finish before the time out.");
00069 
00070 }*/
00071 
00072 
00073 actionlib::SimpleActionClient<ias_drawer_executive::OpenContainerAction> *ac_ = 0;
00074 
00075 actionlib::SimpleActionClient<ias_drawer_executive::CloseContainerAction> *ac_close_ = 0;
00076 
00077 void test_open(int arm_, double x, double y, double z, double ox, double oy, double oz, double ow)
00078 {
00079     if (!ac_)
00080       ac_ = new actionlib::SimpleActionClient<ias_drawer_executive::OpenContainerAction>("open_container_action", true);
00081 
00082     ROS_INFO("Waiting for action server to start.");
00083     // wait for the action server to start
00084     ac_->waitForServer(); //will wait for infinite time
00085 
00086     ROS_INFO("Action server started, sending goal.");
00087 
00088     // send a goal to the action
00089     ias_drawer_executive::OpenContainerGoal goal;
00090     goal.arm = arm_;
00091     goal.position.header.frame_id = "map";
00092     goal.position.pose.position.x = x;
00093     goal.position.pose.position.y = y;
00094     goal.position.pose.position.z = z;
00095     goal.position.pose.orientation.x = ox;
00096     goal.position.pose.orientation.y = oy;
00097     goal.position.pose.orientation.z = oz;
00098     goal.position.pose.orientation.w = ow;
00099 
00100     ac_->sendGoal(goal);
00101 
00102     //wait for the action to return
00103     bool finished_before_timeout = ac_->waitForResult(ros::Duration(1000.0));
00104 
00105     if (finished_before_timeout)
00106     {
00107         actionlib::SimpleClientGoalState state = ac_->getState();
00108         ROS_INFO("Action finished: %s",state.toString().c_str());
00109         boost::shared_ptr<const ias_drawer_executive::OpenContainerResult> result = ac_->getResult();
00110         //ias_drawer_executive::OpenContainerResult result = ac_->getResult();
00111         ROS_INFO("sucess %i", result->success);
00112         ROS_INFO("Trajectory: %i Poses", (unsigned int) result->trajectory.poses.size() );
00113         for (size_t j=0; j < result->trajectory.poses.size(); ++j)
00114         {
00115             ROS_INFO(" Pos %f %f %f, %f %f %f %f", result->trajectory.poses[j].position.x,result->trajectory.poses[j].position.y,result->trajectory.poses[j].position.z,
00116                      result->trajectory.poses[j].orientation.x,result->trajectory.poses[j].orientation.y,result->trajectory.poses[j].orientation.z,result->trajectory.poses[j].orientation.w);
00117         }
00118 
00119         /*int32 success
00120         double32 distance
00121         geometry_msgs/PoseArray trajectory
00122         Header header
00123         uint32 seq
00124         time stamp
00125         string frame_id
00126         geometry_msgs/Pose[] poses
00127         geometry_msgs/Point position
00128           double64 x
00129           double64 y
00130           double64 z
00131         geometry_msgs/Quaternion orientation
00132           double64 x
00133           double64 y
00134           double64 z
00135           double64 w*/
00136         ROS_INFO("TRYING NOW TO CLOSE");
00137 
00138         if (!ac_close_)
00139            ac_close_ = new actionlib::SimpleActionClient<ias_drawer_executive::CloseContainerAction>("close_container_action", true);
00140 
00141         ROS_INFO("Waiting for action server to start.");
00142         // wait for the action server to start
00143         ac_close_->waitForServer(); //will wait for infinite time
00144 
00145         ROS_INFO("Action server started, sending goal.");
00146 
00147         ias_drawer_executive::CloseContainerGoal closeGoal;
00148         closeGoal.arm = arm_;
00149         closeGoal.opening_trajectory = result->trajectory;
00150 
00151         ac_close_->sendGoal(closeGoal);
00152 
00153         bool finished_before_timeout = ac_close_->waitForResult(ros::Duration(1000.0));
00154 
00155         if (finished_before_timeout)
00156         {
00157             actionlib::SimpleClientGoalState state = ac_close_->getState();
00158             ROS_INFO("Action finished: %s",state.toString().c_str());
00159         }
00160     }
00161     else
00162         ROS_INFO("Action did not finish before the time out.");
00163 }
00164 
00165 geometry_msgs::Pose *getPose(double x, double y, double z, double ox, double oy, double oz, double ow)
00166 {
00167         geometry_msgs::Pose *act = new geometry_msgs::Pose();
00168         act->position.x = x;
00169         act->position.y = y;
00170         act->position.z = z;
00171         act->orientation.x = ox;
00172         act->orientation.y = oy;
00173         act->orientation.z = oz;
00174         act->orientation.w = ow;
00175         return act;
00176 }
00177 
00178 
00179 
00180 
00181 void closeonly() {
00182       if (!ac_close_)
00183         ac_close_ = new actionlib::SimpleActionClient<ias_drawer_executive::CloseContainerAction>("close_container_action", true);
00184 
00185      ROS_INFO("Waiting for action server to start.");
00186         // wait for the action server to start
00187         ac_close_->waitForServer(); //will wait for infinite time
00188 
00189         ROS_INFO("Action server started, sending goal.");
00190 
00191         ias_drawer_executive::CloseContainerGoal closeGoal;
00192         closeGoal.arm = 0;
00193 
00194         closeGoal.opening_trajectory.poses.push_back(*getPose( 0.346634, 1.118203, 0.763804, -0.708801, -0.055698, 0.039966, 0.702069));
00195         closeGoal.opening_trajectory.poses.push_back(*getPose( 0.509877, 1.121905, 0.761193, -0.705817, -0.039952, 0.034062, 0.706447));
00196         closeGoal.opening_trajectory.poses.push_back(*getPose( 0.710390, 1.128074, 0.761932, -0.705345, -0.038818, 0.035855, 0.706892));
00197 
00198         ac_close_->sendGoal(closeGoal);
00199 
00200         bool finished_before_timeout = ac_close_->waitForResult(ros::Duration(1000.0));
00201 
00202         if (finished_before_timeout)
00203         {
00204             actionlib::SimpleClientGoalState state = ac_close_->getState();
00205             ROS_INFO("Action finished: %s",state.toString().c_str());
00206         }
00207 }
00208 
00209 /*
00210 [ INFO] [1298048988.832717783]: TRAJ PT 10, 0.710390 1.128074 0.761932, -0.705345 -0.038818 0.035855 0.706892
00211 [ INFO] [1298048988.832773299]: TRAJ PT 10, 0.660410 1.126327 0.761844, -0.705351 -0.038682 0.035990 0.706887
00212 [ INFO] [1298048988.832815850]: TRAJ PT 10, 0.613501 1.124731 0.760803, -0.704437 -0.041014 0.035059 0.707713
00213 [ INFO] [1298048988.832854815]: TRAJ PT 10, 0.558759 1.122694 0.760463, -0.704835 -0.040498 0.034604 0.707369
00214 [ INFO] [1298048988.832893431]: TRAJ PT 10, 0.509877 1.121905 0.761193, -0.705817 -0.039952 0.034062 0.706447
00215 [ INFO] [1298048988.832932724]: TRAJ PT 10, 0.459795 1.119149 0.761111, -0.705787 -0.039522 0.033228 0.706540
00216 [ INFO] [1298048988.833147869]: TRAJ PT 10, 0.410610 1.116605 0.760985, -0.706038 -0.039084 0.032569 0.706344
00217 [ INFO] [1298048988.833254194]: TRAJ PT 10, 0.360722 1.114320 0.760970, -0.706072 -0.038935 0.032037 0.706343
00218 [ INFO] [1298048988.833302466]: TRAJ PT 10, 0.346634 1.118203 0.763804, -0.708801 -0.055698 0.039966 0.702069
00219 [ INFO] [1298048988.833347079]: TRAJ PT 10, 0.336214 1.120235 0.764419, -0.699980 -0.071769 0.059650 0.708039
00220 */
00221 
00222 void quit(int sig)
00223 {
00224     if (ac_)
00225       ac_->cancelAllGoals();
00226     if (ac_close_)
00227       ac_close_->cancelAllGoals();
00228     exit(0);
00229 }
00230 
00231 
00232 int main (int argc, char **argv)
00233 {
00234     ros::init(argc, argv, "test_fibonacci");
00235 
00236     signal(SIGINT,quit);
00237 
00238     //closeonly();
00239 
00240     //test_open(atoi(argv[1]),atof(argv[2]),atof(argv[3]),atof(argv[4]),atof(argv[5]),atof(argv[6]),atof(argv[7]),atof(argv[8]));
00241 
00242 
00243     // create the action client
00244     // true causes the client to spin it's own thread
00245     //exit
00246     return 0;
00247 }