Reorganize console output (#105)

* Reorganize console output

* Colorize error and warning output

* Add function to clear line on atty

* Update output level

include/param.h

@@ -61,17 +61,10 @@ typedef enum
 
 typedef enum
 {
-  OUTPUT_LV_ERROR = 0,
-  OUTPUT_LV_WARNING = 2,
-  OUTPUT_LV_QUIET = OUTPUT_LV_WARNING,
-  OUTPUT_LV_PROCESS = 8,
-  OUTPUT_LV_MODULE = 10,
-  OUTPUT_LV_DEFAULT = OUTPUT_LV_MODULE,
-  OUTPUT_LV_COMMAND = 12,
-  OUTPUT_LV_PARAM = 13,
-  OUTPUT_LV_CONTROL = 14,
-  OUTPUT_LV_VERBOSE = OUTPUT_LV_CONTROL,
-  OUTPUT_LV_DEBUG = 16,
+  OUTPUT_LV_ERROR,
+  OUTPUT_LV_WARNING,
+  OUTPUT_LV_INFO,
+  OUTPUT_LV_DEBUG,
 } ParamOutputLv;
 
 typedef struct _parameters *ParametersPtr;

include/yprintf.h

@@ -25,6 +25,7 @@
 
 #include <param.h>
 
-void yprintf(ParamOutputLv level, const char* format, ...);
+void yprintf(ParamOutputLv level, const char *format, ...);
+void ansi_clear_line(ParamOutputLv level);
 
 #endif  // YPRINTF_H

src/command_param.c

@@ -111,12 +111,12 @@ void param_state_com(int cs, double *data, SpurUserParamsPtr spur)
   if (cs == YP_STATE_MOTOR && data[0] == ENABLE)
   {
     set_param_motor();
-    yprintf(OUTPUT_LV_CONTROL, "COMMAND: Motor Control: enabled\n");
+    yprintf(OUTPUT_LV_DEBUG, "COMMAND: Motor Control: enabled\n");
   }
   if (cs == YP_STATE_VELOCITY && data[0] == ENABLE)
   {
     set_param_velocity();
-    yprintf(OUTPUT_LV_CONTROL, "COMMAND: Velocity Control: enabled\n");
+    yprintf(OUTPUT_LV_DEBUG, "COMMAND: Velocity Control: enabled\n");
   }
   if (cs == YP_STATE_BODY && data[0] == ENABLE)
   {
@@ -126,14 +126,14 @@ void param_state_com(int cs, double *data, SpurUserParamsPtr spur)
     odometry->x = 0;
     odometry->y = 0;
     odometry->theta = 0;
-    yprintf(OUTPUT_LV_CONTROL, "COMMAND: Body Control: enabled\n");
+    yprintf(OUTPUT_LV_DEBUG, "COMMAND: Body Control: enabled\n");
   }
   if (cs == YP_STATE_TRACKING && data[0] == ENABLE)
   {
-    yprintf(OUTPUT_LV_CONTROL, "COMMAND: Trajectory Control: enabled\n");
+    yprintf(OUTPUT_LV_DEBUG, "COMMAND: Trajectory Control: enabled\n");
   }
   if (cs == YP_STATE_GRAVITY && data[0] == ENABLE)
   {
-    yprintf(OUTPUT_LV_CONTROL, "COMMAND: Gravity Compensation: enabled\n");
+    yprintf(OUTPUT_LV_DEBUG, "COMMAND: Gravity Compensation: enabled\n");
   }
 }

src/control_vehicle.c

@@ -454,7 +454,7 @@ double gravity_compensation(OdometryPtr odm, SpurUserParamsPtr spur)
 
 void control_loop_cleanup(void *data)
 {
-  yprintf(OUTPUT_LV_MODULE, "Trajectory control loop stopped.\n");
+  yprintf(OUTPUT_LV_INFO, "Trajectory control loop stopped.\n");
 }
 
 /* 20msごとの割り込みで軌跡追従制御処理を呼び出す */
