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
00123 bool setCallback( const std::string &name, FeedbackCallback feedback_cb,
00124 uint8_t feedback_type=DEFAULT_FEEDBACK_CB );
00125
00128 void applyChanges();
00129
00134 bool get( std::string name, visualization_msgs::InteractiveMarker &int_marker ) const;
00135
00136 private:
00137
00138 struct MarkerContext
00139 {
00140 ros::Time last_feedback;
00141 std::string last_client_id;
00142 FeedbackCallback default_feedback_cb;
00143 boost::unordered_map<uint8_t,FeedbackCallback> feedback_cbs;
00144 visualization_msgs::InteractiveMarker int_marker;
00145 };
00146
00147 typedef boost::unordered_map< std::string, MarkerContext > M_MarkerContext;
00148
00149
00150 struct UpdateContext
00151 {
00152 enum {
00153 FULL_UPDATE,
00154 POSE_UPDATE,
00155 ERASE
00156 } update_type;
00157 visualization_msgs::InteractiveMarker int_marker;
00158 FeedbackCallback default_feedback_cb;
00159 boost::unordered_map<uint8_t,FeedbackCallback> feedback_cbs;
00160 };
00161
00162 typedef boost::unordered_map< std::string, UpdateContext > M_UpdateContext;
00163
00164
00165
00166
00167 void spinThread();
00168
00169
00170 void processFeedback( const FeedbackConstPtr& feedback );
00171
00172
00173 void keepAlive();
00174
00175
00176 void publish( visualization_msgs::InteractiveMarkerUpdate &update );
00177
00178
00179 void publishInit();
00180
00181
00182 void doSetPose( M_UpdateContext::iterator update_it,
00183 const std::string &name,
00184 const geometry_msgs::Pose &pose,
00185 const std_msgs::Header &header );
00186
00187
00188 M_MarkerContext marker_contexts_;
00189
00190
00191 M_UpdateContext pending_updates_;
00192
00193
00194 std::string topic_ns_;
00195
00196 mutable boost::recursive_mutex mutex_;
00197
00198
00199 boost::scoped_ptr<boost::thread> spin_thread_;
00200 ros::NodeHandle node_handle_;
00201 ros::CallbackQueue callback_queue_;
00202 volatile bool need_to_terminate_;
00203
00204
00205 ros::Timer keep_alive_timer_;
00206
00207 ros::Publisher init_pub_;
00208 ros::Publisher update_pub_;
00209 ros::Subscriber feedback_sub_;
00210
00211 uint64_t seq_num_;
00212
00213 std::string server_id_;
00214 };
00215
00216 }
00217
00218 #endif