Motion: Created plotting trajectory from motion.log

Plotting using gnuplot, needs upgrade for more information
In file plot_motion_log.sh

diff --git a/src/motion/test/plot_motion_log.sh b/src/motion/test/plot_motion_log.sh
new file mode 100644 (file)
index 0000000..cff07ee
--- /dev/null
+++ b/src/motion/test/plot_motion_log.sh
@@ -0,0 +1,14 @@
+#!/bin/bash
+
+log_file=$(find ../../../build/linux -path "*.log")
+
+echo plotting logfile: $log_file;
+
+# displays x y points only
+echo plot "'$log_file' using \"%*9[^\n]%*lf%lf%lf%*lf%*lf\"" | gnuplot -persist
+
+#displays phi on time
+#echo plot "'$log_file' using \"%*9[^\n]%lf%*lf%*lf%lf%*lf\" w lines" | gnuplot -persist
+
+
+# motion.log format: gnuplot: tm x y phi style

diff --git a/src/motion/trajectory.cc b/src/motion/trajectory.cc
index d8fc04e2d374fcd298b3d086918aed12d66d3ec5..80197cc2fd960e7e500e9bd596d93116244cea6f 100644 (file)
--- a/src/motion/trajectory.cc
+++ b/src/motion/trajectory.cc
@@ -15,7 +15,6 @@
 #include "trgendbg.h"
 #include <string.h>
 
-//#define MOTION_LOG
 
 // #ifdef MATLAB_MEX_FILE
 // #define S_FUNCTION_LEVEL 2
