moveit2
The MoveIt Motion Planning Framework for ROS 2.
|
A (simple) semantic world representation for pick and place and other tasks. More...
#include <semantic_world.h>
Public Types | |
typedef std::function< void()> | TableCallbackFn |
The signature for a callback on receiving table messages. | |
Public Member Functions | |
SemanticWorld (const rclcpp::Node::SharedPtr &node, 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. | |
object_recognition_msgs::msg::TableArray | getTablesInROI (double minx, double miny, double minz, double maxx, double maxy, double maxz) const |
Get all the tables within a region of interest. | |
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. | |
std::vector< geometry_msgs::msg::PoseStamped > | generatePlacePoses (const std::string &table_name, const shapes::ShapeConstPtr &object_shape, const geometry_msgs::msg::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::msg::PoseStamped > | generatePlacePoses (const object_recognition_msgs::msg::Table &table, const shapes::ShapeConstPtr &object_shape, const geometry_msgs::msg::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::msg::PoseStamped > | generatePlacePoses (const object_recognition_msgs::msg::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. | |
void | clear () |
bool | addTablesToCollisionWorld () |
visualization_msgs::msg::MarkerArray | getPlaceLocationsMarker (const std::vector< geometry_msgs::msg::PoseStamped > &poses) const |
void | addTableCallback (const TableCallbackFn &table_callback) |
std::string | findObjectTable (const geometry_msgs::msg::Pose &pose, double min_distance_from_edge=0.0, double min_vertical_offset=0.0) const |
bool | isInsideTableContour (const geometry_msgs::msg::Pose &pose, const object_recognition_msgs::msg::Table &table, double min_distance_from_edge=0.0, double min_vertical_offset=0.0) const |
A (simple) semantic world representation for pick and place and other tasks.
Definition at line 60 of file semantic_world.h.
typedef std::function<void()> moveit::semantic_world::SemanticWorld::TableCallbackFn |
The signature for a callback on receiving table messages.
Definition at line 64 of file semantic_world.h.
moveit::semantic_world::SemanticWorld::SemanticWorld | ( | const rclcpp::Node::SharedPtr & | node, |
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 68 of file semantic_world.cpp.
|
inline |
Definition at line 123 of file semantic_world.h.
bool moveit::semantic_world::SemanticWorld::addTablesToCollisionWorld | ( | ) |
Definition at line 111 of file semantic_world.cpp.
void moveit::semantic_world::SemanticWorld::clear | ( | ) |
Definition at line 222 of file semantic_world.cpp.
std::string moveit::semantic_world::SemanticWorld::findObjectTable | ( | const geometry_msgs::msg::Pose & | pose, |
double | min_distance_from_edge = 0.0 , |
||
double | min_vertical_offset = 0.0 |
||
) | const |
std::vector< geometry_msgs::msg::PoseStamped > moveit::semantic_world::SemanticWorld::generatePlacePoses | ( | const object_recognition_msgs::msg::Table & | table, |
const shapes::ShapeConstPtr & | object_shape, | ||
const geometry_msgs::msg::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 249 of file semantic_world.cpp.
std::vector< geometry_msgs::msg::PoseStamped > moveit::semantic_world::SemanticWorld::generatePlacePoses | ( | const object_recognition_msgs::msg::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 325 of file semantic_world.cpp.
std::vector< geometry_msgs::msg::PoseStamped > moveit::semantic_world::SemanticWorld::generatePlacePoses | ( | const std::string & | table_name, |
const shapes::ShapeConstPtr & | object_shape, | ||
const geometry_msgs::msg::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 229 of file semantic_world.cpp.
visualization_msgs::msg::MarkerArray moveit::semantic_world::SemanticWorld::getPlaceLocationsMarker | ( | const std::vector< geometry_msgs::msg::PoseStamped > & | poses | ) | const |
Definition at line 84 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 205 of file semantic_world.cpp.
object_recognition_msgs::msg::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 188 of file semantic_world.cpp.
bool moveit::semantic_world::SemanticWorld::isInsideTableContour | ( | const geometry_msgs::msg::Pose & | pose, |
const object_recognition_msgs::msg::Table & | table, | ||
double | min_distance_from_edge = 0.0 , |
||
double | min_vertical_offset = 0.0 |
||
) | const |