Motion: appending trajectories without segfault

diff --git a/src/motion/trajectory.cc b/src/motion/trajectory.cc
index 29e66a4103a30629120019ac614608d3989ab0d5..7c53d2b669e10208e1b4238abdd6328453aff25c 100644 (file)
--- a/src/motion/trajectory.cc
+++ b/src/motion/trajectory.cc
@@ -735,21 +735,21 @@ bool Trajectory::appendTrajectory(Trajectory &traj, double time)
 
 //        printf("swtime> %lf\n", time);
 
-        Pos h1, h2;
+/*        Pos h1, h2;
         for (iterator seg = begin(); seg != end(); ++seg) {
                 (*seg)->getRefPos((*seg)->getT1(), h1);
                 (*seg)->getRefPos((*seg)->getT2(), h2);
-//                printf("puvodni %lf %lf    %lf %lf", h1.x, h1.y, h2.x, h2.y);
-//                printf(" -- times: %lf %lf\n", (*seg)->getT1(), (*seg)->getT2());
+                printf("puvodni %lf %lf    %lf %lf", h1.x, h1.y, h2.x, h2.y);
+                printf(" -- times: %lf %lf\n", (*seg)->getT1(), (*seg)->getT2());
         }
 
         for (iterator seg = traj.begin(); seg != traj.end(); ++seg) {
                 (*seg)->getRefPos((*seg)->getT1(), h1);
                 (*seg)->getRefPos((*seg)->getT2(), h2);
-//                printf("novy   %lf %lf    %lf %lf", h1.x, h1.y, h2.x, h2.y);
-//                printf(" -- times: %lf %lf\n", (*seg)->getT1(), (*seg)->getT2());
+                printf("novy   %lf %lf    %lf %lf", h1.x, h1.y, h2.x, h2.y);
+                printf(" -- times: %lf %lf\n", (*seg)->getT1(), (*seg)->getT2());
         }
-
+*/
         // split & delete
         for (iterator seg = begin(); seg != end(); ++seg) {
                 if ((*seg)->containsTime(time) == 0) {
@@ -783,15 +783,16 @@ bool Trajectory::appendTrajectory(Trajectory &traj, double time)
         for (iterator seg = begin(); seg != end(); ++seg) {
                 t = (*seg)->startAt(t);
         }
-
+/*
         for (iterator seg = begin(); seg != end(); ++seg) {
                 (*seg)->getRefPos((*seg)->getT1(), h1);
                 (*seg)->getRefPos((*seg)->getT2(), h2);
-//                printf("merged  %lf %lf    %lf %lf", h1.x, h1.y, h2.x, h2.y);
-//                printf(" -- times: %lf %lf\n", (*seg)->getT1(), (*seg)->getT2());
+                printf("merged  %lf %lf    %lf %lf", h1.x, h1.y, h2.x, h2.y);
+                printf(" -- times: %lf %lf\n", (*seg)->getT1(), (*seg)->getT2());
         }
 
-
+*/
+        currentSeg = begin();
         prepared = true;        
         return true;
 }

diff --git a/src/robofsm/motion-control.cc b/src/robofsm/motion-control.cc
index f38a8692ab15d112d4374bf25c45e3a47a7011ce..19842ae5def0180c9e15e598f84d5b5b62d1f7e4 100644 (file)
--- a/src/robofsm/motion-control.cc
+++ b/src/robofsm/motion-control.cc
@@ -391,20 +391,19 @@ void go(Trajectory *t, double append_time)
 {
        pthread_mutex_lock(&actual_trajectory_lock);
        Trajectory *old;
-       old = actual_trajectory;
         if (append_time != 0) {
                 // trajectory only connects a new one in some specific time
                 if(!actual_trajectory->appendTrajectory(*t, append_time))
                         DBG("Can not append trajectory\n");
         } else {
                 // trajectory starts from zero time
+               old = actual_trajectory;
                gettimeofday(&tv_start, NULL);
                actual_trajectory = t;
+               if (old)
+                       delete(old);
         }
        pthread_mutex_unlock(&actual_trajectory_lock);
-       if (old)
-               delete(old);
-
 }
 
 /**