Merge pull request #1478 from rdiankov/keep_env_body_index

new api AddKinBody and AddRobot with requested environment body index option
rdiankov · Dec 22, 2024 · 01875e8 · 01875e8
2 parents 72db427 + 92421fd
commit 01875e8
Show file tree

Hide file tree

Showing 8 changed files with 97 additions and 47 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -4,8 +4,8 @@ set( CMAKE_ALLOW_LOOSE_LOOP_CONSTRUCTS TRUE )
 
 # Define here the needed parameters
 set (OPENRAVE_VERSION_MAJOR 0)
-set (OPENRAVE_VERSION_MINOR 159)
-set (OPENRAVE_VERSION_PATCH 1)
+set (OPENRAVE_VERSION_MINOR 160)
+set (OPENRAVE_VERSION_PATCH 0)
 set (OPENRAVE_VERSION ${OPENRAVE_VERSION_MAJOR}.${OPENRAVE_VERSION_MINOR}.${OPENRAVE_VERSION_PATCH})
 set (OPENRAVE_SOVERSION ${OPENRAVE_VERSION_MAJOR}.${OPENRAVE_VERSION_MINOR})
 message(STATUS "Compiling OpenRAVE Version ${OPENRAVE_VERSION}, soversion=${OPENRAVE_SOVERSION}")
@@ -114,22 +114,6 @@ include(GNUInstallDirs)
 
 find_package(PkgConfig) # pkg_check_modules
 
-check_cxx_source_compiles(" 
-#include <mutex> 
-int main() 
-{ 
-    std::scoped_lock lock;
-    return 0;
-}"
-OPENRAVE_STD_SCOPED_LOCK
-)
-# need to convert to int to set config.h
-if(OPENRAVE_STD_SCOPED_LOCK)
-  set(OPENRAVE_STD_SCOPED_LOCK 1)
-else()
-  set(OPENRAVE_STD_SCOPED_LOCK 0)
-endif()
-
 check_cxx_source_compiles(" 
 #include <string_view> 
 int main() 
@@ -139,6 +123,7 @@ int main()
 }"
 OPENRAVE_STD_STRING_VIEW
 )
+# need to convert to int to set config.h
 if(OPENRAVE_STD_STRING_VIEW)
   set(OPENRAVE_STD_STRING_VIEW 1)
 else()

diff --git a/config.h.in b/config.h.in
@@ -71,7 +71,6 @@
 #define OPENRAVE_CURL @OPENRAVE_CURL@
 
 #define OPENRAVE_ENVIRONMENT_RECURSIVE_LOCK @OPENRAVE_ENVIRONMENT_RECURSIVE_LOCK@
-#define OPENRAVE_STD_SCOPED_LOCK @OPENRAVE_STD_SCOPED_LOCK@
 #define OPENRAVE_STD_STRING_VIEW @OPENRAVE_STD_STRING_VIEW@
 
 #endif
diff --git a/docs/source/changelog.rst b/docs/source/changelog.rst
@@ -3,6 +3,12 @@
 ChangeLog
 #########
 
+Version 0.160.0
+===============
+
+- Add new functions for AddKinBody/AddRobot to specify an exact environmentBodyIndex.
+- Use std::unique_lock instead of std::scoped_lock. Remove boost recursive mutex.
+
 Version 0.159.1
 ===============
 

diff --git a/include/openrave/environment.h b/include/openrave/environment.h
@@ -23,23 +23,16 @@
 #define  OPENRAVE_ENVIRONMENTBASE_H
 
 #include <openrave/config.h>
