From c179569130f11e5b68c13f87af128c4c2b7bbe02 Mon Sep 17 00:00:00 2001
From: urrsk <41109954+urrsk@users.noreply.github.com>
Date: Thu, 3 Aug 2023 21:04:39 +0200
Subject: [PATCH] Add more debug print out to the spline example

---
 examples/spline_example.cpp | 20 ++++++++++++++++----
 1 file changed, 16 insertions(+), 4 deletions(-)

diff --git a/examples/spline_example.cpp b/examples/spline_example.cpp
index 47ffa81a..4e7d9641 100644
--- a/examples/spline_example.cpp
+++ b/examples/spline_example.cpp
@@ -202,7 +202,10 @@ int main(int argc, char* argv[])
   ret = g_my_driver->writeJointCommand(vector6d_t(), comm::ControlMode::MODE_FORWARD);
   if (!ret)
   {
-    URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?");
+    std::stringstream lastq;
+    lastq << g_joint_positions;
+    URCL_LOG_ERROR("Could not send joint command. Last q received is %s\n Is the robot in remote control?",
+                   lastq.str().c_str());
     return 1;
   }
 
@@ -247,7 +250,10 @@ int main(int argc, char* argv[])
 
       if (!ret)
       {
-        URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?");
+        std::stringstream lastq;
+        lastq << g_joint_positions;
+        URCL_LOG_ERROR("Could not send joint command. Last q received is %s\n Is the robot in remote control?",
+                       lastq.str().c_str());
         return 1;
       }
     }
@@ -276,7 +282,10 @@ int main(int argc, char* argv[])
 
       if (!ret)
       {
-        URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?");
+        std::stringstream lastq;
+        lastq << g_joint_positions;
+        URCL_LOG_ERROR("Could not send joint command. Last q received is %s\n Is the robot in remote control?",
+                       lastq.str().c_str());
         return 1;
       }
     }
@@ -287,7 +296,10 @@ int main(int argc, char* argv[])
   ret = g_my_driver->writeJointCommand(vector6d_t(), comm::ControlMode::MODE_FORWARD);
   if (!ret)
   {
-    URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?");
+    std::stringstream lastq;
+    lastq << g_joint_positions;
+    URCL_LOG_ERROR("Could not send joint command. Last q received is %s\n Is the robot in remote control?",
+                   lastq.str().c_str());
     return 1;
   }
   return 0;