void modeCb( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
{
menu_handler.setCheckState( h_mode_last, MenuHandler::UNCHECKED );
h_mode_last = feedback->menu_entry_id;
std::string namesss=feedback->control_name;
ROS_INFO("Name: %s", namesss.c_str());
ROS_INFO("last %i", h_mode_last);
menu_handler.setCheckState( h_mode_last, MenuHandler::CHECKED );
menu_handler.reApply( *server );
server->applyChanges();
}
void DroneRvizDisplay::initMenu(int i)
{
std::ostringstream s;
s << "Drone " << i;
h_mode_last = menu_handler.insert( sub_menu_handle, s.str(), &modeCb );
menu_handler.setCheckState( h_mode_last, MenuHandler::UNCHECKED );
h_mode_last = 2;
menu_handler.setCheckState( h_mode_last, MenuHandler::CHECKED ); //try
//
ROS_INFO("INIT");
//obstacles.ns="zero";
}