srs_env_model: srs_env_model: objtree

00001 /******************************************************************************
00002  * \file
00003  *
00004  * $Id:$
00005  *
00006  * Copyright (C) Brno University of Technology
00007  *
00008  * This file is part of software developed by dcgm-robotics@FIT group.
00009  *
00010  * Author: Jan Gorig (xgorig01@stud.fit.vutbr.cz)
00011  * Supervised by: Michal Spanel (spanel@fit.vutbr.cz)
00012  * Date: dd/mm/2012
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 #pragma once
00029 #ifndef SRS_ENV_MODEL_BUT_SERVER_PLUGINS_OBJTREEPLUGIN_H
00030 #define SRS_ENV_MODEL_BUT_SERVER_PLUGINS_OBJTREEPLUGIN_H
00031 
00032 #include <message_filters/subscriber.h>
00033 #include <interactive_markers/interactive_marker_server.h>
00034 
00035 #include <srs_env_model/but_server/server_tools.h>
00036 #include <srs_env_model/but_server/objtree/octree.h>
00037 #include <srs_env_model/GetPlane.h>
00038 #include <srs_env_model/GetAlignedBox.h>
00039 #include <srs_env_model/GetBoundingBox.h>
00040 #include <srs_env_model/InsertPlane.h>
00041 #include <srs_env_model/InsertAlignedBox.h>
00042 #include <srs_env_model/InsertBoundingBox.h>
00043 #include <srs_env_model/InsertPlanes.h>
00044 #include <srs_env_model/ShowObject.h>
00045 #include <srs_env_model/RemoveObject.h>
00046 #include <srs_env_model/ShowObjtree.h>
00047 #include <srs_env_model/GetObjectsInBox.h>
00048 #include <srs_env_model/GetObjectsInHalfspace.h>
00049 #include <srs_env_model/GetObjectsInSphere.h>
00050 
00051 #include <srs_interaction_primitives/services_list.h>
00052 
00053 
00054 namespace srs_env_model
00055 {
00056 
00057 class CObjTreePlugin : public CServerPluginBase
00058 {
00059 public:
00060     typedef boost::shared_ptr<interactive_markers::InteractiveMarkerServer> InteractiveMarkerServerPtr;
00061 
00063         CObjTreePlugin(const std::string & name);
00064 
00066         virtual ~CObjTreePlugin();
00067 
00069         virtual void init(ros::NodeHandle & node_handle);
00070 
00071     virtual void reset();
00072 
00073     enum Operation
00074     {
00075         INSERT,
00076         UPDATE,
00077         GET_SIMILAR
00078     };
00079 
00081     virtual void pause( bool bPause, ros::NodeHandle & node_handle );
00082 
00083 protected:
00085     virtual bool shouldPublish(){return false; }
00087     virtual void publishInternal( const ros::Time & timestamp ){}
00088 
00090     bool srvInsertPlane(srs_env_model::InsertPlane::Request &req, srs_env_model::InsertPlane::Response &res);
00092     bool srvInsertPlaneByPosition(srs_env_model::InsertPlane::Request &req, srs_env_model::InsertPlane::Response &res);
00094     bool srvGetSimilarPlane(srs_env_model::InsertPlane::Request &req, srs_env_model::InsertPlane::Response &res);
00096     bool srvInsertPlanes(srs_env_model::InsertPlanes::Request &req, srs_env_model::InsertPlanes::Response &res);
00098     bool srvInsertABox(srs_env_model::InsertAlignedBox::Request &req, srs_env_model::InsertAlignedBox::Response &res);
00100     bool srvInsertABoxByPosition(srs_env_model::InsertAlignedBox::Request &req, srs_env_model::InsertAlignedBox::Response &res);
00102     bool srvGetSimilarABox(srs_env_model::InsertAlignedBox::Request &req, srs_env_model::InsertAlignedBox::Response &res);
00104     bool srvInsertBBox(srs_env_model::InsertBoundingBox::Request &req, srs_env_model::InsertBoundingBox::Response &res);
00106     bool srvInsertBBoxByPosition(srs_env_model::InsertBoundingBox::Request &req, srs_env_model::InsertBoundingBox::Response &res);
00108     bool srvGetSimilarBBox(srs_env_model::InsertBoundingBox::Request &req, srs_env_model::InsertBoundingBox::Response &res);
00110     bool srvShowObject(srs_env_model::ShowObject::Request &req, srs_env_model::ShowObject::Response &res);
00112     bool srvRemoveObject(srs_env_model::RemoveObject::Request &req, srs_env_model::RemoveObject::Response &res);
00114     bool srvShowObjtree(srs_env_model::ShowObjtree::Request &req, srs_env_model::ShowObjtree::Response &res);
00116     bool srvGetPlane(srs_env_model::GetPlane::Request &req, srs_env_model::GetPlane::Response &res);
00118     bool srvGetABox(srs_env_model::GetAlignedBox::Request &req, srs_env_model::GetAlignedBox::Response &res);
00120     bool srvGetBBox(srs_env_model::GetBoundingBox::Request &req, srs_env_model::GetBoundingBox::Response &res);
00122     bool srvGetObjectsInBox(srs_env_model::GetObjectsInBox::Request &req, srs_env_model::GetObjectsInBox::Response &res);
00124     bool srvGetObjectsInHalfspace(srs_env_model::GetObjectsInHalfspace::Request &req, srs_env_model::GetObjectsInHalfspace::Response &res);
00126     bool srvGetObjectsInSphere(srs_env_model::GetObjectsInSphere::Request &req, srs_env_model::GetObjectsInSphere::Response &res);
00127 
00128     //Helper methods
00129     unsigned int insertPlane(const srs_env_model_msgs::PlaneDesc &plane, Operation op);
00130     unsigned int insertABox(unsigned int id, const geometry_msgs::Point32 &position, const geometry_msgs::Vector3 &scale, Operation op);
00131     unsigned int insertBBox(unsigned int id, const geometry_msgs::Pose &pose, const geometry_msgs::Vector3 &scale, Operation op);
00132     void showObject(unsigned int id);
00133     void removeObject(unsigned int id);
00134     void showObjtree();
00135     void getObjects(const objtree::Filter *filter, std::vector<unsigned int> &output);
00136 
00137     //Service servers
00138     ros::ServiceServer m_serviceGetObjectsInBox;
00139     ros::ServiceServer m_serviceGetObjectsInHalfspace;
00140     ros::ServiceServer m_serviceGetObjectsInSphere;
00141     ros::ServiceServer m_serviceGetPlane;
00142     ros::ServiceServer m_serviceGetABox;
00143     ros::ServiceServer m_serviceGetBBox;
00144     ros::ServiceServer m_serviceInsertPlane;
00145     ros::ServiceServer m_serviceInsertABox;
00146     ros::ServiceServer m_serviceInsertBBox;
00147     ros::ServiceServer m_serviceInsertPlaneByPosition;
00148     ros::ServiceServer m_serviceInsertABoxByPosition;
00149     ros::ServiceServer m_serviceInsertBBoxByPosition;
00150     ros::ServiceServer m_serviceGetSimilarPlane;
00151     ros::ServiceServer m_serviceGetSimilarABox;
00152     ros::ServiceServer m_serviceGetSimilarBBox;
00153     ros::ServiceServer m_serviceInsertPlanes;
00154     ros::ServiceServer m_serviceShowObject;
00155     ros::ServiceServer m_serviceShowObjtree;
00156     ros::ServiceServer m_serviceRemoveObject;
00157     
00158     //Service clients
00159     ros::ServiceClient m_clientAddPlane;
00160     ros::ServiceClient m_clientAddBoundingBox;
00161     ros::ServiceClient m_clientRemovePrimitive;
00162 
00163     ros::Publisher m_markerPub;
00164 
00165     objtree::Octree m_octree;
00166 
00167 private:
00168     void publishLine(visualization_msgs::Marker &lines, float x1, float y1, float z1, float x2, float y2, float z2);
00169     void publishCube(visualization_msgs::Marker &lines, float x, float y, float z, float w, float h, float d);
00170     void publishOctree(const std::list<objtree::Box> &nodes);
00171 
00172     void removePrimitiveMarker(unsigned int id);
00173 };
00174 
00175 }
00176 
00177 #endif //SRS_ENV_MODEL_BUT_SERVER_PLUGINS_OBJTREEPLUGIN_H
objtree_plugin.h
