Skip to content

Commit

Permalink
change function signature for compatibility
Browse files Browse the repository at this point in the history
  • Loading branch information
zirui.xie committed Oct 23, 2024
1 parent e800a93 commit 48a1991
Show file tree
Hide file tree
Showing 5 changed files with 71 additions and 53 deletions.
8 changes: 4 additions & 4 deletions include/openrave/kinbody.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1371,13 +1371,13 @@ class OPENRAVE_API KinBody : public InterfaceBase
///
/// the list is stored inside _GetInfo()._mapExtraGeometries. Note that the pointers are copied and not the data, so
/// any be careful not to modify the geometries afterwards
void SetGroupGeometries(const std::string& groupid, const KinBody::ExtraGeometryInfoPtr& extraGeometry);
void SetGroupGeometries(const std::string& groupid, const std::vector<KinBody::GeometryInfoPtr>& geometries);

private:
/// \brief stores geometries for later retrieval
///
/// This call is identical to SetGroupGeometries except that it does not automatically post a Prop_LinkGeometryGroup update to the body. It will be up to the caller to ensure this occurs.
void _SetGroupGeometriesNoPostprocess(const std::string& name, const KinBody::ExtraGeometryInfoPtr& extraGeometry);
void _SetGroupGeometriesNoPostprocess(const std::string& name, const std::vector<KinBody::GeometryInfoPtr>& geometries);

public:
/// \brief returns the number of geometries stored from a particular key
Expand Down Expand Up @@ -2642,9 +2642,9 @@ class OPENRAVE_API KinBody : public InterfaceBase
/// \param linkgeometries a vector containing a collection of geometry infos ptr for each links
/// Note that the pointers are copied and not the data, so be careful not to modify the geometries afterwards
/// This method is faster than Link::SetGeometriesFromGroup since it makes only one change callback.
virtual void SetLinkGroupGeometries(const std::string& id, const std::vector<KinBody::ExtraGeometryInfoPtr>& linkgeometries);
// virtual void SetLinkGroupGeometries(const std::string& id, const std::vector<KinBody::ExtraGeometryInfoPtr>& linkgeometries);
// DEPRECATED: now group geometry is in ExtraGeometryInfo struct type
// virtual void SetLinkGroupGeometries(const std::string& name, const std::vector< std::vector<KinBody::GeometryInfoPtr> >& linkgeometries);
virtual void SetLinkGroupGeometries(const std::string& name, const std::vector< std::vector<KinBody::GeometryInfoPtr> >& linkgeometries);

/// \brief Unique name of the body.
virtual const std::string& GetName() const {
Expand Down
53 changes: 35 additions & 18 deletions python/bindings/openravepy_kinbody.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1995,14 +1995,17 @@ object PyLink::GetGeometriesFromGroup(const std::string& groupid)
return ogeometryinfos;
}

