shape_marker.h
Go to the documentation of this file.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: Waqas Tanveer, email:Waqas.Tanveer@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 #ifndef SHAPE_MARKER_H_ 00056 #define SHAPE_MARKER_H_ 00057 00058 00059 //################## 00060 //#### includes #### 00061 // standard includes 00062 #include <stdio.h> 00063 #include <sstream> 00064 #include <math.h> 00065 #include <stdlib.h> 00066 #include <vector> 00067 #include <algorithm> 00068 // ROS includes 00069 #include <ros/ros.h> 00070 //#include <sensor_msgs/PointCloud2.h> 00071 #include <visualization_msgs/Marker.h> 00072 #include <visualization_msgs/MarkerArray.h> 00073 #include <visualization_msgs/InteractiveMarker.h> 00074 #include <visualization_msgs/InteractiveMarkerControl.h> 00075 //#include <visualization_msgs/MenuEntry.h> 00076 #include <interactive_markers/interactive_marker_server.h> 00077 #include <interactive_markers/menu_handler.h> 00078 00079 // PCL includes 00080 #include <pcl/pcl_config.h> 00081 #ifdef PCL_VERSION_COMPARE 00082 #include <pcl/common/transforms.h> 00083 #else 00084 #include <pcl/common/transform.h> 00085 #endif 00086 #include <pcl/common/eigen.h> 00087 #include <pcl/point_cloud.h> 00088 #include <pcl/ros/conversions.h> 00089 #include <pcl/point_types.h> 00090 00091 //#include <boost/bind.hpp> 00092 //#include <boost/make_shared.hpp> 00093 #include <boost/shared_ptr.hpp> 00094 00095 // external includes 00096 #include <Eigen/Core> 00097 00098 //#include <cob_3d_mapping_msgs/ShapeArray.h> 00099 #include <cob_3d_visualization/polypartition.h> 00100 #include <cob_3d_mapping_msgs/ModifyMap.h> 00101 #include <cob_3d_mapping_common/ros_msg_conversions.h> 00102 #include "cob_3d_mapping_common/polygon.h" 00103 #include "cob_3d_mapping_common/cylinder.h" 00104 //#include <cob_3d_visualization/shape_visualization.h> 00105 00106 //#define PI 3.14159265 00107 00108 //using namespace cob_3d_mapping ; 00109 class ShapeMarker 00110 { 00111 public: 00112 00113 ShapeMarker(boost::shared_ptr<interactive_markers::InteractiveMarkerServer> im_server, 00114 cob_3d_mapping_msgs::Shape& shape,std::vector<unsigned int>& moved_shapes_indices,std::vector<unsigned int>& interacted_shapes, 00115 std::vector<unsigned int>& deleted_markers_indices, bool arrows,bool deleted); 00116 ~ShapeMarker() 00117 { 00118 if(im_server_->erase(marker_.name)){ 00119 // ROS_INFO("Marker %s erased",marker_.name.c_str()); 00120 stringstream ss; 00121 ss << "normal_" << shape_.id; 00122 im_server_->erase(ss.str()); 00123 ss.str(""); 00124 ss.clear(); 00125 ss << "centroid_" << shape_.id; 00126 im_server_->erase(ss.str()); 00127 } 00128 } 00129 00138 void triangle_refinement(list<TPPLPoly>& i_list,list<TPPLPoly>& o_list); 00139 void getShape (cob_3d_mapping_msgs::Shape& shape); 00144 bool getArrows(); 00149 bool setArrows(); 00154 bool getDeleted(); 00159 bool setDeleted(); 00164 unsigned int getID() ; 00169 void enableMovement (const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback) ; 00173 void displayArrows() ; 00178 void hideArrows(int flag_untick) ; 00182 void createShapeMenu () ; 00187 void deleteMarker(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback) ; 00194 TPPLPoint msgToPoint2D (const pcl::PointXYZ &point) ; 00202 void createMarker (visualization_msgs::InteractiveMarkerControl& im_ctrl); 00206 void createInteractiveMarker () ; 00212 void displayNormalCB (const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback) ; 00216 void displayNormal(); 00221 void hideNormal(int flag_untick); 00227 void displaySymAxisCB (const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback) ; 00231 void displaySymAxis(); 00236 void hideSymAxis(int flag_untick); 00242 void displayCentroidCB (const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback) ; 00246 void displayCentroid(); 00250 void hideCentroid(int flag_untick); 00256 void displayOriginCB (const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback) ; 00260 void displayOrigin(); 00264 void hideOrigin(int flag_untick); 00270 void displayContourCB(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback) ; 00274 void displayContour(); 00278 void hideContour(int flag_untick); 00283 void resetMarker(); 00284 00285 visualization_msgs::Marker& getMarker() {return marker;}; 00286 00287 00288 00289 00290 protected: 00291 visualization_msgs::InteractiveMarker marker_ ; 00292 visualization_msgs::InteractiveMarker imarker_ ; 00293 visualization_msgs::InteractiveMarker deleted_imarker_ ; 00294 visualization_msgs::Marker marker; 00295 00296 // ros::NodeHandle nh_; 00297 // ros::Subscriber feedback_sub_ ; 00298 // ros::Publisher marker_pub_ ; 00299 00300 visualization_msgs::InteractiveMarkerControl im_ctrl; 00301 00302 boost::shared_ptr<interactive_markers::InteractiveMarkerServer> im_server_; 00303 cob_3d_mapping_msgs::Shape shape_; 00304 00305 00306 interactive_markers::MenuHandler menu_handler_; 00307 00308 Eigen::Affine3f transformation_; 00309 Eigen::Affine3f transformation_inv_; 00310 // int shape_ctr_ ; 00311 00312 00313 // int shape_ctr_ ; 00314 unsigned int id_; 00315 std::vector<unsigned int>& moved_shapes_indices_ ; 00316 std::vector<unsigned int>& interacted_shapes_ ; 00317 std::vector<unsigned int>& deleted_markers_indices_ ; 00318 // unsigned int& deleted_ ; 00319 00320 00321 bool arrows_; 00322 bool deleted_ ; 00323 // cob_3d_mapping_msgs::ModifyMap::Request req ; 00324 // cob_3d_mapping_msgs::ModifyMap::Response res; 00325 // // 00326 // Eigen::Quaternionf quatInit ; 00327 // Eigen::Vector3f oldCentroid ; 00328 // Eigen::Matrix4f transInit; 00329 // Eigen::Affine3f affineInit; 00330 // Eigen::Matrix4f transInitInv; 00331 00332 }; 00333 00334 00335 00336 00337 #endif /* SHAPE_MARKER_H_ */