Skip to content

Commit

Permalink
Refactor and bug fixes
Browse files Browse the repository at this point in the history
  • Loading branch information
@LLipter
LLipter committed Jan 11, 2024
1 parent 394abe6 commit 5c1fee3
Show file tree
Hide file tree
Showing 3 changed files with 80 additions and 98 deletions.
2 changes: 2 additions & 0 deletions include/openrave/environment.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -918,6 +918,8 @@ class OPENRAVE_API EnvironmentBase : public boost::enable_shared_from_this<Envir
/// \param updateMode one of UFIM_X
virtual void UpdateFromInfo(const EnvironmentBaseInfo& info, std::vector<KinBodyPtr>& vCreatedBodies, std::vector<KinBodyPtr>& vModifiedBodies, std::vector<KinBodyPtr>& vRemovedBodies, UpdateFromInfoMode updateMode) = 0;

virtual void RegisterCallbackOnModify(std::function<void(EnvironmentBaseInfoPtr)> callback) = 0;

int _revision = 0; ///< environment current revision
std::string _description; ///< environment description
std::vector<std::string> _keywords; ///< some string values for describinging the environment
Expand Down
141 changes: 66 additions & 75 deletions include/openrave/kinbody.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -671,15 +671,6 @@ class OPENRAVE_API KinBody : public InterfaceBase
/// \brief geometry object holding a link parent and wrapping access to a protected geometry info
class OPENRAVE_API Geometry
{

public:
inline void RegisterCallbackOnModify(std::function<void(KinBody::GeometryInfoPtr)> callback) {
_callbackOnModify = callback;
}
private:
std::function<void(KinBody::GeometryInfoPtr)> _callbackOnModify;


public:
/// \deprecated (12/07/16)
static const GeometryType GeomNone RAVE_DEPRECATED = OpenRAVE::GT_None;
Expand Down Expand Up @@ -944,9 +935,14 @@ class OPENRAVE_API KinBody : public InterfaceBase
return 0;
}

inline void RegisterCallbackOnModify(std::function<void(KinBody::GeometryInfoPtr)> callback) {
_callbackOnModify = callback;
}

