robowflex::Scene Class Reference

Wrapper class around the planning scene and collision geometry. More...

#include <scene.h>

Inheritance diagram for robowflex::Scene:

Classes
class	CollisionPluginLoader
	The actual plugin loader for collision plugins. Heavily inspired by code in moveit_ros/planning. More...

Public Member Functions
	Scene (const RobotConstPtr &robot)
	Constructor. More...
	Scene (const robot_model::RobotModelConstPtr &robot)
	Constructor. More...
	Scene (const Scene &scene)
	Copy Constructor. More...
void	operator= (const Scene &scene)
	Assignment Copy Constructor. More...
ScenePtr	deepCopy () const
	Deep Copy. More...
Getters and Setters
const planning_scene::PlanningScenePtr &	getSceneConst () const
	Get a const reference to the planning scene. More...
planning_scene::PlanningScenePtr &	getScene ()
	Get a reference to the planning scene. More...
moveit_msgs::PlanningScene	getMessage () const
	Get the message that describes the current planning scene. More...
robot_state::RobotState &	getCurrentState ()
	Get a reference to the current robot state in the planning scene. More...
const robot_state::RobotState &	getCurrentStateConst () const
	Get a constant reference to the current robot state in the planning scene. More...
collision_detection::AllowedCollisionMatrix &	getACM ()
	Get the current allowed collision matrix of the planning scene. More...
const collision_detection::AllowedCollisionMatrix &	getACMConst () const
	Get the current allowed collision matrix of the planning scene. More...
void	useMessage (const moveit_msgs::PlanningScene &msg, bool diff=false)
	Set the planning scene to be the same as a message. More...
Collision Object Management
void	updateCollisionObject (const std::string &name, const GeometryConstPtr &geometry, const RobotPose &pose)
	Adds or updates collision object in the planning scene. If the geometry reference is the same, the collision object is updated. Otherwise, the old object named name is deleted and a new one is created. More...
bool	hasObject (const std::string &name) const
	Returns true if the object name is in the scene. More...
std::vector< std::string >	getCollisionObjects () const
	Returns a list of all the names of collision objects in the scene. More...
GeometryPtr	getObjectGeometry (const std::string &name) const
	Returns a representation of a collision object in the scene as a Geometry. If the object has multiple geometries, returns the first. More...
void	removeCollisionObject (const std::string &name)
	Removes an object from the planning scene. More...
RobotPose	getObjectPose (const std::string &name) const
	Get the current pose of a collision object. If the object has multiple geometries, returns the pose of the first. More...
RobotPose	getObjectGraspPose (const std::string &name, const RobotPose &offset) const
	Get a pose in the global frame relative to a collision object for grasping. More...
bool	moveAllObjectsGlobal (const RobotPose &transform)
	Move all objects according to a transform specified in world frame. More...
bool	moveObjectGlobal (const std::string &name, const RobotPose &transform)
	Move all shapes in an object according to the given transform specified in world frame. More...
bool	moveObjectLocal (const std::string &name, const RobotPose &transform)
	Move all shapes in an object according to the given transform specified in object frame. If the object has multiple shapes, the first one is considered the local frame. More...
RobotPose	getFramePose (const std::string &id) const
	Get the pose of a particular frame in the scene. Example, use this to get the pose from /world to /base_link. More...
bool	setCollisionDetector (const std::string &detector_name) const
	Attempts to set the collision detector plugin used by the scene to name. In MoveIt by default, 'Hybrid' is the only plugin defined. However, you can define your own libraries that contain classes that extend the collision_detection::CollisionPlugin class, and load them here as well. See http://moveit.ros.org/documentation/plugins/#collisionplugin for more details. More...
bool	attachObjectToState (robot_state::RobotState &state, const std::string &name) const
	Attach the named collision object name to the default end-effector of the given robot state. Only works if there is one end-effector in the system. Uses all end-effector links as allowed touch links. More...
bool	attachObjectToState (robot_state::RobotState &state, const std::string &name, const std::string &ee_link, const std::vector< std::string > &touch_links) const
	Attach the named collision object name to the link ee_link of the given robot state. More...
