Fix crash when plotting an empty trajectory

tesseract-robotics · Oct 9, 2023 · cfbc929 · cfbc929
1 parent da7ce37
commit cfbc929
Showing 1 changed file with 9 additions and 6 deletions.
diff --git a/tesseract_rosutils/src/plotting.cpp b/tesseract_rosutils/src/plotting.cpp
@@ -116,13 +116,16 @@ void ROSPlotting::plotTrajectory(const tesseract_common::JointTrajectory& traj,
 {
   tesseract_msgs::msg::Trajectory msg;
 
-  // Set the initial state
-  for (std::size_t i = 0; i < traj[0].joint_names.size(); ++i)
+  if (!traj.empty())
   {
-    tesseract_msgs::msg::StringDoublePair pair;
-    pair.first = traj[0].joint_names[i];
-    pair.second = traj[0].position[static_cast<Eigen::Index>(i)];
-    msg.initial_state.push_back(pair);
+    // Set the initial state
+    for (std::size_t i = 0; i < traj[0].joint_names.size(); ++i)
+    {
+      tesseract_msgs::msg::StringDoublePair pair;
+      pair.first = traj[0].joint_names[i];
+      pair.second = traj[0].position[static_cast<Eigen::Index>(i)];
+      msg.initial_state.push_back(pair);
+    }
   }
 
   // Set the joint trajectory message