Current Cone Coloring Improvements #393

DiogoProPrayer · 2025-02-17T18:00:01Z

Major improvements when the parameters were modified
Small improvements by the addition of curvature to the cost function: the weight should be small, only to prevent extreme cases with outliers.

…o speedCalc

…ealed

* added test to available dashboards * Added test test to send bucket * canceled previous change and added change to get dashboard layout * same as 2nd commit * Added graph 4 * Fixed errors in code structure and added graph 5 * Fixed errors in code structure and added graph 5 * Tried to add random variables * Same and Debugging * Debugging * Revert "Fixed errors in code structure and added graph 5" This reverts commit 095fe49. * Revert "Tried to add random variables" This reverts commit d05a3f0.

added launch files for more disciplines in pacsim fixed error in control node due to acos not clamped to [-1, 1], resulting in NaN steering fixed car model not appearing in foxglove

…o speedCalc

…tool. Modified some parameters

…eate tests and how to visualize the results

…the first pathpoint in the debug script

* feat: velocity estimation package definition predicting dependencies * feat: velocity estimation node and adapter declaration & main function * feat(common_lib): added Velocities structure to common_lib * feat(custom_interfaces): added Velocities message to custom interfaces * feat(ve): initial launch file * feat(ve): parameters factory & updated main and node & declared adapters * feat(ve): abstract estimator * feat: ve node has estimator * feat(ve): imu callback for PacSim * feat(ve): wss callback for pacsim * feat(ve): steering angle callback for pacsim * feat(ve): simulate resolver data assuming gear ratio of 4:1 * feat(ve): ekf structure * feat(ve): updated CMakeLists * fix(ve): updated package.xml * feat(ve): estimator creation logics & launch file adjustment & node runs! * feat(motion_lib): created motion lib and moved the vehicle dynamics folder from common_lib to it * fix(se): replaced vehicle dynamics functionality from common lib with motion lib in ekf_state_est * fix(control): replaced vehicle dynamics functionality with motion_lib in control * fix(common_lib): removed vehicle dynamics from common lib * fix(se): fixed error in include and removed compiler warnings * fix(motion_lib & common_lib): fixed compiling & added particle model & changed directory structure in motion_lib * fix(control&se): fixed include statements * fix compiling & made prediction step * fix(ve): removed unnecessary import and hierarchy * feat(VE): doxygen comments * fix(ve): fixed case and weird names * feat(VE): readme and diagram * feat(VE): initial prediction step * feat(observation_lib): created observation library * fix(ve):_replaced function with observation_lib * feat(VE): vehicle adapter * fix(motion_lib&observation_lib): dealing well with division by values close to 0 and using doubles * fix(adapters): added vehicle adapter to loader * feat(ve): all matrixes/vectors using doubles and temporary debug logic * fix: removed observation lib * feat(ve): vehicle launch file and publisher to adapter * fix: fixed vehicle adapter * fix: comments, diagram, readme, adapters * fix: action * added velocity estimation to github action * fixed error in compilation * fix: small typos * small fixes * fix: created interface for s2v models, changed topic name, created car params struct * fix: moved publishing velocities logics from adapter to node * fix: changed uses of bicycle model to use new class * fix: removed old bicycle model from motion_lib * fix: made class for particle model * fix: removed redundancy in jacobian * fix: added and improved comments, fixed class names, fixed jacobian * feat: new tests in motion_lib * feat: completed unit tests * fix(motion_lib): added description to the tests --------- Co-authored-by: marhcouto <marhcouto1101@gmail.com>

* hotfix: bunch of bs * fixed control flickering steering: caused by the first pathpoint which is inserted when the ld cant find any intersection (helpful when car is at standstill) fixed by iterating closest point starting at 1 instead of 0 * config done

* tests start * Straight_line working * pcd visualization working -Dpath * all tests + pcds working * Documentation and cleanup * compile error fixed * force included pcd files * removed result lines * parameters change * merge with dev and adjustements * added easy way to clean the result files * added ground truths and major refactor * removed useless files * small file fix * fixed correctness * hotfix * documentation change * Update perception_node.cpp * re-add bugged files * readded result files to try and solve github tests * general cleanup * relaxed some of the tests

Davide64-dev · 2025-02-18T11:27:12Z

