More clear output to test against detection logs

jhirsh

.gitignore

@@ -8,3 +8,4 @@ thirdparty/Qt5
 qt-*
 scripts/install
 armlab-regular/env/
+*.jlp

include/planner/arm_planner.hpp

@@ -101,7 +101,7 @@ class ArmPlanner
             for (size_t i = 0; i < newBalls.detections.size(); ++i)
             {
                 ball_detection_t detection = newBalls.detections[i];
-                Eigen::Vector2d ballPosition( detection.position[0], detection.position[1] );
+                Eigen::Vector2d detectionPosition( detection.position[0], detection.position[1] );
 
                 Ball * closest;
                 double closestDistance = DBL_MAX;
@@ -115,12 +115,12 @@ class ArmPlanner
 
                     Eigen::Vector4d predictionState = ball.predict_coordinate( detection.utime );
                     Eigen::Vector2d prediction( predictionState.x(), predictionState.y() );
-                    double distance = (prediction - ballPosition).norm();
-                    std::cout << "Ball " << ballPosition.x() << " " << ballPosition.y() << std::endl;
-                    std::cout << "Prediction " << prediction.x() << " " << prediction.y() << std::endl;
-                    std::cout << "Distance " << distance;
+                    double distance = (prediction - detectionPosition).norm();
                     if ( distance < corrThreshold && distance < closestDistance )
                         {
+                        std::cout << "Detection " << detectionPosition.x() << " " << detectionPosition.y() << std::endl;
+                        //std::cout << "Prediction " << prediction.x() << " " << prediction.y() << std::endl;
+                        std::cout << "Distance " << distance << std::endl;
                         closest = &ball;
                         closestDistance = distance;
                         }
@@ -144,6 +144,7 @@ class ArmPlanner
                     if ( (*it).odds < -10 )
                         {
                         // purge ball
+                        std::cout << "Purged ball... " << balls.size() << " left";
                         it = balls.erase( it );
                         }
                     else

src/planner/planner.cpp

@@ -52,15 +52,19 @@ class Handler
 
         planner->updateBalls( *ball );
 
+        if(planner->balls.empty()) return;
         std::vector<Vector2d> plan = planner->calculatePlan( );
+        /*
         auto pos =  planner->balls[0].getPos();
         cout << pos << '\n';
         cout << " calculated Plan at " << endl;
-        //planner.publishPlan( plan );
+        planner.publishPlan( plan );
+        */
     }
 
 private:
     shared_ptr<ArmPlanner> planner;
+    int64_t prevTime = 0;
 };
 
 // Handles lcm in a loop until program no longer running