Fix segfault for older PCL version (#3)

* enable full debug logs

* reduce c++ standard to 14, enable debug logs for first target

* lower cpp std to 11

* cpp stadard to 14, remove arraws from viz as pcl versions mismatch

* final logs adjst

CMakeLists.txt

@@ -1,6 +1,6 @@
 cmake_minimum_required(VERSION 2.8 FATAL_ERROR)
 
-add_definitions(-std=c++17)
+add_definitions(-std=c++14)
 
 set(CXX_FLAGS "-Wall")
 set(CMAKE_CXX_FLAGS, "${CXX_FLAGS}")

src/highway.h

@@ -28,13 +28,12 @@ class Highway
 	bool visualize_radar = true;
 	bool visualize_pcd = false;
 	// Predict path in the future using UKF
-	double projectedTime = 2;
-	int projectedSteps = 20;
+	double projectedTime = 1;
+	int projectedSteps = 5;
 	// --------------------------------
 
 	Highway(pcl::visualization::PCLVisualizer::Ptr& viewer)
 	{
-
 		tools = Tools();
 
 		egoCar = Car(Vect3(0, 0, 0), Vect3(4, 2, 2), Color(0, 1, 0), 0, 0, 2, "egoCar");
@@ -54,7 +53,7 @@ class Highway
 		car1.setInstructions(car1_instructions);
 		if( trackCars[0] )
 		{
-			UKF ukf1(true, true, false);
+			UKF ukf1(true, true, true);
 			car1.setUKF(ukf1);
 		}
 		traffic.push_back(car1);
@@ -134,17 +133,25 @@ class Highway
 				tools.ground_truth.push_back(gt);
 				tools.lidarSense(traffic[i], viewer, timestamp, visualize_lidar);
 				tools.radarSense(traffic[i], egoCar, viewer, timestamp, visualize_radar);
+				if (traffic[i].ukf.debug){
+					std::cout << "GT=[" << gt.transpose() << "].\n";
+				}
+				if (traffic[i].ukf.debug){
+					std::cout << "Viz UKF results... ";
+				}
+
 				tools.ukfResults(traffic[i],viewer, projectedTime, projectedSteps);
+				if (traffic[i].ukf.debug){
+					std::cout << "Done.\n";
+				}
+
 				VectorXd estimate(4);
 				double v  = traffic[i].ukf.getState()(2);
     			double yaw = traffic[i].ukf.getState()(3);
     			double v1 = cos(yaw)*v;
     			double v2 = sin(yaw)*v;
 				estimate << traffic[i].ukf.getState()[0], traffic[i].ukf.getState()[1], v1, v2;
 				tools.estimations.push_back(estimate);
-				if (traffic[i].ukf.debug){
-					std::cout << "GT=[" << gt.transpose() << "].\n";
-				}
 
 			}
 		}

src/tools.cpp

@@ -68,9 +68,9 @@ void Tools::ukfResults(Car car, pcl::visualization::PCLVisualizer::Ptr& viewer,
 	UKF ukf = car.ukf;
 	ukf.debug = false;
 	viewer->addSphere(pcl::PointXYZ(ukf.getState()[0],ukf.getState()[1],3.5), 0.5, 0, 1, 0,car.name+"_ukf");
-	viewer->addArrow(pcl::PointXYZ(ukf.getState()[0], ukf.getState()[1],3.5), 
-		pcl::PointXYZ(ukf.getState()[0]+ukf.getState()[2]*cos(ukf.getState()[3]),
-		ukf.getState()[1]+ukf.getState()[2]*sin(ukf.getState()[3]),3.5), 0, 1, 0, car.name+"_ukf_vel");
+	// viewer->addArrow(pcl::PointXYZ(ukf.getState()[0], ukf.getState()[1],3.5), 
+	// 	pcl::PointXYZ(ukf.getState()[0]+ukf.getState()[2]*cos(ukf.getState()[3]),
+	// 	ukf.getState()[1]+ukf.getState()[2]*sin(ukf.getState()[3]),3.5), 0, 1, 0, car.name+"_ukf_vel");
 	if(time > 0)
 	{
 		double dt = time/steps;
@@ -79,11 +79,11 @@ void Tools::ukfResults(Car car, pcl::visualization::PCLVisualizer::Ptr& viewer,
 		{
 			ukf.predict(dt);
 			viewer->addSphere(pcl::PointXYZ(ukf.getState()[0],ukf.getState()[1],3.5), 0.5, 0, 1, 0,car.name+"_ukf"+std::to_string(ct));
-			viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-0.8*(ct/time), car.name+"_ukf"+std::to_string(ct));
-			viewer->addArrow(pcl::PointXYZ(ukf.getState()[0], ukf.getState()[1],3.5), 
-				pcl::PointXYZ(ukf.getState()[0]+ukf.getState()[2]*cos(ukf.getState()[3]),ukf.getState()[1]+ukf.getState()[2]*sin(ukf.getState()[3]),3.5), 
-				0, 1, 0, car.name+"_ukf_vel"+std::to_string(ct));
-			viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-0.8*(ct/time), car.name+"_ukf_vel"+std::to_string(ct));
+			// viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-0.8*(ct/time), car.name+"_ukf"+std::to_string(ct));
+			// viewer->addArrow(pcl::PointXYZ(ukf.getState()[0], ukf.getState()[1],3.5), 
+			// 	pcl::PointXYZ(ukf.getState()[0]+ukf.getState()[2]*cos(ukf.getState()[3]),ukf.getState()[1]+ukf.getState()[2]*sin(ukf.getState()[3]),3.5), 
+			// 	0, 1, 0, car.name+"_ukf_vel"+std::to_string(ct));
+			// viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0-0.8*(ct/time), car.name+"_ukf_vel"+std::to_string(ct));
 			ct += dt;
 		}
 	}

src/ukf.cpp

@@ -88,6 +88,10 @@ void UKF::step(MeasurementPackage meas_package)
     break;
   }
   }
+  if (debug){
+    std::cout << nIter << ": step completed OK.\n";
+  }
+
 }
 
 Eigen::VectorXd UKF::getState() const