00001 #ifndef ARTICULATION_RVIZ_PLUGIN_MODEL_DISPLAY_H
00002 #define ARTICULATION_RVIZ_PLUGIN_MODEL_DISPLAY_H
00003
00004 #include "rviz/display.h"
00005 #include "rviz/common.h"
00006 #include "rviz/properties/forwards.h"
00007 #include "rviz/helpers/color.h"
00008
00009 #include <ros/ros.h>
00010
00011 #include "articulation_msgs/ModelMsg.h"
00012 #include "articulation_msgs/TrackMsg.h"
00013 #include "articulation_models/models/factory.h"
00014
00015 #include <message_filters/subscriber.h>
00016 #include <tf/message_filter.h>
00017 #include <tf/transform_listener.h>
00018
00019 #include <ogre_tools/arrow.h>
00020 #include <ogre_tools/shape.h>
00021 #include <ogre_tools/billboard_line.h>
00022 #include <ogre_tools/movable_text.h>
00023 #include <OGRE/OgreSceneNode.h>
00024 #include <OGRE/OgreSceneManager.h>
00025 #include <boost/thread/mutex.hpp>
00026 #include <boost/shared_ptr.hpp>
00027 namespace articulation_rviz_plugin
00028 {
00029
00034 class ModelDisplay : public rviz::Display
00035 {
00036 public:
00037 articulation_models::MultiModelFactory factory;
00038 ModelDisplay( const std::string& name, rviz::VisualizationManager* manager );
00039 virtual void createProperties();
00040 virtual ~ModelDisplay();
00041
00042 virtual void onEnable();
00043 virtual void onDisable();
00044
00045 std::string model_topic_;
00046 void setTopic( const std::string& topic );
00047 const std::string& getTopic() { return model_topic_; }
00048 rviz::ROSTopicStringPropertyWPtr topic_property_;
00049
00050 enum UniqueStyle
00051 {
00052 u_all,
00053 u_single,
00054 u_track,
00055 u_model,
00056 u_modelstored,
00057 u_modeltype
00058 };
00059
00060 int unique_;
00061 void setUnique(int cs);
00062 int getUnique() { return unique_; }
00063 rviz::EnumPropertyWPtr unique_property_;
00064
00065 rviz::Color color_;
00066 void setColor( const rviz::Color& color );
00067 const rviz::Color& getColor() { return color_; }
00068 rviz::ColorPropertyWPtr color_property_;
00069
00070 float alpha_;
00071 void setAlpha(float a);
00072 float getAlpha() { return alpha_; }
00073 rviz::FloatPropertyWPtr alpha_property_;
00074
00075 float lineWidth_;
00076 void setLineWidth(float a);
00077 float getLineWidth() { return lineWidth_; }
00078 rviz::FloatPropertyWPtr lineWidth_property_;
00079
00080 enum ColorStyle
00081 {
00082 cs_fixed,
00083 cs_channel,
00084 cs_hue,
00085 cs_brightness,
00086 ColorStyleCount,
00087 };
00088
00089 int trackColor_;
00090 void setTrackColor(int cs);
00091 int getTrackColor() { return trackColor_; }
00092 rviz::EnumPropertyWPtr trackColor_property_;
00093
00094 int modelColor_;
00095 void setModelColor(int cs);
00096 int getModelColor() { return modelColor_; }
00097 rviz::EnumPropertyWPtr modelColor_property_;
00098
00099 int poseColor_;
00100 void setPoseColor(int cs);
00101 int getPoseColor() { return poseColor_; }
00102 rviz::EnumPropertyWPtr poseColor_property_;
00103
00104 int modelTypeColor_;
00105 void setModelTypeColor(int cs);
00106 int getModelTypeColor() { return modelTypeColor_; }
00107 rviz::EnumPropertyWPtr modelTypeColor_property_;
00108
00109 int likelihoodColor_;
00110 void setLikelihoodColor(int cs);
00111 int getLikelihoodColor() { return likelihoodColor_; }
00112 rviz::EnumPropertyWPtr likelihoodColor_property_;
00113
00114 int forceColor_;
00115 void setForceColor(int cs);
00116 int getForceColor() { return forceColor_; }
00117 rviz::EnumPropertyWPtr forceColor_property_;
00118
00119 enum DisplayStyle
00120 {
00121 ds_line,
00122 ds_cross_line,
00123 ds_cross,
00124 ds_arrow,
00125 ds_axes,
00126 ds_rectangle,
00127 ds_none,
00128 DisplayStyleCount
00129 };
00130
00131 int displayStyle_;
00132 void setDisplayStyle(int ds);
00133 int getDisplayStyle() { return displayStyle_; }
00134 rviz::EnumPropertyWPtr displayStyle_property_;
00135
00136 bool sampleConfigurations_;
00137 void setSampleConfigurations(bool sc);
00138 bool getSampleConfigurations() { return sampleConfigurations_; }
00139 rviz::BoolPropertyWPtr sampleConfigurations_property_;
00140
00141 bool projectConfigurations_;
00142 void setProjectConfigurations(bool sc);
00143 bool getProjectConfigurations() { return projectConfigurations_; }
00144 rviz::BoolPropertyWPtr projectConfigurations_property_;
00145
00146 bool showJacobians_;
00147 void setShowJacobians(bool sc);
00148 bool getShowJacobians() { return showJacobians_; }
00149 rviz::BoolPropertyWPtr showJacobians_property_;
00150
00151 bool showHessians_;
00152 void setShowHessians(bool sc);
00153 bool getShowHessians() { return showHessians_; }
00154 rviz::BoolPropertyWPtr showHessians_property_;
00155
00156 bool showLatestOnly_;
00157 void setShowLatestOnly(bool sc);
00158 bool getShowLatestOnly() { return showLatestOnly_; }
00159 rviz::BoolPropertyWPtr showLatestOnly_property_;
00160
00161 float sampleDensity_;
00162 void setSampleDensity(float a);
00163 float getSampleDensity() { return sampleDensity_; }
00164 rviz::FloatPropertyWPtr sampleDensity_property_;
00165
00166 bool showModelName_;
00167 void setShowModelName(bool sc);
00168 bool getShowModelName() { return showModelName_; }
00169 rviz::BoolPropertyWPtr showModelName_property_;
00170
00171 bool showForces_;
00172 void setShowForces(bool sc);
00173 bool getShowForces() { return showForces_; }
00174 rviz::BoolPropertyWPtr showForces_property_;
00175
00176 float forceScaling_;
00177 void setForceScaling(float a);
00178 float getForceScaling() { return forceScaling_; }
00179 rviz::FloatPropertyWPtr forceScaling_property_;
00180
00181 virtual void targetFrameChanged();
00182 virtual void fixedFrameChanged();
00183 virtual void reset();
00184 virtual void update(float wall_dt, float ros_dt);
00185
00186 protected:
00187 std::map<int, std::vector<ogre_tools::Arrow*> > arrows;
00188 std::map<int, std::vector<ogre_tools::BillboardLine*> > lines;
00189 std::map<int, std::vector<ogre_tools::MovableText* > > text;
00190 std::vector<ogre_tools::BillboardLine*> recycleLines;
00191 std::vector<ogre_tools::Arrow*> recycleArrows;
00192 std::vector<ogre_tools::MovableText*> recycleText;
00193 ogre_tools::BillboardLine* newBillboardLine();
00194 ogre_tools::Arrow* newArrow();
00195 ogre_tools::MovableText* newText();
00196
00197 void subscribe();
00198 void unsubscribe();
00199
00200 void clearVector(std::vector<ogre_tools::BillboardLine*>& vec);
00201 void clearVector(std::vector<ogre_tools::Arrow*>& vec);
00202 void clearVector(std::vector<ogre_tools::MovableText*>& vec);
00203 void clearMap();
00204 void clearDisplay();
00205 void clearResources(int uniqueId);
00206
00207 void incomingModel(const articulation_msgs::ModelMsg::ConstPtr& msg);
00208
00209 message_filters::Subscriber<articulation_msgs::ModelMsg> sub_;
00210 tf::MessageFilter<articulation_msgs::ModelMsg> tf_filter_;
00211
00212 articulation_msgs::ModelMsg::ConstPtr incoming_model_message_;
00213
00214 typedef std::vector<articulation_msgs::ModelMsg::ConstPtr> V_ModelMsg;
00215 V_ModelMsg message_queue_;
00216 boost::mutex queue_mutex_;
00217
00218 Ogre::SceneNode* scene_node_;
00219
00220 bool transform(const articulation_msgs::ModelMsg::ConstPtr& message, btTransform &transform);
00221 bool transform(const btTransform &pose, const btVector3 &scaleIn,Ogre::Vector3& pos, Ogre::Quaternion& orient, Ogre::Vector3& scaleOut);
00222
00223 void createRectangle(Ogre::Vector3 pos,Ogre::Quaternion orient,Ogre::Vector3 scale,double w,double h,btVector3 color,
00224 std::vector<ogre_tools::BillboardLine*> &vec);
00225
00226 void createAxes(Ogre::Vector3 pos,Ogre::Quaternion orient,Ogre::Vector3 scale,btVector3 color,
00227 std::vector<ogre_tools::BillboardLine*> &vec);
00228
00229 void createLine(Ogre::Vector3 pos,Ogre::Vector3 old_pos,Ogre::Vector3 scale,btVector3 color,
00230 std::vector<ogre_tools::BillboardLine*> &vec,bool add_cross,bool with_line);
00231
00232 void createArrow(Ogre::Vector3 pos,Ogre::Vector3 old_pos,Ogre::Vector3 scale,btVector3 color,
00233 std::vector<ogre_tools::Arrow*> &vec);
00234
00235 void createText(Ogre::Vector3 pos,Ogre::Quaternion orient,Ogre::Vector3 scale,btVector3 color,std::string title,
00236 std::vector<ogre_tools::MovableText*> &vec);
00237
00238 btVector3 modifyColor( btVector3 color, int colorStyle, float f );
00239 };
00240
00241 }
00242
00243 #endif
00244
00245
