00001 00064 #ifndef GEOMETRY_MAP_NODE_H__ 00065 #define GEOMETRY_MAP_NODE_H__ 00066 00067 // ROS includes 00068 #include <ros/ros.h> 00069 #include <dynamic_reconfigure/server.h> 00070 00071 // ROS message includes 00072 //#include <visualization_msgs/Marker.h> 00073 //#include <visualization_msgs/MarkerArray.h> 00074 #include <cob_3d_mapping_msgs/GetGeometryMap.h> 00075 #include <cob_3d_mapping_msgs/SetGeometryMap.h> 00076 #include <cob_3d_mapping_msgs/ModifyMap.h> 00077 #include <cob_3d_mapping_msgs/ShapeArray.h> 00078 #include <cob_srvs/Trigger.h> 00079 00080 #include "cob_3d_mapping_common/polygon.h" 00081 #include "cob_3d_mapping_geometry_map/geometry_map.h" 00082 00083 namespace cob_3d_mapping 00084 { 00088 class GeometryMapNode 00089 { 00090 public: 00094 GeometryMapNode(); 00095 00099 ~GeometryMapNode() { }; 00100 00111 void dynReconfCallback(cob_3d_mapping_geometry_map::geometry_map_nodeConfig &config, uint32_t level); 00112 00120 void shapeCallback(const cob_3d_mapping_msgs::ShapeArray::ConstPtr& sa); 00121 00132 bool clearMap(cob_srvs::Trigger::Request &req, cob_srvs::Trigger::Response &res); 00133 00144 bool getMap(cob_3d_mapping_msgs::GetGeometryMap::Request &req, cob_3d_mapping_msgs::GetGeometryMap::Response &res); 00145 00156 bool 00157 setMap(cob_3d_mapping_msgs::SetGeometryMap::Request &req, cob_3d_mapping_msgs::SetGeometryMap::Response &res); 00158 00169 bool modifyMap(cob_3d_mapping_msgs::ModifyMap::Request &req, cob_3d_mapping_msgs::ModifyMap::Response &res) ; 00170 00176 void publishMap(); 00177 00178 00184 //void publishPrimitives(); 00185 00193 //void fillMarker(Polygon::Ptr p, visualization_msgs::Marker& m, visualization_msgs::Marker& m_t); 00194 00195 00203 //void fillMarker(Cylinder::Ptr c, visualization_msgs::Marker& m, visualization_msgs::Marker& m_t); 00204 00205 00213 //void fillMarker(ShapeCluster::Ptr sc, visualization_msgs::Marker& m, visualization_msgs::Marker& m_t); 00214 00215 ros::NodeHandle n_; 00216 00217 protected: 00218 ros::Subscriber shape_sub_; 00219 ros::Publisher map_pub_; 00220 ros::ServiceServer clear_map_server_; 00221 ros::ServiceServer get_map_server_; 00222 ros::ServiceServer set_map_server_; 00223 ros::ServiceServer modify_map_server_ ; 00224 00225 bool enable_cyl_; 00226 bool enable_poly_; 00227 00231 dynamic_reconfigure::Server<cob_3d_mapping_geometry_map::geometry_map_nodeConfig> config_server_; 00232 00233 GeometryMap geometry_map_; 00234 std::string map_frame_id_; 00235 }; 00236 } 00237 00238 #endif // GEOMETRY_MAP_NODE_H__