Skip to content

Commit

Permalink
Add callback and invoke callback function in setters
Browse files Browse the repository at this point in the history
  • Loading branch information
@LLipter
LLipter committed Jan 9, 2024
1 parent dd0d774 commit 394abe6
Show file tree
Hide file tree
Showing 6 changed files with 329 additions and 2 deletions.
75 changes: 75 additions & 0 deletions include/openrave/kinbody.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -671,6 +671,15 @@ 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 @@ -1072,6 +1081,28 @@ 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 @@ -1628,6 +1659,13 @@ 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 @@ -2392,6 +2430,43 @@ 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
56 changes: 56 additions & 0 deletions src/libopenrave-core/environment-core.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1022,6 +1022,14 @@ 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 @@ -2758,6 +2766,11 @@ class Environment : public EnvironmentBase
virtual void SetUnit(std::pair<std::string, dReal> unit)
{
_unitInfo.lengthUnit = GetLengthUnitFromString(unit.first, LU_Meter);
if (_callbackOnModify != nullptr) {
EnvironmentBaseInfoPtr diffInfo = boost::make_shared<EnvironmentBaseInfo>();
diffInfo->_unitInfo = _unitInfo;
_callbackOnModify(diffInfo);
}
}

virtual UnitInfo GetUnitInfo() const
Expand All @@ -2768,6 +2781,11 @@ class Environment : public EnvironmentBase
virtual void SetUnitInfo(UnitInfo unitInfo)
{
_unitInfo = unitInfo;
if (_callbackOnModify != nullptr) {
EnvironmentBaseInfoPtr diffInfo = boost::make_shared<EnvironmentBaseInfo>();
diffInfo->_unitInfo = _unitInfo;
_callbackOnModify(diffInfo);
}
}

/// \brief similar to GetInfo, but creates a copy of an up-to-date info, safe for caller to manipulate
Expand Down Expand Up @@ -3227,6 +3245,11 @@ class Environment : public EnvironmentBase
void SetDescription(const std::string& sceneDescription) override {
EnvironmentLock lockenv(GetMutex());
_description = sceneDescription;
if (_callbackOnModify != nullptr) {
EnvironmentBaseInfoPtr diffInfo = boost::make_shared<EnvironmentBaseInfo>();
diffInfo->_description = _description;
_callbackOnModify(diffInfo);
}
}

std::string GetDescription() const override {
Expand All @@ -3237,6 +3260,11 @@ class Environment : public EnvironmentBase
void SetKeywords(const std::vector<std::string>& sceneKeywords) override {
EnvironmentLock lockenv(GetMutex());
_keywords = sceneKeywords;
if (_callbackOnModify != nullptr) {
EnvironmentBaseInfoPtr diffInfo = boost::make_shared<EnvironmentBaseInfo>();
diffInfo->_keywords = _keywords;
_callbackOnModify(diffInfo);
}
}

std::vector<std::string> GetKeywords() const override {
Expand All @@ -3247,6 +3275,11 @@ class Environment : public EnvironmentBase
void SetUInt64Parameter(const std::string& parameterName, uint64_t value) override {
EnvironmentLock lockenv(GetMutex());
_mapUInt64Parameters[parameterName] = value;
if (_callbackOnModify != nullptr) {
EnvironmentBaseInfoPtr diffInfo = boost::make_shared<EnvironmentBaseInfo>();
diffInfo->_uInt64Parameters = _mapUInt64Parameters;
_callbackOnModify(diffInfo);
}
}

bool RemoveUInt64Parameter(const std::string& parameterName) override
Expand Down Expand Up @@ -4293,6 +4326,29 @@ 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
std::unordered_map<std::string, int> _mapBodyIdIndex; /// maps body id to env body index of bodies stored in _vecbodies sorted by name. used to lookup kin body by name. protected by _mutexInterfaces
Expand Down
19 changes: 18 additions & 1 deletion src/libopenrave/kinbody.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -879,6 +879,12 @@ void KinBody::SetName(const std::string& newname)
}
_name = newname;
_PostprocessChangedParameters(Prop_Name);
if (_callbackOnModify != nullptr) {
KinBody::KinBodyInfoPtr diffInfo = boost::make_shared<KinBody::KinBodyInfo>();
diffInfo->_id = _id;
diffInfo->_name = _name;
_callbackOnModify(diffInfo);
}
}
}

Expand Down Expand Up @@ -5384,7 +5390,7 @@ bool KinBody::SetVisible(bool visible)
FOREACH(it, _veclinks) {
FOREACH(itgeom,(*it)->_vGeometries) {
if( (*itgeom)->IsVisible() != visible ) {
(*itgeom)->_info._bVisible = visible;
(*itgeom)->SetVisible(visible);
bchanged = true;
}
}
Expand Down Expand Up @@ -6015,6 +6021,12 @@ void KinBody::_InitAndAddLink(LinkPtr plink)
plink->SetReadableInterface(it->first, it->second);
}

plink->RegisterCallbackOnModify(
[this](KinBody::LinkInfoPtr linkInfo) {
_MergeLinksDiff(linkInfo);
}
);

_veclinks.push_back(plink);
_vLinkTransformPointers.clear();
__hashKinematicsGeometryDynamics.resize(0);
Expand Down Expand Up @@ -6070,6 +6082,11 @@ void KinBody::_InitAndAddJoint(JointPtr pjoint)
else {
_vPassiveJoints.push_back(pjoint);
}
pjoint->RegisterCallbackOnModify(
[this](KinBody::JointInfoPtr jointInfo) {
_MergeJointsDiff(jointInfo);
}
);
__hashKinematicsGeometryDynamics.resize(0);
}

