Add lite6 path plotting script

2023-03-02 18:10:54 +02:00
parent 58a27194bc
commit da28b3ad82
3 changed files with 95 additions and 2 deletions
--- a/src/lite6_controller/src/lite6_controller.cpp
+++ b/src/lite6_controller/src/lite6_controller.cpp
@@ -384,7 +384,7 @@ public:
      move_group.execute(ts[0]);

      status = status + "," + pointsToString(&goal->motion.path,0,0,0);
-      //appendLineToFile("OUTPUT.csv", status);
+      appendLineToFile("OUTPUT.csv", status);

      result->result = "success";
      goal_handle->succeed(result);
@@ -394,7 +394,7 @@ public:

    status = "failure";
    status = status + "," + pointsToString(&goal->motion.path,0,0,0);
-    //appendLineToFile("OUTPUT.csv", status);
+    appendLineToFile("OUTPUT.csv", status);

    RCLCPP_ERROR(this->get_logger(), "Planner failed to return trajectory in time");
    result->result = "failure";