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_CLIENT
00033 #define INTERACTIVE_MARKER_CLIENT
00034
00035 #include <boost/shared_ptr.hpp>
00036 #include <boost/thread/mutex.hpp>
00037 #include <boost/function.hpp>
00038 #include <boost/unordered_map.hpp>
00039
00040 #include <string>
00041
00042 #include <ros/subscriber.h>
00043 #include <ros/node_handle.h>
00044
00045 #include <tf/tf.h>
00046
00047 #include <visualization_msgs/InteractiveMarkerInit.h>
00048 #include <visualization_msgs/InteractiveMarkerUpdate.h>
00049
00050 #include "detail/state_machine.h"
00051
00052 namespace interactive_markers
00053 {
00054
00055 class SingleClient;
00056
00067 class InteractiveMarkerClient : boost::noncopyable
00068 {
00069 public:
00070
00071 enum StatusT {
00072 OK = 0,
00073 WARN = 1,
00074 ERROR = 2
00075 };
00076
00077 typedef visualization_msgs::InteractiveMarkerUpdateConstPtr UpdateConstPtr;
00078 typedef visualization_msgs::InteractiveMarkerInitConstPtr InitConstPtr;
00079
00080 typedef boost::function< void ( const UpdateConstPtr& ) > UpdateCallback;
00081 typedef boost::function< void ( const InitConstPtr& ) > InitCallback;
00082 typedef boost::function< void ( const std::string& ) > ResetCallback;
00083 typedef boost::function< void ( StatusT, const std::string&, const std::string& ) > StatusCallback;
00084
00088 InteractiveMarkerClient( tf::Transformer& tf,
00089 const std::string& target_frame = "",
00090 const std::string &topic_ns = "" );
00091
00093 ~InteractiveMarkerClient();
00094
00096 void subscribe( std::string topic_ns );
00097
00099 void shutdown();
00100
00102 void update();
00103
00105 void setTargetFrame( std::string target_frame );
00106
00108 void setInitCb( const InitCallback& cb );
00109
00111 void setUpdateCb( const UpdateCallback& cb );
00112
00114 void setResetCb( const ResetCallback& cb );
00115
00117 void setStatusCb( const StatusCallback& cb );
00118
00119 void setEnableAutocompleteTransparency( bool enable ) { enable_autocomplete_transparency_ = enable;}
00120
00121 private:
00122
00123
00124 template<class MsgConstPtrT>
00125 void process( const MsgConstPtrT& msg );
00126
00127 ros::NodeHandle nh_;
00128
00129 enum StateT
00130 {
00131 IDLE,
00132 INIT,
00133 RUNNING
00134 };
00135
00136 StateMachine<StateT> state_;
00137
00138 std::string topic_ns_;
00139
00140 ros::Subscriber update_sub_;
00141 ros::Subscriber init_sub_;
00142
00143
00144 void subscribeInit();
00145
00146
00147 void subscribeUpdate();
00148
00149 void statusCb( StatusT status, const std::string& server_id, const std::string& msg );
00150
00151 typedef boost::shared_ptr<SingleClient> SingleClientPtr;
00152 typedef boost::unordered_map<std::string, SingleClientPtr> M_SingleClient;
00153 M_SingleClient publisher_contexts_;
00154 boost::mutex publisher_contexts_mutex_;
00155
00156 tf::Transformer& tf_;
00157 std::string target_frame_;
00158
00159 public:
00160
00161 struct CbCollection
00162 {
00163 void initCb( const InitConstPtr& i ) const {
00164 if (init_cb_) init_cb_( i ); }
00165 void updateCb( const UpdateConstPtr& u ) const {
00166 if (update_cb_) update_cb_( u ); }
00167 void resetCb( const std::string& s ) const {
00168 if (reset_cb_) reset_cb_(s); }
00169 void statusCb( StatusT s, const std::string& id, const std::string& m ) const {
00170 if (status_cb_) status_cb_(s,id,m); }
00171
00172 void setInitCb( InitCallback init_cb ) {
00173 init_cb_ = init_cb;
00174 }
00175 void setUpdateCb( UpdateCallback update_cb ) {
00176 update_cb_ = update_cb;
00177 }
00178 void setResetCb( ResetCallback reset_cb ) {
00179 reset_cb_ = reset_cb;
00180 }
00181 void setStatusCb( StatusCallback status_cb ) {
00182 status_cb_ = status_cb;
00183 }
00184
00185 private:
00186 InitCallback init_cb_;
00187 UpdateCallback update_cb_;
00188 ResetCallback reset_cb_;
00189 StatusCallback status_cb_;
00190 };
00191
00192
00193 void processInit( const InitConstPtr& msg );
00194
00195
00196 void processUpdate( const UpdateConstPtr& msg );
00197
00198 private:
00199 CbCollection callbacks_;
00200
00201
00202 StatusCallback status_cb_;
00203
00204
00205 int last_num_publishers_;
00206
00207
00208 bool enable_autocomplete_transparency_;
00209 };
00210
00211
00212
00213 }
00214
00215 #endif