$search
00001 /* 00002 * Visualizer.h 00003 * 00004 * Created on: May 8, 2012 00005 * Author: sdries 00006 */ 00007 00008 #ifndef VISUALIZER_H_ 00009 #define VISUALIZER_H_ 00010 00011 #include "ros/ros.h" 00012 #include <visualization_msgs/Marker.h> 00013 #include <visualization_msgs/MarkerArray.h> 00014 00015 #include <wire_msgs/Property.h> 00016 00017 #include "tf/transform_listener.h" 00018 00019 #include "problib/conversions.h" 00020 00021 00022 00023 #include <map> 00024 00025 // Color struct 00026 struct Color { 00027 double r; 00028 double g; 00029 double b; 00030 00031 Color() {}; 00032 Color(double red, double green, double blue) : r(red), g(green), b(blue) {}; 00033 }; 00034 00035 00036 class Visualizer { 00037 00038 public: 00039 00040 Visualizer(); 00041 00042 virtual ~Visualizer(); 00043 00044 bool setParameters(ros::NodeHandle& n, const std::string& ns); 00045 00046 bool createMarkers(const std_msgs::Header& header, long ID, 00047 const std::vector<wire_msgs::Property>& props, 00048 std::vector<visualization_msgs::Marker>& markers_out, 00049 const std::string& frame_id); 00050 00051 void setTFListener(tf::TransformListener* tf_listener); 00052 00053 protected: 00054 00055 // Structure storing marker information 00056 struct MarkerInfo { 00057 visualization_msgs::Marker shape_marker; 00058 visualization_msgs::Marker text_marker; 00059 visualization_msgs::Marker cov_marker; 00060 bool show_cov; 00061 bool show_text; 00062 }; 00063 00064 // Color mapping 00065 std::map<std::string, Color> color_mapping_; 00066 00067 // Tf listener 00068 tf::TransformListener* tf_listener_; 00069 00070 // Mapping from object class to marker 00071 std::map<std::string, MarkerInfo> object_class_to_marker_map_; 00072 00073 // Mapping that defines per attribute if it has to be shown 00074 std::map<std::string, bool> attribute_map_; 00075 00076 // Get marker parameters 00077 bool getMarkerParameters(ros::NodeHandle& n, const std::string& ns); 00078 00079 // Get attribute settings 00080 bool getAttributeSettings(ros::NodeHandle& n, const std::string& ns); 00081 00082 // Get most probable Gaussian from a pdf 00083 const pbl::Gaussian* getBestGaussian(const pbl::PDF& pdf, double min_weight = 0); 00084 00085 // Set marker type 00086 void setMarkerType(XmlRpc::XmlRpcValue& v, MarkerInfo& m); 00087 00088 // Overloaded get value function 00089 void getStructValue(XmlRpc::XmlRpcValue& s, const std::string& name, float& f, float default_value); 00090 void getStructValue(XmlRpc::XmlRpcValue& s, const std::string& name, double& d, double default_value); 00091 void getStructValue(XmlRpc::XmlRpcValue& s, const std::string& name, bool& b, bool default_value); 00092 void getStructValue(XmlRpc::XmlRpcValue& s, const std::string& name, std::string& str, const std::string& default_value); 00093 }; 00094 00095 #endif /* VISUALIZER_H_ */