protected:
boost::weak_ptr<Link> _parent;
KinBody::GeometryInfo _info; ///< geometry info
std::function<void(KinBody::GeometryInfoPtr)> _callbackOnModify;
#ifdef RAVE_PRIVATE
#ifdef _MSC_VER
friend class OpenRAVEXMLParser::LinkXMLReader;
Expand Down Expand Up @@ -1081,28 +1077,6 @@ class OPENRAVE_API KinBody : public InterfaceBase
/// \brief A rigid body holding all its collision and rendering data.
class OPENRAVE_API Link : public boost::enable_shared_from_this<Link>, public ReadablesContainer
{
public:
inline void RegisterCallbackOnModify(std::function<void(LinkInfoPtr)> callback) {
_callbackOnModify = callback;
for (size_t index=0;index<_vGeometries.size();index++) {
_vGeometries[index]->RegisterCallbackOnModify(
[this](KinBody::GeometryInfoPtr geometryInfo) {
_MergeGeometriesDiff(geometryInfo);
}
);
}
}
private:
std::function<void(LinkInfoPtr)> _callbackOnModify;

inline void _MergeGeometriesDiff(KinBody::GeometryInfoPtr geometryInfo) {
if (_callbackOnModify != nullptr) {
LinkInfoPtr diffInfo = boost::make_shared<KinBody::LinkInfo>();
diffInfo->_vgeometryinfos.push_back(geometryInfo);
_callbackOnModify(diffInfo);
}
}

public:
Link(KinBodyPtr parent); ///< pass in a ODE world
~Link();
Expand Down Expand Up @@ -1423,6 +1397,17 @@ class OPENRAVE_API KinBody : public InterfaceBase
/// \brief update Link according to new LinkInfo, returns false if update cannot be performed and requires InitFromInfo
UpdateFromInfoResult UpdateFromInfo(const KinBody::LinkInfo& info);

inline void RegisterCallbackOnModify(std::function<void(LinkInfoPtr)> callback) {
_callbackOnModify = callback;
for (size_t index=0;index<_vGeometries.size();index++) {
_vGeometries[index]->RegisterCallbackOnModify(
[this](KinBody::GeometryInfoPtr geometryInfo) {
_MergeGeometriesDiff(geometryInfo);
}
);
}
}

protected:
/// \brief enables / disables LinkInfo as well as notifies parent KinBody
void _Enable(bool enable);
Expand All @@ -1432,10 +1417,20 @@ class OPENRAVE_API KinBody : public InterfaceBase
/// \param parameterschanged if true, will
void _Update(bool parameterschanged=true, uint32_t extraParametersChanged=0);

inline void _MergeGeometriesDiff(KinBody::GeometryInfoPtr geometryInfo) {
if (_callbackOnModify != nullptr) {
LinkInfoPtr diffInfo = boost::make_shared<KinBody::LinkInfo>();
diffInfo->_vgeometryinfos.push_back(geometryInfo);
_callbackOnModify(diffInfo);
}
}

std::vector<GeometryPtr> _vGeometries; ///< \see GetGeometries

LinkInfo _info; ///< parameter information of the link

std::function<void(LinkInfoPtr)> _callbackOnModify;

private:
/// Sensitive variables that are auto-generated and should not be modified by the user.
/// @name Private Link Variables
Expand Down Expand Up @@ -1659,13 +1654,6 @@ class OPENRAVE_API KinBody : public InterfaceBase
/// \brief Information about a joint that controls the relationship between two links.
class OPENRAVE_API Joint : public boost::enable_shared_from_this<Joint>, public ReadablesContainer
{
public:
inline void RegisterCallbackOnModify(std::function<void(KinBody::JointInfoPtr)> callback) {
_callbackOnModify = callback;
}
private:
std::function<void(KinBody::JointInfoPtr)> _callbackOnModify;

public:
/// \deprecated 12/10/19
typedef Mimic MIMIC RAVE_DEPRECATED;
Expand Down Expand Up @@ -2144,6 +2132,10 @@ class OPENRAVE_API KinBody : public InterfaceBase
/// \brief update the cached _doflastsetvalues
//virtual void SetDOFLastSetValue(dReal dofvalue, const int iaxis = 0);

inline void RegisterCallbackOnModify(std::function<void(KinBody::JointInfoPtr)> callback) {
_callbackOnModify = callback;
}

protected:
JointInfo _info;

Expand Down Expand Up @@ -2194,6 +2186,8 @@ class OPENRAVE_API KinBody : public InterfaceBase

boost::array<dReal,3> _doflastsetvalues; ///< the last set value by the kinbody (_voffsets not applied). For revolute joints that have a range greater than 2*pi, it is only possible to recover the joint value from the link positions mod 2*pi. In order to recover the branch, multiplies of 2*pi are added/subtracted to this value that is closest to _doflastsetvalues. For circular joints, the last set value can be ignored since they always return a value from [-pi,pi)

std::function<void(KinBody::JointInfoPtr)> _callbackOnModify;

private:
/// Sensitive variables that should not be modified.
/// @name Private Joint Variables
Expand Down Expand Up @@ -2430,43 +2424,6 @@ class OPENRAVE_API KinBody : public InterfaceBase
typedef boost::shared_ptr<KinBodyInfo> KinBodyInfoPtr;
typedef boost::shared_ptr<KinBodyInfo const> KinBodyInfoConstPtr;

public:
inline void RegisterCallbackOnModify(std::function<void(KinBodyInfoPtr)> callback) {
_callbackOnModify = callback;
for (size_t index=0;index<_veclinks.size();index++) {
_veclinks[index]->RegisterCallbackOnModify(
[this](KinBody::LinkInfoPtr linkInfo) {
_MergeLinksDiff(linkInfo);
}
);
}
for (size_t index=0;index<_vecjoints.size();index++) {
_vecjoints[index]->RegisterCallbackOnModify(
[this](KinBody::JointInfoPtr jointInfo) {
_MergeJointsDiff(jointInfo);
}
);
}
}
private:
std::function<void(KinBodyInfoPtr)> _callbackOnModify;

inline void _MergeLinksDiff(KinBody::LinkInfoPtr linkInfo) {
if (_callbackOnModify != nullptr) {
KinBodyInfoPtr diffInfo = boost::make_shared<KinBody::KinBodyInfo>();
diffInfo->_vLinkInfos.push_back(linkInfo);
_callbackOnModify(diffInfo);
}
}
inline void _MergeJointsDiff(KinBody::JointInfoPtr jointInfo) {
if (_callbackOnModify != nullptr) {
KinBodyInfoPtr diffInfo = boost::make_shared<KinBody::KinBodyInfo>();
diffInfo->_vJointInfos.push_back(jointInfo);
_callbackOnModify(diffInfo);
}
}
public:

/// \brief Helper class to save and restore the entire kinbody state.
///
/// Options can be passed to the constructor in order to choose which parameters to save (see \ref SaveParameters)
Expand Down Expand Up @@ -3612,6 +3569,24 @@ class OPENRAVE_API KinBody : public InterfaceBase
return _prAssociatedFileEntries;
}

inline void RegisterCallbackOnModify(std::function<void(KinBodyInfoPtr)> callback) {
_callbackOnModify = callback;
for (size_t index=0;index<_veclinks.size();index++) {
_veclinks[index]->RegisterCallbackOnModify(
[this](KinBody::LinkInfoPtr linkInfo) {
_MergeLinksDiff(linkInfo);
}
);
}
for (size_t index=0;index<_vecjoints.size();index++) {
_vecjoints[index]->RegisterCallbackOnModify(
[this](KinBody::JointInfoPtr jointInfo) {
_MergeJointsDiff(jointInfo);
}
);
}
}

protected:
/// \brief constructors declared protected so that user always goes through environment to create bodies
KinBody(InterfaceType type, EnvironmentBasePtr penv);
Expand Down Expand Up @@ -3706,6 +3681,21 @@ class OPENRAVE_API KinBody : public InterfaceBase

void _SetAdjacentLinksInternal(int linkindex0, int linkindex1);

inline void _MergeLinksDiff(KinBody::LinkInfoPtr linkInfo) {
if (_callbackOnModify != nullptr) {
KinBodyInfoPtr diffInfo = boost::make_shared<KinBody::KinBodyInfo>();
diffInfo->_vLinkInfos.push_back(linkInfo);
_callbackOnModify(diffInfo);
}
}
inline void _MergeJointsDiff(KinBody::JointInfoPtr jointInfo) {
if (_callbackOnModify != nullptr) {
KinBodyInfoPtr diffInfo = boost::make_shared<KinBody::KinBodyInfo>();
diffInfo->_vJointInfos.push_back(jointInfo);
_callbackOnModify(diffInfo);
}
}

std::string _name; ///< name of body

std::vector<JointPtr> _vecjoints; ///< \see GetJoints
Expand Down Expand Up @@ -3767,6 +3757,7 @@ class OPENRAVE_API KinBody : public InterfaceBase
mutable std::string __hashKinematicsGeometryDynamics; ///< hash serializing kinematics, dynamics and geometry properties of the KinBody
int64_t _lastModifiedAtUS=0; ///< us, linux epoch, last modified time of the kinbody when it was originally loaded from the environment.
int64_t _revisionId = 0; ///< the webstack revision for this loaded kinbody
std::function<void(KinBodyInfoPtr)> _callbackOnModify;

private:
mutable std::vector<dReal> _vTempJoints;
Expand Down
35 changes: 12 additions & 23 deletions src/libopenrave-core/environment-core.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1022,14 +1022,6 @@ class Environment : public EnvironmentBase
const uint32_t maskPotentialyChanged(0xffffffff&~KinBody::Prop_JointMimic& ~KinBody::Prop_LinkStatic& ~KinBody::Prop_BodyRemoved& ~KinBody::Prop_LinkGeometry& ~KinBody::Prop_LinkGeometryGroup& ~KinBody::Prop_LinkDynamics);
pbody->_PostprocessChangedParameters(maskPotentialyChanged);
_CallBodyCallbacks(pbody, 1);
EnvironmentBaseInfoPtr diffInfo = boost::make_shared<EnvironmentBaseInfo>();
diffInfo->_description = _description;
_callbackOnModify(diffInfo);
pbody->RegisterCallbackOnModify(
[this](KinBody::KinBodyInfoPtr kinBodyInfo) {
_MergeKinbodyDiff(kinBodyInfo);
}
);
}

