$search
00001 /**************************************************************** 00002 * 00003 * Copyright (c) 2011 00004 * 00005 * Fraunhofer Institute for Manufacturing Engineering 00006 * and Automation (IPA) 00007 * 00008 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 00009 * 00010 * Project name: care-o-bot 00011 * ROS stack name: cob_environment_perception_intern 00012 * ROS package name: cob_3d_mapping_semantics 00013 * Description: 00014 * 00015 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 00016 * 00017 * Author: Georg Arbeiter, email:georg.arbeiter@ipa.fhg.de 00018 * Supervised by: Georg Arbeiter, email:georg.arbeiter@ipa.fhg.de 00019 * 00020 * Date of creation: 11/2012 00021 * ToDo: 00022 * 00023 * 00024 * 00025 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 00026 * 00027 * Redistribution and use in source and binary forms, with or without 00028 * modification, are permitted provided that the following conditions are met: 00029 * 00030 * * Redistributions of source code must retain the above copyright 00031 * notice, this list of conditions and the following disclaimer. 00032 * * Redistributions in binary form must reproduce the above copyright 00033 * notice, this list of conditions and the following disclaimer in the 00034 * documentation and/or other materials provided with the distribution. 00035 * * Neither the name of the Fraunhofer Institute for Manufacturing 00036 * Engineering and Automation (IPA) nor the names of its 00037 * contributors may be used to endorse or promote products derived from 00038 * this software without specific prior written permission. 00039 * 00040 * This program is free software: you can redistribute it and/or modify 00041 * it under the terms of the GNU Lesser General Public License LGPL as 00042 * published by the Free Software Foundation, either version 3 of the 00043 * License, or (at your option) any later version. 00044 * 00045 * This program is distributed in the hope that it will be useful, 00046 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00047 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00048 * GNU Lesser General Public License LGPL for more details. 00049 * 00050 * You should have received a copy of the GNU Lesser General Public 00051 * License LGPL along with this program. 00052 * If not, see <http://www.gnu.org/licenses/>. 00053 * 00054 ****************************************************************/ 00055 00056 //################## 00057 //#### includes #### 00058 #include <algorithm> 00059 00060 // ROS includes 00061 #include <ros/ros.h> 00062 #include <rosbag/bag.h> 00063 //#include <tf_conversions/tf_eigen.h> 00064 //#include <tf/transform_listener.h> 00065 #include <dynamic_reconfigure/server.h> 00066 00067 // ros message includes 00068 #include <sensor_msgs/PointCloud.h> 00069 #include <visualization_msgs/Marker.h> 00070 #include <visualization_msgs/MarkerArray.h> 00071 00072 // PCL includes 00073 #include <pcl/point_cloud.h> 00074 #include <pcl/point_types.h> 00075 #include <pcl/ros/conversions.h> 00076 #include <pcl/io/pcd_io.h> 00077 00078 //internal includes 00079 #include <cob_3d_mapping_msgs/ShapeArray.h> 00080 #include <cob_3d_mapping_msgs/GetGeometryMap.h> 00081 #include <cob_3d_mapping_msgs/GetObjectsOfClass.h> 00082 #include <cob_3d_mapping_msgs/GetTables.h> 00083 //#include <cob_3d_mapping_msgs/MoveToTable.h> 00084 #include <cob_3d_mapping_semantics/supporting_plane_extraction.h> 00085 #include <cob_3d_mapping_semantics/supporting_plane_extraction_nodeConfig.h> 00086 #include <cob_3d_mapping_common/ros_msg_conversions.h> 00087 //#include <tabletop_object_detector/TabletopDetection.h> 00088 00089 using namespace cob_3d_mapping; 00090 00091 class SupportingPlaneExtractionNode 00092 { 00093 public: 00094 00095 // Constructor 00096 SupportingPlaneExtractionNode () : 00097 n_("~") 00098 { 00099 config_server_.setCallback(boost::bind(&SupportingPlaneExtractionNode::dynReconfCallback, this, _1, _2)); 00100 00101 sa_sub_ = n_.subscribe ("shape_array", 10, &SupportingPlaneExtractionNode::callbackShapeArray, this); 00102 sa_pub_ = n_.advertise<cob_3d_mapping_msgs::ShapeArray> ("shape_array_pub", 1); //10 00103 s_marker_pub_ = n_.advertise<visualization_msgs::Marker> ("marker", 10); 00104 00105 //get_server_ = n_.advertiseService ("get_objects_of_class", &SupportingPlaneExtractionNode::getTablesService, this); 00106 } 00107 00108 // Destructor 00109 ~SupportingPlaneExtractionNode () 00110 { 00112 } 00113 00124 void 00125 dynReconfCallback(cob_3d_mapping_semantics::supporting_plane_extraction_nodeConfig &config, uint32_t level) 00126 { 00127 ROS_INFO("[supporting_plane_extraction]: received new parameters"); 00128 spe_.setDistanceMin (config.distance_min); 00129 spe_.setDistanceMax (config.distance_max); 00130 spe_.setAreaMin (config.area_min); 00131 spe_.setAreaMax (config.area_max); 00132 } 00133 00142 void 00143 callbackShapeArray (const cob_3d_mapping_msgs::ShapeArray::ConstPtr sa_ptr) 00144 { 00145 cob_3d_mapping_msgs::ShapeArray sup_planes; 00146 sup_planes.header = sa_ptr->header; 00147 processShapeArray(*sa_ptr, sup_planes); 00148 publishShapeMarker (sup_planes); 00149 sa_pub_.publish (sup_planes); 00150 //ROS_INFO("Found %u tables", (unsigned int)tables.shapes.size()); 00151 } 00152 00153 00162 bool 00163 getTablesService (cob_3d_mapping_msgs::GetObjectsOfClassRequest &req, 00164 cob_3d_mapping_msgs::GetObjectsOfClassResponse &res) 00165 { 00166 ROS_INFO("service get_tables started...."); 00167 00168 cob_3d_mapping_msgs::ShapeArray sa; 00169 if (getMapService (sa)) 00170 { 00171 processShapeArray(sa, res.objects); 00172 publishShapeMarker (res.objects); 00173 ROS_INFO("Found %u tables", (unsigned int)res.objects.shapes.size()); 00174 return true; 00175 } 00176 else 00177 return false; 00178 } 00179 00180 00186 bool 00187 getMapService (cob_3d_mapping_msgs::ShapeArray& sa) 00188 { 00189 ROS_INFO("Waiting for service server to start."); 00190 ros::service::waitForService ("get_geometry_map", 10); //will wait for infinite time 00191 00192 ROS_INFO("Server started, polling map."); 00193 00194 //build message 00195 cob_3d_mapping_msgs::GetGeometryMapRequest req; 00196 cob_3d_mapping_msgs::GetGeometryMapResponse res; 00197 00198 if (ros::service::call ("get_geometry_map", req, res)) 00199 { 00200 ROS_INFO("Service call finished."); 00201 } 00202 else 00203 { 00204 ROS_INFO("Service call failed."); 00205 return 0; 00206 } 00207 sa = res.map; 00208 return true; 00209 } 00210 00218 void 00219 publishShapeMarker (const cob_3d_mapping_msgs::ShapeArray& sa) 00220 { 00221 for(unsigned int i=0; i<sa.shapes.size(); i++) 00222 { 00223 visualization_msgs::Marker marker; 00224 marker.action = visualization_msgs::Marker::ADD; 00225 marker.type = visualization_msgs::Marker::LINE_STRIP; 00226 marker.lifetime = ros::Duration (2); 00227 marker.header = sa.header; 00228 marker.ns = "table_marker"; 00229 //marker.header.stamp = ros::Time::now (); 00230 00231 marker.id = i; 00232 marker.scale.x = 0.05; 00233 marker.scale.y = 0.05; 00234 marker.scale.z = 0; 00235 marker.color.r = 0;//1; 00236 marker.color.g = 0; 00237 marker.color.b = 1; 00238 marker.color.a = 1.0; 00239 00240 00241 // sensor_msgs::PointCloud2 pc2; 00242 pcl::PointCloud<pcl::PointXYZ> cloud; 00243 00244 pcl::fromROSMsg (sa.shapes[i].points[0], cloud); 00245 00246 geometry_msgs::Point p; 00247 //marker.points.resize (cloud.size()+1); 00248 for (unsigned int j = 0; j < cloud.size(); j++) 00249 { 00250 p.x = cloud[j].x; 00251 p.y = cloud[j].y; 00252 p.z = cloud[j].z; 00253 marker.points.push_back(p); 00254 } 00255 p.x = cloud[0].x; 00256 p.y = cloud[0].y; 00257 p.z = cloud[0].z; 00258 marker.points.push_back(p); 00259 s_marker_pub_.publish (marker); 00260 } 00261 } 00262 00271 void 00272 processShapeArray(const cob_3d_mapping_msgs::ShapeArray& sa, cob_3d_mapping_msgs::ShapeArray& sup_planes) 00273 { 00274 std::vector<Polygon::Ptr> polys; 00275 for (unsigned int i = 0; i < sa.shapes.size (); i++) 00276 { 00277 if(sa.shapes[i].type != cob_3d_mapping_msgs::Shape::POLYGON) continue; 00278 Polygon::Ptr poly_ptr (new Polygon()); 00279 fromROSMsg(sa.shapes[i], *poly_ptr); 00280 polys.push_back(poly_ptr); 00281 } 00282 00283 Polygon sp; 00284 if(!spe_.getSupportingPlane(polys, sp)) return; 00285 cob_3d_mapping_msgs::Shape s; 00286 s.header = sa.header; 00287 toROSMsg(sp, s); 00288 /*for (unsigned int i = 0; i < sa.shapes.size (); i++) 00289 { 00290 if(id == sa.shapes[i].id) 00291 { 00292 s = sa.shapes[i]; 00293 break; 00294 } 00295 }*/ 00296 ROS_INFO("Found a supporting plane"); 00297 sup_planes.shapes.push_back (s); 00298 } 00299 00300 ros::NodeHandle n_; 00301 00302 protected: 00303 ros::Subscriber sa_sub_; 00304 ros::Publisher sa_pub_; 00305 ros::Publisher pc2_pub_; 00306 ros::Publisher s_marker_pub_; 00307 //ros::ServiceServer get_tables_server_; 00308 //tf::TransformListener tf_listener_; ///< Retrieves transformations. 00309 00313 dynamic_reconfigure::Server<cob_3d_mapping_semantics::supporting_plane_extraction_nodeConfig> config_server_; 00314 00315 SupportingPlaneExtraction spe_; 00316 00317 }; 00318 00319 int 00320 main (int argc, char** argv) 00321 { 00322 ros::init (argc, argv, "supporting_plane_extraction_node"); 00323 00324 SupportingPlaneExtractionNode sem_exn_node; 00325 //ros::spin (); 00326 00327 ros::Rate loop_rate (10); 00328 while (ros::ok ()) 00329 { 00330 ros::spinOnce (); 00331 loop_rate.sleep (); 00332 } 00333 00334 }