37 #ifndef MOVEIT_PLANNING_INTERFACE_PLANNING_SCENE_INTERFACE_ 38 #define MOVEIT_PLANNING_INTERFACE_PLANNING_SCENE_INTERFACE_ 42 #include <moveit_msgs/ObjectColor.h> 43 #include <moveit_msgs/CollisionObject.h> 44 #include <moveit_msgs/AttachedCollisionObject.h> 45 #include <moveit_msgs/PlanningScene.h> 72 double maxz,
bool with_type, std::vector<std::string>& types);
78 double maxz,
bool with_type =
false)
80 std::vector<std::string> empty_vector_string;
85 std::map<std::string, geometry_msgs::Pose>
getObjectPoses(
const std::vector<std::string>& object_ids);
89 std::map<std::string, moveit_msgs::CollisionObject>
90 getObjects(
const std::vector<std::string>& object_ids = std::vector<std::string>());
94 std::map<std::string, moveit_msgs::AttachedCollisionObject>
95 getAttachedObjects(
const std::vector<std::string>& object_ids = std::vector<std::string>());
104 const std_msgs::ColorRGBA& object_color);
110 const std::vector<moveit_msgs::CollisionObject>& collision_objects,
111 const std::vector<moveit_msgs::ObjectColor>& object_colors = std::vector<moveit_msgs::ObjectColor>());
132 const std::vector<moveit_msgs::CollisionObject>& collision_objects,
133 const std::vector<moveit_msgs::ObjectColor>& object_colors = std::vector<moveit_msgs::ObjectColor>())
const;
std::vector< std::string > getKnownObjectNamesInROI(double minx, double miny, double minz, double maxx, double maxy, double maxz, bool with_type=false)
Get the names of known objects in the world that are located within a bounding region (specified in t...
bool applyCollisionObject(const moveit_msgs::CollisionObject &collision_object)
Apply collision object to the planning scene of the move_group node synchronously. Other PlanningSceneMonitors will NOT receive the update unless they subscribe to move_group's monitored scene.
bool applyCollisionObjects(const std::vector< moveit_msgs::CollisionObject > &collision_objects, const std::vector< moveit_msgs::ObjectColor > &object_colors=std::vector< moveit_msgs::ObjectColor >())
Apply collision objects to the planning scene of the move_group node synchronously. Other PlanningSceneMonitors will NOT receive the update unless they subscribe to move_group's monitored scene. If object_colors do not specify an id, the corresponding object id from collision_objects is used.
std::map< std::string, moveit_msgs::CollisionObject > getObjects(const std::vector< std::string > &object_ids=std::vector< std::string >())
Get the objects identified by the given object ids list. If no ids are provided, return all the known...
void addCollisionObjects(const std::vector< moveit_msgs::CollisionObject > &collision_objects, const std::vector< moveit_msgs::ObjectColor > &object_colors=std::vector< moveit_msgs::ObjectColor >()) const
Add collision objects to the world via /planning_scene. Make sure object.operation is set to object...
PlanningSceneInterfaceImpl * impl_
std::map< std::string, geometry_msgs::Pose > getObjectPoses(const std::vector< std::string > &object_ids)
Get the poses from the objects identified by the given object ids list.
PlanningSceneInterface(const std::string &ns="")
bool applyAttachedCollisionObjects(const std::vector< moveit_msgs::AttachedCollisionObject > &attached_collision_objects)
Apply attached collision objects to the planning scene of the move_group node synchronously. Other PlanningSceneMonitors will NOT receive the update unless they subscribe to move_group's monitored scene.
bool applyPlanningScene(const moveit_msgs::PlanningScene &ps)
Update the planning_scene of the move_group node with the given ps synchronously. Other PlanningScene...
std::map< std::string, moveit_msgs::AttachedCollisionObject > getAttachedObjects(const std::vector< std::string > &object_ids=std::vector< std::string >())
Get the attached objects identified by the given object ids list. If no ids are provided, return all the attached objects.
bool applyAttachedCollisionObject(const moveit_msgs::AttachedCollisionObject &attached_collision_object)
Apply attached collision object to the planning scene of the move_group node synchronously. Other PlanningSceneMonitors will NOT receive the update unless they subscribe to move_group's monitored scene.
std::vector< std::string > getKnownObjectNames(bool with_type=false)
Get the names of all known objects in the world. If with_type is set to true, only return objects tha...
void removeCollisionObjects(const std::vector< std::string > &object_ids) const
Remove collision objects from the world via /planning_scene.
std::vector< std::string > getKnownObjectNamesInROI(double minx, double miny, double minz, double maxx, double maxy, double maxz, bool with_type, std::vector< std::string > &types)
Get the names of known objects in the world that are located within a bounding region (specified in t...
~PlanningSceneInterface()
MOVEIT_CLASS_FORWARD(MoveGroupInterface)