MTC
Classes | Public Types | Public Member Functions | Protected Member Functions | Protected Attributes | List of all members
moveit::task_constructor::stages::ModifyPlanningScene Class Reference

#include <modify_planning_scene.h>

Inheritance diagram for moveit::task_constructor::stages::ModifyPlanningScene:
Inheritance graph
[legend]
Collaboration diagram for moveit::task_constructor::stages::ModifyPlanningScene:
Collaboration graph
[legend]

Classes

struct  CollisionMatrixPairs
 

Public Types

using Names = std::vector< std::string >
 
using ApplyCallback = std::function< void(const planning_scene::PlanningScenePtr &, const PropertyMap &)>
 
- Public Types inherited from moveit::task_constructor::PropagatingEitherWay
enum  Direction { AUTO = 0x00, FORWARD = 0x01, BACKWARD = 0x02 }
 
- Public Types inherited from moveit::task_constructor::Stage
enum  PropertyInitializerSource { DEFAULT = 0, MANUAL = 1, PARENT = 2, INTERFACE = 4 }
 
using pointer = std::unique_ptr< Stage >
 
using SolutionCallback = std::function< void(const SolutionBase &)>
 
using SolutionCallbackList = std::list< SolutionCallback >
 

Public Member Functions

 ModifyPlanningScene (const std::string &name="modify planning scene")
 
void computeForward (const InterfaceState &from) override
 
void computeBackward (const InterfaceState &to) override
 
void setCallback (const ApplyCallback &cb)
 call an arbitrary function
 
void attachObjects (const Names &objects, const std::string &attach_link, bool attach)
 attach or detach a list of objects to the given link
 
void addObject (const moveit_msgs::CollisionObject &collision_object)
 Add an object to the planning scene.
 
void removeObject (const std::string &object_name)
 Remove an object from the planning scene.
 
void moveObject (const moveit_msgs::CollisionObject &collision_object)
 Move an object from the planning scene.
 
void attachObject (const std::string &object, const std::string &link)
 conviency methods accepting a single object name
 
void detachObject (const std::string &object, const std::string &link)
 
template<typename T , typename E = typename std::enable_if_t< is_container<T>::value && std::is_base_of<std::string, typename T::value_type>::value>>
void attachObjects (const T &objects, const std::string &link)
 conviency methods accepting any container of object names
 
template<typename T , typename E = typename std::enable_if_t< is_container<T>::value && std::is_base_of<std::string, typename T::value_type>::value>>
void detachObjects (const T &objects, const std::string &link)
 
void allowCollisions (const Names &first, const Names &second, bool allow)
 allow / forbid collisions for each combination of pairs in first and second lists
 
void allowCollisions (const std::string &first, const std::string &second, bool allow)
 allow / forbid collisions for pair (first, second)
 
void allowCollisions (const std::string &object, bool allow)
 allow / forbid all collisions for given object
 
template<typename T1 , typename T2 , typename E1 = typename std::enable_if_t<is_container<T1>::value && std::is_convertible<typename T1::value_type, std::string>::value>, typename E2 = typename std::enable_if_t<is_container<T2>::value && std::is_convertible<typename T1::value_type, std::string>::value>>
void allowCollisions (const T1 &first, const T2 &second, bool enable_collision)
 conveniency method accepting arbitrary container types
 
template<typename T , typename E = typename std::enable_if_t< is_container<T>::value && std::is_convertible<typename T::value_type, std::string>::value>>
void allowCollisions (const std::string &first, const T &second, bool enable_collision)
 conveniency method accepting std::string and an arbitrary container of names
 
template<typename T , typename E = typename std::enable_if_t< is_container<T>::value && std::is_convertible<typename T::value_type, std::string>::value>>
void allowCollisions (const char *first, const T &second, bool enable_collision)
 conveniency method accepting const char* and an arbitrary container of names
 
void allowCollisions (const std::string &first, const moveit::core::JointModelGroup &jmg, bool allow)
 conveniency method accepting std::string and JointModelGroup
 
- Public Member Functions inherited from moveit::task_constructor::PropagatingEitherWay
 PropagatingEitherWay (const std::string &name="propagating either way")
 
void restrictDirection (Direction dir)
 