@@ -466,7 +466,7 @@ void control_loop(void)
   odometry = get_odometry_ptr();
   spur = get_spur_user_param_ptr();
 
-  yprintf(OUTPUT_LV_MODULE, "Trajectory control loop started.\n");
+  yprintf(OUTPUT_LV_INFO, "Trajectory control loop started.\n");
   pthread_cleanup_push(control_loop_cleanup, NULL);
 
 #if defined(HAVE_LIBRT)  // clock_nanosleepが利用可能

src/ipcommunication.c

@@ -287,7 +287,7 @@ int ipcmd_recv_tcp(struct ipcmd_t *ipcmd, YPSpur_msg *data)
         return -1;
       }
       ipcmd->clients[i] = sock;
-      yprintf(OUTPUT_LV_PROCESS, "Connection %d accepted from %s.\n", i, inet_ntoa(client.sin_addr));
+      yprintf(OUTPUT_LV_INFO, "Connection %d accepted from %s.\n", i, inet_ntoa(client.sin_addr));
     }
 
     recved = -1;
@@ -312,12 +312,12 @@ int ipcmd_recv_tcp(struct ipcmd_t *ipcmd, YPSpur_msg *data)
     {
       if (ipcmd->tcp_type == IPCMD_TCP_CLIENT)
       {
-        yprintf(OUTPUT_LV_PROCESS, "Connection closed.\n");
+        yprintf(OUTPUT_LV_INFO, "Connection closed.\n");
         ipcmd->connection_error = 1;
         shutdown(ipcmd->socket, SOCK_SHUTDOWN_OPTION);
         return -1;
       }
-      yprintf(OUTPUT_LV_PROCESS, "Connection %d closed.\n", i);
+      yprintf(OUTPUT_LV_INFO, "Connection %d closed.\n", i);
       ipcmd->clients[i] = -1;
       continue;
     }

src/libypspur-md.c

@@ -1412,5 +1412,5 @@ double YP_md_get_joint_torque(YPSpur *spur, int id, double *t)
 
 ParamOutputLv output_lv(void)
 {
-  return OUTPUT_LV_DEFAULT;
+  return OUTPUT_LV_INFO;
 }

src/odometry.c

@@ -594,7 +594,7 @@ int odometry_receive_loop(void)
     ret = serial_recieve(odometry_receive, NULL);
     if (param->parameter_applying)
     {
-      yprintf(OUTPUT_LV_MODULE, "Restarting odometry receive loop.\n");
+      yprintf(OUTPUT_LV_INFO, "Restarting odometry receive loop.\n");
       continue;
     }
     break;

src/param.c

@@ -177,7 +177,7 @@ int arg_analyze(int argc, char *argv[])
 
   g_param.option = OPTION_DEFAULT;
   g_param.msq_key = YPSPUR_MSQ_KEY;
-  g_param.output_lv = OUTPUT_LV_DEFAULT;
+  g_param.output_lv = OUTPUT_LV_INFO;
   g_param.speed = 0;
   g_param.ssm_id = 0;
 
@@ -293,11 +293,11 @@ int arg_analyze(int argc, char *argv[])
     }
     else if (!strcmp(argv[i], "--verbose"))
     {
-      g_param.output_lv = OUTPUT_LV_VERBOSE;
+      g_param.output_lv = OUTPUT_LV_DEBUG;
     }
     else if (!strcmp(argv[i], "--quiet"))
     {
-      g_param.output_lv = OUTPUT_LV_QUIET;
+      g_param.output_lv = OUTPUT_LV_WARNING;
     }
     else if (!strcmp(argv[i], "--reconnect"))
     {
@@ -687,7 +687,7 @@ int set_paramptr(FILE *paramfile)
                 g_Pf[param_num][j] = NULL;
               }
             }
-            yprintf(OUTPUT_LV_PARAM, "%d %s %f\n", param_num, param_names[param_num], g_P[param_num][0]);
+            yprintf(OUTPUT_LV_DEBUG, "%d %s %f\n", param_num, param_names[param_num], g_P[param_num][0]);
           }
           else
           {
@@ -700,7 +700,7 @@ int set_paramptr(FILE *paramfile)
               formula_free(g_Pf[param_num][motor_num]);
               g_Pf[param_num][motor_num] = NULL;
             }
