Go to the documentation of this file.00001
00060 #include <ros/ros.h>
00061 #include <actionlib/server/simple_action_server.h>
00062
00063 #include <pcl/point_types.h>
00064 #include <pcl_ros/point_cloud.h>
00065
00066 #include <cob_srvs/Trigger.h>
00067 #include <cob_object_detection_msgs/DetectionArray.h>
00068 #include <cob_3d_mapping_msgs/SetBoundingBoxes.h>
00069 #include <cob_3d_mapping_msgs/TableObjectClusterAction.h>
00070
00071 class TableObjectClusterActionServerNode
00072 {
00073 public:
00074 typedef cob_object_detection_msgs::DetectionArray BoundingBoxes;
00075 typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;
00076
00077 TableObjectClusterActionServerNode(const std::string& name)
00078 : action_name(name)
00079 , action_started(false)
00080 , as(nh, name, false)
00081 , subscribed_(false)
00082 { onInit(); }
00083
00084 void onInit()
00085 {
00086
00087 sub_bba = nh.subscribe<BoundingBoxes>("bounding_box_array", 1,
00088 boost::bind(&TableObjectClusterActionServerNode::outputCallback,this,_1));
00089
00090 pub_pc = nh.advertise<PointCloud>("triggered_point_cloud", 1);
00091
00092 set_bb_client_ = nh.serviceClient<cob_3d_mapping_msgs::SetBoundingBoxes>("set_known_objects");
00093 start_pc_sub_ = nh.advertiseService("start_pc_sub", &TableObjectClusterActionServerNode::startPCSub, this);
00094
00095 as.registerGoalCallback(boost::bind(&TableObjectClusterActionServerNode::goalCallback, this));
00096 as.registerPreemptCallback(boost::bind(&TableObjectClusterActionServerNode::preemptCallback, this));
00097 as.start();
00098 }
00099
00100 bool startPCSub(cob_srvs::Trigger::Request &req, cob_srvs::Trigger::Response &res)
00101 {
00102 if(!subscribed_)
00103 {
00104 sub_pc = nh.subscribe<PointCloud>("point_cloud", 1, boost::bind(&TableObjectClusterActionServerNode::inputCallback,this,_1));
00105 subscribed_ = true;
00106 }
00107 else
00108 {
00109 sub_pc.shutdown();
00110 subscribed_ = false;
00111 }
00112 return true;
00113 }
00114
00115 void goalCallback()
00116 {
00117 if(!last_pc)
00118 {
00119 ROS_INFO("%s: Aborted! No point cloud available for table detection.", action_name.c_str());
00120 as.setAborted();
00121 return;
00122 }
00123 action_timeout = ros::Time::now() + ros::Duration(5.0);
00124 action_started = true;
00125 cob_3d_mapping_msgs::SetBoundingBoxes srv;
00126 srv.request.bounding_boxes = as.acceptNewGoal()->known_objects;
00127 if(set_bb_client_.call(srv))
00128 {
00129 pub_pc.publish(last_pc);
00130 }
00131 else
00132 {
00133 ROS_WARN("Failed to set bounding boxes of known objects");
00134 }
00135 }
00136
00137 void preemptCallback()
00138 {
00139 ROS_INFO("%s: Preempted", action_name.c_str());
00140 as.setPreempted();
00141 }
00142
00143
00144 void inputCallback(const PointCloud::ConstPtr& pc)
00145 {
00146
00147 last_pc = pc;
00148 }
00149
00150
00151 void outputCallback(const BoundingBoxes::ConstPtr& bba)
00152 {
00153 if(as.isActive())
00154 {
00155 cob_3d_mapping_msgs::TableObjectClusterResult result;
00156 result.bounding_boxes = *bba;
00157 as.setSucceeded(result);
00158 }
00159 }
00160
00161
00162 void spinOnce()
00163 {
00164 if( as.isActive() && ros::Time::now() > action_timeout )
00165 {
00166 ROS_INFO("%s: Timeout! No BoundingBoxes received after 5.0s", action_name.c_str());
00167 as.setAborted();
00168 action_started = false;
00169 }
00170 }
00171
00172 private:
00173 ros::NodeHandle nh;
00174 ros::Subscriber sub_pc;
00175 ros::Subscriber sub_bba;
00176 ros::Publisher pub_pc;
00177 ros::ServiceClient set_bb_client_;
00178 ros::ServiceServer start_pc_sub_;
00179
00180 actionlib::SimpleActionServer<cob_3d_mapping_msgs::TableObjectClusterAction> as;
00181
00182 PointCloud::ConstPtr last_pc;
00183
00184 std::string action_name;
00185 bool action_started;
00186 ros::Time action_timeout;
00187 bool subscribed_;
00188 };
00189
00190 int main (int argc, char **argv)
00191 {
00192 ros::init(argc, argv, "table_object_cluster_action_server");
00193
00194 TableObjectClusterActionServerNode tocasn("tabletop_object_cluster_trigger");
00195
00196 ros::Rate loop_rate(10);
00197 while (ros::ok())
00198 {
00199 ros::spinOnce();
00200 tocasn.spinOnce();
00201
00202 loop_rate.sleep();
00203 }
00204
00205 return 0;
00206 }