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