-            yprintf(OUTPUT_LV_PARAM, "%d %s[%d] %f\n", param_num, param_names[param_num], motor_num,
+            yprintf(OUTPUT_LV_DEBUG, "%d %s[%d] %f\n", param_num, param_names[param_num], motor_num,
                     g_P[param_num][motor_num]);
           }
           param_num = YP_PARAM_NUM;
@@ -746,10 +746,10 @@ int set_paramptr(FILE *paramfile)
               {
                 g_Pf[param_num][j] = formula_optimize(g_Pf[param_num][j]);
               }
-              yprintf(OUTPUT_LV_PARAM, "%d %s ", param_num, param_names[param_num]);
-              if (output_lv() >= OUTPUT_LV_PARAM)
+              yprintf(OUTPUT_LV_DEBUG, "%d %s ", param_num, param_names[param_num]);
+              if (output_lv() >= OUTPUT_LV_DEBUG)
                 formula_print(stderr, g_Pf[param_num][0]);
-              yprintf(OUTPUT_LV_PARAM, "\n");
+              yprintf(OUTPUT_LV_DEBUG, "\n");
             }
           }
           else
@@ -767,10 +767,10 @@ int set_paramptr(FILE *paramfile)
             else
             {
               g_Pf[param_num][motor_num] = formula_optimize(g_Pf[param_num][motor_num]);
-              yprintf(OUTPUT_LV_PARAM, "%d %s[%d] ", param_num, param_names[param_num], motor_num);
-              if (output_lv() >= OUTPUT_LV_PARAM)
+              yprintf(OUTPUT_LV_DEBUG, "%d %s[%d] ", param_num, param_names[param_num], motor_num);
+              if (output_lv() >= OUTPUT_LV_DEBUG)
                 formula_print(stderr, g_Pf[param_num][motor_num]);
-              yprintf(OUTPUT_LV_PARAM, "\n");
+              yprintf(OUTPUT_LV_DEBUG, "\n");
             }
           }
           param_num = YP_PARAM_NUM;
@@ -882,7 +882,7 @@ int set_paramptr(FILE *paramfile)
   enable_state(YP_STATE_BODY);
   enable_state(YP_STATE_TRACKING);
 
-  yprintf(OUTPUT_LV_PARAM, "Info: %d motors defined\n", g_param.num_motor_enable);
+  yprintf(OUTPUT_LV_DEBUG, "Info: %d motors defined\n", g_param.num_motor_enable);
 
   return 1;
 }
@@ -902,7 +902,7 @@ int set_param(char *filename, char *concrete_path)
     FILE *fd;
 #endif  // HAVE_PKG_CONFIG
 
-    yprintf(OUTPUT_LV_PARAM, "Warn: File [%s] is not exist.\n", filename);
+    yprintf(OUTPUT_LV_DEBUG, "Warn: File [%s] is not exist.\n", filename);
 
 #if HAVE_PKG_CONFIG
     if (!strchr(filename, '/'))
