Robot driving with gnuplot

robot.cpp

// @file      main.cpp
// @author    Donghee Park
//
// Copyright (c) 2024 Donghee Park, all rights reserved

#include <cstdlib>
#include <fstream>
#include <iostream>
#include <unistd.h>
#include <math.h>
#include <signal.h>

#define PI 3.14159265

int init(std::string path_file, std::string body_file) {
  std::ofstream pathFile(path_file);
  std::ofstream bodyFile(body_file);

  if (!pathFile.is_open()) {
    return 1;
  }

  if (!bodyFile.is_open()) {
    return 1;
  }

  pathFile << "3.000000 0.000000\n";
  pathFile << "2.985013 0.299500\n";
  pathFile << "2.940200 0.596008\n";
  pathFile << "2.866009 0.886561\n";
  pathFile << "2.763183 1.168255\n";
  pathFile << "2.632748 1.438277\n";
  pathFile << "2.476007 1.693928\n";
  pathFile << "2.294527 1.932653\n";
  pathFile << "2.090120 2.152068\n";
  pathFile << "1.864830 2.349981\n";
  pathFile << "1.620907 2.524413\n";
  pathFile << "1.360788 2.673622\n";
  pathFile << "1.087073 2.796117\n";
  pathFile << "0.802496 2.890675\n";
  pathFile << "0.509901 2.956349\n";
  pathFile << "0.212211 2.992485\n";
  pathFile << "-0.087599 2.998721\n";
  pathFile << "-0.386534 2.974994\n";
  pathFile << "-0.681607 2.921543\n";
  pathFile << "-0.969870 2.838900\n";
  pathFile << "-1.248441 2.727892\n";
  pathFile << "-1.514539 2.589628\n";
  pathFile << "-1.765503 2.425489\n";
  pathFile << "-1.998828 2.237116\n";
  pathFile << "-2.212181 2.026390\n";
  pathFile << "-2.403430 1.795417\n";
  pathFile << "-2.570666 1.546505\n";
  pathFile << "-2.712216 1.282141\n";
  pathFile << "-2.826667 1.004966\n";
  pathFile << "-2.912874 0.717750\n";
  pathFile << "-2.969977 0.423362\n";
  pathFile << "-2.997406 0.124744\n";
  pathFile << "-2.994884 -0.175120\n";
  pathFile << "-2.962440 -0.473234\n";
  pathFile << "-2.900395 -0.766620\n";
  pathFile << "-2.809371 -1.052346\n";
  pathFile << "-2.690277 -1.327558\n";
  pathFile << "-2.544302 -1.589505\n";
  pathFile << "-2.372906 -1.835570\n";
  pathFile << "-2.177800 -2.063295\n";
  pathFile << "-1.960935 -2.270404\n";
  pathFile << "-1.724476 -2.454829\n";
  pathFile << "-1.470787 -2.614725\n";
  pathFile << "-1.202402 -2.748496\n";
  pathFile << "-0.922004 -2.854805\n";
  pathFile << "-0.632393 -2.932589\n";
  pathFile << "-0.336464 -2.981072\n";
  pathFile << "-0.037172 -2.999770\n";
  pathFile << "0.262490 -2.988494\n";
  pathFile << "0.559530 -2.947359\n";
  pathFile << "0.850980 -2.876775\n";
  pathFile << "1.133926 -2.777447\n";
  pathFile << "1.405543 -2.650368\n";
  pathFile << "1.663116 -2.496807\n";
  pathFile << "1.904072 -2.318299\n";
  pathFile << "2.126003 -2.116627\n";
  pathFile << "2.326692 -1.893807\n";
  pathFile << "2.504133 -1.652064\n";
  pathFile << "2.656554 -1.393815\n";
  pathFile << "2.782432 -1.121639\n";
  pathFile << "2.880508 -0.838256\n";
  pathFile << "2.949803 -0.546498\n";
  pathFile << "2.989625 -0.249279\n";
  pathFile << "3.000000 0.000000\n";

  bodyFile << "3.000000 1.000000\n";
  bodyFile << "2.500000 -0.500000\n";
  bodyFile << "3.500000 -0.500000\n";
  bodyFile << "3.000000 1.000000\n";

  pathFile.close();
  bodyFile.close();
  return 0;
}

int body(std::string body_file, float body_x, float body_y, float body_theta) {
  std::ofstream bodyFile(body_file);
  if (!bodyFile.is_open()) {
    return 1;
  }

  float x11 = 1.0 * cos(body_theta) - 0.0 * sin(body_theta);
  float y11 = 1.0 * sin(body_theta) + 0.0 * cos(body_theta);
  float x22 = -0.5 * cos(body_theta) - 0.5 * sin(body_theta);
  float y22 = -0.5 * sin(body_theta) + 0.5 * cos(body_theta);
  float x33 = -0.5 * cos(body_theta) + 0.5 * sin(body_theta);
  float y33 = -0.5 * sin(body_theta) - 0.5 * cos(body_theta);

  bodyFile << body_x + x11 << " " << body_y + y11 << "\n";
  bodyFile << body_x + x22 << " " << body_y + y22 << "\n";
  bodyFile << body_x + x33 << " " << body_y + y33 << "\n";
  bodyFile << body_x + x11 << " " << body_y + y11 << "\n";

  // without rotation
  // bodyFile << body_x << " " << body_y << "\n";
  // bodyFile << body_x - 0.5 << " "  << body_y - 0.5 << "\n";
  // bodyFile << body_x + 0.5 << " " << body_y - 0.5 << "\n";
  // bodyFile << body_x << " " << body_y << "\n";

  bodyFile.close();
  return 0;
}

void signal_handler(int sig) {
  system("killall gnuplot");
  system("killall gnuplot_qt");
  exit(0);
}

int main() {
  signal(SIGINT, signal_handler);

  std::string path_file = "path.txt";
  std::string body_file = "body.txt";
  float body_x = 3.0;
  float body_y = 2.0;
  float body_theta = PI / 2.0;
  float dt = 0.1f;

  if (init(path_file, body_file) != 0) {
    return 1;
  }

  FILE *gp = popen("gnuplot -persist ", "w");
  if (gp == NULL) {
    return 1;
  }
  fprintf(gp, "set colors classic \n");
  fprintf(gp, "set grid \n");
  fprintf(gp, "unset autoscale \n");
  fprintf(gp, "set xlabel \"X [m]\" \n");
  fprintf(gp, "set ylabel \"Y [m]\" \n");

  for (float t = 0; t < 10.0 * PI; t += dt) {
    body(body_file, body_x, body_y, body_theta);
    // body_x += 0.01; body_y += 0.01;
    body_theta += dt;
    body_x = 3 * cos(body_theta - PI / 2.0);
    body_y = 3 * sin(body_theta - PI / 2.0);

    fprintf(gp, "plot \"%s\" lt -1 w l t \"Path\", \"%s\" lt -1 w l t \"Body\" \n", path_file.c_str(), body_file.c_str());
    fflush(gp);

    usleep(dt * 1000000);
  }

  pclose(gp);

  return 0;
}