bool	attachObject (const std::string &name)
	Helper function that attaches object to internal state and removes from scene. More...
bool	attachObject (robot_state::RobotState &state, const std::string &name)
	Helper function that attaches object provided state and removes from scene. More...
bool	attachObject (robot_state::RobotState &state, const std::string &name, const std::string &ee_link, const std::vector< std::string > &touch_links)
	Helper function that attaches object provided state and removes from scene. More...
bool	detachObject (const std::string &name)
	Helper function that detaches the object from the internal scene state. More...
bool	detachObject (robot_state::RobotState &state, const std::string &name)
	Detach an object name from the robot state. More...
Checking Collisions
collision_detection::CollisionResult	checkCollision (const robot_state::RobotState &state, const collision_detection::CollisionRequest &request={}) const
	Check if a robot state is in collision. More...
double	distanceToCollision (const robot_state::RobotState &state) const
	Get the distance to collision for a robot state. More...
double	distanceToObject (const robot_state::RobotState &state, const std::string &object) const
	Get the distance to collision to a specific object. More...
double	distanceBetweenObjects (const std::string &one, const std::string &two) const
	Get the distance to collision between two collision objects in the scene. More...
double	distanceACM (const robot_state::RobotState &state, const collision_detection::AllowedCollisionMatrix &acm) const
	Get the distance to collision using an ACM. More...
void	clearACM (collision_detection::AllowedCollisionMatrix &acm) const
	Disables collision between all entries in the ACM (all robot links and objects in the scene) More...
moveit::core::GroupStateValidityCallbackFn	getGSVCF (bool verbose) const
	Get the group state validity callback function that uses this scene. More...
IO
bool	toYAMLFile (const std::string &file) const
	Serialize the current planning scene to a YAML file. More...
bool	fromYAMLFile (const std::string &file)
	Load a planning scene from a YAML file. More...
bool	fromOpenRAVEXMLFile (const std::string &file, std::string models_dir="")
Public Member Functions inherited from robowflex::ID
	ID ()
	Constructor. More...
const std::string &	getID () const
	Get the unique ID for this object. More...
std::size_t	getVersion () const
	Get the current version of this object. More...
Key	getKey () const
	Get this ID as a Key. More...
bool	operator== (const ID &b) const
	Compare with another ID object. More...
bool	operator== (const Key &b) const
	Compare with an ID Key. More...

Private Member Functions
void	fixCollisionObjectFrame (moveit_msgs::PlanningScene &msg)
	Corrects frame mismatches on loaded scenes by using the current root frame instead. More...

Private Attributes
CollisionPluginLoaderPtr	loader_
	Plugin loader that sets collision detectors for the scene. More...
planning_scene::PlanningScenePtr	scene_
	Underlying planning scene. More...

Additional Inherited Members
Public Types inherited from robowflex::ID
using	Key = std::pair< std::string, std::size_t >
	A snapshot of the state of an ID. Can be compared against another ID. More...
Static Public Member Functions inherited from robowflex::ID
static Key	getNullKey ()
	Get a null key for initialization. More...
Protected Member Functions inherited from robowflex::ID
void	incrementVersion ()
	Increment the version number of this object. More...

Detailed Description

Wrapper class around the planning scene and collision geometry.

The Scene class is a wrapper around MoveIt!'s planning_scene::PlanningScene, providing access to set and manipulate collision objects, attach and detach objects to the robot, and so on. There are also utilities to load and save planning scenes from YAML files (toYAMLFile() and fromYAMLFile()). Note that this class has its own robot state, separate from the one in the provided Robot. Information between this state and the Robot's scratch state are not synchronized, you must do this manually.

Definition at line 47 of file scene.h.

Constructor & Destructor Documentation

Scene() [1/3]

Scene::Scene

(

const RobotConstPtr &

robot

)

Constructor.

Parameters

[in]

robot

Robot to construct planning scene for.