- Public Member Functions inherited from moveit::task_constructor::Stage
virtual void reset ()
 reset stage, clearing all solutions, interfaces, inherited properties.
 
virtual void init (const moveit::core::RobotModelConstPtr &robot_model)
 
const ContainerBaseparent () const
 
const std::string & name () const
 
void setName (const std::string &name)
 
uint32_t introspectionId () const
 
Introspectionintrospection () const
 
void setTimeout (double timeout)
 
double timeout () const
 timeout of stage per computation
 
void setMarkerNS (const std::string &marker_ns)
 
const std::string & markerNS ()
 marker namespace of solution markers
 
void setTrajectoryExecutionInfo (TrajectoryExecutionInfo trajectory_execution_info)
 Set and get info to use when executing the stage's trajectory.
 
TrajectoryExecutionInfo trajectoryExecutionInfo () const
 
void forwardProperties (const InterfaceState &source, InterfaceState &dest)
 forwarding of properties between interface states
 
std::set< std::string > forwardedProperties () const
 
void setForwardedProperties (const std::set< std::string > &names)
 
SolutionCallbackList::const_iterator addSolutionCallback (SolutionCallback &&cb)
 add function to be called for every newly found solution or failure
 
void removeSolutionCallback (SolutionCallbackList::const_iterator which)
 remove function callback
 
void setCostTerm (const CostTermConstPtr &term)
 
template<typename T , typename = std::enable_if_t<std::is_constructible<LambdaCostTerm, T>::value>>
void setCostTerm (T term)
 
const ordered< SolutionBaseConstPtr > & solutions () const
 
const std::list< SolutionBaseConstPtr > & failures () const
 
size_t numFailures () const
 
void silentFailure ()
 Call to increase number of failures w/o storing a (failure) trajectory.
 
bool storeFailures () const
 Should we generate failure solutions? Note: Always report a failure!
 
virtual void explainFailure (std::ostream &os) const
 
PropertyMapproperties ()
 Get the stage's property map.
 
const PropertyMapproperties () const
 
void setProperty (const std::string &name, const boost::any &value)
 Set a previously declared property to a new value.
 
void setProperty (const std::string &name, const char *value)
 overload: const char* values are stored as std::string
 
void reportPropertyError (const Property::error &e)
 Analyze source of error and report accordingly.
 
double getTotalComputeTime () const
 

Protected Member Functions

std::pair< InterfaceState, SubTrajectoryapply (const InterfaceState &from, bool invert)
 
void processCollisionObject (planning_scene::PlanningScene &scene, const moveit_msgs::CollisionObject &object, bool invert)
 
void attachObjects (planning_scene::PlanningScene &scene, const std::pair< std::string, std::pair< Names, bool >> &pair, bool invert)
 
void allowCollisions (planning_scene::PlanningScene &scene, const CollisionMatrixPairs &pairs, bool invert)
 
- Protected Member Functions inherited from moveit::task_constructor::PropagatingEitherWay
 PropagatingEitherWay (PropagatingEitherWayPrivate *impl)
 
template<Interface::Direction dir>
void send (const InterfaceState &start, InterfaceState &&end, SubTrajectory &&trajectory)
 
void sendForward (const InterfaceState &from, InterfaceState &&to, SubTrajectory &&trajectory)
 
void sendBackward (InterfaceState &&from, const InterfaceState &to, SubTrajectory &&trajectory)
 
- Protected Member Functions inherited from moveit::task_constructor::ComputeBase
 ComputeBase (ComputeBasePrivate *impl)
 ComputeBase can only be instantiated by derived classes in stage.cpp.
 
- Protected Member Functions inherited from moveit::task_constructor::Stage
 Stage (StagePrivate *impl)
 Stage can only be instantiated through derived classes.
 
 Stage (const Stage &)=delete
 Stage cannot be copied.
 

Protected Attributes

std::map< std::string, std::pair< Names, bool > > attach_objects_
 
std::vector< moveit_msgs::CollisionObject > collision_objects_
 
std::list< CollisionMatrixPairscollision_matrix_edits_
 
ApplyCallback callback_
 
- Protected Attributes inherited from moveit::task_constructor::Stage
StagePrivate * pimpl_
 

Detailed Description

Allow modification of planning scene

This stage takes the incoming planning scene and applies previously scheduled changes to it, for example:


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