void PyLink::SetGroupGeometries(const std::string& groupid, object oextrageometryinfo)
void PyLink::SetGroupGeometries(const std::string& groupid, object ogeometryinfos)
{
PyExtraGeometryInfoPtr pyextrageom = py::extract<PyExtraGeometryInfoPtr>(oextrageometryinfo);
if( !pyextrageom ) {
throw OPENRAVE_EXCEPTION_FORMAT0(_("cannot cast to KinBody.ExtraGeometryInfo"),ORE_InvalidArguments);
std::vector<KinBody::GeometryInfoPtr> geometries(len(ogeometryinfos));
for (size_t i = 0; i < geometries.size(); ++i) {
PyGeometryInfoPtr pygeom = py::extract<PyGeometryInfoPtr>(ogeometryinfos[py::to_object(i)]);
if ( !pygeom ) {
throw OPENRAVE_EXCEPTION_FORMAT0(_("cannot cast to KinBody.GeometryInfo"),ORE_InvalidArguments);
}
geometries[i] = pygeom->GetGeometryInfo();
}
KinBody::ExtraGeometryInfoPtr extraGeometry( pyextrageom->GetExtraGeometryInfo() );
_plink->SetGroupGeometries(groupid, extraGeometry);
_plink->SetGroupGeometries(groupid, geometries);
}

int PyLink::GetGroupNumGeometries(const std::string& groupid)
Expand Down Expand Up @@ -2986,20 +2989,34 @@ void PyKinBody::SetLinkGeometriesFromGroup(const std::string& geomname, const bo

void PyKinBody::SetLinkGroupGeometries(const std::string& geomname, object olinkgeometryinfos)
{
std::vector< KinBody::ExtraGeometryInfoPtr > linkgeometries(len(olinkgeometryinfos));

std::vector< std::vector<KinBody::GeometryInfoPtr> > linkgeometries(len(olinkgeometryinfos));
for(size_t i = 0; i < linkgeometries.size(); ++i) {
linkgeometries[i] = KinBody::ExtraGeometryInfoPtr(new KinBody::ExtraGeometryInfo());
PyExtraGeometryInfoPtr infoi = extract<PyExtraGeometryInfoPtr>(olinkgeometryinfos[py::to_object(i)]);
if ( !infoi ) {
throw OPENRAVE_EXCEPTION_FORMAT0(_("cannot cast to KinBody.ExtraGeometryInfo"), ORE_InvalidArguments);
}
linkgeometries[i]->_id = py::extract<std::string>(infoi->_id).empty() ? geomname : py::extract<std::string>(infoi->_id);
linkgeometries[i]->_name = py::extract<std::string>(infoi->_name).empty() ? geomname : py::extract<std::string>(infoi->_name);
std::vector<KinBody::GeometryInfoPtr>& geometries = linkgeometries[i]->_vgeometryinfos;
geometries.resize(len(infoi->_vgeometryinfos));
std::vector<KinBody::GeometryInfoPtr>& geometries = linkgeometries[i];
object infoi = extract<py::object>(olinkgeometryinfos[py::to_object(i)]);
geometries.resize(len(infoi));

// linkgeometries[i] = KinBody::ExtraGeometryInfoPtr(new KinBody::ExtraGeometryInfo());
// PyExtraGeometryInfoPtr infoi = extract<PyExtraGeometryInfoPtr>(olinkgeometryinfos[py::to_object(i)]);
// if ( !infoi ) {
// throw OPENRAVE_EXCEPTION_FORMAT0(_("cannot cast to KinBody.ExtraGeometryInfo"), ORE_InvalidArguments);
// }
// if (olinkgeometryinfos[py::to_object(i)].attr("_id").is_none()) {
// linkgeometries[i]->_id = geomname;
// }
// else {
// linkgeometries[i]->_id = py::extract<std::string>(infoi->_id).empty() ? geomname : py::extract<std::string>(infoi->_id);
// }
// if (olinkgeometryinfos[py::to_object(i)].attr("_name").is_none()) {
// linkgeometries[i]->_name = geomname;
// }
// else {
// linkgeometries[i]->_name = py::extract<std::string>(infoi->_name).empty() ? geomname : py::extract<std::string>(infoi->_name);
// }
// linkgeometries[i]->_name = py::extract<std::string>(infoi->_name).empty() ? geomname : py::extract<std::string>(infoi->_name);
// std::vector<KinBody::GeometryInfoPtr>& geometries = linkgeometries[i]->_vgeometryinfos;
// geometries.resize(len(infoi->_vgeometryinfos));
for(size_t j = 0; j < geometries.size(); ++j) {
PyGeometryInfoPtr pygeom = py::extract<PyGeometryInfoPtr>(infoi->_vgeometryinfos[py::to_object(j)]);
PyGeometryInfoPtr pygeom = py::extract<PyGeometryInfoPtr>(infoi[py::to_object(j)]);
if( !pygeom ) {
throw OPENRAVE_EXCEPTION_FORMAT0(_("cannot cast to KinBody.GeometryInfo"),ORE_InvalidArguments);
}
Expand Down
7 changes: 2 additions & 5 deletions src/libopenrave-core/colladaparser/colladareader.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5075,15 +5075,12 @@ class ColladaReader : public daeErrorHandler
}
FOREACH(itlinkgeomgroups, mapGeometryGroups) {
FOREACH(itgeomgroup, itlinkgeomgroups->second) {
KinBody::ExtraGeometryInfoPtr extrageom(new KinBody::ExtraGeometryInfo());
extrageom->_id = itgeomgroup->first;
extrageom->_name = itgeomgroup->first;
std::vector<KinBody::GeometryInfoPtr>& vgeometries = extrageom->_vgeometryinfos;
std::vector<KinBody::GeometryInfoPtr> vgeometries;
vgeometries.reserve(itgeomgroup->second.size());
FOREACH(itgeominfo, itgeomgroup->second) {
vgeometries.push_back(boost::make_shared<KinBody::GeometryInfo>(*itgeominfo));
}
itlinkgeomgroups->first->SetGroupGeometries(itgeomgroup->first, extrageom);
itlinkgeomgroups->first->SetGroupGeometries(itgeomgroup->first, vgeometries);
}
}
}
Expand Down
23 changes: 11 additions & 12 deletions src/libopenrave/kinbody.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -710,21 +710,20 @@ void KinBody::SetLinkGeometriesFromGroup(const std::string& geomname, const bool
}

// DEPRECATED: now using ExtraGeometryInfo type instead of std::vector<GeometryInfoPtr>
// void KinBody::SetLinkGroupGeometries(const std::string& geomname, const std::vector< std::vector<KinBody::GeometryInfoPtr> >& linkgeometries)
// void KinBody::SetLinkGroupGeometries(const std::string& id, const std::vector<KinBody::ExtraGeometryInfoPtr>& linkgeometries)

void KinBody::SetLinkGroupGeometries(const std::string& id, const std::vector<KinBody::ExtraGeometryInfoPtr>& linkgeometries)
void KinBody::SetLinkGroupGeometries(const std::string& geomname, const std::vector< std::vector<KinBody::GeometryInfoPtr> >& linkgeometries)
{
OPENRAVE_ASSERT_OP( linkgeometries.size(), ==, _veclinks.size() );
for (size_t idx=0; idx < _veclinks.size(); ++idx) {
Link& link = *_veclinks.at(idx);
link._info._mapExtraGeometries[id] = KinBody::ExtraGeometryInfoPtr(new KinBody::ExtraGeometryInfo());
link._info._mapExtraGeometries[id]->_id = linkgeometries.at(idx)->_id.empty() ? id : linkgeometries.at(idx)->_id;
link._info._mapExtraGeometries[id]->_name = linkgeometries.at(idx)->_name.empty() ? id : linkgeometries.at(idx)->_name;

const std::vector<GeometryInfoPtr>& inputgeometries = linkgeometries.at(idx)->_vgeometryinfos;
std::vector<GeometryInfoPtr>& vgeometries = link._info._mapExtraGeometries[id]->_vgeometryinfos;
vgeometries.resize(inputgeometries.size());
std::copy(inputgeometries.begin(), inputgeometries.end(), vgeometries.begin());
FOREACH(itlink, _veclinks){
Link& link = **itlink;
KinBody::ExtraGeometryInfoPtr new_extrageom( new KinBody::ExtraGeometryInfo() );
new_extrageom->_id = geomname;
new_extrageom->_name = geomname;
std::map< std::string, KinBody::ExtraGeometryInfoPtr >::iterator it = link._info._mapExtraGeometries.insert(make_pair(geomname, new_extrageom)).first;
const std::vector<KinBody::GeometryInfoPtr>& geometries = linkgeometries.at(link.GetIndex());
it->second->_vgeometryinfos.resize(geometries.size());
std::copy(geometries.begin(), geometries.end(), it->second->_vgeometryinfos.begin());
}
_PostprocessChangedParameters(Prop_LinkGeometryGroup); // have to notify collision checkers that the geometry info they are caching could have changed.
}
Expand Down
33 changes: 19 additions & 14 deletions src/libopenrave/kinbodylink.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -780,9 +780,7 @@ void KinBody::Link::_InitGeometriesInternal(bool bForceRecomputeMeshCollision) {
}
_info._mapExtraGeometries.clear();
// have to reset the self group! cannot use geometries directly since we require exclusive access to the GeometryInfo objects
KinBody::ExtraGeometryInfoPtr selfGeom(new ExtraGeometryInfo());
selfGeom->_id = "self";
std::vector<KinBody::GeometryInfoPtr>& vgeometryinfos = selfGeom->_vgeometryinfos;
std::vector<KinBody::GeometryInfoPtr> vgeometryinfos;
vgeometryinfos.resize(_vGeometries.size());
for(int index = 0; index < (int)vgeometryinfos.size(); ++index) {
vgeometryinfos[index].reset(new KinBody::GeometryInfo());
Expand All @@ -791,7 +789,7 @@ void KinBody::Link::_InitGeometriesInternal(bool bForceRecomputeMeshCollision) {

// SetGroupGeometries calls PostprocessChangedParameters on the parent, which would increment the body update stamp, and then we would invoke PostprocessChangedParameters _again_ in _Update here, resulting in duplicated work in callbacks.
// Coalesce these update calls into one by calling the NoPostprocess version and adding the Prop_LinkGeometryGroup flag to our main _Update call instead.
_SetGroupGeometriesNoPostprocess("self", selfGeom);
_SetGroupGeometriesNoPostprocess("self", vgeometryinfos);
_Update(/* parametersChanged */ true, /* extraParametersChanged */ Prop_LinkGeometryGroup);
}

Expand Down Expand Up @@ -852,24 +850,31 @@ const std::vector<KinBody::GeometryInfoPtr>& KinBody::Link::GetGeometriesFromGro
return it->second->_vgeometryinfos;
}

void KinBody::Link::_SetGroupGeometriesNoPostprocess(const std::string& groupid, const KinBody::ExtraGeometryInfoPtr& extraGeometry)
void KinBody::Link::_SetGroupGeometriesNoPostprocess(const std::string& groupid, const std::vector<KinBody::GeometryInfoPtr>& geometries)
{
FOREACH(itgeominfo, extraGeometry->_vgeometryinfos) {
FOREACH(itgeominfo, geometries) {
if (!(*itgeominfo)) {
int igeominfo = itgeominfo - extraGeometry->_vgeometryinfos.begin();
int igeominfo = itgeominfo - geometries.begin();
throw OPENRAVE_EXCEPTION_FORMAT("GeometryInfo index %d is invalid for body %s", igeominfo % GetParent()->GetName(), ORE_InvalidArguments);
}
}
// check group id validity
if (_info._mapExtraGeometries.find(groupid) != _info._mapExtraGeometries.end()) {
throw OPENRAVE_EXCEPTION_FORMAT("ExtraGeometry id %s is invalid as already exists", groupid, ORE_InvalidArguments);
}
_info._mapExtraGeometries.insert(make_pair(groupid, boost::shared_ptr<ExtraGeometryInfo>(extraGeometry)));
KinBody::ExtraGeometryInfoPtr new_extrageom( new KinBody::ExtraGeometryInfo() );
new_extrageom->_id = groupid;
new_extrageom->_name = groupid;
std::map<std::string, KinBody::ExtraGeometryInfoPtr>::iterator it = _info._mapExtraGeometries.insert(make_pair(groupid, new_extrageom)).first;
it->second->_vgeometryinfos.resize(geometries.size());
std::copy(geometries.begin(), geometries.end(), it->second->_vgeometryinfos.begin());

// // check group id validity
// if (_info._mapExtraGeometries.find(groupid) != _info._mapExtraGeometries.end()) {
// throw OPENRAVE_EXCEPTION_FORMAT("ExtraGeometry id %s is invalid as already exists", groupid, ORE_InvalidArguments);
// }
// _info._mapExtraGeometries.insert(make_pair(groupid, boost::shared_ptr<ExtraGeometryInfo>(extraGeometry)));
}

void KinBody::Link::SetGroupGeometries(const std::string& groupid, const KinBody::ExtraGeometryInfoPtr& extraGeometry)
void KinBody::Link::SetGroupGeometries(const std::string& groupid, const std::vector<KinBody::GeometryInfoPtr>& geometries)
{
_SetGroupGeometriesNoPostprocess(groupid, extraGeometry);
_SetGroupGeometriesNoPostprocess(groupid, geometries);
GetParent()->_PostprocessChangedParameters(Prop_LinkGeometryGroup); // have to notify collision checkers that the geometry info they are caching could have changed.
}

Expand Down

0 comments on commit 48a1991

Please sign in to comment.