cob_3d_visualization: cob_3d_visualization: table

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
00018  * Author: Shaghayegh Nazari, email:georg.arbeiter@ipa.fhg.de
00019  * \author
00020  * Supervised by: Georg Arbeiter, email:georg.arbeiter@ipa.fhg.de
00021  *
00022  * Date of creation: 09/2012
00023  * ToDo:
00024  *
00025  *
00026  * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
00027  *
00028  * Redistribution and use in source and binary forms, with or without
00029  * modification, are permitted provided that the following conditions are met:
00030  *
00031  * * Redistributions of source code must retain the above copyright
00032  * notice, this list of conditions and the following disclaimer.
00033  * * Redistributions in binary form must reproduce the above copyright
00034  * notice, this list of conditions and the following disclaimer in the
00035  * documentation and/or other materials provided with the distribution.
00036  * * Neither the name of the Fraunhofer Institute for Manufacturing
00037  * Engineering and Automation (IPA) nor the names of its
00038  * contributors may be used to endorse or promote products derived from
00039  * this software without specific prior written permission.
00040  *
00041  * This program is free software: you can redistribute it and/or modify
00042  * it under the terms of the GNU Lesser General Public License LGPL as
00043  * published by the Free Software Foundation, either version 3 of the
00044  * License, or (at your option) any later version.
00045  *
00046  * This program is distributed in the hope that it will be useful,
00047  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00048  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
00049  * GNU Lesser General Public License LGPL for more details.
00050  *
00051  * You should have received a copy of the GNU Lesser General Public
00052  * License LGPL along with this program.
00053  * If not, see <http://www.gnu.org/licenses/>.
00054  *
00055  ****************************************************************/
00056 
00057 #include <cob_3d_visualization/table_marker.h>
00058 
00059 TableMarker::TableMarker (boost::shared_ptr<interactive_markers::InteractiveMarkerServer> server,cob_3d_mapping_msgs::Shape& table,int ctr/*,
00060     tabletop_object_detector::Table& tableMsg*/)
00061 {
00062   id_ = ctr ;
00063   table_im_server_ = server ;
00064   table_ = table ;
00065   //goal_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("/move_base_linear_simple/goal", 1);
00066 
00067   createTableMenu();
00068   createInteractiveMarkerForTable();
00069   //Table msg
00070   //table_msg_ = tableMsg ;
00071 }
00072 
00073 void TableMarker::createInteractiveMarkerForTable ()
00074 {
00075   cob_3d_mapping::Polygon p;
00076   cob_3d_mapping::fromROSMsg (table_, p);
00077 
00078 
00079 
00080   /* create interactive marker for *this shape */
00081   stringstream ss;
00082   ss << "table_"<< id_ ; //ctr_ ;
00083   table_int_marker_.name = ss.str ();
00084   table_int_marker_.header = table_.header;
00085   table_int_marker_.header.stamp = ros::Time::now() ;
00086 
00087   ss.str ("");
00088   im_ctrl.always_visible = true;
00089   ss << "table_" << id_ << "_control";
00090   im_ctrl.name = ss.str ();
00091   im_ctrl.description = "table_markers";
00092   im_ctrl.interaction_mode = visualization_msgs::InteractiveMarkerControl::BUTTON;
00093 
00094   /* create marker */
00095   createMarkerforTable (im_ctrl);
00096 
00097   table_int_marker_.controls.push_back (im_ctrl);
00098   table_im_server_->insert (table_int_marker_);
00099 
00100 
00101   table_menu_handler_.apply (*table_im_server_, table_int_marker_.name);
00102   table_im_server_ ->applyChanges() ;
00103 
00104 }
00105 
00106 void
00107 TableMarker::createMarkerforTable (visualization_msgs::InteractiveMarkerControl& im_ctrl)
00108 {
00109   float offset(0.1);
00110 
00111   //  /* transform shape points to 2d and store 2d point in triangle list */
00112   TPPLPartition pp;
00113   list<TPPLPoly> polys, tri_list;
00114 
00115   Eigen::Vector3f v, normal, origin;
00116   if (table_.params.size () == 4)
00117   {
00118     normal (0) = table_.params[0];
00119     normal (1) = table_.params[1];
00120     normal (2) = table_.params[2];
00121     origin (0) = table_.centroid.x + offset ;
00122     origin (1) = table_.centroid.y + offset ;
00123     origin (2) = table_.centroid.z + offset ;
00124     v = normal.unitOrthogonal ();
00125 
00126     pcl::getTransformationFromTwoUnitVectorsAndOrigin (v, normal, origin, transformation_);
00127     transformation_inv_ = transformation_.inverse ();
00128   }
00129 
00130   for (size_t i = 0; i < table_.points.size (); i++)
00131   {
00132     pcl::PointCloud<pcl::PointXYZ> pc;
00133     TPPLPoly poly;
00134     pcl::fromROSMsg (table_.points[i], pc);
00135     poly.Init (pc.points.size ());
00136     poly.SetHole (table_.holes[i]);
00137 
00138     for (size_t j = 0; j < pc.points.size (); j++)
00139     {
00140       poly[j] = msgToPoint2DforTable (pc[j]);
00141     }
00142     if (table_.holes[i])
00143       poly.SetOrientation (TPPL_CW);
00144     else
00145       poly.SetOrientation (TPPL_CCW);
00146 
00147     polys.push_back (poly);
00148   }
00149   pp.Triangulate_EC (&polys, &tri_list);
00150   TPPLPoint pt;
00151   for (std::list<TPPLPoly>::iterator it = tri_list.begin (); it != tri_list.end (); it++)
00152   {
00153     table_marker_.id = id_ ; //ctr_;
00154 
00155     table_marker_.header = table_.header;
00156     table_marker_.header.stamp = ros::Time::now() ;
00157 
00158     table_marker_.type = visualization_msgs::Marker::TRIANGLE_LIST;
00159     table_marker_.ns = "table visualization";
00160     table_marker_.action = visualization_msgs::Marker::ADD;
00161     table_marker_.lifetime = ros::Duration ();
00162 
00163     //set color
00164     table_marker_.color.r = 1 ;
00165     table_marker_.color.g = 0;
00166     table_marker_.color.b = 0;
00167     table_marker_.color.a = 1;
00168 
00169     //set scale
00170     table_marker_.scale.x = 1;
00171     table_marker_.scale.y = 1;
00172     table_marker_.scale.z = 1;
00173 
00174     //set pose
00175     Eigen::Quaternionf quat (transformation_inv_.rotation ());
00176     Eigen::Vector3f trans (transformation_inv_.translation ());
00177 
00178     table_marker_.pose.position.x = trans (0);
00179     table_marker_.pose.position.y = trans (1);
00180     table_marker_.pose.position.z = trans (2);
00181 
00182     table_marker_.pose.orientation.x = quat.x ();
00183     table_marker_.pose.orientation.y = quat.y ();
00184     table_marker_.pose.orientation.z = quat.z ();
00185     table_marker_.pose.orientation.w = quat.w ();
00186 
00187     //draw each triangle
00188     table_marker_.points.resize (it->GetNumPoints ());
00189     for (long i = 0; i < it->GetNumPoints (); i++)
00190     {
00191       pt = it->GetPoint (i);
00192       table_marker_.points[i].x = pt.x;
00193       table_marker_.points[i].y = pt.y;
00194       table_marker_.points[i].z = 0;
00195     }
00196     im_ctrl.markers.push_back (table_marker_);
00197   }
00198 }
00199 
00200 TPPLPoint
00201 TableMarker::msgToPoint2DforTable (const pcl::PointXYZ &point)
00202 {
00203   //ROS_INFO(" transform 3D point to 2D ");
00204   TPPLPoint pt;
00205   Eigen::Vector3f p = transformation_ * point.getVector3fMap ();
00206   pt.x = p (0);
00207   pt.y = p (1);
00208   //std::cout << "\n transformed point : \n" << p << std::endl;
00209   return pt;
00210 }
00211 
00212 void TableMarker::tableFeedbackCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback) {
00213 
00214   ROS_INFO("%s position : x= %f, y= %f, z= %f", table_int_marker_.name.c_str(), table_.centroid.x,table_.centroid.y,table_.centroid.z);
00215 
00216 }
00217 void TableMarker::createTableMenu() {
00218 
00219   interactive_markers::MenuHandler::EntryHandle eh_1;
00220   eh_1 = table_menu_handler_.insert ("Move to this table",boost::bind (&TableMarker::MoveToTheTable, this, _1));
00221 
00222   table_menu_handler_.setVisible (eh_1, true);
00223   table_menu_handler_.setCheckState (eh_1, interactive_markers::MenuHandler::NO_CHECKBOX);
00224 }
00225 
00226 void TableMarker::MoveToTheTable(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback) {
00227   std::cout << "in MovingToTable..." << "\n" ;
00228 
00229   ROS_INFO("Moving to table %d...",id_) ;
00230 
00231   cob_3d_mapping_msgs::MoveToTable::Request  reqMoveToTable;
00232   cob_3d_mapping_msgs::MoveToTable::Response resMoveToTable;
00233 
00234   //reqMoveToTable.targetTable = table_ ;
00235   /*reqMoveToTable.tableCentroid.position.x = table_.centroid.x ;
00236   reqMoveToTable.tableCentroid.position.y = table_.centroid.y ;
00237   reqMoveToTable.tableCentroid.position.z = table_.centroid.z ;*/
00238 
00239   if (ros::service::call("/move_to_table",reqMoveToTable,resMoveToTable)){
00240     // Calling move_to_table Service...
00241   }
00242 
00243 }
00244
table_marker.cpp
