Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #ifndef MOVEIT_SEMANTIC_WORLD_
00038 #define MOVEIT_SEMANTIC_WORLD_
00039
00040 #include <ros/ros.h>
00041 #include <moveit/planning_scene/planning_scene.h>
00042 #include <object_recognition_msgs/TableArray.h>
00043 #include <moveit_msgs/CollisionObject.h>
00044 #include <geometric_shapes/shapes.h>
00045
00046 #include <boost/thread/mutex.hpp>
00047
00048 namespace moveit
00049 {
00050
00051 namespace semantic_world
00052 {
00053
00057 class SemanticWorld
00058 {
00059 public:
00060
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,
00074 double maxx, double maxy, double maxz) const;
00075
00079 std::vector<std::string> getTableNamesInROI(double minx, double miny, double minz,
00080 double maxx, double maxy, 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,
00091 double delta_height = 0.01,
00092 unsigned int num_heights = 2) const;
00093
00099 std::vector<geometry_msgs::PoseStamped> generatePlacePoses(const object_recognition_msgs::Table &table,
00100 const shapes::ShapeConstPtr& object_shape,
00101 const geometry_msgs::Quaternion &object_orientation,
00102 double resolution,
00103 double delta_height = 0.01,
00104 unsigned int num_heights = 2) const;
00112 std::vector<geometry_msgs::PoseStamped> generatePlacePoses(const object_recognition_msgs::Table &table,
00113 double resolution,
00114 double height_above_table,
00115 double delta_height = 0.01,
00116 unsigned int num_heights = 2,
00117 double min_distance_from_edge = 0.10) const;
00118
00119
00120 void clear();
00121
00122 bool addTablesToCollisionWorld();
00123
00124 visualization_msgs::MarkerArray getPlaceLocationsMarker(const std::vector<geometry_msgs::PoseStamped> &poses) const;
00125
00126 void addTableCallback(const TableCallbackFn &table_callback)
00127 {
00128 table_callback_ = table_callback;
00129 }
00130
00131 std::string findObjectTable(const geometry_msgs::Pose &pose,
00132 double min_distance_from_edge = 0.0,
00133 double min_vertical_offset = 0.0) const;
00134
00135 bool isInsideTableContour(const geometry_msgs::Pose &pose,
00136 const object_recognition_msgs::Table &table,
00137 double min_distance_from_edge = 0.0,
00138 double min_vertical_offset = 0.0) const;
00139
00140 private:
00141
00142 shapes::Mesh* createSolidMeshFromPlanarPolygon (const shapes::Mesh& polygon, double thickness) const;
00143
00144 shapes::Mesh* orientPlanarPolygon (const shapes::Mesh& polygon) const;
00145
00146 void tableCallback(const object_recognition_msgs::TableArrayPtr &msg);
00147
00148 void transformTableArray(object_recognition_msgs::TableArray &table_array) const;
00149
00150 planning_scene::PlanningSceneConstPtr planning_scene_;
00151
00152 ros::NodeHandle node_handle_;
00153
00154 object_recognition_msgs::TableArray table_array_;
00155
00156 std::vector<geometry_msgs::PoseStamped> place_poses_;
00157
00158 std::map<std::string, object_recognition_msgs::Table> current_tables_in_collision_world_;
00159
00160
00161
00162 ros::Subscriber table_subscriber_;
00163
00164 ros::Publisher visualization_publisher_, collision_object_publisher_;
00165
00166 TableCallbackFn table_callback_;
00167
00168 ros::Publisher planning_scene_diff_publisher_;
00169
00170 };
00171
00172 }
00173
00174 }
00175
00176 #endif