srs_interaction_primitives: srs_interaction_primitives: bounding

00001 /******************************************************************************
00002  * \file
00003  *
00004  * $Id: BoundingBox.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: 27/11/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/bounding_box.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 BoundingBox::BoundingBox(InteractiveMarkerServerPtr server, string frame_id, string name) :
00040     Primitive(server, frame_id, name, srs_interaction_primitives::PrimitiveType::BOUNDING_BOX)
00041 {
00042   description_ = "";
00043 }
00044 
00045 void BoundingBox::bboxCallback(const InteractiveMarkerFeedbackConstPtr &feedback)
00046 {
00047   if (feedback->control_name != BOUNDING_BOX_CONTROL_NAME)
00048   {
00049     /*if ((feedback->event_type == InteractiveMarkerFeedback::MOUSE_UP))
00050      {
00051      InteractiveMarker o;
00052      if (server->get(name, o))
00053      {
00054      pose = o.pose;
00055      object.pose = pose;
00056      }
00057 
00058      ROS_INFO("New position:");
00059      cout << feedback->pose.position << endl;
00060      ROS_INFO("New orientation:");
00061      cout << feedback->pose.orientation << endl;
00062 
00063      // Interaction with object from Interactive Marker Server
00064      InteractiveMarker obj;
00065      if (server->get(attachedObjectName, obj))
00066      {
00067      server->erase(attachedObjectName);
00068      obj.pose = feedback->pose;
00069      server->insert(obj);
00070      }
00071 
00072      server->insert(object);
00073      menu_handler.reApply(*server);
00074      server->applyChanges();
00075      }*/
00076     defaultCallback(feedback);
00077   }
00078 }
00079 
00080 void BoundingBox::menuCallback(const InteractiveMarkerFeedbackConstPtr &feedback)
00081 {
00082   MenuHandler::EntryHandle handle = feedback->menu_entry_id;
00083   MenuHandler::CheckState state;
00084   string title;
00085   menu_handler_.getCheckState(handle, state);
00086   menu_handler_.getTitle(handle, title);
00087 
00088   updatePublisher_->publishMenuClicked(title, state);
00089 
00090  /* InteractiveMarker o;
00091   if (server_->get(name_, o))
00092   {
00093     pose_ = o.pose;
00094     object_.pose = pose_;
00095   }*/
00096 
00097   switch (feedback->menu_entry_id)
00098   {
00099     case 1:
00100       /*
00101        * Bounding box visibility
00102        */
00103       if (state == MenuHandler::CHECKED)
00104       {
00105         showBoundingBoxControl(false);
00106         menu_handler_.setCheckState(handle, MenuHandler::UNCHECKED);
00107       }
00108       else
00109       {
00110         showBoundingBoxControl(true);
00111         menu_handler_.setCheckState(handle, MenuHandler::CHECKED);
00112       }
00113       break;
00114     case 2:
00115       /*
00116        * Bounding box description
00117        */
00118       if (state == MenuHandler::CHECKED)
00119       {
00120         removeDescriptionControl();
00121         menu_handler_.setCheckState(handle, MenuHandler::UNCHECKED);
00122       }
00123       else
00124       {
00125         addDescriptionControl();
00126         menu_handler_.setCheckState(handle, MenuHandler::CHECKED);
00127       }
00128       break;
00129     case 3:
00130       /*
00131        * Bounding box measure
00132        */
00133       if (state == MenuHandler::CHECKED)
00134       {
00135         removeMeasureControl();
00136         menu_handler_.setCheckState(handle, MenuHandler::UNCHECKED);
00137       }
00138       else
00139       {
00140         addMeasureControl();
00141         menu_handler_.setCheckState(handle, MenuHandler::CHECKED);
00142       }
00143       break;
00144     case 5:
00145       /*
00146        * Movement controls
00147        */
00148       if (state == MenuHandler::CHECKED)
00149       {
00150         removeMovementControls();
00151         menu_handler_.setCheckState(handle, MenuHandler::UNCHECKED);
00152       }
00153       else
00154       {
00155         addMovementControls();
00156         menu_handler_.setCheckState(handle, MenuHandler::CHECKED);
00157       }
00158       break;
00159     case 6:
00160       /*
00161        * Rotation controls
00162        */
00163       if (state == MenuHandler::CHECKED)
00164       {
00165         removeRotationControls();
00166         menu_handler_.setCheckState(handle, MenuHandler::UNCHECKED);
00167       }
00168       else
00169       {
00170         addRotationControls();
00171         menu_handler_.setCheckState(handle, MenuHandler::CHECKED);
00172       }
00173       break;
00174     case 8:
00175       /*
00176        * Take object action
00177        */
00178       ROS_INFO("Take object");
00179       break;
00180     case 9:
00181       /*
00182        * Throw object action
00183        */
00184       ROS_INFO("Throw object");
00185       break;
00186   }
00187 
00188   server_->insert(object_);
00189   menu_handler_.reApply(*server_);
00190   server_->applyChanges();
00191 }
00192 
00193 void BoundingBox::createMenu()
00194 {
00195   if (!menu_created_)
00196   {
00197     menu_created_ = true;
00198     menu_handler_.setCheckState(
00199         menu_handler_.insert("Show Bounding Box", boost::bind(&BoundingBox::menuCallback, this, _1)),
00200         MenuHandler::CHECKED);
00201     menu_handler_.setCheckState(
00202         menu_handler_.insert("Show description", boost::bind(&BoundingBox::menuCallback, this, _1)),
00203         MenuHandler::UNCHECKED);
00204     menu_handler_.setCheckState(menu_handler_.insert("Show measure", boost::bind(&BoundingBox::menuCallback, this, _1)),
00205                                 MenuHandler::UNCHECKED);
00206 
00207     MenuHandler::EntryHandle sub_menu_handle = menu_handler_.insert("Interaction");
00208     menu_handler_.setCheckState(
00209         menu_handler_.insert(sub_menu_handle, "Movement", boost::bind(&BoundingBox::menuCallback, this, _1)),
00210         MenuHandler::UNCHECKED);
00211     menu_handler_.setCheckState(
00212         menu_handler_.insert(sub_menu_handle, "Rotation", boost::bind(&BoundingBox::menuCallback, this, _1)),
00213         MenuHandler::UNCHECKED);
00214 
00215     sub_menu_handle = menu_handler_.insert("Actions");
00216     menu_handler_.insert(sub_menu_handle, "Take object", boost::bind(&BoundingBox::menuCallback, this, _1));
00217     menu_handler_.insert(sub_menu_handle, "Throw object", boost::bind(&BoundingBox::menuCallback, this, _1));
00218   }
00219 }
00220 
00221 void BoundingBox::createBoundingBoxControl()
00222 {
00223   createBoundingBoxControl(0.0f, 0.0f, 0.0f);
00224 }
00225 
00226 void BoundingBox::createBoundingBoxControl(float trans_x, float trans_y, float trans_z)
00227 {
00228   Point p1, p2;
00229   double sx = scale_.x / 2;
00230   double sy = scale_.y / 2;
00231   double sz = scale_.z / 2;
00232 
00233   bounding_box_.type = Marker::CUBE;
00234   bounding_box_.pose.position.x = trans_x;
00235   bounding_box_.pose.position.y = trans_y;
00236   bounding_box_.pose.position.z = trans_z;
00237   bounding_box_.scale = scale_;
00238   bounding_box_.color = color_;
00239   bounding_box_.color.a = BBOX_MAX_ALPHA;
00240 
00241   wire_.points.clear();
00242 
00243   wire_.type = Marker::LINE_LIST;
00244   wire_.scale = scale_;
00245   wire_.color.r = color_.b;
00246   wire_.color.g = color_.r;
00247   wire_.color.b = color_.g;
00248   wire_.color.a = BBOX_MAX_ALPHA;
00249   wire_.scale.x = 0.002;
00250 
00251   p1.x = -sx + trans_x;
00252   p1.y = -sy + trans_y;
00253   p1.z = -sz + trans_z;
00254   p2.x = -sx + trans_x;
00255   p2.y = sy + trans_y;
00256   p2.z = -sz + trans_z;
00257   wire_.points.push_back(p1);
00258   wire_.points.push_back(p2);
00259   p2.x = -sx + trans_x;
00260   p2.y = -sy + trans_y;
00261   p2.z = sz + trans_z;
00262   wire_.points.push_back(p1);
00263   wire_.points.push_back(p2);
00264   p1.x = -sx + trans_x;
00265   p1.y = sy + trans_y;
00266   p1.z = sz + trans_z;
00267   p2.x = -sx + trans_x;
00268   p2.y = sy + trans_y;
00269   p2.z = -sz + trans_z;
00270   wire_.points.push_back(p1);
00271   wire_.points.push_back(p2);
00272   p2.x = -sx + trans_x;
00273   p2.y = -sy + trans_y;
00274   p2.z = sz + trans_z;
00275   wire_.points.push_back(p1);
00276   wire_.points.push_back(p2);
00277 
00278   p1.x = sx + trans_x;
00279   p1.y = -sy + trans_y;
00280   p1.z = -sz + trans_z;
00281   p2.x = sx + trans_x;
00282   p2.y = sy + trans_y;
00283   p2.z = -sz + trans_z;
00284   wire_.points.push_back(p1);
00285   wire_.points.push_back(p2);
00286   p2.x = sx + trans_x;
00287   p2.y = -sy + trans_y;
00288   p2.z = sz + trans_z;
00289   wire_.points.push_back(p1);
00290   wire_.points.push_back(p2);
00291   p1.x = sx + trans_x;
00292   p1.y = sy + trans_y;
00293   p1.z = sz + trans_z;
00294   p2.x = sx + trans_x;
00295   p2.y = sy + trans_y;
00296   p2.z = -sz + trans_z;
00297   wire_.points.push_back(p1);
00298   wire_.points.push_back(p2);
00299   p2.x = sx + trans_x;
00300   p2.y = -sy + trans_y;
00301   p2.z = sz + trans_z;
00302   wire_.points.push_back(p1);
00303   wire_.points.push_back(p2);
00304 
00305   p1.x = sx + trans_x;
00306   p1.y = sy + trans_y;
00307   p1.z = sz + trans_z;
00308   p2.x = -sx + trans_x;
00309   p2.y = sy + trans_y;
00310   p2.z = sz + trans_z;
00311   wire_.points.push_back(p1);
00312   wire_.points.push_back(p2);
00313 
00314   p1.x = sx + trans_x;
00315   p1.y = -sy + trans_y;
00316   p1.z = sz + trans_z;
00317   p2.x = -sx + trans_x;
00318   p2.y = -sy + trans_y;
00319   p2.z = sz + trans_z;
00320   wire_.points.push_back(p1);
00321   wire_.points.push_back(p2);
00322 
00323   p1.x = sx + trans_x;
00324   p1.y = sy + trans_y;
00325   p1.z = -sz + trans_z;
00326   p2.x = -sx + trans_x;
00327   p2.y = sy + trans_y;
00328   p2.z = -sz + trans_z;
00329   wire_.points.push_back(p1);
00330   wire_.points.push_back(p2);
00331 
00332   p1.x = sx + trans_x;
00333   p1.y = -sy + trans_y;
00334   p1.z = -sz + trans_z;
00335   p2.x = -sx + trans_x;
00336   p2.y = -sy + trans_y;
00337   p2.z = -sz + trans_z;
00338   wire_.points.push_back(p1);
00339   wire_.points.push_back(p2);
00340 
00341   control_.name = BOUNDING_BOX_CONTROL_NAME;
00342   control_.markers.push_back(bounding_box_);
00343   control_.markers.push_back(wire_);
00344   control_.interaction_mode = InteractiveMarkerControl::BUTTON;
00345   control_.always_visible = true;
00346 
00347   object_.controls.push_back(control_);
00348 }
00349 
00350 void BoundingBox::showBoundingBoxControl(bool show)
00351 {
00352   for (unsigned int i = 0; i < control_.markers.size(); i++)
00353     control_.markers[i].color.a = show ? BBOX_MAX_ALPHA : BBOX_MIN_ALPHA;
00354 
00355   removeControl(BOUNDING_BOX_CONTROL_NAME);
00356   object_.controls.push_back(control_);
00357 }
00358 
00359 void BoundingBox::create()
00360 {
00361   clearObject();
00362 
00363   object_.header.frame_id = frame_id_;
00364   object_.name = name_;
00365 //  object_.description = description_;
00366   object_.pose = pose_;
00367   object_.scale = srs_interaction_primitives::maxScale(scale_);
00368 
00369   createBoundingBoxControl();
00370 
00371   createMenu();
00372 }
00373 
00374 void BoundingBox::insert()
00375 {
00376   create();
00377 
00378   server_->insert(object_, boost::bind(&BoundingBox::bboxCallback, this, _1));
00379   menu_handler_.apply(*server_, name_);
00380 }
00381 
00382 }
bounding_box.cpp
