semantic_world.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Sachin Chitta */
36 
37 #ifndef MOVEIT_SEMANTIC_WORLD_
38 #define MOVEIT_SEMANTIC_WORLD_
39 
40 #include <ros/ros.h>
43 #include <object_recognition_msgs/TableArray.h>
44 #include <moveit_msgs/CollisionObject.h>
45 #include <boost/thread/mutex.hpp>
47 
48 namespace shapes
49 {
51 }
52 
53 namespace moveit
54 {
55 namespace semantic_world
56 {
58 
63 {
64 public:
66  typedef boost::function<void()> TableCallbackFn;
67 
72  SemanticWorld(const planning_scene::PlanningSceneConstPtr& planning_scene);
73 
77  object_recognition_msgs::TableArray getTablesInROI(double minx, double miny, double minz, double maxx, double maxy,
78  double maxz) const;
79 
83  std::vector<std::string> getTableNamesInROI(double minx, double miny, double minz, double maxx, double maxy,
84  double maxz) const;
85 
91  std::vector<geometry_msgs::PoseStamped> generatePlacePoses(const std::string& table_name,
92  const shapes::ShapeConstPtr& object_shape,
93  const geometry_msgs::Quaternion& object_orientation,
94  double resolution, double delta_height = 0.01,
95  unsigned int num_heights = 2) const;
96 
102  std::vector<geometry_msgs::PoseStamped> generatePlacePoses(const object_recognition_msgs::Table& table,
103  const shapes::ShapeConstPtr& object_shape,
104  const geometry_msgs::Quaternion& object_orientation,
105  double resolution, double delta_height = 0.01,
106  unsigned int num_heights = 2) const;
114  std::vector<geometry_msgs::PoseStamped> generatePlacePoses(const object_recognition_msgs::Table& table,
115  double resolution, double height_above_table,
116  double delta_height = 0.01, unsigned int num_heights = 2,
117  double min_distance_from_edge = 0.10) const;
118 
119  void clear();
120 
121  bool addTablesToCollisionWorld();
122 
123  visualization_msgs::MarkerArray getPlaceLocationsMarker(const std::vector<geometry_msgs::PoseStamped>& poses) const;
124 
125  void addTableCallback(const TableCallbackFn& table_callback)
126  {
127  table_callback_ = table_callback;
128  }
129 
130  std::string findObjectTable(const geometry_msgs::Pose& pose, double min_distance_from_edge = 0.0,
131  double min_vertical_offset = 0.0) const;
132 
133  bool isInsideTableContour(const geometry_msgs::Pose& pose, const object_recognition_msgs::Table& table,
134  double min_distance_from_edge = 0.0, double min_vertical_offset = 0.0) const;
135 
136 private:
137  shapes::Mesh* createSolidMeshFromPlanarPolygon(const shapes::Mesh& polygon, double thickness) const;
138 
139  shapes::Mesh* orientPlanarPolygon(const shapes::Mesh& polygon) const;
140 
141  void tableCallback(const object_recognition_msgs::TableArrayPtr& msg);
142 
143  void transformTableArray(object_recognition_msgs::TableArray& table_array) const;
144 
145  planning_scene::PlanningSceneConstPtr planning_scene_;
146 
148 
149  object_recognition_msgs::TableArray table_array_;
150 
151  std::vector<geometry_msgs::PoseStamped> place_poses_;
152 
153  std::map<std::string, object_recognition_msgs::Table> current_tables_in_collision_world_;
154 
155  // boost::mutex table_lock_;
156 
158 
159  ros::Publisher visualization_publisher_, collision_object_publisher_;
160 
161  TableCallbackFn table_callback_;
162 
164 };
165 }
166 }
167 
168 #endif
planning_scene::PlanningSceneConstPtr planning_scene_
std::map< std::string, object_recognition_msgs::Table > current_tables_in_collision_world_
object_recognition_msgs::TableArray table_array_
MOVEIT_CLASS_FORWARD(Shape)
A (simple) semantic world representation for pick and place and other tasks.
void addTableCallback(const TableCallbackFn &table_callback)
std::vector< geometry_msgs::PoseStamped > place_poses_
boost::function< void()> TableCallbackFn
The signature for a callback on receiving table messages.
std::shared_ptr< const Shape > ShapeConstPtr


perception
Author(s): Ioan Sucan , Jon Binney , Suat Gedikli
autogenerated on Sun Oct 18 2020 13:17:23