+#include <mutex>
 
 namespace OpenRAVE {
 
 #if OPENRAVE_ENVIRONMENT_RECURSIVE_LOCK
-#if OPENRAVE_STD_SCOPED_LOCK
-#include <mutex>
 using EnvironmentMutex = ::std::recursive_mutex;
-using EnvironmentLock  = ::std::scoped_lock<std::recursive_mutex>;
+using EnvironmentLock  = ::std::unique_lock<std::recursive_mutex>;
 using defer_lock_t     = ::std::defer_lock_t;
 using try_to_lock_t    = ::std::try_to_lock_t;
 #else
-using EnvironmentMutex = ::boost::recursive_try_mutex;
-using EnvironmentLock  = EnvironmentMutex::scoped_lock;
-using defer_lock_t     = ::boost::defer_lock_t;
-using try_to_lock_t    = ::boost::try_to_lock_t;
-#endif // OPENRAVE_STD_SCOPED_LOCK
-#else
 using EnvironmentMutex = ::std::mutex;
 using EnvironmentLock  = ::std::unique_lock<std::mutex>;
 using defer_lock_t     = ::std::defer_lock_t;
@@ -482,6 +475,22 @@ class OPENRAVE_API EnvironmentBase : public boost::enable_shared_from_this<Envir
 
     virtual void Add(InterfaceBasePtr pinterface, bool bAnonymous, const std::string& cmdargs=std::string()) RAVE_DEPRECATED;
 
+    /** \brief Add an body to the environment
+        \param[in] pbody the pointer to an initialized body
+        \param[in] addMode One of IAM_X
+        \param[in] requestedEnvironmentBodyIndex if positive and none of existing body uses it, this is assigned to pbody. If positive and existing body uses it, exception is thrown. If non-positive, environment body index is decided internally.
+        \throw openrave_exception Throw if interface is invalid or already added
+     */
+    virtual void AddKinBody(KinBodyPtr pbody, InterfaceAddMode addMode, int requestedEnvironmentBodyIndex) = 0;
+
+    /** \brief Add an robot to the environment
+        \param[in] probot the pointer to an initialized robot
+        \param[in] addMode One of IAM_X
+        \param[in] requestedEnvironmentBodyIndex if positive and none of existing body uses it, this is assigned to probot. If positive and existing body uses it, exception is thrown. If non-positive, environment body index is decided internally.
+        \throw openrave_exception Throw if interface is invalid or already added
+     */
+    virtual void AddRobot(RobotBasePtr probot, InterfaceAddMode addMode, int requestedEnvironmentBodyIndex) = 0;
+
     /// \brief bodycallback(body, action)
     ///
     /// \param body KinBodyPtr

diff --git a/include/openrave/openraveexception.h b/include/openrave/openraveexception.h
@@ -50,6 +50,7 @@ enum OpenRAVEErrorCode
     ORE_TimeDurationUnitInvalid = 18, ///< Cannot find the specific TimeDurationUnit
     ORE_AngleUnitInvalid = 19, ///< Cannot find the specific AngleUnit
     ORE_TimeStampUnitInvalid = 20, ///< Cannot find the specific TimeStampUnit
+    ORE_EnvironmentBodyIndexConflict=21, ///< body with same environment body index is trying to be added to the environment
 
     ORE_EnvironmentFormatUnrecognized = 0x0100, ///< the environment format to load is not recognized.
 

diff --git a/python/bindings/include/openravepy/openravepy_environmentbase.h b/python/bindings/include/openravepy/openravepy_environmentbase.h
@@ -217,8 +217,10 @@ class OPENRAVEPY_API PyEnvironmentBase : public OPENRAVE_ENABLE_SHARED_FROM_THIS
 
     void AddKinBody(PyKinBodyPtr pbody);
     void AddKinBody(PyKinBodyPtr pbody, bool bAnonymous);
+    void AddKinBody(PyKinBodyPtr pbody, py::object oAddMode, int requestedEnvironmentBodyIndex);
     void AddRobot(PyRobotBasePtr robot);
     void AddRobot(PyRobotBasePtr robot, bool bAnonymous);
+    void AddRobot(PyRobotBasePtr robot, py::object oAddMode, int requestedEnvironmentBodyIndex);
     void AddSensor(PySensorBasePtr sensor);
     void AddSensor(PySensorBasePtr sensor, bool bAnonymous);
     void AddViewer(PyViewerBasePtr viewer);

diff --git a/python/bindings/openravepy_int.cpp b/python/bindings/openravepy_int.cpp
@@ -2156,20 +2156,28 @@ object PyEnvironmentBase::ReadTrimeshData(const std::string& data, const std::st
     return toPyTriMesh(*ptrimesh);
 }
 
-void PyEnvironmentBase::Add(PyInterfaceBasePtr pinterface, py::object oAddMode, const std::string& cmdargs)
+
+InterfaceAddMode _ExtractAddMode(const py::object oAddMode, PyInterfaceBasePtr pinterface, const EnvironmentBasePtr& penv)
 {
     InterfaceAddMode addMode = IAM_StrictNameChecking;
     if( !IS_PYTHONOBJECT_NONE(oAddMode) ) {
         if (PyBool_Check(oAddMode.ptr())) {
             addMode = py::extract<bool>(oAddMode) ? IAM_AllowRenaming : IAM_StrictNameChecking;
-            if( pinterface->GetInterfaceType() != PT_Module ) {
-                RAVELOG_WARN_FORMAT("env=%s trying to use 'anonymous' flag when adding object '%s' of interface type '%d' via Add", _penv->GetNameId()%pinterface->GetXMLId()%(int)pinterface->GetInterfaceType());
+            const InterfaceType type = pinterface->GetInterfaceType();
+            if( type != PT_Module && type != PT_Robot && type != PT_KinBody ) {
+                RAVELOG_WARN_FORMAT("env=%s trying to use 'anonymous' flag when adding object '%s' of interface type '%d' via Add", penv->GetNameId()%pinterface->GetXMLId()%(int)pinterface->GetInterfaceType());
             }
         }
         else {
             addMode = py::extract<InterfaceAddMode>(oAddMode);
         }
     }
+    return addMode;
+}
+
+void PyEnvironmentBase::Add(PyInterfaceBasePtr pinterface, py::object oAddMode, const std::string& cmdargs)
+{
+    const InterfaceAddMode addMode = _ExtractAddMode(oAddMode, pinterface, _penv);
     PythonThreadSaver threadsaver;
     _penv->Add(pinterface->GetInterfaceBase(), addMode, cmdargs);
 }
@@ -2181,6 +2189,10 @@ void PyEnvironmentBase::AddKinBody(PyKinBodyPtr pbody, bool bAnonymous) {
     RAVELOG_WARN("Calling AddKinBody with bAnonymous, should switch to IAM_X signals");
     CHECK_POINTER(pbody); _penv->Add(openravepy::GetKinBody(pbody),bAnonymous ? IAM_AllowRenaming : IAM_StrictNameChecking);
 }
+void PyEnvironmentBase::AddKinBody(PyKinBodyPtr pbody, py::object oAddMode, int requestedEnvironmentBodyIndex) {
+    CHECK_POINTER(pbody);
+    _penv->AddKinBody(openravepy::GetKinBody(pbody), _ExtractAddMode(oAddMode, pbody, _penv), requestedEnvironmentBodyIndex);
+}
 void PyEnvironmentBase::AddRobot(PyRobotBasePtr robot) {
     CHECK_POINTER(robot);
     _penv->Add(openravepy::GetRobot(robot), IAM_StrictNameChecking);
@@ -2190,6 +2202,10 @@ void PyEnvironmentBase::AddRobot(PyRobotBasePtr robot, bool bAnonymous) {
     CHECK_POINTER(robot);
     _penv->Add(openravepy::GetRobot(robot), bAnonymous ? IAM_AllowRenaming : IAM_StrictNameChecking);
 }
+void PyEnvironmentBase::AddRobot(PyRobotBasePtr robot, py::object oAddMode, int requestedEnvironmentBodyIndex) {
+    CHECK_POINTER(robot);
+    _penv->AddRobot(openravepy::GetRobot(robot), _ExtractAddMode(oAddMode, robot, _penv), requestedEnvironmentBodyIndex);
+}
 void PyEnvironmentBase::AddSensor(PySensorBasePtr sensor) {
     CHECK_POINTER(sensor);
     _penv->Add(openravepy::GetSensor(sensor), IAM_StrictNameChecking);
@@ -3662,8 +3678,10 @@ Because race conditions can pop up when trying to lock the openrave environment
 #endif
         void (PyEnvironmentBase::*addkinbody1)(PyKinBodyPtr) = &PyEnvironmentBase::AddKinBody;
         void (PyEnvironmentBase::*addkinbody2)(PyKinBodyPtr,bool) = &PyEnvironmentBase::AddKinBody;
+        void (PyEnvironmentBase::*addkinbody3)(PyKinBodyPtr,object,int) = &PyEnvironmentBase::AddKinBody;
         void (PyEnvironmentBase::*addrobot1)(PyRobotBasePtr) = &PyEnvironmentBase::AddRobot;
         void (PyEnvironmentBase::*addrobot2)(PyRobotBasePtr,bool) = &PyEnvironmentBase::AddRobot;
+        void (PyEnvironmentBase::*addrobot3)(PyRobotBasePtr,object,int) = &PyEnvironmentBase::AddRobot;
         void (PyEnvironmentBase::*addsensor1)(PySensorBasePtr) = &PyEnvironmentBase::AddSensor;
         void (PyEnvironmentBase::*addsensor2)(PySensorBasePtr,bool) = &PyEnvironmentBase::AddSensor;
         void (PyEnvironmentBase::*setuserdata1)(PyUserData) = &PyEnvironmentBase::SetUserData;
@@ -3835,8 +3853,10 @@ Because race conditions can pop up when trying to lock the openrave environment
 #endif
                      .def("AddKinBody",addkinbody1, PY_ARGS("body") DOXY_FN(EnvironmentBase,AddKinBody))
                      .def("AddKinBody",addkinbody2, PY_ARGS("body","anonymous") DOXY_FN(EnvironmentBase,AddKinBody))
+                     .def("AddKinBody",addkinbody3, PY_ARGS("body","addMode","requestedEnvironmentBodyIndex") DOXY_FN(EnvironmentBase,AddKinBody))
                      .def("AddRobot",addrobot1, PY_ARGS("robot") DOXY_FN(EnvironmentBase,AddRobot))
                      .def("AddRobot",addrobot2, PY_ARGS("robot","anonymous") DOXY_FN(EnvironmentBase,AddRobot))
+                     .def("AddRobot",addrobot3, PY_ARGS("robot","addMode","requestedEnvironmentBodyIndex") DOXY_FN(EnvironmentBase,AddRobot))
                      .def("AddSensor",addsensor1, PY_ARGS("sensor") DOXY_FN(EnvironmentBase,AddSensor))
                      .def("AddSensor",addsensor2, PY_ARGS("sensor","anonymous") DOXY_FN(EnvironmentBase,AddSensor))
                      .def("AddViewer",addsensor2, PY_ARGS("sensor","anonymous") DOXY_FN(EnvironmentBase,AddViewer))

diff --git a/src/libopenrave-core/environment-core.h b/src/libopenrave-core/environment-core.h
@@ -976,10 +976,10 @@ class Environment : public EnvironmentBase
     {
         CHECK_INTERFACE(pinterface);
         switch(pinterface->GetInterfaceType()) {
-        case PT_Robot: _AddRobot(RaveInterfaceCast<RobotBase>(pinterface),addMode); break;
-        case PT_KinBody: _AddKinBody(RaveInterfaceCast<KinBody>(pinterface),addMode); break;
+        case PT_Robot: _AddRobot(RaveInterfaceCast<RobotBase>(pinterface), addMode); break;
+        case PT_KinBody: _AddKinBody(RaveInterfaceCast<KinBody>(pinterface), addMode); break;
         case PT_Module: {
-            int ret = AddModule(RaveInterfaceCast<ModuleBase>(pinterface),cmdargs);
+            int ret = AddModule(RaveInterfaceCast<ModuleBase>(pinterface), cmdargs);
             OPENRAVE_ASSERT_OP_FORMAT(ret,==,0,"module '%s' failed with args: '%s'",pinterface->GetXMLId()%cmdargs,ORE_InvalidArguments);
             break;
         }
@@ -990,7 +990,17 @@ class Environment : public EnvironmentBase
         }
     }
 
-    virtual void _AddKinBody(KinBodyPtr pbody, InterfaceAddMode addMode)
+    virtual void AddKinBody(KinBodyPtr pbody, InterfaceAddMode addMode, int requestedEnvironmentBodyIndex) override
+    {
+        _AddKinBody(pbody, addMode, requestedEnvironmentBodyIndex);
+    }
+
+    virtual void AddRobot(RobotBasePtr probot, InterfaceAddMode addMode, int requestedEnvironmentBodyIndex) override
+    {
+        _AddRobot(probot, addMode, requestedEnvironmentBodyIndex);
+    }
+
+    virtual void _AddKinBody(KinBodyPtr pbody, InterfaceAddMode addMode, int requestedEnvironmentBodyIndex = 0)
     {
         EnvironmentLock lockenv(GetMutex());
         CHECK_INTERFACE(pbody);
@@ -1011,7 +1021,7 @@ class Environment : public EnvironmentBase
         }
         {
             ExclusiveLock lock969(_mutexInterfaces);
-            const int newBodyIndex = _AssignEnvironmentBodyIndex(pbody);
+            const int newBodyIndex = _AssignEnvironmentBodyIndex(pbody, requestedEnvironmentBodyIndex);
             _AddKinBodyInternal(pbody, newBodyIndex);
             _nBodiesModifiedStamp++;
         }
@@ -1028,7 +1038,7 @@ class Environment : public EnvironmentBase
         _CallBodyCallbacks(pbody, 1);
     }
 
-    virtual void _AddRobot(RobotBasePtr robot, InterfaceAddMode addMode)
+    virtual void _AddRobot(RobotBasePtr robot, InterfaceAddMode addMode, int requestedEnvironmentBodyIndex = 0)
     {
         EnvironmentLock lockenv(GetMutex());
         CHECK_INTERFACE(robot);
@@ -1052,7 +1062,7 @@ class Environment : public EnvironmentBase
         }
         {
             ExclusiveLock lock823(_mutexInterfaces);
-            const int newBodyIndex = _AssignEnvironmentBodyIndex(robot);
+            const int newBodyIndex = _AssignEnvironmentBodyIndex(robot, requestedEnvironmentBodyIndex);
             _AddKinBodyInternal(robot, newBodyIndex);
             _nBodiesModifiedStamp++;
         }
@@ -4203,20 +4213,38 @@ class Environment : public EnvironmentBase
     }
 
     /// assumes _mutexInterfaces is locked
-    virtual int _AssignEnvironmentBodyIndex(KinBodyPtr pbody)
+    virtual int _AssignEnvironmentBodyIndex(KinBodyPtr pbody, int requestedEnvironmentBodyIndex = 0)
     {
         const bool bRecycleId = !_environmentIndexRecyclePool.empty();
         int envBodyIndex = 0;
-        if (bRecycleId) {
-            std::set<int>::iterator smallestIt = _environmentIndexRecyclePool.begin();
-            envBodyIndex = *smallestIt;
-            _environmentIndexRecyclePool.erase(smallestIt);
-            RAVELOG_VERBOSE_FORMAT("env=%s, recycled body envBodyIndex=%d for '%s'. %d remaining in pool", GetNameId()%envBodyIndex%pbody->GetName()%_environmentIndexRecyclePool.size());
+        if (requestedEnvironmentBodyIndex > 0) {
+            // user requested env body index
+            std::set<int>::iterator smallestIt = _environmentIndexRecyclePool.find(requestedEnvironmentBodyIndex);
+            if (smallestIt != _environmentIndexRecyclePool.end()) {
+                envBodyIndex = *smallestIt;
+                _environmentIndexRecyclePool.erase(smallestIt);
+                RAVELOG_VERBOSE_FORMAT("env=%s, recycled body envBodyIndex=%d for '%s'. %d remaining in pool", GetNameId()%envBodyIndex%pbody->GetName()%_environmentIndexRecyclePool.size());
+            }
+            else {
+                envBodyIndex = requestedEnvironmentBodyIndex;
+                if ((size_t) envBodyIndex < _vecbodies.size() && !!_vecbodies.at(envBodyIndex)) {
+                    throw OPENRAVE_EXCEPTION_FORMAT(_("env=%s, environmentBodyIndex=%d is used by existing body=%s while trying to add new body=%s"), GetNameId() % requestedEnvironmentBodyIndex % _vecbodies.at(envBodyIndex)->GetName() % pbody->GetName(), ORE_EnvironmentBodyIndexConflict);
+                }
+                RAVELOG_VERBOSE_FORMAT("env=%s, use requested envBodyIndex=%d for '%s'. %d remaining in pool", GetNameId()%envBodyIndex%pbody->GetName()%_environmentIndexRecyclePool.size());
+            }
         }
         else {
-            envBodyIndex = _vecbodies.empty() ? 1 : _vecbodies.size(); // skip 0
-            if( envBodyIndex > 200 ) { // give some number sufficiently big so that leaking of objects can be detected rather than spamming the log
-                RAVELOG_DEBUG_FORMAT("env=%s, assigned new body envBodyIndex=%d for '%s', this should not happen unless total number of bodies in env keeps increasing", GetNameId()%envBodyIndex%pbody->GetName());
+            if (bRecycleId) {
+                std::set<int>::iterator smallestIt = _environmentIndexRecyclePool.begin();
+                envBodyIndex = *smallestIt;
+                _environmentIndexRecyclePool.erase(smallestIt);
+                RAVELOG_VERBOSE_FORMAT("env=%s, recycled body envBodyIndex=%d for '%s'. %d remaining in pool", GetNameId()%envBodyIndex%pbody->GetName()%_environmentIndexRecyclePool.size());
+            }
+            else {
+                envBodyIndex = _vecbodies.empty() ? 1 : _vecbodies.size(); // skip 0
+                if( envBodyIndex > 200 ) { // give some number sufficiently big so that leaking of objects can be detected rather than spamming the log
+                    RAVELOG_DEBUG_FORMAT("env=%s, assigned new body envBodyIndex=%d for '%s', this should not happen unless total number of bodies in env keeps increasing", GetNameId()%envBodyIndex%pbody->GetName());
+                }
             }
         }
         pbody->_environmentBodyIndex = envBodyIndex;