00001 00060 #ifndef SHAPE_VISUALIZATION_H_ 00061 #define SHAPE_VISUALIZATION_H_ 00062 00063 //################## 00064 //#### includes #### 00065 // standard includes 00066 #include <stdio.h> 00067 #include <sstream> 00068 00069 // ROS includes 00070 #include <ros/ros.h> 00071 //#include <sensor_msgs/PointCloud2.h> 00072 #include <visualization_msgs/Marker.h> 00073 #include <visualization_msgs/MarkerArray.h> 00074 #include <visualization_msgs/InteractiveMarker.h> 00075 #include <visualization_msgs/InteractiveMarkerControl.h> 00076 #include <interactive_markers/interactive_marker_server.h> 00077 #include <interactive_markers/menu_handler.h> 00078 #include <cob_3d_mapping_msgs/ModifyMap.h> 00079 #include <cob_3d_visualization/shape_marker.h> 00080 //#include <cob_3d_visualization/table_marker.h> 00081 #include <cob_3d_mapping_msgs/GetTables.h> 00082 #include <cob_3d_mapping_msgs/GetObjectsOfClass.h> 00083 #include <tf/transform_listener.h> 00084 00085 00086 00087 00088 #include <boost/shared_ptr.hpp> 00089 00090 00091 class ShapeVisualization 00092 { 00093 public: 00094 // Constructor 00095 ShapeVisualization () ; 00096 // Destructor 00097 ~ShapeVisualization () 00098 { 00100 } 00106 void shapeArrayCallback (const cob_3d_mapping_msgs::ShapeArrayPtr& sa) ; 00111 void setShapePosition(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); 00115 void moreOptions(); 00121 void displayAllNormals(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); 00127 void displayAllCentroids (const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback) ; 00133 void displayAllContours (const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); 00138 void resetAll(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback) ; 00143 void applyModifications (const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback) ; 00147 void optionMenu() ; 00148 // void findTables(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback); 00149 00150 protected: 00151 00152 ros::NodeHandle nh_; 00153 // ros::Publisher shape_pub_ ; 00154 ros::Subscriber shape_array_sub_; // sub for shape array msgs 00155 ros::Subscriber feedback_sub_ ; 00156 ros::Publisher marker_pub_; 00157 std::vector<boost::shared_ptr<ShapeMarker> > v_sm_; 00158 cob_3d_mapping_msgs::ShapeArray sha ; 00159 interactive_markers::MenuHandler menu_handler_for_text_; 00160 00161 int ctr_for_shape_indexes; 00162 std::vector<unsigned int> moved_shapes_indices_; 00163 std::vector<unsigned int> interacted_shapes_; 00164 std::vector<unsigned int> deleted_markers_indices_; 00165 cob_3d_mapping_msgs::ShapeArray modified_shapes_; 00166 std::string frame_id_; 00167 std::vector<unsigned int> marker_ids_; 00168 std::vector<unsigned int> contour_ids_; 00169 bool show_contours_; 00170 // unsigned int deleted_ ; 00171 00172 00173 Eigen::Quaternionf quatInit ; 00174 Eigen::Vector3f oldCentroid ; 00175 Eigen::Matrix4f transInit; 00176 Eigen::Affine3f affineInit; 00177 Eigen::Matrix4f transInitInv; 00178 // cob_3d_mapping_msgs::ModifyMap::Request req ; 00179 // cob_3d_mapping_msgs::ModifyMap::Response res; 00180 00181 00182 00183 boost::shared_ptr<interactive_markers::InteractiveMarkerServer> im_server_; // server for interactive markers 00184 00185 }; 00186 00187 00188 #endif /* SHAPE_VISUALIZATION_H_ */