virtual void _AddRobot(RobotBasePtr robot, InterfaceAddMode addMode)
Expand Down Expand Up @@ -3350,6 +3342,17 @@ class Environment : public EnvironmentBase
return true;
}

inline void RegisterCallbackOnModify(std::function<void(EnvironmentBaseInfoPtr)> callback) override {
_callbackOnModify = callback;
for (size_t index=0;index<_vecbodies.size();index++) {
_vecbodies[index]->RegisterCallbackOnModify(
[this](KinBody::KinBodyInfoPtr kinBodyInfo) {
_MergeKinbodyDiff(kinBodyInfo);
}
);
}
}

protected:

void _Init()
Expand Down Expand Up @@ -4326,28 +4329,13 @@ class Environment : public EnvironmentBase
_prLoadEnvAlloc->Clear();
}

public:
inline void RegisterCallbackOnModify(std::function<void(EnvironmentBaseInfoPtr)> callback) {
_callbackOnModify = callback;
for (size_t index=0;index<_vecbodies.size();index++) {
_vecbodies[index]->RegisterCallbackOnModify(
[this](KinBody::KinBodyInfoPtr kinBodyInfo) {
_MergeKinbodyDiff(kinBodyInfo);
}
);
}
}
private:
std::function<void(EnvironmentBaseInfoPtr)> _callbackOnModify;