Definition at line 145 of file scene.cpp.

  : loader_(new CollisionPluginLoader()), scene_(new planning_scene::PlanningScene(robot->getModelConst()))
{
}

Scene() [2/3]

Scene::Scene

(

const robot_model::RobotModelConstPtr &

robot

)

Constructor.

Parameters

[in]

robot

Robot to construct planning scene for.

Definition at line 150 of file scene.cpp.

  : loader_(new CollisionPluginLoader()), scene_(new planning_scene::PlanningScene(robot))
{
}

Scene() [3/3]

Scene::Scene

(

const Scene &

scene

)

Copy Constructor.

Parameters

[in]

scene

Scene to copy.

Definition at line 155 of file scene.cpp.

                               : loader_(new CollisionPluginLoader()), scene_(other.getSceneConst())
{
}

Member Function Documentation

attachObject() [1/3]

bool Scene::attachObject

(

const std::string &

name

)

Helper function that attaches object to internal state and removes from scene.

Parameters

[in]

name

Name of object to attach to robot and remove from scene.

Returns

True on success, false on failure.

Definition at line 403 of file scene.cpp.

{
    return attachObject(getCurrentState(), name);
}

attachObject() [2/3]

bool Scene::attachObject	(	robot_state::RobotState &	state,
		const std::string &	name
	)

Helper function that attaches object provided state and removes from scene.

Parameters

[in]

name

Name of object to attach to robot and remove from scene.

Returns

True on success, false on failure.

Definition at line 408 of file scene.cpp.

{
    if (attachObjectToState(state, name))
    {
        removeCollisionObject(name);
        return true;
    }
 
    return false;
}

attachObject() [3/3]

bool Scene::attachObject	(	robot_state::RobotState &	state,
		const std::string &	name,
		const std::string &	ee_link,
		const std::vector< std::string > &	touch_links
	)

Helper function that attaches object provided state and removes from scene.

Parameters

[in]	state	State of the robot to attach to.
[in]	name	Name of object to attach to.
[in]	ee_link	Link to attach object to.
[in]	touch_links	Links the object is allowed to touch.

Returns

True on success, false on failure.

Definition at line 419 of file scene.cpp.

{
    if (attachObjectToState(state, name, ee_link, touch_links))
    {
        removeCollisionObject(name);
        return true;
    }
 
    return false;
}

attachObjectToState() [1/2]

bool Scene::attachObjectToState	(	robot_state::RobotState &	state,
		const std::string &	name
	)		const

Attach the named collision object name to the default end-effector of the given robot state. Only works if there is one end-effector in the system. Uses all end-effector links as allowed touch links.

Parameters

[in]	name	Name of collision to attach.
[in]	state	State of robot the object will be attached to

Returns

True on success, false on failure.

Definition at line 354 of file scene.cpp.

{
    const auto &robot = state.getRobotModel();
    const auto &ee = robot->getEndEffectors();
 
    // One end-effector
    if (ee.size() == 1)
    {
        const auto &links = ee[0]->getLinkModelNames();
        return attachObjectToState(state, name, links[0], links);
    }
 
    return false;
}

attachObjectToState() [2/2]

bool Scene::attachObjectToState	(	robot_state::RobotState &	state,
		const std::string &	name,
		const std::string &	ee_link,
		const std::vector< std::string > &	touch_links
	)		const

Attach the named collision object name to the link ee_link of the given robot state.

Parameters

[in]	state	State of the robot to attach.
[in]	name	Name of object to attach.
[in]	ee_link	Link to attach object to.
[in]	touch_links	Links the object is allowed to touch.

Returns

True on success, false on failure.

Definition at line 369 of file scene.cpp.

