randomized parking spot generation

Objects/car.cpp

@@ -12,9 +12,7 @@ Car::Car()
       velocity(0),
       angle(0),
       acceleration(0),
-      steering(0),
-      rolling_instace(0),
-      front_center_distance(0),
+      steering(0), rollingInstance(0), frontCenterDistance(0),
       control_index(0),
       sinNegAngle(0),
       cosNegAngle(0) {}
@@ -25,8 +23,7 @@ Car::Car(
     ): position(setPosition),
       dimensions(setDimensions),
       color(setColor),
-      angle(setAngle),
-      front_center_distance(setFrontCenterDistance),
+      angle(setAngle), frontCenterDistance(setFrontCenterDistance),
       name(std::move(setName)) {
   orientation = Eigen::Quaternionf(Eigen::AngleAxisf(angle, Eigen::Vector3f::UnitZ()));
   sinNegAngle = sin(- angle);
@@ -35,7 +32,7 @@ Car::Car(
   steering = 0;
   velocity = 0;
   control_index = 0;
-  rolling_instace = 0.5;
+  rollingInstance = 0.5;
 }
 
 void Car::renderBottom(pcl::visualization::PCLVisualizer::Ptr& viewer) const {
@@ -129,7 +126,7 @@ void Car::control(const std::vector<Control>& c) {
 
 void Car::move(float dt, int time_us) {
   if (!controls.empty() && control_index < (int) controls.size() - 1) {
-    if (time_us >= controls[control_index + 1].time_us) {
+    if (time_us >= controls[control_index + 1].timeUs) {
       accelerate(controls[control_index + 1].acceleration);
       steer(controls[control_index + 1].steering);
       control_index++;
@@ -138,19 +135,19 @@ void Car::move(float dt, int time_us) {
 
   position.x += velocity * cos(angle) * dt;
   position.y += velocity * sin(angle) * dt;
-  angle += velocity * steering * dt / front_center_distance;
+  angle += velocity * steering * dt / frontCenterDistance;
   orientation = Eigen::Quaternionf(Eigen::AngleAxisf(angle, Eigen::Vector3f::UnitZ()));
   velocity += acceleration * dt;
 
   // Apply rolling instance if there is no acceleration input
   if (acceleration == 0) {
     if (velocity > 0) {
-      velocity -= rolling_instace;
+      velocity -= rollingInstance;
       if (velocity < 0) {
         velocity = 0;
       }
     } else if (velocity < 0) {
-      velocity += rolling_instace;
+      velocity += rollingInstance;
       if (velocity > 0) {
         velocity = 0;
       }

Objects/car.hpp

@@ -42,11 +42,11 @@ class Vect3 {
 
 class Control {
 public:
-  long long time_us;
+  long long timeUs;
   float acceleration;
   float steering;
   Control(long long t, float acc, float s)
-      : time_us(t), acceleration(acc), steering(s)
+      : timeUs(t), acceleration(acc), steering(s)
   {}
 };
 
@@ -61,8 +61,8 @@ class Car {
   float angle;
   float acceleration;
   float steering;
-  float front_center_distance;
-  float rolling_instace;
+  float frontCenterDistance;
+  float rollingInstance;
   std::vector<Control> controls;
   int control_index;
   double sinNegAngle;

Objects/road.cpp

@@ -52,13 +52,6 @@ void Road::renderPoles(pcl::visualization::PCLVisualizer::Ptr& viewer) const {
 
 void Road::computeDummies() {
 
-  parkingSpots = {
-      pcl::PointXYZ(-15, -30, 0),
-      pcl::PointXYZ(-15, 30, 0),
-      pcl::PointXYZ(15, -30, 0),
-      pcl::PointXYZ(15, 30, 0)
-  };
-
   std::vector<pcl::PointXYZ> dummyObstacles = {
       pcl::PointXYZ(-15, -15, 0),
       pcl::PointXYZ(-15, 15, 0),
@@ -69,10 +62,22 @@ void Road::computeDummies() {
   std::random_device rd;
   std::mt19937 gen(rd());
   std::uniform_real_distribution<> disDim(1.5, 2.0);
+  std::uniform_real_distribution<> disLoc(-30.0, 30.0);
+
+  std::vector<pcl::PointXYZ> dummyParkingSpots = {
+      pcl::PointXYZ(disLoc(gen), -disLoc(gen), 0),
+      pcl::PointXYZ(-disLoc(gen), disLoc(gen), 0),
+      pcl::PointXYZ(disLoc(gen), -disLoc(gen), 0),
+      pcl::PointXYZ(disLoc(gen), disLoc(gen), 0)
+  };
 
   for (const auto& location : dummyObstacles) {
     double dimension = disDim(gen);
-    obstacles.emplace_back(location, dimension);
+    obstacles.emplace_back(dimension, location);
+  }
+
+  for (auto& location : dummyParkingSpots) {
+    parkingSpots.push_back(location);
   }
 }
 
@@ -90,8 +95,8 @@ void Road::renderDummies(pcl::visualization::PCLVisualizer::Ptr& viewer) const {
   }
 
   for (size_t i = 0; i < obstacles.size(); ++i) {
-    pcl::PointXYZ location = obstacles[i].first;
-    double dimension = obstacles[i].second;
+    double dimension = obstacles[i].first;
+    pcl::PointXYZ location = obstacles[i].second;
     viewer->addCube(location.x, location.x + dimension,
                     location.y, location.y + dimension,
                     location.z, location.z + dimension,

Objects/road.hpp

@@ -28,7 +28,7 @@ class Road {
   std::vector<pcl::ModelCoefficients> laneCoefficients;
   std::vector<pcl::ModelCoefficients> poleCoefficients;
   std::vector<pcl::PointXYZ> parkingSpots;
-  std::vector<std::pair<pcl::PointXYZ, double>> obstacles;
+  std::vector<std::pair<double, pcl::PointXYZ>> obstacles;
 };
 
 void renderPointCloud(pcl::visualization::PCLVisualizer::Ptr& viewer, const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, const std::string& name, Color color = Color(1, 1, 1));

Scene/scene.cpp

@@ -19,14 +19,16 @@ Scene::Scene(pcl::visualization::PCLVisualizer::Ptr& viewer) {
     obstacles.push_back(obstacle);
   }
 
-  parkingSpots.emplace_back(pcl::PointXYZ(-30, -30, 0), 4.0, 2.0, 0.1, 1);
-  parkingSpots.back().setName("parkingSpot1");
-  parkingSpots.emplace_back(pcl::PointXYZ(-30, 30, 0), 4.0, 2.0, 0.1, 2);
-  parkingSpots.back().setName("parkingSpot2");
-  parkingSpots.emplace_back(pcl::PointXYZ(30, -30, 0), 4.0, 2.0, 0.1, 3);
-  parkingSpots.back().setName("parkingSpot3");
-  parkingSpots.emplace_back(pcl::PointXYZ(30, 30, 0), 4.0, 2.0, 0.1, 4);
-  parkingSpots.back().setName("parkingSpot4");
+  double parkingSpotWidth = 2.0;
+  double parkingSpotLength = 4.0;
+  double parkingSpotHeight = 0.1;
+
+  for (int i = 0; i <= 3; ++i) {
+    pcl::PointXYZ position(disPos(gen), disPos(gen), 0.0);
+    ParkingSpot parkingSpot(position, parkingSpotLength, parkingSpotWidth, parkingSpotHeight, i);
+    parkingSpot.setName("parkingSpot" + std::to_string(i));
+    parkingSpots.push_back(parkingSpot);
+  }
 
   Car car1 = Car(
       Vect3(-10, 4, 0),  // Position