00001 00059 /* 00060 * plane_extraction_action_client.cpp 00061 * 00062 * Created on: 26.08.2011 00063 * Author: goa 00064 */ 00065 00066 00067 #include <ros/ros.h> 00068 #include <actionlib/client/simple_action_client.h> 00069 #include <actionlib/client/terminal_state.h> 00070 #include <cob_3d_mapping_msgs/TableObjectClusterAction.h> 00071 #include <pcl/point_types.h> 00072 00073 #include <cob_3d_mapping_common/stop_watch.h> 00074 00075 int main (int argc, char **argv) 00076 { 00077 ros::init(argc, argv, "table_object_cluster_action_client"); 00078 ros::NodeHandle nh; 00079 00080 00081 // create the action client 00082 // true causes the client to spin its own thread 00083 actionlib::SimpleActionClient<cob_3d_mapping_msgs::TableObjectClusterAction> ac("tabletop_object_cluster_trigger", true); 00084 00085 ROS_INFO("Waiting for action server to start."); 00086 // wait for the action server to start 00087 ac.waitForServer(); //will wait for infinite time 00088 00089 ROS_INFO("Action server started, sending goal."); 00090 // send a goal to the action 00091 cob_3d_mapping_msgs::TableObjectClusterGoal goal; // no information needed 00092 ac.sendGoal(goal); 00093 PrecisionStopWatch sw; 00094 sw.precisionStart(); 00095 //wait for the action to return 00096 bool finished_before_timeout = ac.waitForResult(ros::Duration(30.0)); 00097 ROS_INFO("Action took %f seconds", sw.precisionStop()); 00098 if (finished_before_timeout) 00099 { 00100 actionlib::SimpleClientGoalState state = ac.getState(); 00101 ROS_INFO("Action finished: %s",state.toString().c_str()); 00102 } 00103 else 00104 { 00105 ROS_INFO("Action did not finish before the time out."); 00106 return 0; 00107 } 00108 cob_3d_mapping_msgs::TableObjectClusterResultConstPtr res = ac.getResult(); 00109 00110 //exit 00111 return 0; 00112 } 00113