From 30c10c087f54d495187c6ab764ca5f48369b58a7 Mon Sep 17 00:00:00 2001
From: Louise Poubel <louise@openrobotics.org>
Date: Mon, 26 Apr 2021 17:57:27 -0700
Subject: [PATCH] Debugging actor crashes

Signed-off-by: Louise Poubel <louise@openrobotics.org>
---
 examples/worlds/actor.sdf     | 62 -----------------------------------
 src/rendering/SceneManager.cc | 28 +++++++++++++---
 2 files changed, 23 insertions(+), 67 deletions(-)

diff --git a/examples/worlds/actor.sdf b/examples/worlds/actor.sdf
index e69a66513f..44c945758f 100644
--- a/examples/worlds/actor.sdf
+++ b/examples/worlds/actor.sdf
@@ -150,68 +150,6 @@
       <uri>https://fuel.ignitionrobotics.org/1.0/Mingfei/models/actor</uri>
     </include>
 
-    <actor name="actor_talking">
-      <skin>
-        <filename>https://fuel.ignitionrobotics.org/1.0/Mingfei/models/actor/tip/files/meshes/talk_b.dae</filename>
-        <scale>1.0</scale>
-      </skin>
-      <animation name="talk_b">
-        <filename>https://fuel.ignitionrobotics.org/1.0/Mingfei/models/actor/tip/files/meshes/talk_b.dae</filename>
-        <scale>0.055</scale>
-        <interpolate_x>true</interpolate_x>
-      </animation>
-      <script>
-        <loop>true</loop>
-        <auto_start>true</auto_start>
-        <trajectory id="0" type="talk_b">
-          <waypoint>
-            <time>0</time>
-            <pose>2 -2 1.0 0 0 0</pose>
-          </waypoint>
-          <waypoint>
-            <time>30</time>
-            <pose>2 -2 1.0 0 0 0</pose>
-          </waypoint>
-        </trajectory>
-      </script>
-    </actor>
-
-    <model name="box">
-      <pose>5.0 0 0.7 0 0 0</pose>
-      <link name="box_link">
-        <inertial>
-          <inertia>
-            <ixx>1</ixx>
-            <ixy>0</ixy>
-            <ixz>0</ixz>
-            <iyy>1</iyy>
-            <iyz>0</iyz>
-            <izz>1</izz>
-          </inertia>
-          <mass>1.0</mass>
-        </inertial>
-        <collision name="box_collision">
-          <geometry>
-            <box>
-              <size>1 1 1</size>
-            </box>
-          </geometry>
-        </collision>
-
-        <visual name="box_visual">
-          <geometry>
-            <box>
-              <size>1 1 1</size>
-            </box>
-          </geometry>
-          <material>
-            <ambient>0.3 0.3 0.3 1</ambient>
-            <diffuse>0.3 0.3 0.3 1</diffuse>
-            <specular>0.3 0.5 0.3 1</specular>
-          </material>
-        </visual>
-      </link>
-    </model>
 
     <model name="camera">
       <static>true</static>
diff --git a/src/rendering/SceneManager.cc b/src/rendering/SceneManager.cc
index 41c8a5a81c..4e79d01623 100644
--- a/src/rendering/SceneManager.cc
+++ b/src/rendering/SceneManager.cc
@@ -674,7 +674,12 @@ rendering::VisualPtr SceneManager::CreateActor(Entity _id,
                 << "]" << std::endl;
         return nullptr;
       }
-      meshSkel->AddAnimation(firstAnim);
+
+      // Copy animation so that meshSkel can own it
+      auto newAnim = new common::SkeletonAnimation(firstAnim->Name());
+      *newAnim = *firstAnim;
+
+      meshSkel->AddAnimation(newAnim);
       mapAnimNameId[animName] = numAnims++;
     }
   }
@@ -950,10 +955,18 @@ std::map<std::string, math::Matrix4d> SceneManager::ActorMeshAnimationAt(
   if (this->dataPtr->actorTrajectories.find(_id)
       == this->dataPtr->actorTrajectories.end())
   {
+    ignerr << "Found no trajectories for entity [" << _id << "]" << std::endl;
     return allFrames;
   }
 
   auto trajs = this->dataPtr->actorTrajectories[_id];
+
+  if (trajs.empty())
+  {
+    ignerr << "Empty trajectory for entity [" << _id << "]" << std::endl;
+    return allFrames;
+  }
+
   bool followTraj = true;
   if (1 == trajs.size() && nullptr == trajs[0].Waypoints())
   {
@@ -963,7 +976,7 @@ std::map<std::string, math::Matrix4d> SceneManager::ActorMeshAnimationAt(
   auto firstTraj = trajs.begin();
   auto poseFrame = common::PoseKeyFrame(0.0);
 
-  common::TrajectoryInfo traj = trajs[0];
+  common::TrajectoryInfo &traj = trajs[0];
 
   using TP = std::chrono::steady_clock::time_point;
   auto totalTime = trajs.rbegin()->EndTime() - trajs.begin()->StartTime();
@@ -1018,6 +1031,12 @@ std::map<std::string, math::Matrix4d> SceneManager::ActorMeshAnimationAt(
   {
     auto skel = vIt->second;
     unsigned int animIndex = traj.AnimIndex();
+    auto anim = skel->Animation(animIndex);
+    if (nullptr == anim)
+    {
+      return allFrames;
+    }
+
     std::map<std::string, math::Matrix4d> rawFrames;
 
     double timeSeconds = std::chrono::duration<double>(_time).count();
@@ -1027,12 +1046,11 @@ std::map<std::string, math::Matrix4d> SceneManager::ActorMeshAnimationAt(
       double distance = traj.DistanceSoFar(_time);
       if (distance < 0.1)
       {
-        rawFrames = skel->Animation(animIndex)->PoseAt(timeSeconds, !noLoop);
+        rawFrames = anim->PoseAt(timeSeconds, !noLoop);
       }
       else
       {
-        rawFrames = skel->Animation(animIndex)->PoseAtX(distance,
-                                        skel->RootNode()->Name());
+        rawFrames = anim->PoseAtX(distance, skel->RootNode()->Name());
       }
     }
     else