From 5c1fee38a0312afd70dc266054832f48490eb3ed Mon Sep 17 00:00:00 2001 From: "ran.liao" Date: Wed, 10 Jan 2024 10:15:34 +0900 Subject: [PATCH] Refactor and bug fixes --- include/openrave/environment.h | 2 + include/openrave/kinbody.h | 141 +++++++++++------------- src/libopenrave-core/environment-core.h | 35 ++---- 3 files changed, 80 insertions(+), 98 deletions(-) diff --git a/include/openrave/environment.h b/include/openrave/environment.h index 61d4340647..3a356a054c 100644 --- a/include/openrave/environment.h +++ b/include/openrave/environment.h @@ -918,6 +918,8 @@ class OPENRAVE_API EnvironmentBase : public boost::enable_shared_from_this& vCreatedBodies, std::vector& vModifiedBodies, std::vector& vRemovedBodies, UpdateFromInfoMode updateMode) = 0; + virtual void RegisterCallbackOnModify(std::function callback) = 0; + int _revision = 0; ///< environment current revision std::string _description; ///< environment description std::vector _keywords; ///< some string values for describinging the environment diff --git a/include/openrave/kinbody.h b/include/openrave/kinbody.h index a48f4b6b23..0276db1ba1 100644 --- a/include/openrave/kinbody.h +++ b/include/openrave/kinbody.h @@ -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 callback) { - _callbackOnModify = callback; - } -private: - std::function _callbackOnModify; - - public: /// \deprecated (12/07/16) static const GeometryType GeomNone RAVE_DEPRECATED = OpenRAVE::GT_None; @@ -944,9 +935,14 @@ class OPENRAVE_API KinBody : public InterfaceBase return 0; } + inline void RegisterCallbackOnModify(std::function callback) { + _callbackOnModify = callback; + } + protected: boost::weak_ptr _parent; KinBody::GeometryInfo _info; ///< geometry info + std::function _callbackOnModify; #ifdef RAVE_PRIVATE #ifdef _MSC_VER friend class OpenRAVEXMLParser::LinkXMLReader; @@ -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, public ReadablesContainer { -public: - inline void RegisterCallbackOnModify(std::function callback) { - _callbackOnModify = callback; - for (size_t index=0;index<_vGeometries.size();index++) { - _vGeometries[index]->RegisterCallbackOnModify( - [this](KinBody::GeometryInfoPtr geometryInfo) { - _MergeGeometriesDiff(geometryInfo); - } - ); - } - } -private: - std::function _callbackOnModify; - - inline void _MergeGeometriesDiff(KinBody::GeometryInfoPtr geometryInfo) { - if (_callbackOnModify != nullptr) { - LinkInfoPtr diffInfo = boost::make_shared(); - diffInfo->_vgeometryinfos.push_back(geometryInfo); - _callbackOnModify(diffInfo); - } - } - public: Link(KinBodyPtr parent); ///< pass in a ODE world ~Link(); @@ -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 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); @@ -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(); + diffInfo->_vgeometryinfos.push_back(geometryInfo); + _callbackOnModify(diffInfo); + } + } + std::vector _vGeometries; ///< \see GetGeometries LinkInfo _info; ///< parameter information of the link + std::function _callbackOnModify; + private: /// Sensitive variables that are auto-generated and should not be modified by the user. /// @name Private Link Variables @@ -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, public ReadablesContainer { -public: - inline void RegisterCallbackOnModify(std::function callback) { - _callbackOnModify = callback; - } -private: - std::function _callbackOnModify; - public: /// \deprecated 12/10/19 typedef Mimic MIMIC RAVE_DEPRECATED; @@ -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 callback) { + _callbackOnModify = callback; + } + protected: JointInfo _info; @@ -2194,6 +2186,8 @@ class OPENRAVE_API KinBody : public InterfaceBase boost::array _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 _callbackOnModify; + private: /// Sensitive variables that should not be modified. /// @name Private Joint Variables @@ -2430,43 +2424,6 @@ class OPENRAVE_API KinBody : public InterfaceBase typedef boost::shared_ptr KinBodyInfoPtr; typedef boost::shared_ptr KinBodyInfoConstPtr; -public: - inline void RegisterCallbackOnModify(std::function 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 _callbackOnModify; - - inline void _MergeLinksDiff(KinBody::LinkInfoPtr linkInfo) { - if (_callbackOnModify != nullptr) { - KinBodyInfoPtr diffInfo = boost::make_shared(); - diffInfo->_vLinkInfos.push_back(linkInfo); - _callbackOnModify(diffInfo); - } - } - inline void _MergeJointsDiff(KinBody::JointInfoPtr jointInfo) { - if (_callbackOnModify != nullptr) { - KinBodyInfoPtr diffInfo = boost::make_shared(); - 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) @@ -3612,6 +3569,24 @@ class OPENRAVE_API KinBody : public InterfaceBase return _prAssociatedFileEntries; } + inline void RegisterCallbackOnModify(std::function 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); @@ -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(); + diffInfo->_vLinkInfos.push_back(linkInfo); + _callbackOnModify(diffInfo); + } + } + inline void _MergeJointsDiff(KinBody::JointInfoPtr jointInfo) { + if (_callbackOnModify != nullptr) { + KinBodyInfoPtr diffInfo = boost::make_shared(); + diffInfo->_vJointInfos.push_back(jointInfo); + _callbackOnModify(diffInfo); + } + } + std::string _name; ///< name of body std::vector _vecjoints; ///< \see GetJoints @@ -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 _callbackOnModify; private: mutable std::vector _vTempJoints; diff --git a/src/libopenrave-core/environment-core.h b/src/libopenrave-core/environment-core.h index 7d30ead63f..5885baac47 100644 --- a/src/libopenrave-core/environment-core.h +++ b/src/libopenrave-core/environment-core.h @@ -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(); - diffInfo->_description = _description; - _callbackOnModify(diffInfo); - pbody->RegisterCallbackOnModify( - [this](KinBody::KinBodyInfoPtr kinBodyInfo) { - _MergeKinbodyDiff(kinBodyInfo); - } - ); } virtual void _AddRobot(RobotBasePtr robot, InterfaceAddMode addMode) @@ -3350,6 +3342,17 @@ class Environment : public EnvironmentBase return true; } + inline void RegisterCallbackOnModify(std::function 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() @@ -4326,20 +4329,6 @@ class Environment : public EnvironmentBase _prLoadEnvAlloc->Clear(); } -public: - inline void RegisterCallbackOnModify(std::function callback) { - _callbackOnModify = callback; - for (size_t index=0;index<_vecbodies.size();index++) { - _vecbodies[index]->RegisterCallbackOnModify( - [this](KinBody::KinBodyInfoPtr kinBodyInfo) { - _MergeKinbodyDiff(kinBodyInfo); - } - ); - } - } -private: - std::function _callbackOnModify; - inline void _MergeKinbodyDiff(KinBody::KinBodyInfoPtr kinBodyInfo) { if (_callbackOnModify != nullptr) { EnvironmentBaseInfoPtr diffInfo = boost::make_shared(); @@ -4347,7 +4336,6 @@ class Environment : public EnvironmentBase _callbackOnModify(diffInfo); } } -protected: std::vector _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 _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 @@ -4394,6 +4382,7 @@ class Environment : public EnvironmentBase std::map _mapUInt64Parameters; ///< a custom user-driven parameters std::vector _vRapidJsonLoadBuffer; boost::shared_ptr > _prLoadEnvAlloc; ///< allocator used for loading environments + std::function _callbackOnModify; bool _bInit; ///< environment is initialized bool _bEnableSimulation; ///< enable simulation loop