From 3d07ea1d13eea34fba9baeff8739d53f8ad4b37f Mon Sep 17 00:00:00 2001 From: AndreiMoraru123 <81184255+AndreiMoraru123@users.noreply.github.com> Date: Sun, 18 Jun 2023 22:52:46 +0300 Subject: [PATCH] randomized parking spot generation --- Objects/car.cpp | 17 +++++++---------- Objects/car.hpp | 8 ++++---- Objects/road.cpp | 25 +++++++++++++++---------- Objects/road.hpp | 2 +- Scene/scene.cpp | 18 ++++++++++-------- 5 files changed, 37 insertions(+), 33 deletions(-) diff --git a/Objects/car.cpp b/Objects/car.cpp index aa69264..f2c0826 100644 --- a/Objects/car.cpp +++ b/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& 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; } diff --git a/Objects/car.hpp b/Objects/car.hpp index a6ae5f7..25da33c 100644 --- a/Objects/car.hpp +++ b/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 controls; int control_index; double sinNegAngle; diff --git a/Objects/road.cpp b/Objects/road.cpp index c15e676..9f08589 100644 --- a/Objects/road.cpp +++ b/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 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 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, diff --git a/Objects/road.hpp b/Objects/road.hpp index 5ef5faf..ddb1021 100644 --- a/Objects/road.hpp +++ b/Objects/road.hpp @@ -28,7 +28,7 @@ class Road { std::vector laneCoefficients; std::vector poleCoefficients; std::vector parkingSpots; - std::vector> obstacles; + std::vector> obstacles; }; void renderPointCloud(pcl::visualization::PCLVisualizer::Ptr& viewer, const pcl::PointCloud::Ptr& cloud, const std::string& name, Color color = Color(1, 1, 1)); diff --git a/Scene/scene.cpp b/Scene/scene.cpp index cc2a2a1..bb876e9 100644 --- a/Scene/scene.cpp +++ b/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