{
    const auto &world = scene_->getWorld();
    if (!world->hasObject(name))
    {
        RBX_ERROR("World does not have object `%s`", name);
        return false;
    }
 
    const auto &obj = world->getObject(name);
    if (!obj)
    {
        RBX_ERROR("Could not get object `%s`", name);
        return false;
    }
 
    const auto &tf = state.getGlobalLinkTransform(ee_link);
 
    RobotPoseVector poses;
    for (const auto &pose : obj->shape_poses_)
        poses.push_back(tf.inverse() * pose);
 
    std::set<std::string> touch_links_set(touch_links.begin(), touch_links.end());
    auto *body = attachObjectHelper<moveit::core::AttachedBody>(state.getLinkModel(ee_link),  //
                                                                name,                         //
                                                                obj->shapes_,                 //
                                                                poses,                        //
                                                                touch_links_set);
    state.attachBody(body);
 
    return true;
}

checkCollision()

collision_detection::CollisionResult Scene::checkCollision	(	const robot_state::RobotState &	state,
		const collision_detection::CollisionRequest &	request = {}
	)		const

Check if a robot state is in collision.

Parameters

[in]	state	State to check for collision.
[in]	request	Optional request parameters for collision checking.

Returns

The collision result.

Definition at line 466 of file scene.cpp.

{
    collision_detection::CollisionResult result;
    scene_->checkCollision(request, result, state);
 
    return result;
}

clearACM()

void Scene::clearACM

(

collision_detection::AllowedCollisionMatrix &

acm

)

const

Disables collision between all entries in the ACM (all robot links and objects in the scene)

Parameters

[in,out]

acm

ACM to clear.

Definition at line 496 of file scene.cpp.

{
    const auto &links = getCurrentStateConst().getRobotModel()->getLinkModelNames();
    const auto &objs = getCollisionObjects();
 
    // No self-collision distances
    for (unsigned int i = 0; i < links.size(); ++i)
        for (unsigned int j = i + 1; j < links.size(); ++j)
            acm.setEntry(links[i], links[j], true);
 
    // No obstacle collisions
    for (unsigned int i = 0; i < objs.size(); ++i)
        for (unsigned int j = i + 1; j < objs.size(); ++j)
            acm.setEntry(objs[i], objs[j], true);
 
    // Ignore all other objects
    for (const auto &link : links)
        for (const auto &obj : objs)
            acm.setEntry(link, obj, true);
}

deepCopy()

ScenePtr Scene::deepCopy

(

)

const

Deep Copy.

Returns

The deep copied planning scene.

Definition at line 165 of file scene.cpp.

{
    auto scene = std::make_shared<Scene>(scene_->getRobotModel());
    scene->useMessage(getMessage());
 
    return scene;
}

detachObject() [1/2]

bool Scene::detachObject

(

const std::string &

name

)

Helper function that detaches the object from the internal scene state.

Parameters

[in]

name

Name of collision to detach.

Returns

True on success, false on failure.

Definition at line 437 of file scene.cpp.

{
    return detachObject(getCurrentState(), name);
}

detachObject() [2/2]

bool Scene::detachObject	(	robot_state::RobotState &	state,
		const std::string &	name
	)

Detach an object name from the robot state.

Parameters

[in]	state	State to detatch the object from.
[in]	name	Name of collision to detach.

Returns

True on success, false on failure.

Definition at line 442 of file scene.cpp.

{
    incrementVersion();
 
    const auto &world = scene_->getWorldNonConst();
    const auto &body = state.getAttachedBody(name);
 
    if (!body)
    {
        RBX_ERROR("Robot does not have attached object `%s`", name);
        return false;
    }
 
    world->addToObject(name, body->getShapes(), body->getGlobalCollisionBodyTransforms());
 
    if (not state.clearAttachedBody(name))
    {
        RBX_ERROR("Could not detach object `%s`", name);
        return false;
    }
 
    return true;
}

distanceACM()

double Scene::distanceACM	(	const robot_state::RobotState &	state,
		const collision_detection::AllowedCollisionMatrix &	acm
	)		const

Get the distance to collision using an ACM.

Parameters

[in]	state	State of the robot.
[in]	one	acm ACM to use for distance check.

Returns

The distance between all colliding entries in ACM. On error, returns NaN.

Definition at line 480 of file scene.cpp.

