$search
00001 /**************************************************************** 00002 * 00003 * Copyright (c) 2010 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_common 00013 * Description: 00014 * 00015 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 00016 * 00017 * Author: Shaghayegh Nazari, email:georg.arbeiter@ipa.fhg.de 00018 * Supervised by: Georg Arbeiter, email:georg.arbeiter@ipa.fhg.de 00019 * 00020 * Date of creation: 09/2012 00021 * ToDo: 00022 * 00023 * 00024 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 00025 * 00026 * Redistribution and use in source and binary forms, with or without 00027 * modification, are permitted provided that the following conditions are met: 00028 * 00029 * * Redistributions of source code must retain the above copyright 00030 * notice, this list of conditions and the following disclaimer. 00031 * * Redistributions in binary form must reproduce the above copyright 00032 * notice, this list of conditions and the following disclaimer in the 00033 * documentation and/or other materials provided with the distribution. 00034 * * Neither the name of the Fraunhofer Institute for Manufacturing 00035 * Engineering and Automation (IPA) nor the names of its 00036 * contributors may be used to endorse or promote products derived from 00037 * this software without specific prior written permission. 00038 * 00039 * This program is free software: you can redistribute it and/or modify 00040 * it under the terms of the GNU Lesser General Public License LGPL as 00041 * published by the Free Software Foundation, either version 3 of the 00042 * License, or (at your option) any later version. 00043 * 00044 * This program is distributed in the hope that it will be useful, 00045 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00046 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00047 * GNU Lesser General Public License LGPL for more details. 00048 * 00049 * You should have received a copy of the GNU Lesser General Public 00050 * License LGPL along with this program. 00051 * If not, see <http://www.gnu.org/licenses/>. 00052 * 00053 ****************************************************************/ 00054 00055 00056 #ifndef TABLE_MARKER_H_ 00057 #define TABLE_MARKER_H_ 00058 00059 //################## 00060 //#### includes #### 00061 // standard includes 00062 #include <stdio.h> 00063 #include <sstream> 00064 00065 // ROS includes 00066 #include <ros/ros.h> 00067 //#include <sensor_msgs/PointCloud2.h> 00068 #include <visualization_msgs/Marker.h> 00069 #include <visualization_msgs/MarkerArray.h> 00070 #include <visualization_msgs/InteractiveMarker.h> 00071 #include <visualization_msgs/InteractiveMarkerControl.h> 00072 #include <visualization_msgs/InteractiveMarkerFeedback.h> 00073 //#include <visualization_msgs/MenuEntry.h> 00074 #include <interactive_markers/interactive_marker_server.h> 00075 #include <interactive_markers/menu_handler.h> 00076 00077 // PCL includes 00078 #include <pcl/pcl_config.h> 00079 #ifdef PCL_VERSION_COMPARE 00080 #include <pcl/common/transforms.h> 00081 #else 00082 #include <pcl/common/transform.h> 00083 #endif 00084 #include <pcl/common/eigen.h> 00085 #include <pcl/point_cloud.h> 00086 #include <pcl/ros/conversions.h> 00087 #include <pcl/point_types.h> 00088 #include <boost/shared_ptr.hpp> 00089 00090 // external includes 00091 #include <Eigen/Core> 00092 00093 //#include <cob_3d_visualization/polypartition.h> 00094 //#include <cob_3d_mapping_msgs/ShapeArray.h> 00095 #include <cob_3d_visualization/shape_marker.h> 00096 //#include <cob_3d_mapping_msgs/ModifyMap.h> 00097 00098 #include <cob_3d_mapping_common/ros_msg_conversions.h> 00099 #include "cob_3d_mapping_common/polygon.h" 00100 #include <cob_3d_mapping_msgs/MoveToTable.h> 00101 00102 00103 00104 00105 00106 class TableMarker 00107 { 00108 public: 00109 // Constructor 00110 TableMarker (boost::shared_ptr<interactive_markers::InteractiveMarkerServer> server,cob_3d_mapping_msgs::Shape& table,int ctr/*, 00111 tabletop_object_detector::Table& tableMsg*/) ; 00112 // Destructor 00113 ~TableMarker () 00114 { 00115 00116 } 00124 void createMarkerforTable (visualization_msgs::InteractiveMarkerControl& im_ctrl); 00128 void createInteractiveMarkerForTable(); 00135 TPPLPoint msgToPoint2DforTable (const pcl::PointXYZ &point); 00140 void tableFeedbackCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); 00144 void createTableMenu(); 00149 void MoveToTheTable(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); 00150 00151 protected: 00152 visualization_msgs::InteractiveMarker table_int_marker_ ; 00153 visualization_msgs::Marker table_marker_; 00154 00155 ros::NodeHandle nh_; 00156 00157 cob_3d_mapping_msgs::Shape table_; 00158 00159 ros::Publisher goal_pub_ ; 00160 00161 00162 visualization_msgs::InteractiveMarkerControl im_ctrl; 00163 boost::shared_ptr<interactive_markers::InteractiveMarkerServer> table_im_server_; 00164 interactive_markers::MenuHandler table_menu_handler_; 00165 00166 Eigen::Affine3f transformation_; 00167 Eigen::Affine3f transformation_inv_; 00168 00169 int id_; 00170 00171 // Table Parameters 00172 //tabletop_object_detector::Table table_msg_; 00173 00174 }; 00175 00176 00177 #endif /* TABLE_MARKER_H_ */