semantic_world.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Sachin Chitta */
00036 
00037 #ifndef MOVEIT_SEMANTIC_WORLD_
00038 #define MOVEIT_SEMANTIC_WORLD_
00039 
00040 #include <ros/ros.h>
00041 #include <moveit/macros/class_forward.h>
00042 #include <moveit/planning_scene/planning_scene.h>
00043 #include <object_recognition_msgs/TableArray.h>
00044 #include <moveit_msgs/CollisionObject.h>
00045 #include <geometric_shapes/shapes.h>
00046 
00047 #include <boost/thread/mutex.hpp>
00048 
00049 namespace moveit
00050 {
00051 namespace semantic_world
00052 {
00053 MOVEIT_CLASS_FORWARD(SemanticWorld);
00054 
00058 class SemanticWorld
00059 {
00060 public:
00062   typedef boost::function<void()> TableCallbackFn;
00063 
00068   SemanticWorld(const planning_scene::PlanningSceneConstPtr& planning_scene);
00069 
00073   object_recognition_msgs::TableArray getTablesInROI(double minx, double miny, double minz, double maxx, double maxy,
00074                                                      double maxz) const;
00075 
00079   std::vector<std::string> getTableNamesInROI(double minx, double miny, double minz, double maxx, double maxy,
00080                                               double maxz) const;
00081 
00087   std::vector<geometry_msgs::PoseStamped> generatePlacePoses(const std::string& table_name,
00088                                                              const shapes::ShapeConstPtr& object_shape,
00089                                                              const geometry_msgs::Quaternion& object_orientation,
00090                                                              double resolution, double delta_height = 0.01,
00091                                                              unsigned int num_heights = 2) const;
00092 
00098   std::vector<geometry_msgs::PoseStamped> generatePlacePoses(const object_recognition_msgs::Table& table,
00099                                                              const shapes::ShapeConstPtr& object_shape,
00100                                                              const geometry_msgs::Quaternion& object_orientation,
00101                                                              double resolution, double delta_height = 0.01,
00102                                                              unsigned int num_heights = 2) const;
00110   std::vector<geometry_msgs::PoseStamped> generatePlacePoses(const object_recognition_msgs::Table& table,
00111                                                              double resolution, double height_above_table,
00112                                                              double delta_height = 0.01, unsigned int num_heights = 2,
00113                                                              double min_distance_from_edge = 0.10) const;
00114 
00115   void clear();
00116 
00117   bool addTablesToCollisionWorld();
00118 
00119   visualization_msgs::MarkerArray getPlaceLocationsMarker(const std::vector<geometry_msgs::PoseStamped>& poses) const;
00120 
00121   void addTableCallback(const TableCallbackFn& table_callback)
00122   {
00123     table_callback_ = table_callback;
00124   }
00125 
00126   std::string findObjectTable(const geometry_msgs::Pose& pose, double min_distance_from_edge = 0.0,
00127                               double min_vertical_offset = 0.0) const;
00128 
00129   bool isInsideTableContour(const geometry_msgs::Pose& pose, const object_recognition_msgs::Table& table,
00130                             double min_distance_from_edge = 0.0, double min_vertical_offset = 0.0) const;
00131 
00132 private:
00133   shapes::Mesh* createSolidMeshFromPlanarPolygon(const shapes::Mesh& polygon, double thickness) const;
00134 
00135   shapes::Mesh* orientPlanarPolygon(const shapes::Mesh& polygon) const;
00136 
00137   void tableCallback(const object_recognition_msgs::TableArrayPtr& msg);
00138 
00139   void transformTableArray(object_recognition_msgs::TableArray& table_array) const;
00140 
00141   planning_scene::PlanningSceneConstPtr planning_scene_;
00142 
00143   ros::NodeHandle node_handle_;
00144 
00145   object_recognition_msgs::TableArray table_array_;
00146 
00147   std::vector<geometry_msgs::PoseStamped> place_poses_;
00148 
00149   std::map<std::string, object_recognition_msgs::Table> current_tables_in_collision_world_;
00150 
00151   //  boost::mutex table_lock_;
00152 
00153   ros::Subscriber table_subscriber_;
00154 
00155   ros::Publisher visualization_publisher_, collision_object_publisher_;
00156 
00157   TableCallbackFn table_callback_;
00158 
00159   ros::Publisher planning_scene_diff_publisher_;
00160 };
00161 }
00162 }
00163 
00164 #endif


perception
Author(s): Ioan Sucan , Jon Binney , Suat Gedikli
autogenerated on Mon Apr 23 2018 10:25:10