{
    collision_detection::DistanceRequest req;
    collision_detection::DistanceResult res;
 
    req.acm = &acm;
 
#if ROBOWFLEX_MOVEIT_VERSION >= ROBOWFLEX_MOVEIT_VERSION_COMPUTE(1, 1, 0)
    scene_->getCollisionEnv()->distanceRobot(req, res, state);
#else
    scene_->getCollisionWorld()->distanceRobot(req, res, *scene_->getCollisionRobot(), state);
#endif
    return res.minimum_distance.distance;
}

distanceBetweenObjects()

double Scene::distanceBetweenObjects	(	const std::string &	one,
		const std::string &	two
	)		const

Get the distance to collision between two collision objects in the scene.

Parameters

[in]	one	One of the objects to check.
[in]	two	The other object to check.

Returns

The distance between the objects. On error, returns NaN.

Definition at line 535 of file scene.cpp.

{
    if (one == two)  // Early terminate if they are the same
        return 0.;
 
    if (not hasObject(one))
    {
        RBX_ERROR("World does not have object `%s`", one);
        return std::numeric_limits<double>::quiet_NaN();
    }
 
    if (not hasObject(two))
    {
        RBX_ERROR("World does not have object `%s`", two);
        return std::numeric_limits<double>::quiet_NaN();
    }
 
    robot_state::RobotState copy = getCurrentStateConst();
    attachObjectToState(copy, one);
 
    collision_detection::AllowedCollisionMatrix acm;
    clearACM(acm);
    acm.setEntry(one, one, true);
    acm.setEntry(one, two, false);
 
    return distanceACM(copy, acm);
}

distanceToCollision()

double Scene::distanceToCollision

(

const robot_state::RobotState &

state

)

const

Get the distance to collision for a robot state.

Parameters

[in]

state

State to get distance to collision for.

Returns

The distance of the state to collision.

Definition at line 475 of file scene.cpp.

{
    return scene_->distanceToCollision(state);
}

distanceToObject()

double Scene::distanceToObject	(	const robot_state::RobotState &	state,
		const std::string &	object
	)		const

Get the distance to collision to a specific object.

Parameters

[in]	state	State of the robot.
[in]	object	Object to check against.

Returns

The distance to collision to the object. On error, returns NaN.

Definition at line 517 of file scene.cpp.

{
    if (not hasObject(object))
    {
        RBX_ERROR("World does not have object `%s`", object);
        return std::numeric_limits<double>::quiet_NaN();
    }
 
    collision_detection::AllowedCollisionMatrix acm;
    clearACM(acm);
 
    // Enable collision to the object of interest
    for (const auto &link : getCurrentStateConst().getRobotModel()->getLinkModelNames())
        acm.setEntry(link, object, false);
 
    return distanceACM(state, acm);
}

fixCollisionObjectFrame()

void Scene::fixCollisionObjectFrame ( moveit_msgs::PlanningScene &  msg )

private

Corrects frame mismatches on loaded scenes by using the current root frame instead.

Parameters

[in,out]

msg

Message to correct frame names for.

Definition at line 223 of file scene.cpp.

{
    for (auto &co : msg.world.collision_objects)
        if (co.header.frame_id.empty() or not scene_->knowsFrameTransform(co.header.frame_id))
            co.header.frame_id = scene_->getRobotModel()->getRootLinkName();
}

fromOpenRAVEXMLFile()

bool Scene::fromOpenRAVEXMLFile	(	const std::string &	file,
		std::string	models_dir = ""
	)

Definition at line 611 of file scene.cpp.

{
    if (models_dir.empty())
        models_dir = IO::resolveParent(file);
 
    moveit_msgs::PlanningScene msg;
    if (!openrave::fromXMLFile(msg, file, models_dir))
        return false;
 
    scene_->usePlanningSceneMsg(msg);
    return true;
}

fromYAMLFile()

bool Scene::fromYAMLFile

(

const std::string &

file

)

Load a planning scene from a YAML file.

Parameters

[in]

file

File to load planning scene from.

Returns

True on success, false on failure.

Definition at line 589 of file scene.cpp.