@@ -969,14 +969,14 @@ void init_param_update_thread(pthread_t *thread, char *filename)
 
 void param_update_loop_cleanup(void *data)
 {
-  yprintf(OUTPUT_LV_MODULE, "Parameter updater stopped.\n");
+  yprintf(OUTPUT_LV_INFO, "Parameter updater stopped.\n");
 }
 
 void param_update(void *filename)
 {
   struct stat prev_status;
 
-  yprintf(OUTPUT_LV_MODULE, "Parameter updater started. Watching %s\n", filename);
+  yprintf(OUTPUT_LV_INFO, "Parameter updater started. Watching %s\n", filename);
   pthread_cleanup_push(param_update_loop_cleanup, filename);
 
   stat(filename, &prev_status);
@@ -1014,7 +1014,7 @@ void param_update(void *filename)
 /* パラメータ適用 */
 int apply_robot_params()
 {
-  yprintf(OUTPUT_LV_MODULE, "Applying parameters.\n");
+  yprintf(OUTPUT_LV_INFO, "Applying parameters.\n");
 
   int j;
   // ウォッチドックタイマの設定
@@ -1208,7 +1208,7 @@ int set_param_motor(void)
       ki = (double)(65536.0 * g_P[YP_PARAM_PWM_MAX][j] * g_P[YP_PARAM_MOTOR_R][j] /
                     (g_P[YP_PARAM_TORQUE_UNIT][j] * g_P[YP_PARAM_MOTOR_TC][j] *
                      g_P[YP_PARAM_VOLT][j]));
-      yprintf(OUTPUT_LV_PARAM, "Info: TORQUE_CONSTANT[%d]: %d\n", j, (int)ki);
+      yprintf(OUTPUT_LV_DEBUG, "Info: TORQUE_CONSTANT[%d]: %d\n", j, (int)ki);
       if (ki == 0)
       {
         yprintf(OUTPUT_LV_ERROR, "ERROR: TORQUE_CONSTANT[%d] fixed point value underflow\n", j);
@@ -1340,7 +1340,7 @@ int set_param_velocity(void)
     ffr = 256.0 * 2.0 * M_PI * g_P[YP_PARAM_TORQUE_UNIT][0] / (g_P[YP_PARAM_COUNT_REV][0] * fabs(g_P[YP_PARAM_GEAR][0]));
     if (ischanged_p(YP_PARAM_GAIN_A, 0) || ffr_changed)
     {
-      yprintf(OUTPUT_LV_PARAM, "Info: GAIN_A: %d\n",
+      yprintf(OUTPUT_LV_DEBUG, "Info: GAIN_A: %d\n",
               (int)(g_P[YP_PARAM_GAIN_A][0] * ffr));
       if (abs(g_P[YP_PARAM_GAIN_A][0] * ffr) == 0)
       {
@@ -1364,7 +1364,7 @@ int set_param_velocity(void)
     ffl = 256.0 * 2.0 * M_PI * g_P[YP_PARAM_TORQUE_UNIT][1] / (g_P[YP_PARAM_COUNT_REV][1] * fabs(g_P[YP_PARAM_GEAR][1]));
     if (ischanged_p(YP_PARAM_GAIN_B, 0) || ffl_changed)
     {
-      yprintf(OUTPUT_LV_PARAM, "Info: GAIN_B: %d\n",
+      yprintf(OUTPUT_LV_DEBUG, "Info: GAIN_B: %d\n",
               (int)(g_P[YP_PARAM_GAIN_A][0] * ffl));
       if (abs(g_P[YP_PARAM_GAIN_B][0] * ffl) == 0)
       {
@@ -1407,7 +1407,7 @@ int set_param_velocity(void)
       }
       if (ischanged_p(YP_PARAM_INERTIA_SELF, j) || ff_changed)
       {
-        yprintf(OUTPUT_LV_PARAM, "Info: INERTIA_SELF[%d]: %d\n", j,
+        yprintf(OUTPUT_LV_DEBUG, "Info: INERTIA_SELF[%d]: %d\n", j,
                 (int)(g_P[YP_PARAM_INERTIA_SELF][j] * ff));
         if (abs(g_P[YP_PARAM_INERTIA_SELF][j] * ff) == 0)
         {

src/serial.c

@@ -236,7 +236,7 @@ int serial_change_baudrate(int baud)
   {
     errnum = errno;
     yprintf(OUTPUT_LV_ERROR, "Error: Failed to set input baud rate to %d\n", baud);
-    yprintf(OUTPUT_LV_VERBOSE, "Error: cfsetispeed: %s(%d)\n", strerror(errnum), errnum);
+    yprintf(OUTPUT_LV_DEBUG, "Error: cfsetispeed: %s(%d)\n", strerror(errnum), errnum);
     return 0;
   }
   errno = 0;
@@ -245,7 +245,7 @@ int serial_change_baudrate(int baud)
   {
     errnum = errno;
     yprintf(OUTPUT_LV_ERROR, "Error: Failed to set output baud rate to %d\n", baud);
-    yprintf(OUTPUT_LV_VERBOSE, "Error: cfsetospeed: %s(%d)\n", strerror(errnum), errnum);
+    yprintf(OUTPUT_LV_DEBUG, "Error: cfsetospeed: %s(%d)\n", strerror(errnum), errnum);
     return 0;
   }
   SER_BAUDRATE = (double)baud;
@@ -259,7 +259,7 @@ int serial_change_baudrate(int baud)
   {
     errnum = errno;
     yprintf(OUTPUT_LV_ERROR, "Error: Failed to set attribute of serial port\n");
-    yprintf(OUTPUT_LV_VERBOSE, "Error: tcsetattr: %s(%d)\n", strerror(errnum), errnum);
+    yprintf(OUTPUT_LV_DEBUG, "Error: tcsetattr: %s(%d)\n", strerror(errnum), errnum);
     return 0;
   }
 
@@ -273,7 +273,7 @@ int serial_change_baudrate(int baud)
     {
       errnum = errno;
       yprintf(OUTPUT_LV_ERROR, "Error: Failed to get  attribute of serial port\n");
-      yprintf(OUTPUT_LV_VERBOSE, "Error: tcgetattr: %s(%d)\n", strerror(errnum), errnum);
+      yprintf(OUTPUT_LV_DEBUG, "Error: tcgetattr: %s(%d)\n", strerror(errnum), errnum);
       return 0;
     }
     isp = cfgetispeed(&term);
@@ -444,12 +444,12 @@ int serial_recieve(int (*serial_event)(char *, int, double, void *), void *data)
     {
       int errnum;
       errnum = errno;
-      yprintf(OUTPUT_LV_VERBOSE, "Error: Select in serial_recieve failed. (%s)\n", strerror(errnum));
+      yprintf(OUTPUT_LV_DEBUG, "Error: Select in serial_recieve failed. (%s)\n", strerror(errnum));
       return -1;
     }
     else if (retval == 0)
     {
-      yprintf(OUTPUT_LV_VERBOSE, "Error: Select timed out\n");
+      yprintf(OUTPUT_LV_DEBUG, "Error: Select timed out\n");
       return -1;
     }
     receive_time = get_time();
@@ -459,12 +459,12 @@ int serial_recieve(int (*serial_event)(char *, int, double, void *), void *data)
     {
       int errnum;
       errnum = errno;
-      yprintf(OUTPUT_LV_VERBOSE, "Error: Read in serial_recieve failed. (%s)\n", strerror(errnum));
+      yprintf(OUTPUT_LV_DEBUG, "Error: Read in serial_recieve failed. (%s)\n", strerror(errnum));
       return -1;
     }
     else if (len == 0)
     {
-      yprintf(OUTPUT_LV_VERBOSE, "Error: Read timed out\n");
+      yprintf(OUTPUT_LV_DEBUG, "Error: Read timed out\n");
       return -1;
     }
 #else

src/ssm_spur_handler.c

@@ -54,7 +54,7 @@ void init_ypspurSSM(int ssm_id)
 #ifdef HAVE_SSM
   g_ssm_id = ssm_id;
 
-  yprintf(OUTPUT_LV_MODULE, "    with SSM\n");
+  yprintf(OUTPUT_LV_INFO, "    with SSM\n");
   if (!initSSM())
   {
     /* SSMが起動していない */
@@ -159,7 +159,7 @@ void coordinate_synchronize(Odometry *odm, SpurUserParamsPtr spur)
         if (g_odm_adj_sid > 0)
         {  // openできた
           g_ssm_adj_enable = 1;
-          yprintf(OUTPUT_LV_MODULE, "SSMInfo: Find adjust information.\n");
+          yprintf(OUTPUT_LV_INFO, "SSMInfo: Find adjust information.\n");
         }
         else
         {