srs_interaction_primitives: srs_interaction_primitives: plane.cpp Source File

00001 /******************************************************************************
00002  * \file
00003  *
00004  * $Id: Plane.cpp 676 2012-04-19 18:32:07Z xlokaj03 $
00005  *
00006  * Copyright (C) Brno University of Technology
00007  *
00008  * This file is part of software developed by Robo@FIT group.
00009  *
00010  * Author: Tomas Lokaj (xlokaj03@stud.fit.vutbr.cz)
00011  * Supervised by: Michal Spanel (spanel@fit.vutbr.cz)
00012  * Date: 2/12/2011
00013  * 
00014  * This file is free software: you can redistribute it and/or modify
00015  * it under the terms of the GNU Lesser General Public License as published by
00016  * the Free Software Foundation, either version 3 of the License, or
00017  * (at your option) any later version.
00018  * 
00019  * This file is distributed in the hope that it will be useful,
00020  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00021  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00022  * GNU Lesser General Public License for more details.
00023  * 
00024  * You should have received a copy of the GNU Lesser General Public License
00025  * along with this file.  If not, see <http://www.gnu.org/licenses/>.
00026  */
00027 
00028 #include <srs_interaction_primitives/plane.h>
00029 
00030 using namespace std;
00031 using namespace interactive_markers;
00032 using namespace visualization_msgs;
00033 using namespace geometry_msgs;
00034 using namespace std_msgs;
00035 
00036 namespace srs_interaction_primitives
00037 {
00038 
00039 Plane::Plane(InteractiveMarkerServerPtr server, string frame_id, string name) :
00040     Primitive(server, frame_id, name, srs_interaction_primitives::PrimitiveType::PLANE)
00041 {
00042 }
00043 
00044 void Plane::menuCallback(const InteractiveMarkerFeedbackConstPtr &feedback)
00045 {
00046   MenuHandler::EntryHandle handle = feedback->menu_entry_id;
00047   MenuHandler::CheckState state;
00048   string title;
00049   menu_handler_.getCheckState(handle, state);
00050   menu_handler_.getTitle(handle, title);
00051 
00052   updatePublisher_->publishMenuClicked(title, state);
00053 
00054   switch (feedback->menu_entry_id)
00055   {
00056     case 1:
00057       /*
00058        * Plane tag description
00059        */
00060       if (state == MenuHandler::CHECKED)
00061       {
00062         removeDescriptionControl();
00063         menu_handler_.setCheckState(handle, MenuHandler::UNCHECKED);
00064       }
00065       else
00066       {
00067         description_ = tag_;
00068         addDescriptionControl();
00069         menu_handler_.setCheckState(handle, MenuHandler::CHECKED);
00070       }
00071       break;
00072     case 3:
00073       /*
00074        * Plane tag
00075        */
00076       tag_ = "Unknown";
00077       updatePublisher_->publishTagChanged(tag_);
00078       if (state == MenuHandler::UNCHECKED)
00079       {
00080         menu_handler_.setCheckState(3, MenuHandler::CHECKED);
00081         menu_handler_.setCheckState(4, MenuHandler::UNCHECKED);
00082         menu_handler_.setCheckState(5, MenuHandler::UNCHECKED);
00083         menu_handler_.setCheckState(6, MenuHandler::UNCHECKED);
00084         if (show_description_control_)
00085         {
00086           removeDescriptionControl();
00087           description_ = tag_;
00088           addDescriptionControl();
00089         }
00090       }
00091       break;
00092     case 4:
00093       /*
00094        * Plane tag
00095        */
00096       tag_ = "Wall";
00097       updatePublisher_->publishTagChanged(tag_);
00098       if (state == MenuHandler::UNCHECKED)
00099       {
00100         menu_handler_.setCheckState(3, MenuHandler::UNCHECKED);
00101         menu_handler_.setCheckState(4, MenuHandler::CHECKED);
00102         menu_handler_.setCheckState(5, MenuHandler::UNCHECKED);
00103         menu_handler_.setCheckState(6, MenuHandler::UNCHECKED);
00104         if (show_description_control_)
00105         {
00106           removeDescriptionControl();
00107           description_ = tag_;
00108           addDescriptionControl();
00109         }
00110       }
00111       break;
00112     case 5:
00113       /*
00114        * Plane tag
00115        */
00116       tag_ = "Door";
00117       updatePublisher_->publishTagChanged(tag_);
00118       if (state == MenuHandler::UNCHECKED)
00119       {
00120         menu_handler_.setCheckState(3, MenuHandler::UNCHECKED);
00121         menu_handler_.setCheckState(4, MenuHandler::UNCHECKED);
00122         menu_handler_.setCheckState(5, MenuHandler::CHECKED);
00123         menu_handler_.setCheckState(6, MenuHandler::UNCHECKED);
00124         if (show_description_control_)
00125         {
00126           removeDescriptionControl();
00127           description_ = tag_;
00128           addDescriptionControl();
00129         }
00130       }
00131       break;
00132     case 6:
00133       /*
00134        * Plane tag
00135        */
00136       tag_ = "Table desk";
00137       updatePublisher_->publishTagChanged(tag_);
00138       if (state == MenuHandler::UNCHECKED)
00139       {
00140         menu_handler_.setCheckState(3, MenuHandler::UNCHECKED);
00141         menu_handler_.setCheckState(4, MenuHandler::UNCHECKED);
00142         menu_handler_.setCheckState(5, MenuHandler::UNCHECKED);
00143         menu_handler_.setCheckState(6, MenuHandler::CHECKED);
00144         if (show_description_control_)
00145         {
00146           removeDescriptionControl();
00147           description_ = tag_;
00148           addDescriptionControl();
00149         }
00150       }
00151       break;
00152   }
00153 
00154   server_->insert(object_);
00155   menu_handler_.reApply(*server_);
00156   server_->applyChanges();
00157 }
00158 
00159 void Plane::createMenu()
00160 {
00161   if (!menu_created_)
00162   {
00163     menu_created_ = true;
00164     menu_handler_.setCheckState(menu_handler_.insert("Show description", boost::bind(&Plane::menuCallback, this, _1)),
00165                                 MenuHandler::UNCHECKED);
00166     MenuHandler::EntryHandle sub_menu_handle = menu_handler_.insert("Tag");
00167     menu_handler_.setCheckState(
00168         menu_handler_.insert(sub_menu_handle, "Unknown", boost::bind(&Plane::menuCallback, this, _1)),
00169         MenuHandler::UNCHECKED);
00170     menu_handler_.setCheckState(
00171         menu_handler_.insert(sub_menu_handle, "Wall", boost::bind(&Plane::menuCallback, this, _1)),
00172         MenuHandler::UNCHECKED);
00173     menu_handler_.setCheckState(
00174         menu_handler_.insert(sub_menu_handle, "Door", boost::bind(&Plane::menuCallback, this, _1)),
00175         MenuHandler::UNCHECKED);
00176     menu_handler_.setCheckState(
00177         menu_handler_.insert(sub_menu_handle, "Table desk", boost::bind(&Plane::menuCallback, this, _1)),
00178         MenuHandler::UNCHECKED);
00179   }
00180 }
00181 
00182 void Plane::create()
00183 {
00184   clearObject();
00185 
00186   object_.header.frame_id = frame_id_;
00187   object_.name = name_;
00188 //  object_.description = name_ + " plane";
00189   object_.pose = pose_;
00190 
00191   mesh_.type = Marker::CUBE;
00192   mesh_.color = color_;
00193   mesh_.scale = scale_;
00194   mesh_.scale.z = 0.001;
00195 
00196   control_.always_visible = true;
00197   control_.interaction_mode = InteractiveMarkerControl::BUTTON;
00198   control_.markers.push_back(mesh_);
00199   object_.controls.push_back(control_);
00200 
00201   createMenu();
00202 }
00203 
00204 void Plane::insert()
00205 {
00206   create();
00207 
00208   server_->insert(object_);
00209   menu_handler_.apply(*server_, name_);
00210 }
00211 
00212 }