{
    moveit_msgs::PlanningScene msg;
    if (!IO::fromYAMLFile(msg, file))
        return false;
 
    fixCollisionObjectFrame(msg);
 
    // Add robot_state if loaded scene does not contain one.
    if (msg.robot_state.joint_state.position.empty())
        moveit::core::robotStateToRobotStateMsg(scene_->getCurrentState(), msg.robot_state);
 
    auto acm(getACM());
    useMessage(msg);
 
    // Update ACM only if anything specified.
    if (msg.allowed_collision_matrix.entry_names.empty())
        getACM() = acm;
 
    return true;
}

getACM()

collision_detection::AllowedCollisionMatrix & Scene::getACM

(

)

Get the current allowed collision matrix of the planning scene.

Returns

The allowed collision matrix.

Definition at line 202 of file scene.cpp.

{
    incrementVersion();
    return scene_->getAllowedCollisionMatrixNonConst();
}

getACMConst()

const collision_detection::AllowedCollisionMatrix & Scene::getACMConst

(

)

const

Get the current allowed collision matrix of the planning scene.

Returns

The allowed collision matrix.

Definition at line 208 of file scene.cpp.

{
    return scene_->getAllowedCollisionMatrix();
}

getCollisionObjects()

std::vector< std::string > Scene::getCollisionObjects

(

)

const

Returns a list of all the names of collision objects in the scene.

Returns

A list of all the collision objects in the scene.

Definition at line 247 of file scene.cpp.

{
    const auto &world = scene_->getWorld();
    return world->getObjectIds();
}

getCurrentState()

robot_state::RobotState & Scene::getCurrentState

(

)

Get a reference to the current robot state in the planning scene.

Returns

The planning scene robot.

Definition at line 191 of file scene.cpp.

{
    incrementVersion();
    return scene_->getCurrentStateNonConst();
}

getCurrentStateConst()

const robot_state::RobotState & Scene::getCurrentStateConst

(

)

const

Get a constant reference to the current robot state in the planning scene.

Returns

The planning scene robot.

Definition at line 197 of file scene.cpp.

{
    return scene_->getCurrentState();
}

getFramePose()

RobotPose Scene::getFramePose

(

const std::string &

id

)

const

Get the pose of a particular frame in the scene. Example, use this to get the pose from /world to /base_link.

Parameters

[in]

id

The ID of the frame to look for.

Returns

Pose of the object, Identity if frame is not present.

Definition at line 333 of file scene.cpp.

{
    if (not scene_->knowsFrameTransform(id))
        RBX_WARN("Frame %s in not present in the scene!", id);
 
    return scene_->getFrameTransform(id);
}

getGSVCF()

moveit::core::GroupStateValidityCallbackFn Scene::getGSVCF

(

bool

verbose

)

const

Get the group state validity callback function that uses this scene.

Parameters

[in]

verbose

If true, will have verbose collision output.

Returns

The group state validity function. This function will return true if there is no collision, and false otherwise.

Definition at line 563 of file scene.cpp.

{
    return [this, verbose](robot_state::RobotState *state,            //
                           const moveit::core::JointModelGroup *jmg,  //
                           const double *values)                      //
    {
        state->setJointGroupPositions(jmg, values);
        state->updateCollisionBodyTransforms();
 
        collision_detection::CollisionRequest request;
        request.verbose = verbose;
 
        auto result = this->checkCollision(*state, request);
        return not result.collision;
    };
}

getMessage()

moveit_msgs::PlanningScene Scene::getMessage

(

)

const

Get the message that describes the current planning scene.

Returns

The planning scene message.

Definition at line 184 of file scene.cpp.

{
    moveit_msgs::PlanningScene msg;
    scene_->getPlanningSceneMsg(msg);
    return msg;
}

getObjectGeometry()

GeometryPtr Scene::getObjectGeometry

(

const std::string &

name

)

const

Returns a representation of a collision object in the scene as a Geometry. If the object has multiple geometries, returns the first.

Parameters

[in]

name

