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 #include "interactive_markers/menu_handler.h"
00033
00034 #include <boost/bind.hpp>
00035 #include <boost/make_shared.hpp>
00036
00037 namespace interactive_markers
00038 {
00039
00040 MenuHandler::MenuHandler() :
00041 current_handle_(1)
00042 {
00043
00044 }
00045
00046 MenuHandler::EntryHandle MenuHandler::insert( const std::string &title,
00047 const FeedbackCallback &feedback_cb )
00048 {
00049 EntryHandle handle = doInsert( title, visualization_msgs::MenuEntry::FEEDBACK, "", feedback_cb );
00050 top_level_handles_.push_back( handle );
00051 return handle;
00052 }
00053
00054 MenuHandler::EntryHandle MenuHandler::insert( const std::string &title,
00055 const uint8_t command_type,
00056 const std::string &command )
00057 {
00058 EntryHandle handle = doInsert( title, command_type, command, FeedbackCallback() );
00059 top_level_handles_.push_back( handle );
00060 return handle;
00061 }
00062
00063
00064 MenuHandler::EntryHandle MenuHandler::insert( EntryHandle parent, const std::string &title,
00065 const FeedbackCallback &feedback_cb )
00066 {
00067 boost::unordered_map<EntryHandle, EntryContext>::iterator parent_context =
00068 entry_contexts_.find( parent );
00069
00070 ROS_ASSERT_MSG ( parent_context != entry_contexts_.end(), "Parent menu entry %u not found.", parent );
00071
00072 EntryHandle handle = doInsert( title, visualization_msgs::MenuEntry::FEEDBACK, "", feedback_cb );
00073 parent_context->second.sub_entries.push_back( handle );
00074 return handle;
00075 }
00076
00077
00078 MenuHandler::EntryHandle MenuHandler::insert( EntryHandle parent, const std::string &title,
00079 const uint8_t command_type,
00080 const std::string &command )
00081 {
00082 boost::unordered_map<EntryHandle, EntryContext>::iterator parent_context =
00083 entry_contexts_.find( parent );
00084
00085 ROS_ASSERT_MSG ( parent_context != entry_contexts_.end(), "Parent menu entry %u not found.", parent );
00086
00087 EntryHandle handle = doInsert( title, command_type, command, FeedbackCallback() );
00088 parent_context->second.sub_entries.push_back( handle );
00089 return handle;
00090 }
00091
00092
00093 bool MenuHandler::setVisible( EntryHandle handle, bool visible )
00094 {
00095 boost::unordered_map<EntryHandle, EntryContext>::iterator context =
00096 entry_contexts_.find( handle );
00097
00098 if ( context == entry_contexts_.end() )
00099 {
00100 return false;
00101 }
00102
00103 context->second.visible = visible;
00104 return true;
00105 }
00106
00107
00108 bool MenuHandler::setCheckState( EntryHandle handle, CheckState check_state )
00109 {
00110 boost::unordered_map<EntryHandle, EntryContext>::iterator context =
00111 entry_contexts_.find( handle );
00112
00113 if ( context == entry_contexts_.end() )
00114 {
00115 return false;
00116 }
00117
00118 context->second.check_state = check_state;
00119 return true;
00120 }
00121
00122
00123 bool MenuHandler::getCheckState( EntryHandle handle, CheckState &check_state ) const
00124 {
00125 boost::unordered_map<EntryHandle, EntryContext>::const_iterator context =
00126 entry_contexts_.find( handle );
00127
00128 if ( context == entry_contexts_.end() )
00129 {
00130 check_state = NO_CHECKBOX;
00131 return false;
00132 }
00133
00134 check_state = context->second.check_state;
00135 return true;
00136 }
00137
00138
00139 bool MenuHandler::apply( InteractiveMarkerServer &server, const std::string &marker_name )
00140 {
00141 visualization_msgs::InteractiveMarker int_marker;
00142
00143 if ( !server.get( marker_name, int_marker ) )
00144 {
00145
00146 managed_markers_.erase( marker_name );
00147 return false;
00148 }
00149
00150 int_marker.menu_entries.clear();
00151
00152 pushMenuEntries( top_level_handles_, int_marker.menu_entries, 0 );
00153
00154 server.insert( int_marker );
00155 server.setCallback( marker_name, boost::bind( &MenuHandler::processFeedback, this, _1 ), visualization_msgs::InteractiveMarkerFeedback::MENU_SELECT );
00156 managed_markers_.insert( marker_name );
00157 return true;
00158 }
00159
00160 bool MenuHandler::pushMenuEntries( std::vector<EntryHandle>& handles_in,
00161 std::vector<visualization_msgs::MenuEntry>& entries_out,
00162 EntryHandle parent_handle )
00163 {
00164 for ( unsigned t = 0; t < handles_in.size(); t++ )
00165 {
00166 EntryHandle handle = handles_in[t];
00167 boost::unordered_map<EntryHandle, EntryContext>::iterator context_it =
00168 entry_contexts_.find( handle );
00169
00170 if ( context_it == entry_contexts_.end() )
00171 {
00172 ROS_ERROR( "Internal error: context handle not found! This is a bug in MenuHandler." );
00173 return false;
00174 }
00175
00176 EntryContext& context = context_it->second;
00177
00178 if ( !context.visible )
00179 {
00180 continue;
00181 }
00182
00183 entries_out.push_back( makeEntry( context, handle, parent_handle ));
00184 if( false == pushMenuEntries( context.sub_entries, entries_out, handle ))
00185 {
00186 return false;
00187 }
00188 }
00189 return true;
00190 }
00191
00192 bool MenuHandler::reApply( InteractiveMarkerServer &server )
00193 {
00194 bool success = true;
00195 std::set<std::string>::iterator it = managed_markers_.begin();
00196 while ( it != managed_markers_.end() )
00197 {
00198
00199
00200 std::set<std::string>::iterator next_it = it;
00201 next_it++;
00202 success = apply( server, *it ) && success;
00203 it = next_it;
00204 }
00205 return success;
00206 }
00207
00208 MenuHandler::EntryHandle MenuHandler::doInsert( const std::string &title,
00209 const uint8_t command_type,
00210 const std::string &command,
00211 const FeedbackCallback &feedback_cb )
00212 {
00213 EntryHandle handle = current_handle_;
00214 current_handle_++;
00215
00216 EntryContext context;
00217 context.title = title;
00218 context.command = command;
00219 context.command_type = command_type;
00220 context.visible = true;
00221 context.check_state = NO_CHECKBOX;
00222 context.feedback_cb = feedback_cb;
00223
00224 entry_contexts_[handle] = context;
00225 return handle;
00226 }
00227
00228 visualization_msgs::MenuEntry MenuHandler::makeEntry( EntryContext& context, EntryHandle handle, EntryHandle parent_handle )
00229 {
00230 visualization_msgs::MenuEntry menu_entry;
00231
00232 switch ( context.check_state )
00233 {
00234 case NO_CHECKBOX:
00235 menu_entry.title = context.title;
00236 break;
00237 case CHECKED:
00238 menu_entry.title = "[x] "+context.title;
00239 break;
00240 case UNCHECKED:
00241 menu_entry.title = "[ ] "+context.title;
00242 break;
00243 }
00244
00245 menu_entry.command = context.command;
00246 menu_entry.command_type = context.command_type;
00247 menu_entry.id = handle;
00248 menu_entry.parent_id = parent_handle;
00249
00250 return menu_entry;
00251 }
00252
00253
00254 void MenuHandler::processFeedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
00255 {
00256 boost::unordered_map<EntryHandle, EntryContext>::iterator context =
00257 entry_contexts_.find( (EntryHandle) feedback->menu_entry_id );
00258
00259 if ( context != entry_contexts_.end() && context->second.feedback_cb )
00260 {
00261 context->second.feedback_cb( feedback );
00262 }
00263 }
00264
00265 bool MenuHandler::getTitle( EntryHandle handle, std::string &title ) const
00266 {
00267 boost::unordered_map<EntryHandle, EntryContext>::const_iterator context =
00268 entry_contexts_.find( handle );
00269
00270 if ( context == entry_contexts_.end() )
00271 {
00272 return false;
00273 }
00274
00275 title = context->second.title;
00276 return true;
00277 }
00278
00279
00280
00281 }