inline void _MergeKinbodyDiff(KinBody::KinBodyInfoPtr kinBodyInfo) {
if (_callbackOnModify != nullptr) {
EnvironmentBaseInfoPtr diffInfo = boost::make_shared<EnvironmentBaseInfo>();
diffInfo->_vBodyInfos.push_back(kinBodyInfo);
_callbackOnModify(diffInfo);
}
}
protected:

std::vector<KinBodyPtr> _vecbodies; ///< all objects that are collidable (includes robots) sorted by env body index ascending order. Note that some element can be nullptr, and size of _vecbodies should be kept unchanged when body is removed from env. protected by _mutexInterfaces. [0] should always be kept null since 0 means no assignment.
std::unordered_map<std::string, int> _mapBodyNameIndex; /// maps body name to env body index of bodies stored in _vecbodies sorted by name. used to lookup kin body by name. protected by _mutexInterfaces
Expand Down Expand Up @@ -4394,6 +4382,7 @@ class Environment : public EnvironmentBase
std::map<std::string, uint64_t> _mapUInt64Parameters; ///< a custom user-driven parameters
std::vector<uint8_t> _vRapidJsonLoadBuffer;
boost::shared_ptr<rapidjson::MemoryPoolAllocator<> > _prLoadEnvAlloc; ///< allocator used for loading environments
std::function<void(EnvironmentBaseInfoPtr)> _callbackOnModify;

bool _bInit; ///< environment is initialized
bool _bEnableSimulation; ///< enable simulation loop
Expand Down

0 comments on commit 5c1fee3

Please sign in to comment.