Name of the object to extract.

Returns

A representation of the collision object as a Geometry.

Definition at line 253 of file scene.cpp.

{
    const auto &world = scene_->getWorld();
 
    const auto &obj = world->getObject(name);
    if (obj)
        return std::make_shared<Geometry>(*obj->shapes_[0]);
 
    RBX_WARN("Object %s does not exist in scene!", name);
    return nullptr;
}

getObjectGraspPose()

RobotPose Scene::getObjectGraspPose	(	const std::string &	name,
		const RobotPose &	offset
	)		const

Get a pose in the global frame relative to a collision object for grasping.

Parameters

[in]	name	Name of object to get pose for.
[in]	offset	The offset of the grasp in the object's frame.

Returns

Pose of the grasp offset from the object.

Definition at line 286 of file scene.cpp.

{
    if (not hasObject(name))
        throw Exception(1, log::format("Object `%1%` not in scene!", name));
 
    return getObjectPose(name) * offset;
}

getObjectPose()

RobotPose Scene::getObjectPose

(

const std::string &

name

)

const

Get the current pose of a collision object. If the object has multiple geometries, returns the pose of the first.

Parameters

[in]

name

Name of object to get pose for.

Returns

Pose of the object.

Definition at line 270 of file scene.cpp.

{
    const auto &world = scene_->getWorldNonConst();
    const auto &obj = world->getObject(name);
    if (obj)
    {
#if ROBOWFLEX_MOVEIT_VERSION >= ROBOWFLEX_MOVEIT_VERSION_COMPUTE(1, 1, 6)
        return obj->pose_ * obj->shape_poses_[0];
#else
        return obj->shape_poses_[0];
#endif
    }
 
    return RobotPose::Identity();
}

getScene()

planning_scene::PlanningScenePtr & Scene::getScene

(

)

Get a reference to the planning scene.

Returns

The planning scene.

Definition at line 178 of file scene.cpp.

{
    incrementVersion();
    return scene_;
}

getSceneConst()

const planning_scene::PlanningScenePtr & Scene::getSceneConst

(

)

const

Get a const reference to the planning scene.

Returns

The planning scene.

Definition at line 173 of file scene.cpp.

{
    return scene_;
}

hasObject()

bool Scene::hasObject

(

const std::string &

name

)

const

Returns true if the object name is in the scene.

Parameters

[in]

name

Name of the object to look for.

Returns

True if object is in scene, false otherwise.

Definition at line 431 of file scene.cpp.

{
    const auto &world = scene_->getWorld();
    return world->hasObject(name);
}

moveAllObjectsGlobal()

bool Scene::moveAllObjectsGlobal

(

const RobotPose &

transform

)

Move all objects according to a transform specified in world frame.

Parameters

[in]

transform

The transform to move objects in world frame.

Returns

True on success.

Definition at line 294 of file scene.cpp.

{
    bool r = true;
    for (const auto &obj : getCollisionObjects())
    {
        r &= moveObjectGlobal(obj, transform);
        if (not r)
            return r;
    }
 
    return r;
}

moveObjectGlobal()

bool Scene::moveObjectGlobal	(	const std::string &	name,
		const RobotPose &	transform
	)

Move all shapes in an object according to the given transform specified in world frame.

Parameters

[in]	name	Name of the object to move.
[in]	transform	The transform to move the object in world frame.

Returns

True on success.

Definition at line 307 of file scene.cpp.

{
    incrementVersion();
 
    bool success = false;
#if ROBOWFLEX_AT_LEAST_KINETIC
    const auto &world = scene_->getWorldNonConst();
    success = world->moveObject(name, transform);
#endif
    if (not success)
        RBX_ERROR("Failed to move object %s", name);
 
    return success;
}

moveObjectLocal()

bool Scene::moveObjectLocal	(	const std::string &	name,
		const RobotPose &	transform
	)

Move all shapes in an object according to the given transform specified in object frame. If the object has multiple shapes, the first one is considered the local frame.

Parameters

