cob_3d_visualization: cob_3d_visualization: shape

00001 /****************************************************************
00002  *
00003  * Copyright (c) 2010
00004  *
00005  * Fraunhofer Institute for Manufacturing Engineering
00006  * and Automation (IPA)
00007  *
00008  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00009  *
00010  * Project name: care-o-bot
00011  * ROS stack name: cob_environment_perception_intern
00012  * ROS package name: cob_3d_mapping_common
00013  * Description:
00014  *
00015  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00016  *
00017  * Author: Georg Arbeiter, email:georg.arbeiter@ipa.fhg.de
00018  * Supervised by: Georg Arbeiter, email:georg.arbeiter@ipa.fhg.de
00019  *
00020  * Date of creation: 09/2012
00021  * ToDo:
00022  *
00023  *
00024  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00025  *
00026  * Redistribution and use in source and binary forms, with or without
00027  * modification, are permitted provided that the following conditions are met:
00028  *
00029  * * Redistributions of source code must retain the above copyright
00030  * notice, this list of conditions and the following disclaimer.
00031  * * Redistributions in binary form must reproduce the above copyright
00032  * notice, this list of conditions and the following disclaimer in the
00033  * documentation and/or other materials provided with the distribution.
00034  * * Neither the name of the Fraunhofer Institute for Manufacturing
00035  * Engineering and Automation (IPA) nor the names of its
00036  * contributors may be used to endorse or promote products derived from
00037  * this software without specific prior written permission.
00038  *
00039  * This program is free software: you can redistribute it and/or modify
00040  * it under the terms of the GNU Lesser General Public License LGPL as
00041  * published by the Free Software Foundation, either version 3 of the
00042  * License, or (at your option) any later version.
00043  *
00044  * This program is distributed in the hope that it will be useful,
00045  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00046  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
00047  * GNU Lesser General Public License LGPL for more details.
00048  *
00049  * You should have received a copy of the GNU Lesser General Public
00050  * License LGPL along with this program.
00051  * If not, see <http://www.gnu.org/licenses/>.
00052  *
00053  ****************************************************************/
00054 
00055 //##################
00056 //#### includes ####
00057 
00058 #include <cob_3d_visualization/shape_marker.h>
00059 
00060 ShapeMarker::ShapeMarker(boost::shared_ptr<interactive_markers::InteractiveMarkerServer> im_server,
00061     cob_3d_mapping_msgs::Shape& shape,std::vector<unsigned int>& moved_shapes_indices,std::vector<unsigned int>& interacted_shapes,
00062     std::vector<unsigned int>& deleted_markers_indices, bool arrows,bool deleted) ://, unsigned int& deleted) :
00063     interacted_shapes_(interacted_shapes) , moved_shapes_indices_(moved_shapes_indices) , deleted_markers_indices_(deleted_markers_indices)
00064 {
00065   arrows_ = arrows ;
00066   deleted_ = deleted ;
00067   im_server_ = im_server;
00068   shape_ = shape;
00069   id_ = shape.id;
00070   createShapeMenu ();
00071   createInteractiveMarker();
00072 }
00073 
00074 void
00075 ShapeMarker::triangle_refinement(list<TPPLPoly>& i_list,list<TPPLPoly>& o_list){
00076   int n_circle = 20;
00077 
00078   TPPLPoly tri_new,tri_temp;
00079   TPPLPoint ptM,ptM01,ptM12,ptM20;
00080   for (std::list<TPPLPoly>::iterator it = i_list.begin (); it != i_list.end (); it++){
00081     int n[4]={0,1,2,0};
00082 
00083     ptM.x =(it->GetPoint(n[0]).x+it->GetPoint(n[1]).x+it->GetPoint(n[2]).x)/3;
00084     ptM.y =(it->GetPoint(n[0]).y+it->GetPoint(n[1]).y+it->GetPoint(n[2]).y)/3;
00085 
00086     ptM01.x=(it->GetPoint(n[0]).x+it->GetPoint(n[1]).x)/2;
00087     ptM01.y=(it->GetPoint(n[0]).y+it->GetPoint(n[1]).y)/2;
00088 
00089     ptM12.x=(it->GetPoint(n[1]).x+it->GetPoint(n[2]).x)/2;
00090     ptM12.y=(it->GetPoint(n[1]).y+it->GetPoint(n[2]).y)/2;
00091 
00092     ptM20.x=(it->GetPoint(n[2]).x+it->GetPoint(n[3]).x)/2;
00093     ptM20.y=(it->GetPoint(n[2]).y+it->GetPoint(n[3]).y)/2;
00094 
00095     tri_temp.Triangle(ptM01,ptM12,ptM20);
00096 
00097 
00098     double thresh = shape_.params[9]/6;
00099     if(fabs(it->GetPoint(n[0]).x-ptM.x)>thresh || fabs(it->GetPoint(n[1]).x-ptM.x)>thresh || fabs(it->GetPoint(n[2]).x-ptM.x)>thresh){
00100       //for every old triangle 6! new triangles are created
00101       for (long i = 0; i < it->GetNumPoints (); i++){
00102         tri_new.Triangle(tri_temp.GetPoint(n[i]),ptM,it->GetPoint(n[i]));
00103         //push new triangle in trinagle list
00104         o_list.push_back(tri_new);
00105         tri_new.Triangle(tri_temp.GetPoint(n[i]),it->GetPoint(n[i+1]),ptM);
00106         //push new triangle in trinagle list
00107         o_list.push_back(tri_new);
00108       }
00109     }
00110     else{
00111       tri_new.Triangle(it->GetPoint(n[0]),it->GetPoint(n[1]),it->GetPoint(n[2]));
00112       o_list.push_back(tri_new);
00113     }
00114   }
00115 }
00116 
00117 void ShapeMarker::getShape (cob_3d_mapping_msgs::Shape& shape) {
00118   shape_ = shape ;
00119 }
00120 
00121 bool ShapeMarker::getArrows (){
00122   return arrows_ ;
00123 }
00124 
00125 bool ShapeMarker::setArrows (){
00126   arrows_ = false ;
00127   return arrows_ ;
00128 }
00129 
00130 bool ShapeMarker::getDeleted (){
00131   return deleted_ ;
00132 }
00133 
00134 bool ShapeMarker::setDeleted (){
00135   deleted_ = false ;
00136   return deleted_ ;
00137 }
00138 
00139 unsigned int ShapeMarker::getID(){
00140   return id_;
00141 }
00142 
00143 void ShapeMarker::deleteMarker(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback) {
00144 
00145   deleted_ = true ;
00146 
00147   stringstream ss;
00148   ss << shape_.id ;// ctr_ ;
00149   deleted_markers_indices_.push_back(shape_.id) ;
00150 
00151   visualization_msgs::InteractiveMarker interactiveMarker;
00152   interactiveMarker.name = ss.str() ;
00153 
00154   std::cout << "Marker" << interactiveMarker.name << " deleted..."<< std::endl ;
00155 
00156 
00157   im_server_->erase(ss.str());
00158   im_server_->applyChanges ();
00159 
00160   // create a transparent interactive marker
00161   createInteractiveMarker () ;
00162   im_server_->applyChanges ();
00163 
00164 }
00165 
00166 
00167 void ShapeMarker::enableMovement (const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
00168 {
00169   interactive_markers::MenuHandler::CheckState check_state;
00170   menu_handler_.getCheckState (feedback->menu_entry_id, check_state);
00171 
00172   shape_.header = marker_.header ;
00173   shape_.header.frame_id = "/map" ;
00174 
00175   if (check_state == interactive_markers::MenuHandler::UNCHECKED)
00176   {
00177     //    im_server_->setCallback(marker_.name,boost::bind (&ShapeMarker::setShapePosition, this, _1)
00178     //                              ,visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE);
00179     menu_handler_.setCheckState (feedback->menu_entry_id, interactive_markers::MenuHandler::CHECKED);
00180     displayArrows();
00181   }
00182   else if (check_state == interactive_markers::MenuHandler::CHECKED)
00183   {
00184     menu_handler_.setCheckState (feedback->menu_entry_id, interactive_markers::MenuHandler::UNCHECKED);
00185     hideArrows(1);
00186   }
00187   menu_handler_.reApply (*im_server_);
00188   im_server_->applyChanges();
00189 }
00190 
00191 
00192 void ShapeMarker::displayArrows()
00193 {
00194   arrows_ = true ;
00195   moved_shapes_indices_.push_back(shape_.id) ;
00196 
00197   /*Creating a transparent marker at the original position*/
00198   createInteractiveMarker () ;
00199   
00200 
00201   visualization_msgs::InteractiveMarkerControl im_ctrl;
00202 
00203   //    stringstream ss;
00204   //    ss.str("");
00205   //    ss.clear();
00206 
00207   //  ss << "arrows_" << shape_.id;
00208   //  marker_.name = ss.str() ;
00209 
00210   ROS_INFO("Adding the arrows... ");
00211   im_ctrl.name = "arrow_markers" ;
00212 
00213   im_ctrl.orientation.w = 1;
00214   im_ctrl.orientation.x = 1;
00215   im_ctrl.orientation.y = 0;
00216   im_ctrl.orientation.z = 0;
00217   im_ctrl.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
00218   marker_.controls.push_back (im_ctrl);
00219   im_ctrl.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00220   marker_.controls.push_back (im_ctrl);
00221 
00222   im_ctrl.orientation.w = 1;
00223   im_ctrl.orientation.x = 0;
00224   im_ctrl.orientation.y = 1;
00225   im_ctrl.orientation.z = 0;
00226   im_ctrl.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
00227   marker_.controls.push_back (im_ctrl);
00228   im_ctrl.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00229   marker_.controls.push_back (im_ctrl);
00230 
00231   im_ctrl.orientation.w = 1;
00232   im_ctrl.orientation.x = 0;
00233   im_ctrl.orientation.y = 0;
00234   im_ctrl.orientation.z = 1;
00235   im_ctrl.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
00236   marker_.controls.push_back (im_ctrl);
00237   im_ctrl.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_AXIS;
00238   marker_.controls.push_back (im_ctrl);
00239 
00240 
00241   im_server_->insert (marker_);
00242   im_server_->applyChanges() ;
00243 
00244 
00245 }
00246 
00247 
00248 void ShapeMarker::hideArrows(int untick)
00249 
00250 {
00251   arrows_ = false ;
00252   stringstream ss;
00253   stringstream aa;
00254   std::vector<unsigned int>::iterator iter;
00255 
00257   if(marker_.controls.size()>1){
00258     ROS_INFO ("Deleting the Arrows ...") ;
00259     marker_.controls.erase(marker_.controls.end()-6,marker_.controls.end()) ;
00260     im_server_->insert(marker_) ;
00261     //    im_server_->applyChanges() ;
00262 
00263     if (!untick){ // when the ResetAll option is used
00264       menu_handler_.setCheckState (5, interactive_markers::MenuHandler::UNCHECKED);
00265       menu_handler_.reApply (*im_server_);
00266       //      im_server_->applyChanges() ;
00267     }
00268 
00270     ss.str("");
00271     ss.clear();
00272     ss << "second_marker_"<< shape_.id;
00273     im_server_->erase(ss.str());
00274 
00275     im_server_->applyChanges() ;
00277   }
00278   if (untick){
00279     iter = find (moved_shapes_indices_.begin(), moved_shapes_indices_.end(), shape_.id) ;
00280     if (iter!=moved_shapes_indices_.end()){
00281       moved_shapes_indices_.erase(moved_shapes_indices_.begin()+(iter-moved_shapes_indices_.begin())) ;
00282     }
00283   }
00284 }
00285 
00286 void ShapeMarker::resetMarker(){
00287 
00288   stringstream aa;
00289   stringstream ss;
00290 
00291   hideNormal(0);
00292   hideCentroid(0);
00293   hideContour(0);
00294   if (shape_.type == cob_3d_mapping_msgs::Shape::CYLINDER){
00295     hideSymAxis(0);
00296     hideOrigin(0);
00297   }
00298 }
00299 
00300   //  interacted_shapes_.pop_back() ;
00301 
00302 void
00303 ShapeMarker::createShapeMenu ()
00304 {
00305   //  ROS_INFO(" creating menu .....");
00306 
00307   interactive_markers::MenuHandler::EntryHandle eh_1, eh_2, eh_3, eh_4, eh_5, eh_6;
00308 
00309   eh_1 = menu_handler_.insert ("Menu");
00310   eh_2 = menu_handler_.insert (eh_1, "Display Normal",boost::bind (&ShapeMarker::displayNormalCB, this, _1));
00311   eh_3 = menu_handler_.insert (eh_1, "Display Centroid",boost::bind (&ShapeMarker::displayCentroidCB, this, _1));
00312   eh_4 = menu_handler_.insert (eh_1, "Display Contour",boost::bind (&ShapeMarker::displayContourCB, this, _1));
00313   eh_5 = menu_handler_.insert (eh_1, "Enable Movement",boost::bind (&ShapeMarker::enableMovement, this, _1));
00314   eh_6 = menu_handler_.insert (eh_1, "Delete Marker",boost::bind (&ShapeMarker::deleteMarker, this, _1));
00315   //    eh_6 = menu_handler_.insert (eh_1, "Fix to this Position",boost::bind (&ShapeMarker::setShapePosition, this, _1));
00316 
00317   menu_handler_.setVisible (eh_1, true);
00318   menu_handler_.setCheckState (eh_1, interactive_markers::MenuHandler::NO_CHECKBOX);
00319 
00320   menu_handler_.setVisible (eh_2, true);
00321   menu_handler_.setCheckState (eh_2, interactive_markers::MenuHandler::UNCHECKED);
00322 
00323   menu_handler_.setVisible (eh_3, true);
00324   menu_handler_.setCheckState (eh_3, interactive_markers::MenuHandler::UNCHECKED);
00325 
00326   menu_handler_.setVisible (eh_4, true);
00327   menu_handler_.setCheckState (eh_4, interactive_markers::MenuHandler::UNCHECKED);
00328 
00329   menu_handler_.setVisible (eh_5, true);
00330   menu_handler_.setCheckState (eh_5, interactive_markers::MenuHandler::UNCHECKED);
00331 
00332   menu_handler_.setVisible (eh_6, true);
00333   menu_handler_.setCheckState (eh_6, interactive_markers::MenuHandler::NO_CHECKBOX);
00334 
00335 
00336   if(shape_.type==cob_3d_mapping_msgs::Shape::CYLINDER){
00337     interactive_markers::MenuHandler::EntryHandle eh_7,eh_8;
00338 
00339     eh_7 = menu_handler_.insert (eh_1, "Show Symmetry Axis",boost::bind (&ShapeMarker::displaySymAxisCB, this, _1));
00340     menu_handler_.setVisible (eh_7, true);
00341     menu_handler_.setCheckState (eh_7, interactive_markers::MenuHandler::UNCHECKED);
00342 
00343     eh_8 = menu_handler_.insert (eh_1, "Show Cylinder Origin",boost::bind (&ShapeMarker::displayOriginCB, this, _1));
00344     menu_handler_.setVisible (eh_8,true);
00345     menu_handler_.setCheckState (eh_8, interactive_markers::MenuHandler::UNCHECKED);
00346   }
00347 }
00348 
00349 void
00350 ShapeMarker::createMarker (visualization_msgs::InteractiveMarkerControl& im_ctrl)
00351 {
00352   /* transform shape points to 2d and store 2d point in triangle list */
00353   TPPLPartition pp;
00354   list<TPPLPoly> polys, tri_list;
00355 
00356   Eigen::Vector3f v, normal, origin;
00357 
00358   if(shape_.type== cob_3d_mapping_msgs::Shape::CYLINDER)
00359   {
00360     cob_3d_mapping::Cylinder c;
00361     cob_3d_mapping::fromROSMsg (shape_, c);
00362     c.ParamsFromShapeMsg();
00363     // make trinagulated cylinder strip
00364     //transform cylinder in local coordinate system
00365     c.makeCyl2D();
00366     c.TransformContours(c.transform_from_world_to_plane);
00367     //c.transform2tf(c.transform_from_world_to_plane);
00368     //TODO: WATCH OUT NO HANDLING FOR MULTY CONTOUR CYLINDERS AND HOLES
00369     TPPLPoly poly;
00370     TPPLPoint pt;
00371 
00372 
00373     for(size_t j=0;j<c.contours.size();j++){
00374 
00375       poly.Init(c.contours[j].size());
00376       poly.SetHole (shape_.holes[j]);
00377 
00378 
00379       for(size_t i=0;i<c.contours[j].size();++i){
00380 
00381         pt.x=c.contours[j][i][0];
00382         pt.y=c.contours[j][i][1];
00383 
00384         poly[i]=pt;
00385 
00386       }
00387       if (shape_.holes[j])
00388         poly.SetOrientation (TPPL_CW);
00389       else
00390         poly.SetOrientation (TPPL_CCW);
00391       polys.push_back(poly);
00392     }
00393     // triangualtion itno monotone triangles
00394     pp.Triangulate_EC (&polys, &tri_list);
00395 
00396     transformation_inv_ = c.transform_from_world_to_plane.inverse();
00397     // optional refinement step
00398     list<TPPLPoly> refined_tri_list;
00399     triangle_refinement(tri_list,refined_tri_list);
00400     tri_list=refined_tri_list;
00401 
00402   }
00403   if(shape_.type== cob_3d_mapping_msgs::Shape::POLYGON)
00404   {
00405     cob_3d_mapping::Polygon p;
00406 
00407     if (shape_.params.size () == 4)
00408     {
00409       cob_3d_mapping::fromROSMsg (shape_, p);
00410       normal (0) = shape_.params[0];
00411       normal (1) = shape_.params[1];
00412       normal (2) = shape_.params[2];
00413       origin (0) = shape_.centroid.x;
00414       origin (1) = shape_.centroid.y;
00415       origin (2) = shape_.centroid.z;
00416       v = normal.unitOrthogonal ();
00417 
00418       pcl::getTransformationFromTwoUnitVectorsAndOrigin (v, normal, origin, transformation_);
00419       transformation_inv_ = transformation_.inverse ();
00420     }
00421 
00422     for (size_t i = 0; i < shape_.points.size (); i++)
00423     {
00424       pcl::PointCloud<pcl::PointXYZ> pc;
00425       TPPLPoly poly;
00426       pcl::fromROSMsg (shape_.points[i], pc);
00427       poly.Init (pc.points.size ());
00428       poly.SetHole (shape_.holes[i]);
00429 
00430       for (size_t j = 0; j < pc.points.size (); j++)
00431       {
00432         poly[j] = msgToPoint2D (pc[j]);
00433       }
00434       if (shape_.holes[i])
00435         poly.SetOrientation (TPPL_CW);
00436       else
00437         poly.SetOrientation (TPPL_CCW);
00438 
00439       polys.push_back (poly);
00440     }
00441     pp.Triangulate_EC (&polys, &tri_list);
00442 
00443   }//Polygon
00444 
00445   //ROS_INFO(" creating markers for this shape.....");
00446   marker.id = shape_.id;
00447 
00448   marker.header = shape_.header;
00449   //marker.header.stamp = ros::Time::now() ;
00450 
00451   marker.type = visualization_msgs::Marker::TRIANGLE_LIST;
00452   marker.ns = "shape visualization";
00453   marker.action = visualization_msgs::Marker::ADD;
00454   marker.lifetime = ros::Duration ();
00455 
00456   //set color
00457   marker.color.g = shape_.color.g;
00458   marker.color.b = shape_.color.b;
00459   marker.color.r = shape_.color.r;
00460   if (arrows_ || deleted_){
00461     marker.color.a = 0.5;
00462   }
00463   else
00464   {
00465     marker.color.a = shape_.color.a;
00466     //      marker.color.r = shape_.color.r;
00467   }
00468 
00469   //set scale
00470   marker.scale.x = 1;
00471   marker.scale.y = 1;
00472   marker.scale.z = 1;
00473 
00474   //set pose
00475   Eigen::Quaternionf quat (transformation_inv_.rotation ());
00476   Eigen::Vector3f trans (transformation_inv_.translation ());
00477 
00478   marker.pose.position.x = trans (0);
00479   marker.pose.position.y = trans (1);
00480   marker.pose.position.z = trans (2);
00481 
00482   marker.pose.orientation.x = quat.x ();
00483   marker.pose.orientation.y = quat.y ();
00484   marker.pose.orientation.z = quat.z ();
00485   marker.pose.orientation.w = quat.w ();
00486   marker.points.resize (/*it->GetNumPoints ()*/tri_list.size()*3);
00487   TPPLPoint pt;
00488   int ctr=0;
00489   for (std::list<TPPLPoly>::iterator it = tri_list.begin (); it != tri_list.end (); it++)
00490   {
00491 
00492     //draw each triangle
00493     switch(shape_.type)
00494     {
00495       case(cob_3d_mapping_msgs::Shape::POLYGON):
00496       {
00497         for (long i = 0; i < it->GetNumPoints (); i++)
00498         {
00499           pt = it->GetPoint (i);
00500           marker.points[3*ctr+i].x = pt.x;
00501           marker.points[3*ctr+i].y = pt.y;
00502           marker.points[3*ctr+i].z = 0;
00503         }
00504         //std::cout << marker.points.size() << std::endl;
00505       }
00506       case(cob_3d_mapping_msgs::Shape::CYLINDER):
00507       {
00508         for (long i = 0; i < it->GetNumPoints (); i++)
00509         {
00510           pt = it->GetPoint(i);
00511 
00512           //apply rerolling of cylinder analogous to cylinder class
00513           if(shape_.params.size()!=10){
00514             break;
00515           }
00516 
00517           double alpha=pt.x/shape_.params[9];
00518 
00519 
00520           marker.points[3*ctr+i].x = shape_.params[9]*sin(-alpha);
00521           marker.points[3*ctr+i].y = pt.y;
00522           marker.points[3*ctr+i].z = shape_.params[9]*cos(-alpha);
00523 
00525           //marker.points[i].x = pt.x;
00526           //marker.points[i].y = pt.y;
00527           //marker.points[i].z = 0;
00528         }
00529       }
00530     }
00531     ctr++;
00532   }
00533   im_ctrl.markers.push_back (marker);
00534 
00535   //  if(!arrows_) {
00536   // Added For displaying the arrows on Marker Position
00537   marker_.pose.position.x = marker.pose.position.x ;
00538   marker_.pose.position.y = marker.pose.position.y ;
00539   marker_.pose.position.z = marker.pose.position.z ;
00540 
00541   marker_.pose.orientation.x = marker.pose.orientation.x ;
00542   marker_.pose.orientation.y = marker.pose.orientation.y ;
00543   marker_.pose.orientation.z = marker.pose.orientation.z ;
00544   // end
00545 }
00546 
00547 TPPLPoint
00548 ShapeMarker::msgToPoint2D (const pcl::PointXYZ &point)
00549 {
00550   //ROS_INFO(" transform 3D point to 2D ");
00551   TPPLPoint pt;
00552   Eigen::Vector3f p = transformation_ * point.getVector3fMap ();
00553   pt.x = p (0);
00554   pt.y = p (1);
00555   //std::cout << "\n transformed point : \n" << p << std::endl;
00556   return pt;
00557 }
00558 
00559 void
00560 ShapeMarker::createInteractiveMarker ()
00561 {
00562   visualization_msgs::InteractiveMarker imarker ;
00563   // ROS_INFO("\tcreating interactive marker for shape < %d >", shape_.id);
00564 
00565   stringstream ss;
00566   if(!arrows_ && !deleted_) {
00567     ss.str("");
00568     ss.clear() ;
00569     ss << shape_.id ;
00570     marker_.name = ss.str ();
00571     marker_.header = shape_.header;
00572     marker_.header.stamp = ros::Time::now() ;
00573   }
00574 
00575   else if(arrows_)
00576   {
00577     ROS_INFO("Second Marker... ") ;
00578     ss.str("");
00579     ss.clear() ;
00580     ss << "second_marker_" <<shape_.id ;
00581     imarker_.name = ss.str ();
00582     imarker_.header = shape_.header;
00583     imarker_.header.stamp = ros::Time::now() ;
00584   }
00585   else if(deleted_)
00586   {
00587     ROS_INFO("Deleted Marker... ") ;
00588     ss.str("");
00589     ss.clear() ;
00590     ss << "deleted_marker_" <<shape_.id ;
00591     deleted_imarker_.name = ss.str ();
00592     deleted_imarker_.header = shape_.header;
00593     deleted_imarker_.header.stamp = ros::Time::now() ;
00594   }
00595   visualization_msgs::InteractiveMarkerControl im_ctrl_for_second_marker;
00596 
00597   /* create marker */
00598   if (!deleted_ && !arrows_) {
00599     ss.str("");
00600     ss.clear() ;
00601     ss.str ("");
00602     im_ctrl.always_visible = true;
00603     ss << "shape_" << shape_.id << "_control";
00604     im_ctrl.name = ss.str ();
00605     im_ctrl.description = "shape_markers";
00606     im_ctrl.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
00607 
00608     createMarker (im_ctrl);
00609     marker_.controls.push_back(im_ctrl) ;
00610     im_server_->insert (marker_ );
00611     im_server_ ->applyChanges() ;
00612     menu_handler_.apply (*im_server_, marker_.name);
00613   }
00614   else if (arrows_)
00615   {
00616     createMarker (im_ctrl_for_second_marker);
00617     imarker_.controls.push_back(im_ctrl_for_second_marker) ;
00618     im_server_->insert (imarker_ );
00619     im_server_ ->applyChanges() ;
00620     //    menu_handler_.apply (*im_server_, imarker_.name);
00621   }
00622   else if(deleted_){
00623     im_ctrl_for_second_marker.always_visible = true;
00624     im_ctrl_for_second_marker.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
00625 
00626     createMarker (im_ctrl_for_second_marker);
00627     deleted_imarker_.controls.push_back(im_ctrl_for_second_marker) ;
00628     im_server_->insert (deleted_imarker_ );
00629     im_server_ ->applyChanges() ;
00630     menu_handler_.apply (*im_server_, deleted_imarker_.name);
00631   }
00632 }
00633 
00634 
00635 void
00636 ShapeMarker::displayNormalCB (const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
00637 {
00638 
00639   interactive_markers::MenuHandler::CheckState check_state;
00640 
00641   menu_handler_.getCheckState (feedback->menu_entry_id, check_state);
00642   if (check_state == interactive_markers::MenuHandler::UNCHECKED)
00643   {
00644     //ROS_INFO(" entry state changed ");
00645     menu_handler_.setCheckState (feedback->menu_entry_id, interactive_markers::MenuHandler::CHECKED);
00646 
00647     displayNormal();
00648   }
00649   else if (check_state == interactive_markers::MenuHandler::CHECKED)
00650   {
00651     //ROS_INFO(" entry state changed ");
00652     menu_handler_.setCheckState (feedback->menu_entry_id, interactive_markers::MenuHandler::UNCHECKED);
00653     hideNormal(1);
00654   }
00655   menu_handler_.reApply (*im_server_);
00656   im_server_->applyChanges ();
00657 
00658 }
00659 
00660 /*void ShapeMarker::displaySymAxis(){
00661 
00662   ROS_INFO(" displaySymAxis from shape[ %d ]...", shape_.id);
00663 
00664   std::vector<unsigned int>::iterator iter;
00665   visualization_msgs::InteractiveMarker imarker;
00666   stringstream ss;
00667 
00668   ss << "symaxis_" << shape_.id;
00669   imarker.name = ss.str();
00670   imarker.header = shape_.header;
00671   ss.str("");
00672   ss.clear();
00673 
00674   visualization_msgs::Marker marker;
00675   marker.header = shape_.header;
00676   marker.type = visualization_msgs::Marker::ARROW;
00677   marker.action = visualization_msgs::Marker::ADD;
00678   marker.lifetime = ros::Duration ();
00679 
00680   //set color
00681   marker.color.r = 1;
00682   marker.color.g = 1;
00683   marker.color.b = 0;
00684   marker.color.a = 1;
00685 
00686   //set scale
00687   marker.scale.x = 0.05;
00688   marker.scale.y = 0.1;
00689   marker.scale.z = 0.1;
00690 
00691   //set pose
00692   marker.points.resize (2);
00693 
00694   marker.points[0].x = shape_.params[6];
00695   marker.points[0].y = shape_.params[7];
00696   marker.points[0].z = shape_.params[8];
00697 
00698   marker.points[1].x = shape_.params[6] - shape_.params[3];
00699   marker.points[1].y = shape_.params[7] - shape_.params[4];
00700   marker.points[1].z = shape_.params[8] - shape_.params[5];
00701 
00702   visualization_msgs::InteractiveMarkerControl im_ctrl_n;
00703 
00704   ss << "symaxis_ctrl_" << shape_.id;
00705   im_ctrl_n.name = ss.str ();
00706   im_ctrl_n.description = "display_symaxis";
00707 
00708   im_ctrl_n.markers.push_back (marker);
00709   imarker.controls.push_back (im_ctrl_n);
00710   im_server_->insert (imarker);
00711 
00712   interacted_shapes_.push_back(shape_.id) ;
00713 
00714 
00715 }*/
00716 
00717 /*void ShapeMarker::hideSymAxis(int untick){
00718 
00719   stringstream ss;
00720   std::vector<unsigned int>::iterator iter;
00721 
00722   ss << "symaxis_" << shape_.id;
00723   im_server_->erase(ss.str());
00724   im_server_->applyChanges ();
00725 
00726   if(untick){
00727     // updating interacted_shapes_ vector
00728     iter = find (interacted_shapes_.begin(), interacted_shapes_.end(), shape_.id) ;
00729     if (iter!=interacted_shapes_.end()){
00730       interacted_shapes_.erase(interacted_shapes_.begin()+(iter-interacted_shapes_.begin())) ;
00731     }
00732   }
00733 }*/
00737 void ShapeMarker::displayNormal(){
00738 
00739   ROS_INFO(" displayNormalCB from shape[ %d ]...", shape_.id);
00740 
00741   std::vector<unsigned int>::iterator iter;
00742   visualization_msgs::InteractiveMarker imarker;
00743   stringstream ss;
00744 
00745   ss << "normal_" << shape_.id;
00746   imarker.name = ss.str();
00747   imarker.header = shape_.header;
00748   ss.str("");
00749   ss.clear();
00750 
00751   visualization_msgs::Marker marker;
00752   marker.header = shape_.header;
00753   marker.type = visualization_msgs::Marker::ARROW;
00754   marker.action = visualization_msgs::Marker::ADD;
00755   marker.lifetime = ros::Duration ();
00756 
00757   //set color
00758   marker.color.r = 1;
00759   marker.color.g = 0;
00760   marker.color.b = 0;
00761   marker.color.a = 1;
00762 
00763   //set scale
00764   marker.scale.x = 0.05;
00765   marker.scale.y = 0.1;
00766   marker.scale.z = 0.1;
00767 
00768   //set pose
00769   marker.points.resize (2);
00770   marker.points[0].x = shape_.centroid.x;
00771   marker.points[0].y = shape_.centroid.y;
00772   marker.points[0].z = shape_.centroid.z;
00773 
00774   marker.points[1].x = shape_.centroid.x + shape_.params[0];
00775   marker.points[1].y = shape_.centroid.y + shape_.params[1];
00776   marker.points[1].z = shape_.centroid.z + shape_.params[2];
00777 
00778   visualization_msgs::InteractiveMarkerControl im_ctrl_n;
00779 
00780   ss << "normal_ctrl_" << shape_.id;
00781   im_ctrl_n.name = ss.str ();
00782   im_ctrl_n.description = "display_normal";
00783 
00784   //im_ctrl_n.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
00785   im_ctrl_n.markers.push_back (marker);
00786   imarker.controls.push_back (im_ctrl_n);
00787   im_server_->insert (imarker);
00788 
00789   interacted_shapes_.push_back(shape_.id) ;
00790 }
00791 
00792 void ShapeMarker::hideNormal(int untick){
00793 
00794   stringstream ss;
00795   std::vector<unsigned int>::iterator iter;
00796 
00797   ss << "normal_" << shape_.id;
00798   im_server_->erase(ss.str());
00799   im_server_->applyChanges ();
00800   if(!untick){ // when ResetAll is activated
00801     menu_handler_.setCheckState (2, interactive_markers::MenuHandler::UNCHECKED);//second menu Entry is display Contour
00802     menu_handler_.reApply (*im_server_);
00803     im_server_->applyChanges() ;
00804   }
00805 
00806   if(untick){
00807     // updating interacted_shapes_ vector
00808     iter = find (interacted_shapes_.begin(), interacted_shapes_.end(), shape_.id) ;
00809     if (iter!=interacted_shapes_.end()){
00810       interacted_shapes_.erase(interacted_shapes_.begin()+(iter-interacted_shapes_.begin())) ;
00811     }
00812   }
00813 }
00814 //
00815 
00816 /*void
00817 ShapeMarker::displayOriginCB (const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
00818 {
00819   stringstream ss;
00820   interactive_markers::MenuHandler::CheckState check_state;
00821   menu_handler_.getCheckState (feedback->menu_entry_id, check_state);
00822   if (check_state == interactive_markers::MenuHandler::UNCHECKED)
00823   {
00824     //ROS_INFO(" entry state changed ");
00825     menu_handler_.setCheckState (feedback->menu_entry_id, interactive_markers::MenuHandler::CHECKED);
00826     displayOrigin();
00827   }
00828   if (check_state == interactive_markers::MenuHandler::CHECKED)
00829   {
00830     //ROS_INFO(" entry state changed ");
00831     menu_handler_.setCheckState (feedback->menu_entry_id, interactive_markers::MenuHandler::UNCHECKED);
00832     hideOrigin(1);
00833   }
00834   menu_handler_.reApply (*im_server_);
00835   im_server_->applyChanges ();
00836 
00837 
00838 }*/
00839 
00840 /*void ShapeMarker::displayOrigin(){
00841 
00842   ROS_INFO(" displayOriginCB from shape[ %d ]...", shape_.id);
00843   std::vector<unsigned int>::iterator iter;
00844 
00845   stringstream ss;
00846   ss.clear();
00847   ss.str("");
00848   visualization_msgs::InteractiveMarker imarker;
00849   ss << "origin_" << shape_.id;
00850   imarker.name = ss.str();
00851   imarker.header = shape_.header;
00852   ss.str("");
00853   ss.clear();
00854 
00855   visualization_msgs::Marker marker;
00856   marker.header = shape_.header;
00857 
00858   marker.type = visualization_msgs::Marker::SPHERE;
00859   marker.action = visualization_msgs::Marker::ADD;
00860   marker.lifetime = ros::Duration ();
00861 
00862   //set color
00863   marker.color.r = 1;
00864   marker.color.g = 0;
00865   marker.color.b = 1;
00866   marker.color.a = 1;
00867 
00868   //set scale
00869   marker.scale.x = 0.04;
00870   marker.scale.y = 0.04;
00871   marker.scale.z = 0.04;
00872 
00873   //set pose
00874   marker.pose.position.x = shape_.params[6];
00875   marker.pose.position.y = shape_.params[7];
00876   marker.pose.position.z = shape_.params[8];
00877 
00878 
00879   visualization_msgs::InteractiveMarkerControl im_ctrl;
00880   im_ctrl.always_visible = true;
00881   ss << "origin_ctrl_" << shape_.id;
00882   im_ctrl.name = ss.str ();
00883   im_ctrl.markers.push_back (marker);
00884   imarker.controls.push_back (im_ctrl);
00885   im_server_->insert (imarker);
00886 
00887   interacted_shapes_.push_back(shape_.id) ;
00888 
00889 }*/
00890 
00891 /*void ShapeMarker::hideOrigin(int untick){
00892   stringstream ss;
00893   std::vector<unsigned int>::iterator iter;
00894 
00895   ss.clear();
00896   ss.str("");
00897   ss << "origin_" << shape_.id;
00898   im_server_->erase(ss.str());
00899   im_server_->applyChanges ();
00900 
00901   if(untick){
00902     // updating interacted_shapes_ vector
00903     iter = find (interacted_shapes_.begin(), interacted_shapes_.end(), shape_.id) ;
00904     if (iter!=interacted_shapes_.end()){
00905       interacted_shapes_.erase(interacted_shapes_.begin()+(iter-interacted_shapes_.begin())) ;
00906     }
00907   }
00908   //
00909 }*/
00910 
00911 
00917 void
00918 ShapeMarker::displayCentroidCB (const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
00919 {
00920   stringstream ss;
00921   interactive_markers::MenuHandler::CheckState check_state;
00922   menu_handler_.getCheckState (feedback->menu_entry_id, check_state);
00923   if (check_state == interactive_markers::MenuHandler::UNCHECKED)
00924   {
00925     //ROS_INFO(" entry state changed ");
00926     menu_handler_.setCheckState (feedback->menu_entry_id, interactive_markers::MenuHandler::CHECKED);
00927     displayCentroid();
00928   }
00929   if (check_state == interactive_markers::MenuHandler::CHECKED)
00930   {
00931     //ROS_INFO(" entry state changed ");
00932     menu_handler_.setCheckState (feedback->menu_entry_id, interactive_markers::MenuHandler::UNCHECKED);
00933     hideCentroid(1);
00934   }
00935   menu_handler_.reApply (*im_server_);
00936   im_server_->applyChanges ();
00937 
00938 
00939 }
00940 
00941 void ShapeMarker::displayCentroid(){
00942 
00943   ROS_INFO(" displayCentroidCB from shape[ %d ]...", shape_.id);
00944   std::vector<unsigned int>::iterator iter;
00945 
00946   stringstream ss;
00947   ss.clear();
00948   ss.str("");
00949   visualization_msgs::InteractiveMarker imarker;
00950   ss << "centroid_" << shape_.id;
00951   imarker.name = ss.str();
00952   imarker.header = shape_.header;
00953   ss.str("");
00954   ss.clear();
00955 
00956   visualization_msgs::Marker marker;
00957   marker.header = shape_.header;
00958 
00959   marker.type = visualization_msgs::Marker::SPHERE;
00960   marker.action = visualization_msgs::Marker::ADD;
00961   marker.lifetime = ros::Duration ();
00962 
00963   //set color
00964   marker.color.r = 0;
00965   marker.color.g = 0;
00966   marker.color.b = 1;
00967   marker.color.a = 1;
00968 
00969   //set scale
00970   marker.scale.x = 0.04;
00971   marker.scale.y = 0.04;
00972   marker.scale.z = 0.04;
00973 
00974   //set pose
00975   marker.pose.position.x = shape_.centroid.x;
00976   marker.pose.position.y = shape_.centroid.y;
00977   marker.pose.position.z = shape_.centroid.z;
00978 
00979 
00980   visualization_msgs::InteractiveMarkerControl im_ctrl;
00981   im_ctrl.always_visible = true;
00982   ss << "centroid_ctrl_" << shape_.id;
00983   im_ctrl.name = ss.str ();
00984   im_ctrl.markers.push_back (marker);
00985   imarker.controls.push_back (im_ctrl);
00986   im_server_->insert (imarker);
00987 
00988   interacted_shapes_.push_back(shape_.id) ;
00989 }
00990 
00991 void ShapeMarker::hideCentroid(int untick){
00992   stringstream ss;
00993   std::vector<unsigned int>::iterator iter;
00994 
00995   ss.clear();
00996   ss.str("");
00997   ss << "centroid_" << shape_.id;
00998   im_server_->erase(ss.str());
00999   im_server_->applyChanges ();
01000 
01001   if(!untick){ // when ResetAll is activated
01002     menu_handler_.setCheckState (3, interactive_markers::MenuHandler::UNCHECKED); //third menu Entry is display Contour
01003     menu_handler_.reApply (*im_server_);
01004     im_server_->applyChanges() ;
01005   }
01006 
01007   if(untick){
01008     // updating interacted_shapes_ vector
01009     iter = find (interacted_shapes_.begin(), interacted_shapes_.end(), shape_.id) ;
01010     if (iter!=interacted_shapes_.end()){
01011       interacted_shapes_.erase(interacted_shapes_.begin()+(iter-interacted_shapes_.begin())) ;
01012     }
01013   }
01014 }
01015 
01016 void ShapeMarker::displayContourCB(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback) {
01017 
01018   interactive_markers::MenuHandler::CheckState check_state;
01019   menu_handler_.getCheckState (feedback->menu_entry_id, check_state);
01020   if (check_state == interactive_markers::MenuHandler::UNCHECKED)
01021   {
01022     //ROS_INFO(" entry state changed ");
01023     menu_handler_.setCheckState (feedback->menu_entry_id, interactive_markers::MenuHandler::CHECKED);
01024     displayContour();
01025   }
01026   else if (check_state == interactive_markers::MenuHandler::CHECKED)
01027   {
01028     menu_handler_.setCheckState (feedback->menu_entry_id, interactive_markers::MenuHandler::UNCHECKED);
01029     hideContour(1);
01030   }
01031   menu_handler_.reApply (*im_server_);
01032   im_server_->applyChanges ();
01033 }
01034 
01035 
01036 void ShapeMarker::displayContour(){
01037   ROS_INFO(" displayContourCB from shape[ %d ]...", shape_.id);
01038   std::vector<unsigned int>::iterator iter ;
01039 
01040   stringstream aa;
01041   stringstream ss;
01042   int ctr = 0;
01043 
01044   visualization_msgs::Marker marker;
01045   marker.action = visualization_msgs::Marker::ADD;
01046   marker.type = visualization_msgs::Marker::LINE_STRIP;
01047   marker.lifetime = ros::Duration();
01048   marker.header = shape_.header ;
01049   marker.header.frame_id = "/map";
01050   marker.ns = "contours" ;
01051 
01052   marker.scale.x = 0.02;
01053   marker.scale.y = 0.02;
01054   marker.scale.z = 1;
01055 
01056   marker.color.r = 0;
01057   marker.color.g = 0;
01058   marker.color.b = 1;
01059   marker.color.a = 1.0;
01060 
01061   cob_3d_mapping::Polygon p;
01062   cob_3d_mapping::fromROSMsg (shape_, p);
01063 
01064   visualization_msgs::InteractiveMarker imarker;
01065   visualization_msgs::InteractiveMarkerControl im_ctrl_ ;
01066   for(unsigned int i=0; i<p.contours.size(); i++)
01067   {
01068     marker.id = ctr ;
01069     ctr ++ ;
01070     for(unsigned int j=0; j<p.contours[i].size(); j++)
01071     {
01072       marker.points.resize(p.contours[i].size()+1);
01073 
01074       marker.points[j].x = p.contours[i][j](0);
01075       marker.points[j].y = p.contours[i][j](1);
01076       marker.points[j].z = p.contours[i][j](2);
01077     }
01078     marker.points[p.contours[i].size()].x = p.contours[i][0](0);
01079     marker.points[p.contours[i].size()].y = p.contours[i][0](1);
01080     marker.points[p.contours[i].size()].z = p.contours[i][0](2);
01081     im_ctrl_.markers.push_back(marker);
01082 
01083   }
01084 
01085   im_ctrl_.always_visible = true ;
01086   im_ctrl_.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
01087 
01088   ss << "contour_" << shape_.id;
01089   imarker.name = ss.str() ;
01090 
01091   imarker.header  = shape_.header ;
01092   imarker.controls.push_back(im_ctrl_);
01093   im_server_->insert (imarker);
01094 
01095   interacted_shapes_.push_back(shape_.id) ;
01096 
01097 }
01098 
01099 void ShapeMarker::hideContour(int untick){
01100   stringstream ss;
01101   std::vector<unsigned int>::iterator iter;
01102 
01103   ss.clear() ;
01104   ss.str("");
01105   ss << "contour_" << shape_.id;
01106   im_server_->erase(ss.str());
01107   im_server_->applyChanges ();
01108   if(!untick){ // when ResetAll is activated
01109     menu_handler_.setCheckState (4, interactive_markers::MenuHandler::UNCHECKED);  //4th menu Entry is display Contour
01110     menu_handler_.reApply (*im_server_);
01111     im_server_->applyChanges() ;
01112   }
01113 
01114   if(untick){
01115     iter = find (interacted_shapes_.begin(), interacted_shapes_.end(), shape_.id) ;
01116     if (iter!=interacted_shapes_.end()){
01117       interacted_shapes_.erase(interacted_shapes_.begin()+(iter-interacted_shapes_.begin())) ;
01118     }
01119   }
01120 
01121 }
01122 
01123 void
01124 ShapeMarker::displaySymAxisCB (const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
01125 {
01126 
01127   interactive_markers::MenuHandler::CheckState check_state;
01128 
01129   menu_handler_.getCheckState (feedback->menu_entry_id, check_state);
01130   if (check_state == interactive_markers::MenuHandler::UNCHECKED)
01131   {
01132     //ROS_INFO(" entry state changed ");
01133     menu_handler_.setCheckState (feedback->menu_entry_id, interactive_markers::MenuHandler::CHECKED);
01134     displaySymAxis();
01135   }
01136   else if (check_state == interactive_markers::MenuHandler::CHECKED)
01137   {
01138     //ROS_INFO(" entry state changed ");
01139     menu_handler_.setCheckState (feedback->menu_entry_id, interactive_markers::MenuHandler::UNCHECKED);
01140     hideSymAxis(1);
01141   }
01142   menu_handler_.reApply (*im_server_);
01143   im_server_->applyChanges ();
01144 
01145 }
01146 
01147 void ShapeMarker::displaySymAxis(){
01148 
01149   ROS_INFO(" displaySymAxis from shape[ %d ]...", shape_.id);
01150 
01151   visualization_msgs::InteractiveMarker imarker;
01152   stringstream ss;
01153 
01154   ss << "symaxis_" << shape_.id;
01155   imarker.name = ss.str();
01156   imarker.header = shape_.header;
01157   ss.str("");
01158   ss.clear();
01159 
01160   visualization_msgs::Marker marker;
01161   marker.header = shape_.header;
01162   marker.type = visualization_msgs::Marker::ARROW;
01163   marker.action = visualization_msgs::Marker::ADD;
01164   marker.lifetime = ros::Duration ();
01165 
01166   //set color
01167   marker.color.r = 1;
01168   marker.color.g = 1;
01169   marker.color.b = 0;
01170   marker.color.a = 1;
01171 
01172   //set scale
01173   marker.scale.x = 0.05;
01174   marker.scale.y = 0.1;
01175   marker.scale.z = 0.1;
01176 
01177   //set pose
01178   marker.points.resize (2);
01179 
01180   marker.points[0].x = shape_.params[6];
01181   marker.points[0].y = shape_.params[7];
01182   marker.points[0].z = shape_.params[8];
01183 
01184   marker.points[1].x = shape_.params[6] - shape_.params[3];
01185   marker.points[1].y = shape_.params[7] - shape_.params[4];
01186   marker.points[1].z = shape_.params[8] - shape_.params[5];
01187 
01188   visualization_msgs::InteractiveMarkerControl im_ctrl_n;
01189 
01190   ss << "symaxis_ctrl_" << shape_.id;
01191   im_ctrl_n.name = ss.str ();
01192   im_ctrl_n.description = "display_symaxis";
01193 
01194   im_ctrl_n.markers.push_back (marker);
01195   imarker.controls.push_back (im_ctrl_n);
01196   im_server_->insert (imarker);
01197 
01198   interacted_shapes_.push_back(shape_.id) ;
01199 
01200 
01201 }
01202 
01203 void ShapeMarker::hideSymAxis(int untick){
01204 
01205   stringstream ss;
01206   std::vector<unsigned int>::iterator iter;
01207 
01208   ss << "symaxis_" << shape_.id;
01209   im_server_->erase(ss.str());
01210   im_server_->applyChanges ();
01211 
01212   if(untick){
01213     // updating interacted_shapes_ vector
01214     iter = find (interacted_shapes_.begin(), interacted_shapes_.end(), shape_.id) ;
01215     if (iter!=interacted_shapes_.end()){
01216       interacted_shapes_.erase(interacted_shapes_.begin()+(iter-interacted_shapes_.begin())) ;
01217     }
01218   }
01219   if(!untick){ // when ResetAll is activated
01220     menu_handler_.setCheckState (7, interactive_markers::MenuHandler::UNCHECKED);//second menu Entry is display Contour
01221     menu_handler_.reApply (*im_server_);
01222     im_server_->applyChanges() ;
01223   }
01224 }
01225 
01226 void
01227 ShapeMarker::displayOriginCB (const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)
01228 {
01229   stringstream ss;
01230   interactive_markers::MenuHandler::CheckState check_state;
01231   menu_handler_.getCheckState (feedback->menu_entry_id, check_state);
01232   if (check_state == interactive_markers::MenuHandler::UNCHECKED)
01233   {
01234     //ROS_INFO(" entry state changed ");
01235     menu_handler_.setCheckState (feedback->menu_entry_id, interactive_markers::MenuHandler::CHECKED);
01236     displayOrigin();
01237   }
01238   if (check_state == interactive_markers::MenuHandler::CHECKED)
01239   {
01240     //ROS_INFO(" entry state changed ");
01241     menu_handler_.setCheckState (feedback->menu_entry_id, interactive_markers::MenuHandler::UNCHECKED);
01242     hideOrigin(1);
01243   }
01244   menu_handler_.reApply (*im_server_);
01245   im_server_->applyChanges ();
01246 
01247 
01248 }
01249 
01250 void ShapeMarker::displayOrigin(){
01251 
01252   ROS_INFO(" displayOriginCB from shape[ %d ]...", shape_.id);
01253   std::vector<unsigned int>::iterator iter;
01254 
01255   stringstream ss;
01256   ss.clear();
01257   ss.str("");
01258   visualization_msgs::InteractiveMarker imarker;
01259   ss << "origin_" << shape_.id;
01260   imarker.name = ss.str();
01261   imarker.header = shape_.header;
01262   ss.str("");
01263   ss.clear();
01264 
01265   visualization_msgs::Marker marker;
01266   marker.header = shape_.header;
01267 
01268   marker.type = visualization_msgs::Marker::SPHERE;
01269   marker.action = visualization_msgs::Marker::ADD;
01270   marker.lifetime = ros::Duration ();
01271 
01272   //set color
01273   marker.color.r = 1;
01274   marker.color.g = 0;
01275   marker.color.b = 1;
01276   marker.color.a = 1;
01277 
01278   //set scale
01279   marker.scale.x = 0.04;
01280   marker.scale.y = 0.04;
01281   marker.scale.z = 0.04;
01282 
01283   //set pose
01284   marker.pose.position.x = shape_.params[6];
01285   marker.pose.position.y = shape_.params[7];
01286   marker.pose.position.z = shape_.params[8];
01287 
01288 
01289   visualization_msgs::InteractiveMarkerControl im_ctrl;
01290   im_ctrl.always_visible = true;
01291   ss << "origin_ctrl_" << shape_.id;
01292   im_ctrl.name = ss.str ();
01293   im_ctrl.markers.push_back (marker);
01294   imarker.controls.push_back (im_ctrl);
01295   im_server_->insert (imarker);
01296 
01297   interacted_shapes_.push_back(shape_.id) ;
01298 
01299 }
01300 
01301 void ShapeMarker::hideOrigin(int untick){
01302   stringstream ss;
01303   std::vector<unsigned int>::iterator iter;
01304 
01305   ss.clear();
01306   ss.str("");
01307   ss << "origin_" << shape_.id;
01308   im_server_->erase(ss.str());
01309   im_server_->applyChanges ();
01310 
01311   if(untick){
01312     // updating interacted_shapes_ vector
01313     iter = find (interacted_shapes_.begin(), interacted_shapes_.end(), shape_.id) ;
01314     if (iter!=interacted_shapes_.end()){
01315       interacted_shapes_.erase(interacted_shapes_.begin()+(iter-interacted_shapes_.begin())) ;
01316     }
01317   }
01318   if(!untick){ // when ResetAll is activated
01319     menu_handler_.setCheckState (8, interactive_markers::MenuHandler::UNCHECKED);//second menu Entry is display Contour
01320     menu_handler_.reApply (*im_server_);
01321     im_server_->applyChanges() ;
01322   }
01323   //
01324 }
01325
shape_marker.cpp
