Go to the documentation of this file.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
00031
00032 #ifndef INTERACTIVE_MARKER_SERVER
00033 #define INTERACTIVE_MARKER_SERVER
00034
00035 #include <visualization_msgs/InteractiveMarkerUpdate.h>
00036 #include <visualization_msgs/InteractiveMarkerFeedback.h>
00037
00038 #include <boost/scoped_ptr.hpp>
00039 #include <boost/thread/thread.hpp>
00040 #include <boost/thread/recursive_mutex.hpp>
00041
00042 #include <ros/ros.h>
00043 #include <ros/callback_queue.h>
00044
00045
00046 #include <boost/function.hpp>
00047 #include <boost/unordered_map.hpp>
00048
00049 namespace interactive_markers
00050 {
00051
00056 class InteractiveMarkerServer : boost::noncopyable
00057 {
00058 public:
00059
00060 typedef visualization_msgs::InteractiveMarkerFeedbackConstPtr FeedbackConstPtr;
00061 typedef boost::function< void ( const FeedbackConstPtr& ) > FeedbackCallback;
00062
00063 static const uint8_t DEFAULT_FEEDBACK_CB = 255;
00064
00072 InteractiveMarkerServer( const std::string &topic_ns, const std::string &server_id="", bool spin_thread = false );
00073
00075 ~InteractiveMarkerServer();
00076
00081 void insert( const visualization_msgs::InteractiveMarker &int_marker );
00082
00089 void insert( const visualization_msgs::InteractiveMarker &int_marker,
00090 FeedbackCallback feedback_cb,
00091 uint8_t feedback_type=DEFAULT_FEEDBACK_CB );
00092
00099 bool setPose( const std::string &name,
00100 const geometry_msgs::Pose &pose,
00101 const std_msgs::Header &header=std_msgs::Header() );
00102
00107 bool erase( const std::string &name );
00108
00111 void clear();
00112
00116 bool empty() const;
00117
00121 std::size_t size() const;
00122
00133 bool setCallback( const std::string &name, FeedbackCallback feedback_cb,
00134 uint8_t feedback_type=DEFAULT_FEEDBACK_CB );
00135
00138 void applyChanges();
00139
00144 bool get( std::string name, visualization_msgs::InteractiveMarker &int_marker ) const;
00145
00146 private:
00147
00148 struct MarkerContext
00149 {
00150 ros::Time last_feedback;
00151 std::string last_client_id;
00152 FeedbackCallback default_feedback_cb;
00153 boost::unordered_map<uint8_t,FeedbackCallback> feedback_cbs;
00154 visualization_msgs::InteractiveMarker int_marker;
00155 };
00156
00157 typedef boost::unordered_map< std::string, MarkerContext > M_MarkerContext;
00158
00159
00160 struct UpdateContext
00161 {
00162 enum {
00163 FULL_UPDATE,
00164 POSE_UPDATE,
00165 ERASE
00166 } update_type;
00167 visualization_msgs::InteractiveMarker int_marker;
00168 FeedbackCallback default_feedback_cb;
00169 boost::unordered_map<uint8_t,FeedbackCallback> feedback_cbs;
00170 };
00171
00172 typedef boost::unordered_map< std::string, UpdateContext > M_UpdateContext;
00173
00174
00175
00176
00177 void spinThread();
00178
00179
00180 void processFeedback( const FeedbackConstPtr& feedback );
00181
00182
00183 void keepAlive();
00184
00185
00186 void publish( visualization_msgs::InteractiveMarkerUpdate &update );
00187
00188
00189 void publishInit();
00190
00191
00192 void doSetPose( M_UpdateContext::iterator update_it,
00193 const std::string &name,
00194 const geometry_msgs::Pose &pose,
00195 const std_msgs::Header &header );
00196
00197
00198 M_MarkerContext marker_contexts_;
00199
00200
00201 M_UpdateContext pending_updates_;
00202
00203
00204 std::string topic_ns_;
00205
00206 mutable boost::recursive_mutex mutex_;
00207
00208
00209 boost::scoped_ptr<boost::thread> spin_thread_;
00210 ros::NodeHandle node_handle_;
00211 ros::CallbackQueue callback_queue_;
00212 volatile bool need_to_terminate_;
00213
00214
00215 ros::Timer keep_alive_timer_;
00216
00217 ros::Publisher init_pub_;
00218 ros::Publisher update_pub_;
00219 ros::Subscriber feedback_sub_;
00220
00221 uint64_t seq_num_;
00222
00223 std::string server_id_;
00224 };
00225
00226 }
00227
00228 #endif