A (simple) semantic world representation for pick and place and other tasks. More...
#include <semantic_world.h>
Public Types | |
typedef boost::function< void()> | TableCallbackFn |
The signature for a callback on receiving table messages. | |
Public Member Functions | |
void | addTableCallback (const TableCallbackFn &table_callback) |
bool | addTablesToCollisionWorld () |
void | clear () |
std::string | findObjectTable (const geometry_msgs::Pose &pose, double min_distance_from_edge=0.0, double min_vertical_offset=0.0) const |
std::vector < geometry_msgs::PoseStamped > | generatePlacePoses (const std::string &table_name, const shapes::ShapeConstPtr &object_shape, const geometry_msgs::Quaternion &object_orientation, double resolution, double delta_height=0.01, unsigned int num_heights=2) const |
Generate possible place poses on the table for a given object. This chooses appropriate values for min_distance_from_edge and for height_above_table based on the object properties. The assumption is that the object is represented by a mesh. | |
std::vector < geometry_msgs::PoseStamped > | generatePlacePoses (const object_recognition_msgs::Table &table, const shapes::ShapeConstPtr &object_shape, const geometry_msgs::Quaternion &object_orientation, double resolution, double delta_height=0.01, unsigned int num_heights=2) const |
Generate possible place poses on the table for a given object. This chooses appropriate values for min_distance_from_edge and for height_above_table based on the object properties. The assumption is that the object is represented by a mesh. | |
std::vector < geometry_msgs::PoseStamped > | generatePlacePoses (const object_recognition_msgs::Table &table, double resolution, double height_above_table, double delta_height=0.01, unsigned int num_heights=2, double min_distance_from_edge=0.10) const |
Generate possible place poses on the table. This samples locations in a grid on the table at the given resolution (in meters) in both X and Y directions. The locations are sampled at the specified height above the table (in meters) and then at subsequent additional heights (num_heights times) incremented by delta_height. Locations are only accepted if they are at least min_distance_from_edge meters from the edge of the table. | |
visualization_msgs::MarkerArray | getPlaceLocationsMarker (const std::vector< geometry_msgs::PoseStamped > &poses) const |
std::vector< std::string > | getTableNamesInROI (double minx, double miny, double minz, double maxx, double maxy, double maxz) const |
Get all the tables within a region of interest. | |
object_recognition_msgs::TableArray | getTablesInROI (double minx, double miny, double minz, double maxx, double maxy, double maxz) const |
Get all the tables within a region of interest. | |
bool | isInsideTableContour (const geometry_msgs::Pose &pose, const object_recognition_msgs::Table &table, double min_distance_from_edge=0.0, double min_vertical_offset=0.0) const |
SemanticWorld (const planning_scene::PlanningSceneConstPtr &planning_scene) | |
A (simple) semantic world representation for pick and place and other tasks. Currently this is used only to represent tables. | |
Private Member Functions | |
shapes::Mesh * | createSolidMeshFromPlanarPolygon (const shapes::Mesh &polygon, double thickness) const |
shapes::Mesh * | orientPlanarPolygon (const shapes::Mesh &polygon) const |
void | tableCallback (const object_recognition_msgs::TableArrayPtr &msg) |
void | transformTableArray (object_recognition_msgs::TableArray &table_array) const |
Private Attributes | |
ros::Publisher | collision_object_publisher_ |
std::map< std::string, object_recognition_msgs::Table > | current_tables_in_collision_world_ |
ros::NodeHandle | node_handle_ |
std::vector < geometry_msgs::PoseStamped > | place_poses_ |
planning_scene::PlanningSceneConstPtr | planning_scene_ |
ros::Publisher | planning_scene_diff_publisher_ |
object_recognition_msgs::TableArray | table_array_ |
TableCallbackFn | table_callback_ |
ros::Subscriber | table_subscriber_ |
ros::Publisher | visualization_publisher_ |
A (simple) semantic world representation for pick and place and other tasks.
Definition at line 58 of file semantic_world.h.
typedef boost::function<void()> moveit::semantic_world::SemanticWorld::TableCallbackFn |
The signature for a callback on receiving table messages.
Definition at line 62 of file semantic_world.h.
moveit::semantic_world::SemanticWorld::SemanticWorld | ( | const planning_scene::PlanningSceneConstPtr & | planning_scene | ) |
A (simple) semantic world representation for pick and place and other tasks. Currently this is used only to represent tables.
Definition at line 57 of file semantic_world.cpp.
void moveit::semantic_world::SemanticWorld::addTableCallback | ( | const TableCallbackFn & | table_callback | ) | [inline] |
Definition at line 121 of file semantic_world.h.
Definition at line 94 of file semantic_world.cpp.
void moveit::semantic_world::SemanticWorld::clear | ( | void | ) |
Definition at line 205 of file semantic_world.cpp.
shapes::Mesh * moveit::semantic_world::SemanticWorld::createSolidMeshFromPlanarPolygon | ( | const shapes::Mesh & | polygon, |
double | thickness | ||
) | const [private] |
Definition at line 559 of file semantic_world.cpp.
std::string moveit::semantic_world::SemanticWorld::findObjectTable | ( | const geometry_msgs::Pose & | pose, |
double | min_distance_from_edge = 0.0 , |
||
double | min_vertical_offset = 0.0 |
||
) | const |
Definition at line 454 of file semantic_world.cpp.
std::vector< geometry_msgs::PoseStamped > moveit::semantic_world::SemanticWorld::generatePlacePoses | ( | const std::string & | table_name, |
const shapes::ShapeConstPtr & | object_shape, | ||
const geometry_msgs::Quaternion & | object_orientation, | ||
double | resolution, | ||
double | delta_height = 0.01 , |
||
unsigned int | num_heights = 2 |
||
) | const |
Generate possible place poses on the table for a given object. This chooses appropriate values for min_distance_from_edge and for height_above_table based on the object properties. The assumption is that the object is represented by a mesh.
Definition at line 212 of file semantic_world.cpp.
std::vector< geometry_msgs::PoseStamped > moveit::semantic_world::SemanticWorld::generatePlacePoses | ( | const object_recognition_msgs::Table & | table, |
const shapes::ShapeConstPtr & | object_shape, | ||
const geometry_msgs::Quaternion & | object_orientation, | ||
double | resolution, | ||
double | delta_height = 0.01 , |
||
unsigned int | num_heights = 2 |
||
) | const |
Generate possible place poses on the table for a given object. This chooses appropriate values for min_distance_from_edge and for height_above_table based on the object properties. The assumption is that the object is represented by a mesh.
Definition at line 231 of file semantic_world.cpp.
std::vector< geometry_msgs::PoseStamped > moveit::semantic_world::SemanticWorld::generatePlacePoses | ( | const object_recognition_msgs::Table & | table, |
double | resolution, | ||
double | height_above_table, | ||
double | delta_height = 0.01 , |
||
unsigned int | num_heights = 2 , |
||
double | min_distance_from_edge = 0.10 |
||
) | const |
Generate possible place poses on the table. This samples locations in a grid on the table at the given resolution (in meters) in both X and Y directions. The locations are sampled at the specified height above the table (in meters) and then at subsequent additional heights (num_heights times) incremented by delta_height. Locations are only accepted if they are at least min_distance_from_edge meters from the edge of the table.
Definition at line 306 of file semantic_world.cpp.
visualization_msgs::MarkerArray moveit::semantic_world::SemanticWorld::getPlaceLocationsMarker | ( | const std::vector< geometry_msgs::PoseStamped > & | poses | ) | const |
Definition at line 67 of file semantic_world.cpp.
std::vector< std::string > moveit::semantic_world::SemanticWorld::getTableNamesInROI | ( | double | minx, |
double | miny, | ||
double | minz, | ||
double | maxx, | ||
double | maxy, | ||
double | maxz | ||
) | const |
Get all the tables within a region of interest.
Definition at line 188 of file semantic_world.cpp.
object_recognition_msgs::TableArray moveit::semantic_world::SemanticWorld::getTablesInROI | ( | double | minx, |
double | miny, | ||
double | minz, | ||
double | maxx, | ||
double | maxy, | ||
double | maxz | ||
) | const |
Get all the tables within a region of interest.
Definition at line 171 of file semantic_world.cpp.
bool moveit::semantic_world::SemanticWorld::isInsideTableContour | ( | const geometry_msgs::Pose & | pose, |
const object_recognition_msgs::Table & | table, | ||
double | min_distance_from_edge = 0.0 , |
||
double | min_vertical_offset = 0.0 |
||
) | const |
Definition at line 387 of file semantic_world.cpp.
shapes::Mesh * moveit::semantic_world::SemanticWorld::orientPlanarPolygon | ( | const shapes::Mesh & | polygon | ) | const [private] |
Definition at line 505 of file semantic_world.cpp.
void moveit::semantic_world::SemanticWorld::tableCallback | ( | const object_recognition_msgs::TableArrayPtr & | msg | ) | [private] |
Definition at line 467 of file semantic_world.cpp.
void moveit::semantic_world::SemanticWorld::transformTableArray | ( | object_recognition_msgs::TableArray & | table_array | ) | const [private] |
Definition at line 480 of file semantic_world.cpp.
Definition at line 155 of file semantic_world.h.
std::map<std::string, object_recognition_msgs::Table> moveit::semantic_world::SemanticWorld::current_tables_in_collision_world_ [private] |
Definition at line 149 of file semantic_world.h.
Definition at line 143 of file semantic_world.h.
std::vector<geometry_msgs::PoseStamped> moveit::semantic_world::SemanticWorld::place_poses_ [private] |
Definition at line 147 of file semantic_world.h.
planning_scene::PlanningSceneConstPtr moveit::semantic_world::SemanticWorld::planning_scene_ [private] |
Definition at line 141 of file semantic_world.h.
Definition at line 159 of file semantic_world.h.
object_recognition_msgs::TableArray moveit::semantic_world::SemanticWorld::table_array_ [private] |
Definition at line 145 of file semantic_world.h.
Definition at line 157 of file semantic_world.h.
Definition at line 153 of file semantic_world.h.
Definition at line 155 of file semantic_world.h.