Attempt to fix #1769 (#1778)

dartsim · Dec 19, 2023 · b7f5dd1 · b7f5dd1
1 parent 3e8133c
commit b7f5dd1
Show file tree

Hide file tree

Showing 5 changed files with 28 additions and 4 deletions.
diff --git a/dart/dynamics/BodyNode.cpp b/dart/dynamics/BodyNode.cpp
@@ -176,7 +176,10 @@ ConstSkeletonPtr SkeletonRefCountingBase::getSkeleton() const
 /// SET_FLAGS : A version of SKEL_SET_FLAGS that assumes a SkeletonPtr named
 /// 'skel' has already been locked
 #define SET_FLAGS(X)                                                           \
-  skel->mTreeCache[mTreeIndex].mDirty.X = true;                                \
+  if (mTreeIndex != INVALID_INDEX)                                             \
+  {                                                                            \
+    skel->mTreeCache[mTreeIndex].mDirty.X = true;                              \
+  }                                                                            \
   skel->mSkelCache.mDirty.X = true;
 
 /// CHECK_FLAG : Check if the dirty flag X for the tree of this BodyNode is

diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp
@@ -2357,6 +2357,13 @@ void Skeleton::registerNode(Node* _newNode)
 //==============================================================================
 void Skeleton::destructOldTree(std::size_t tree)
 {
+  // Invalidate the tree indices of every BodyNode that is being removed
+  DataCache& treeToDestroy = mTreeCache[tree];
+  for (auto& bodyNode : treeToDestroy.mBodyNodes)
+  {
+    bodyNode->mTreeIndex = INVALID_INDEX;
+  }
+
   mTreeCache.erase(mTreeCache.begin() + tree);
   mTreeNodeMaps.erase(mTreeNodeMaps.begin() + tree);
 

diff --git a/dart/gui/osg/Viewer.cpp b/dart/gui/osg/Viewer.cpp
@@ -52,6 +52,19 @@ namespace dart {
 namespace gui {
 namespace osg {
 
+std::string toString(CameraMode mode)
+{
+  switch (mode)
+  {
+    case CameraMode::RGBA:
+      return "RGBA";
+    case CameraMode::DEPTH:
+      return "DEPTH";
+    default:
+      return "UNKNOWN";
+  }
+}
+
 class SaveScreen : public ::osg::Camera::DrawCallback
 {
 public:

diff --git a/dart/gui/osg/Viewer.hpp b/dart/gui/osg/Viewer.hpp
@@ -83,10 +83,12 @@ enum class CameraMode
 
   /// To render the depth buffer
   ///
-  /// \warning The DEPTH mode currently not compatible with the ImGui wigets.
+  /// \warning The DEPTH mode currently not compatible with the ImGui widgets.
   DEPTH,
 };
 
+[[nodiscard]] std::string toString(CameraMode mode);
+
 DART_DECLARE_CLASS_WITH_VIRTUAL_BASE_BEGIN
 class ViewerAttachment : public virtual ::osg::Group
 {

diff --git a/dart/gui/osg/detail/CameraModeCallback.cpp b/dart/gui/osg/detail/CameraModeCallback.cpp
@@ -131,8 +131,7 @@ void CameraModeCallback::setCameraMode(CameraMode mode)
   if (mode != CameraMode::RGBA && mode != CameraMode::DEPTH)
   {
     DART_WARN(
-        "Unsupported camera mode '{}'. Use RGBA or DEPTH.",
-        static_cast<int>(mode));
+        "Unsupported camera mode '{}'. Use RGBA or DEPTH.", toString(mode));
     return;
   }