Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 #include <ros/ros.h>
00037 #include <actionlib/client/simple_action_client.h>
00038 #include <actionlib/client/terminal_state.h>
00039 #include <object_manipulation_msgs/FindContainerAction.h>
00040
00041 #include <pcl/io/pcd_io.h>
00042 #include <pcl/point_types.h>
00043 #include <pcl_ros/transforms.h>
00044
00045 #include <tf/transform_listener.h>
00046
00047 int main (int argc, char **argv)
00048 {
00049 ros::init(argc, argv, "test_find_container_action");
00050
00051 if(argc < 2)
00052 {
00053 ROS_ERROR("Usage: test_find_container_action <point_cloud_topic>");
00054 exit(-1);
00055 }
00056 tf::TransformListener tfl;
00057 ros::Duration(1.0).sleep();
00058
00059
00060
00061 actionlib::SimpleActionClient<object_manipulation_msgs::FindContainerAction> ac("find_container_action", true);
00062
00063 ROS_INFO("Waiting for action server to start.");
00064
00065 ac.waitForServer();
00066
00067 sensor_msgs::PointCloud2::ConstPtr cloud = ros::topic::waitForMessage<sensor_msgs::PointCloud2>(argv[1], ros::Duration(5.0));
00068 if(!cloud)
00069 {
00070 ROS_INFO("No message received on topic %s", argv[1]);
00071 exit(-2);
00072 }
00073
00074
00075
00076 ROS_INFO("Action server started, sending goal.");
00077
00078
00079 object_manipulation_msgs::FindContainerGoal goal;
00080
00081
00082 tfl.waitForTransform("/base_link", cloud->header.frame_id, cloud->header.stamp, ros::Duration(2.0));
00083 pcl_ros::transformPointCloud("/base_link", *cloud, goal.cloud, tfl);
00084
00085
00086 goal.box_pose.header.frame_id = "/base_link";
00087 goal.box_pose.pose.position.x = 0.25;
00088 goal.box_pose.pose.position.y = -0.5;
00089 goal.box_pose.pose.position.z = 0.53;
00090 goal.box_dims.x = 0.5;
00091 goal.box_dims.y = 0.4;
00092 goal.box_dims.z = 0.6;
00093
00094
00095
00096 ac.sendGoal(goal);
00097
00098
00099 bool finished_before_timeout = ac.waitForResult(ros::Duration(30.0));
00100
00101 if (finished_before_timeout)
00102 {
00103 actionlib::SimpleClientGoalState state = ac.getState();
00104 ROS_INFO("Action finished: %s",state.toString().c_str());
00105 }
00106 else
00107 ROS_INFO("Action did not finish before the time out.");
00108
00109
00110 return 0;
00111 }