@DiogoProPrayer approved. Please solve conflicts wait for the action to approve.

* feat: velocity estimation package definition predicting dependencies * feat: velocity estimation node and adapter declaration & main function * feat(common_lib): added Velocities structure to common_lib * feat(custom_interfaces): added Velocities message to custom interfaces * feat(ve): initial launch file * feat(ve): parameters factory & updated main and node & declared adapters * feat(ve): abstract estimator * feat: ve node has estimator * feat(ve): imu callback for PacSim * feat(ve): wss callback for pacsim * feat(ve): steering angle callback for pacsim * feat(ve): simulate resolver data assuming gear ratio of 4:1 * feat(ve): ekf structure * feat(ve): updated CMakeLists * fix(ve): updated package.xml * feat(ve): estimator creation logics & launch file adjustment & node runs! * feat(motion_lib): created motion lib and moved the vehicle dynamics folder from common_lib to it * fix(se): replaced vehicle dynamics functionality from common lib with motion lib in ekf_state_est * fix(control): replaced vehicle dynamics functionality with motion_lib in control * fix(common_lib): removed vehicle dynamics from common lib * fix(se): fixed error in include and removed compiler warnings * fix(motion_lib & common_lib): fixed compiling & added particle model & changed directory structure in motion_lib * fix(control&se): fixed include statements * fix compiling & made prediction step * fix(ve): removed unnecessary import and hierarchy * feat(VE): doxygen comments * fix(ve): fixed case and weird names * feat(VE): readme and diagram * feat(slam): base SLAM node compiling * feat(slam): include base structure and documentation and fixes in base node * FSF-7573 Graph slam (#392) * feat(slam): base graph slam structure, missing motion model and completing functions * feat(motion_lib): constant velocity motion model * feat(slam): long description... - finished constant velocity motion model - changed structures and messages to contain necessary noises and timestamps - changed node structure and completed missing code - created data association library (perception sensor shit) - completed code for adding motion prior (with some holes) - unit tests for constant velocity motion model * fix(slam/perception_sensor_lib): corrected parameter names * fix(slam): added gtsam installation and changed a name * fix(actions): added slam workflow and corrected two others * fix(actions): added lib * fix(slam): changed name of variable * fix(install): added missing command in GTSAM installation * fix(slam): added updating the last_pose_update timestamp * fix(actions): killed slam action, too slow * FSF-7159 Ekf slam (#384) * feat(slam): ekf prediction step * feat(slam): added comments to ekf slam functions * feat(slam): EKF correction step and state augmentation * fix(slam): added slam to action and fixed the compilation of ekf_state_est and control * feat(slam): base graph slam structure, missing motion model and completing functions * feat(motion_lib): constant velocity motion model * fix: changed oberservation noise matrix, rotation matrix as auxiliary function * feat(slam): long description... - finished constant velocity motion model - changed structures and messages to contain necessary noises and timestamps - changed node structure and completed missing code - created data association library (perception sensor shit) - completed code for adding motion prior (with some holes) - unit tests for constant velocity motion model * feat: asynchronous correction and prediction steps * feat: new tests * fix: strange things --------- Co-authored-by: marhcouto <marhcouto1101@gmail.com> * fix(slam/motion_lib): fixes after merge of EKF and Davide review * fix: solved last merge conflicts * fix(ekf_state_est): fixed the path --------- Co-authored-by: Lourenço Rodrigues <up202306331@up.pt> Co-authored-by: lourenco31 <150086800+lourenco31@users.noreply.github.com>

lourenco31 · 2025-02-19T14:14:10Z

src/planning/include/adapter_planning/parameters_factory.hpp

@@ -33,7 +33,7 @@ std::string load_adapter_parameters(PlanningParameters& params) {
  params.path_calculation_dist_threshold_ =
      adapter_node->declare_parameter("path_calculation_dist_threshold", 7.0);
  params.smoothing_spline_order_ =
-      static_cast<int>(adapter_node->declare_parameter("smoothing_spline_order", 3));
+      static_cast<int>(adapter_node->declare_parameter("smoothing_spline_order", 4));


Please check if this makes a difference in execution time. Fitting splines is usually slow. Even if it does increase execution time you don't need to reverse it, but it's better to document that now than to later try to find what's causing high execution time

lourenco31 · 2025-02-19T14:15:52Z

src/planning/include/planning/cone_coloring.hpp

-   */
-  void place_second_cones(std::unordered_set<Cone, std::hash<Cone>>& uncolored_cones,
-                          const Pose& car_pose, int& n_colored_cones);
+  Cone find_best_initial_cone_ransac(const std::unordered_set<Cone, std::hash<Cone>>& cones,


Remove this declaration

lourenco31 · 2025-02-19T14:21:00Z

src/planning/src/planning/cone_coloring.cpp

+    if (colored_cones.back() == this->colored_blue_cones_.back()) {
+      for (auto& colored_cone : this->colored_yellow_cones_) {
+        if (cone.position.euclidean_distance(colored_cone.position) < 2.5) {
+          continue;


Make this minimum distance a parameter of the node

lourenco31 · 2025-02-19T14:23:08Z

src/planning/include/planning/cone_coloring.hpp

@@ -102,7 +105,7 @@ class ConeColoring {
   * @param colored_to_input_cones_ratio ration of cones which have been colored
   * @return double cost
   */
-  double calculate_cost(const Cone& next_cone, const Cone& last_cone,
+  double calculate_cost(const Cone& next_cone, const Cone& last_cone, const Cone& second_last_cone,


Since the "second last" cone is also passed to the function, please refactor this function so that "previous_to_last_vector" is no longer passed as an argument, it is redundant

lourenco31 · 2025-02-19T14:24:13Z

src/planning/src/planning/cone_coloring.cpp

                                    const TwoDVector& previous_to_last_vector,
                                    const double& colored_to_input_cones_ratio) const {
  AngleAndNorms angle_and_norms = common_lib::maths::angle_and_norms(
      previous_to_last_vector, Position{next_cone.position.x - last_cone.position.x,
                                        next_cone.position.y - last_cone.position.y});
  double distance = angle_and_norms.norm2_;
  double angle = angle_and_norms.angle_;
+  double curvature = get_curvature(next_cone, last_cone, second_last_cone);
+  double curvature_weight = 5;


Make this a parameter of the node

lourenco31 · 2025-02-19T14:26:40Z

src/planning/src/planning/cone_coloring.cpp

 double ConeColoring::calculate_cost(const Cone& next_cone, const Cone& last_cone,
+                                    const Cone& second_last_cone,
                                    const TwoDVector& previous_to_last_vector,
                                    const double& colored_to_input_cones_ratio) const {
  AngleAndNorms angle_and_norms = common_lib::maths::angle_and_norms(


You can ignore this for now, but I think this should be refactored

lourenco31 · 2025-02-19T14:27:19Z

src/planning/src/planning/cone_coloring.cpp

@@ -211,17 +211,48 @@ void ConeColoring::place_second_cones(std::unordered_set<Cone, std::hash<Cone>>&
  }
 }

+double get_curvature(Cone point1, Cone point2, Cone point3) {


Move this to common_lib and use Positions instead of Cones as arguments

This reverts commit fd86a7c.

This reverts commit 992082d.

sonarqubecloud · 2025-02-20T09:51:23Z

Quality Gate failed

Failed conditions
148 New issues

See analysis details on SonarQube Cloud

Catch issues before they fail your Quality Gate with our IDE extension SonarQube for IDE

Davide64-dev · 2025-02-21T10:31:55Z

@DiogoProPrayer don't forget this pls 😭😭😭

DiogoProPrayer and others added 30 commits November 15, 2024 13:11

Bugged Velocity Planning

5735584

Fixed velocity planning compilation errors

3eba743

Bugged Velocity Planning

b7c7705

Fixed velocity planning compilation errors

8787eb7

Merge branch 'speedCalc' of github.com:fs-feup/autonomous-systems int…

3269956

…o speedCalc

Velocity Planning Working correctly

368a826

Added tests for Velocity Planning and fixed issues that the tests rev…

f65d878

…ealed

Added launchfile settings for velocity planning

33a0dfe

Added launch files for vehicle and sims

6f49a67

Merge with main

c49e1fe

FSF-4929 | launch files + fixed control (#365)

fcd8f85

added launch files for more disciplines in pacsim fixed error in control node due to acos not clamped to [-1, 1], resulting in NaN steering fixed car model not appearing in foxglove

Update with main

0b66e58

Skidpad and acceleration working on simulation

d8e578b

Fixed reviewed mistakes

3eb78d5

Merge branch 'speedCalc' of github.com:fs-feup/autonomous-systems int…

242b967

…o speedCalc

Final Velocity Planning, including reviews

f8aa2c1

feat: skidpad working in module

fb5697a

feat: Skidpad and acceleration planning with temporary hardcoded flags

d3f9dcf

fix: changed the folder of the auxiliary files

3bdf851

fix: conflit with testing

299af56

Test Compiling

30b3e7c

First test passing

7ca6c63

Added map generation tool and test visualization tool + second test

cc47857

Added testing scenarios and rosbag scenarios. Improved visualisation …

467de01

…tool. Modified some parameters

Added testing Readme indicating the structure of the files, how to cr…

5f8529f

…eate tests and how to visualize the results

Merge branch 'dev' into planning/testing

aff83ac

Repeated a test but in a position where cone coloring would break

a9b247d

Merge

b41f9a5

Reverted parameters

b475c6d

DiogoProPrayer added 10 commits January 13, 2025 16:00

Added a green dot for initial position and an orange dot to indicate …

fa256a2

…the first pathpoint in the debug script

Improvements to the debug tool

c209a3f

Added descriptions to each test

e1b518b

Removed result files

0cdf10d

Added gitignore for result files

0e3cf26

parameters modification

7aa6b86

Curvature added to the cost function

d6a97de

added a new test

2725a91

Fixed curvature weight in 5

7608c15

Modified vehicle and pacsim parameters

64e66c3

DiogoProPrayer requested review from Davide64-dev and lourenco31 February 17, 2025 18:01

lourenco31 and others added 3 commits February 17, 2025 19:47

Update model to 02 (#391)

439ed24

* hotfix: bunch of bs * fixed control flickering steering: caused by the first pathpoint which is inserted when the ld cant find any intersection (helpful when car is at standstill) fixed by iterating closest point starting at 1 instead of 0 * config done

lourenco31 requested changes Feb 19, 2025

View reviewed changes

DiogoProPrayer added 5 commits February 20, 2025 09:02

Cone coloring on the side with the lowest cost

fd86a7c

Merge branch 'conecoloring_remake' into dev

0f1d986

Revert "Cone coloring on the side with the lowest cost"

f0c2fc9

This reverts commit fd86a7c.

Revert "FSF-7053 | FSF-7052 | FSF-7573 | Slam (#394)"

38aac4f

This reverts commit 992082d.

Merge branch 'dev' into conecoloring_improvements

15364e6

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Current Cone Coloring Improvements #393

Current Cone Coloring Improvements #393

DiogoProPrayer commented Feb 17, 2025

Davide64-dev commented Feb 18, 2025

lourenco31 Feb 19, 2025

lourenco31 Feb 19, 2025 •

edited

Loading

lourenco31 Feb 19, 2025

lourenco31 Feb 19, 2025

lourenco31 Feb 19, 2025

lourenco31 Feb 19, 2025

lourenco31 Feb 19, 2025

sonarqubecloud bot commented Feb 20, 2025

Davide64-dev commented Feb 21, 2025

Current Cone Coloring Improvements #393

Are you sure you want to change the base?

Current Cone Coloring Improvements #393

Conversation

DiogoProPrayer commented Feb 17, 2025

Davide64-dev commented Feb 18, 2025

lourenco31 Feb 19, 2025

Choose a reason for hiding this comment

lourenco31 Feb 19, 2025 • edited Loading

Choose a reason for hiding this comment

lourenco31 Feb 19, 2025

Choose a reason for hiding this comment

lourenco31 Feb 19, 2025

Choose a reason for hiding this comment

lourenco31 Feb 19, 2025

Choose a reason for hiding this comment

lourenco31 Feb 19, 2025

Choose a reason for hiding this comment

lourenco31 Feb 19, 2025

Choose a reason for hiding this comment

sonarqubecloud bot commented Feb 20, 2025

Quality Gate failed

Davide64-dev commented Feb 21, 2025

lourenco31 Feb 19, 2025 •

edited

Loading