00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #ifndef RVIZ_INTERACTIVE_MARKER_DISPLAY_H
00031 #define RVIZ_INTERACTIVE_MARKER_DISPLAY_H
00032
00033 #include <map>
00034 #include <set>
00035
00036 #include <visualization_msgs/InteractiveMarker.h>
00037 #include <visualization_msgs/InteractiveMarkerUpdate.h>
00038 #include <visualization_msgs/InteractiveMarkerInit.h>
00039
00040 #include <message_filters/subscriber.h>
00041 #include <tf/message_filter.h>
00042
00043 #include "rviz/display.h"
00044 #include "rviz/selection/forwards.h"
00045
00046 #include "rviz/default_plugin/interactive_markers/interactive_marker.h"
00047 #include "rviz/default_plugin/interactive_markers/interactive_marker_client.h"
00048
00049 namespace Ogre
00050 {
00051 class SceneManager;
00052 class SceneNode;
00053 }
00054
00055 namespace rviz
00056 {
00057 class BoolProperty;
00058 class MarkerSelectionHandler;
00059 class Object;
00060 class RosTopicProperty;
00061 class MarkerBase;
00062
00063 typedef boost::shared_ptr<MarkerSelectionHandler> MarkerSelectionHandlerPtr;
00064 typedef boost::shared_ptr<MarkerBase> MarkerBasePtr;
00065 typedef std::pair<std::string, int32_t> MarkerID;
00066
00073 class InteractiveMarkerDisplay : public Display, public InteractiveMarkerReceiver
00074 {
00075 Q_OBJECT
00076 public:
00077 InteractiveMarkerDisplay();
00078 virtual ~InteractiveMarkerDisplay();
00079
00080 virtual void onInitialize();
00081
00082 virtual void update(float wall_dt, float ros_dt);
00083
00084 virtual void fixedFrameChanged();
00085 virtual void reset();
00086
00088 void setStatusOk(const std::string& name, const std::string& text);
00089 void setStatusWarn(const std::string& name, const std::string& text);
00090 void setStatusError(const std::string& name, const std::string& text);
00091
00096 bool subscribeToInit();
00097
00098
00099 void clearMarkers();
00100
00101
00102 void processMarkerChanges( const std::vector<visualization_msgs::InteractiveMarker>* markers = NULL,
00103 const std::vector<visualization_msgs::InteractiveMarkerPose>* poses = NULL,
00104 const std::vector<std::string>* erases = NULL );
00105
00106 void unsubscribeFromInit();
00107
00108 protected:
00109 virtual void onEnable();
00110 virtual void onDisable();
00111
00112 protected Q_SLOTS:
00113 void updateTopic();
00114 void updateShowDescriptions();
00115 void updateShowAxes();
00116
00117 private:
00118
00119 void subscribe();
00120
00121
00122 void unsubscribe();
00123
00124
00125 void tfMarkerSuccess(const visualization_msgs::InteractiveMarker::ConstPtr& marker);
00126
00127
00128 void tfMarkerFail(const visualization_msgs::InteractiveMarker::ConstPtr& marker, tf::FilterFailureReason reason);
00129
00130
00131 void tfPoseSuccess(const visualization_msgs::InteractiveMarkerPose::ConstPtr& marker_pose);
00132
00133
00134 void tfPoseFail(const visualization_msgs::InteractiveMarkerPose::ConstPtr& marker_pose, tf::FilterFailureReason reason);
00135
00136 void updateMarker( visualization_msgs::InteractiveMarker::ConstPtr& marker );
00137 void updatePose( visualization_msgs::InteractiveMarkerPose::ConstPtr& pose );
00138
00139 InteractiveMarkerClient im_client_;
00140
00141
00142 Ogre::SceneNode* scene_node_;
00143
00144 typedef boost::shared_ptr<InteractiveMarker> InteractiveMarkerPtr;
00145 typedef std::map< std::string, InteractiveMarkerPtr > M_StringToInteractiveMarkerPtr;
00146 M_StringToInteractiveMarkerPtr interactive_markers_;
00147
00148
00149
00150 tf::MessageFilter<visualization_msgs::InteractiveMarker>* tf_filter_;
00151 tf::MessageFilter<visualization_msgs::InteractiveMarkerPose>* tf_pose_filter_;
00152
00153 ros::Subscriber marker_update_sub_;
00154 ros::Subscriber marker_init_sub_;
00155
00156
00157 typedef std::vector<visualization_msgs::InteractiveMarker::ConstPtr> V_InteractiveMarkerMessage;
00158 V_InteractiveMarkerMessage marker_queue_;
00159 typedef std::vector<visualization_msgs::InteractiveMarkerPose::ConstPtr> V_InteractiveMarkerPoseMessage;
00160 V_InteractiveMarkerPoseMessage pose_queue_;
00161 boost::mutex queue_mutex_;
00162
00163 unsigned num_publishers_;
00164
00165 std::string client_id_;
00166
00167
00168 RosTopicProperty* marker_update_topic_property_;
00169 BoolProperty* show_descriptions_property_;
00170 BoolProperty* show_axes_property_;
00171 };
00172
00173 }
00174
00175 #endif