@@ -121,7 +120,7 @@ Trajectory::points2segments()
            if ((*p1)->distanceTo(**p2) < 0.001) {
                #ifdef MOTION_LOG
                char ccc[150];
-               sprintf(ccc, "One of identical points x %lf y %lf erased\n", (*p2)->x, (*p2)->y);
+               sprintf(ccc, "#One of identical points x %lf y %lf erased\n", (*p2)->x, (*p2)->y);
                log(ccc);
                #endif
                p2 = wayPoints.erase(p2);
@@ -144,7 +143,7 @@ Trajectory::points2segments()
                    wayPoints.insert(p2, pt);
                    #ifdef MOTION_LOG
                    char ccc[150];
-                   sprintf(ccc, "Sharp edge smoothed %lf %lf, %lf %lf, %lf %lf\n", 
+                   sprintf(ccc, "#Sharp edge smoothed %lf %lf, %lf %lf, %lf %lf\n", 
                        (**p1).x, (**p1).y, (**p2).x, (**p2).y, (**p3).x, (**p3).y);
                    log(ccc);
                    #endif 
@@ -155,7 +154,7 @@ Trajectory::points2segments()
     #ifdef MOTION_LOG
     char c[200];
     for (p1 = wayPoints.begin(); p1 != wayPoints.end(); p1++) {
-       sprintf(c, "wayPoints: x %lf y %lf\n", (*p1)->x, (*p1)->y);
+       sprintf(c, "#wayPoints: x %lf y %lf\n", (*p1)->x, (*p1)->y);
        log(c);
     }
     #endif
@@ -364,7 +363,7 @@ Trajectory::corners2splines()
         segPrev = insert(segNext, new Spline(newEnd1, newEnd2, edge));
         #ifdef MOTION_LOG
        char c[200];
-       sprintf(c, "splineToBeCounted: begin %lf %lf end %lf %lf edge %lf %lf\n", 
+       sprintf(c, "#splineToBeCounted: begin %lf %lf end %lf %lf edge %lf %lf\n", 
                newEnd1->x, newEnd1->y, newEnd2->x, newEnd2->y, edge->x, edge->y);
        log(c);
        #endif
@@ -374,7 +373,7 @@ Trajectory::corners2splines()
     TrajectoryPoints::iterator p1;
     char c[200];
     for (p1 = wayPoints.begin(); p1 != wayPoints.end(); p1++) {
-       sprintf(c, "wayPointsSplined: x %lf y %lf\n", (*p1)->x, (*p1)->y);
+       sprintf(c, "#wayPointsSplined: x %lf y %lf\n", (*p1)->x, (*p1)->y);
        log(c);
     }
     #endif
@@ -758,7 +757,7 @@ Trajectory::getRefPos(double time, Pos &rp)
        seg = *currentSeg;
        #ifdef MOTION_LOG
        char c[100];
-       sprintf(c, "slength %lf\n", seg->getLength()); // gnuplot middle part
+       sprintf(c, "#slength %lf\n", seg->getLength()); // gnuplot middle part
         log(c);
        #endif
         if (d==0)
@@ -890,14 +889,14 @@ bool Trajectory::appendTrajectory(Trajectory &traj, double time)
 //     printf("-difference real %lf %lf\n", p.x, p.y);
 
        #ifdef MOTION_LOG
-       log("trajectory APPENDED (merged two trajectories togeather)\n");
+       log("#trajectory APPENDED (merged two trajectories togeather)\n");
 
        char c[200];
        Pos p1, p2;
        for (iterator seg = begin(); seg != end(); seg++) { // logging segment parameters
                (*seg)->getRefPos((*seg)->getT1(), p1);
                (*seg)->getRefPos((*seg)->getT2(), p2);
-               sprintf(c, "segment: begin %lf %lf, end  %lf %lf, angles %lf %lf, v1 %lf, v2 %lf, t1 %lf, t2 %lf, len %lf\n", 
+               sprintf(c, "#segment: begin %lf %lf, end  %lf %lf, angles %lf %lf, v1 %lf, v2 %lf, t1 %lf, t2 %lf, len %lf\n", 
                        p1.x, p1.y, p2.x, p2.y, p1.phi, p2.phi,(*seg)->v1, (*seg)->v2, (*seg)->getT1(), (*seg)->getT2(), (*seg)->getLength());
                log(c);
        }
@@ -977,7 +976,7 @@ Trajectory::logTraj(double start_time)
     for (iterator seg = begin(); seg != end(); seg++) { // logging segment parameters
        (*seg)->getRefPos((*seg)->getT1(), p1);
        (*seg)->getRefPos((*seg)->getT2(), p2);
-       sprintf(c, "segment: begin %lf %lf, end  %lf %lf, angles %lf %lf, v1 %lf, v2 %lf, t1 %lf, t2 %lf, len %lf\n", 
+       sprintf(c, "#segment: begin %lf %lf, end  %lf %lf, angles %lf %lf, v1 %lf, v2 %lf, t1 %lf, t2 %lf, len %lf\n", 
                p1.x, p1.y, p2.x, p2.y, p1.phi, p2.phi,(*seg)->v1, (*seg)->v2, (*seg)->getT1(), (*seg)->getT2(), (*seg)->getLength());
        log(c);
        sprintf(c, "gnuplot: %lf %lf %lf %lf %d\n", start_time+(*seg)->getT1(), p1.x, p1.y, p1.phi, 1); //gnuplot start point

diff --git a/src/motion/trgen.h b/src/motion/trgen.h
index 714b3d8b16fde4b6dfdd61546b22a5c3cd917d21..d5780d8b25c37935e42e405583c08457a395c252 100644 (file)
--- a/src/motion/trgen.h
+++ b/src/motion/trgen.h
@@ -26,6 +26,10 @@
 #include <math.h>
 #include "trgenconstr.h"
 
+//#define MOTION_LOG
+
+
+
 /**
  * Represents a state of the robot.
  */

diff --git a/src/motion/trgendbg.h b/src/motion/trgendbg.h
index a516681abd07f0bbc5220dcb500fd23e5e07fa54..1677d9e2356a1b8f2c8be23e736c401cb4514fcd 100644 (file)
--- a/src/motion/trgendbg.h
+++ b/src/motion/trgendbg.h
@@ -20,5 +20,4 @@
 #endif
 
 
-
 #endif