[in]	name	Name of the object to move.
[in]	transform	The transform to move the object in object frame.

Returns

True on success.

Definition at line 322 of file scene.cpp.

{
    incrementVersion();
 
    const auto pose = getObjectPose(name);
    const auto global_tf = pose * transform * pose.inverse();
 
    bool success = moveObjectGlobal(name, global_tf);
    return success;
}

operator=()

void Scene::operator=

(

const Scene &

scene

)

Assignment Copy Constructor.

Parameters

[in]

scene

Scene to copy.

Definition at line 159 of file scene.cpp.

{
    incrementVersion();
    scene_ = other.getSceneConst();
}

removeCollisionObject()

void Scene::removeCollisionObject

(

const std::string &

name

)

Removes an object from the planning scene.

Parameters

[in]

name

Name of object to remove.

Definition at line 265 of file scene.cpp.

{
    scene_->getWorldNonConst()->removeObject(name);
}

setCollisionDetector()

bool Scene::setCollisionDetector

(

const std::string &

detector_name

)

const

Attempts to set the collision detector plugin used by the scene to name. In MoveIt by default, 'Hybrid' is the only plugin defined. However, you can define your own libraries that contain classes that extend the collision_detection::CollisionPlugin class, and load them here as well. See http://moveit.ros.org/documentation/plugins/#collisionplugin for more details.

Parameters

[in]

detector_name

Name of the collision detection plugin.

Returns

True if sucessful, false otherwise.

Definition at line 341 of file scene.cpp.

{
    bool success = true;
    if (not loader_->activate(detector_name, scene_, true))
    {
        success = false;
        RBX_WARN("Was not able to load collision detector plugin '%s'", detector_name);
    }
 
    RBX_INFO("Using collision detector: %s", scene_->getActiveCollisionDetectorName());
    return success;
}

toYAMLFile()

bool Scene::toYAMLFile

(

const std::string &

file

)

const

Serialize the current planning scene to a YAML file.

Parameters

[in]

file

File to serialize planning scene to.

Returns

True on success, false on failure.

Definition at line 580 of file scene.cpp.

{
    moveit_msgs::PlanningScene msg;
    scene_->getPlanningSceneMsg(msg);
 
    YAML::Node node = IO::toNode(msg);
    return IO::YAMLToFile(node, file);
}

updateCollisionObject()

void Scene::updateCollisionObject	(	const std::string &	name,
		const GeometryConstPtr &	geometry,
		const RobotPose &	pose
	)

Adds or updates collision object in the planning scene. If the geometry reference is the same, the collision object is updated. Otherwise, the old object named name is deleted and a new one is created.

Parameters

[in]	name	Name of object to add or update.
[in]	geometry	Geometry of object.
[in]	pose	Pose of object.

Definition at line 230 of file scene.cpp.

{
    incrementVersion();
 
    const auto &world = scene_->getWorldNonConst();
    if (world->hasObject(name))
    {
        if (!world->moveShapeInObject(name, geometry->getShape(), pose))
            world->removeObject(name);
        else
            return;
    }
 
    world->addToObject(name, geometry->getShape(), pose);
}

useMessage()

void Scene::useMessage	(	const moveit_msgs::PlanningScene &	msg,
		bool	diff = false
	)

Set the planning scene to be the same as a message.

Parameters

[in]	msg	Message to use to set planning scene.
[in]	diff	If true, uses the message as a diff.

Definition at line 213 of file scene.cpp.

{
    incrementVersion();
 
    if (!diff)
        scene_->setPlanningSceneMsg(msg);
    else
        scene_->setPlanningSceneDiffMsg(msg);
}

Member Data Documentation

loader_

CollisionPluginLoaderPtr robowflex::Scene::loader_

private

Plugin loader that sets collision detectors for the scene.

Definition at line 350 of file scene.h.

scene_

planning_scene::PlanningScenePtr robowflex::Scene::scene_

private

Underlying planning scene.

Definition at line 351 of file scene.h.

---

The documentation for this class was generated from the following files:

• scene.h
• scene.cpp