Add more debug print out to the spline example

UniversalRobots · Aug 3, 2023 · c179569 · c179569
1 parent 4ef931d
commit c179569
Showing 1 changed file with 16 additions and 4 deletions.
diff --git 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;