Expand Down
54 changes: 54 additions & 0 deletions src/libopenrave/kinbodygeometry.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1821,6 +1821,12 @@ bool KinBody::Geometry::SetVisible(bool visible)
LinkPtr parent(_parent);
parent->GetParent()->_PostprocessChangedParameters(Prop_LinkDraw);
return true;
if (_callbackOnModify != nullptr) {
KinBody::GeometryInfoPtr diffInfo = boost::make_shared<KinBody::GeometryInfo>();
diffInfo->_id = _info._id;
diffInfo->_bVisible = _info._bVisible;
_callbackOnModify(diffInfo);
}
}
return false;
}
Expand All @@ -1830,48 +1836,90 @@ void KinBody::Geometry::SetTransparency(float f)
LinkPtr parent(_parent);
_info._fTransparency = f;
parent->GetParent()->_PostprocessChangedParameters(Prop_LinkDraw);
if (_callbackOnModify != nullptr) {
KinBody::GeometryInfoPtr diffInfo = boost::make_shared<KinBody::GeometryInfo>();
diffInfo->_id = _info._id;
diffInfo->_fTransparency = _info._fTransparency;
_callbackOnModify(diffInfo);
}
}

void KinBody::Geometry::SetDiffuseColor(const RaveVector<float>& color)
{
LinkPtr parent(_parent);
_info._vDiffuseColor = color;
parent->GetParent()->_PostprocessChangedParameters(Prop_LinkDraw);
if (_callbackOnModify != nullptr) {
KinBody::GeometryInfoPtr diffInfo = boost::make_shared<KinBody::GeometryInfo>();
diffInfo->_id = _info._id;
diffInfo->_vDiffuseColor = _info._vDiffuseColor;
_callbackOnModify(diffInfo);
}
}

void KinBody::Geometry::SetAmbientColor(const RaveVector<float>& color)
{
LinkPtr parent(_parent);
_info._vAmbientColor = color;
parent->GetParent()->_PostprocessChangedParameters(Prop_LinkDraw);
if (_callbackOnModify != nullptr) {
KinBody::GeometryInfoPtr diffInfo = boost::make_shared<KinBody::GeometryInfo>();
diffInfo->_id = _info._id;
diffInfo->_vAmbientColor = _info._vAmbientColor;
_callbackOnModify(diffInfo);
}
}

void KinBody::Geometry::SetNegativeCropContainerMargins(const Vector& negativeCropContainerMargins)
{
LinkPtr parent(_parent);
_info._vNegativeCropContainerMargins = negativeCropContainerMargins;
parent->GetParent()->_PostprocessChangedParameters(Prop_LinkDraw);
if (_callbackOnModify != nullptr) {
KinBody::GeometryInfoPtr diffInfo = boost::make_shared<KinBody::GeometryInfo>();
diffInfo->_id = _info._id;
diffInfo->_vNegativeCropContainerMargins = _info._vNegativeCropContainerMargins;
_callbackOnModify(diffInfo);
}
}

void KinBody::Geometry::SetPositiveCropContainerMargins(const Vector& positiveCropContainerMargins)
{
LinkPtr parent(_parent);
_info._vPositiveCropContainerMargins = positiveCropContainerMargins;
parent->GetParent()->_PostprocessChangedParameters(Prop_LinkDraw);
if (_callbackOnModify != nullptr) {
KinBody::GeometryInfoPtr diffInfo = boost::make_shared<KinBody::GeometryInfo>();
diffInfo->_id = _info._id;
diffInfo->_vPositiveCropContainerMargins = _info._vPositiveCropContainerMargins;
_callbackOnModify(diffInfo);
}
}

void KinBody::Geometry::SetNegativeCropContainerEmptyMargins(const Vector& negativeCropContainerEmptyMargins)
{
LinkPtr parent(_parent);
_info._vNegativeCropContainerEmptyMargins = negativeCropContainerEmptyMargins;
parent->GetParent()->_PostprocessChangedParameters(Prop_LinkDraw);
if (_callbackOnModify != nullptr) {
KinBody::GeometryInfoPtr diffInfo = boost::make_shared<KinBody::GeometryInfo>();
diffInfo->_id = _info._id;
diffInfo->_vNegativeCropContainerEmptyMargins = _info._vNegativeCropContainerEmptyMargins;
_callbackOnModify(diffInfo);
}
}

void KinBody::Geometry::SetPositiveCropContainerEmptyMargins(const Vector& positiveCropContainerEmptyMargins)
{
LinkPtr parent(_parent);
_info._vPositiveCropContainerEmptyMargins = positiveCropContainerEmptyMargins;
parent->GetParent()->_PostprocessChangedParameters(Prop_LinkDraw);
if (_callbackOnModify != nullptr) {
KinBody::GeometryInfoPtr diffInfo = boost::make_shared<KinBody::GeometryInfo>();
diffInfo->_id = _info._id;
diffInfo->_vPositiveCropContainerEmptyMargins = _info._vPositiveCropContainerEmptyMargins;
_callbackOnModify(diffInfo);
}
}

/*
Expand Down Expand Up @@ -1993,6 +2041,12 @@ void KinBody::Geometry::SetName(const std::string& name)
LinkPtr parent(_parent);
_info._name = name;
parent->GetParent()->_PostprocessChangedParameters(Prop_LinkGeometry);
if (_callbackOnModify != nullptr) {
KinBody::GeometryInfoPtr diffInfo = boost::make_shared<KinBody::GeometryInfo>();
diffInfo->_id = _info._id;
diffInfo->_name = name;
_callbackOnModify(diffInfo);
}
}

void KinBody::Geometry::UpdateInfo()
Expand Down
Loading

0 comments on commit 394abe6

Please sign in to comment.