Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Feature/waypoint downsample #905

Merged
merged 7 commits into from
Oct 12, 2020
Merged

Conversation

kjrush
Copy link
Contributor

@kjrush kjrush commented Oct 12, 2020

PR Details

Description

Related Issue

Motivation and Context

How Has This Been Tested?

Types of changes

  • Defect fix (non-breaking change that fixes an issue)
  • New feature (non-breaking change that adds functionality)
  • Breaking change (fix or feature that cause existing functionality to change)

Checklist:

  • I have added any new packages to the sonar-scanner.properties file
  • My change requires a change to the documentation.
  • I have updated the documentation accordingly.
  • I have read the CONTRIBUTING document.
    CARMA Contributing Guide
  • I have added tests to cover my changes.
  • All new and existing tests passed.

@kjrush kjrush self-assigned this Oct 12, 2020
for (int i = 0; i < downsampled.lanes.size(); i++) {
for (int j = 0; j < downsampled.lanes[i].waypoints.size(); j++) {
if (j % ratio != 0) {
downsampled.lanes[i].waypoints.erase(downsampled.lanes[i].waypoints.begin() + j);
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Are you confident this is a safe loop. I think normally when changing the size of the vector while iterating over it you want to use iterator syntax.

https://stackoverflow.com/questions/9927163/erase-element-in-vector-while-iterating-the-same-vector

@msmcconnell msmcconnell merged commit 71bb9f2 into fix/testing Oct 12, 2020
@msmcconnell msmcconnell deleted the feature/waypoint-downsample branch October 12, 2020 17:45
kjrush added a commit that referenced this pull request Nov 20, 2020
* Add routing mechanism (#502)

* initial route manager implementation

* update sonarqube file

* add route state pub

* add more unit test

* address comments

* Update carma msg branch

* Update Autoware image version

* initial implementation

* add unit test

* add doc

* update sonarcloud file

* Fix CI test

* Update test_route_generator.cpp

* address comments

* add in lane cruising tactical plugin

* update planning stack to use route manager

* updated email info

* update route and guidance

* update test cases

* update names

* update unit tests

* Update guidance.launch

* Update route_generator_worker.cpp

* fix defects

* handle async issue

* add logic to handle route.msg

* update unit test

* Update WMListenerWorker.cpp

* fix compile error

* fixed unused args bug

* Feature/ui information (#636)

* Add logic to populate speed limit

* Fix syntax errors in route_generator_worker speed limit changes

* update unit tests

* Updated unit test to use the lanelet2_extension libraries (#725)

* Updated unit test to use the lanelet2_extension libraries for the DigitalSpeedLimit regulatory element, and changed the test to use lanelet IDs instead of starting/ending points because that was not working correctly

* Updated code to fix sonarcloud issues

* Made file links relative

Co-authored-by: Michael McConnell <Michael.McConnell-2@leidos.com>
Co-authored-by: afrye-leidos <alexander.frye@leidos.com>

* first commit

* route package builds, but seg error

* checkpoint

* fix te

* correct the msg branch

* Squashed commits

* clean

* Fix checkout.bash for integration/routing branch (#757)

* Fix checkout.bash for integration/routing branch

* Add seg fault fix to WMListenerWorker

* Remove debug prints from listener worker test

* Feature/control request (#771)


Co-authored-by: mishkamn <bayartsem@gmail.com>

* Merge implementation for new geofence spec into routing (#797)

* Feature/platoon strategic plugin (#739)

* add platooning strategic plugin

* removed readme

* updated code

* add to sonar scanner

* update sonar scanner

* removed files

* removed fules

* updated files

* updates

* addressed code smells

* fix code smells

* Created Mobility messages

* Created mobility messages

* addressed comments

* removed extra files

* addressed comments

* update code

* update

* add comments

Co-authored-by: Jerry Sun <Jerry.Sun@leidos.com>

* Add missing dependancies for cav_srvs

* updated package.xml Files (#765)

Co-authored-by: Jerry Sun <Jerry.Sun@leidos.com>
Co-authored-by: Michael McConnell <msmcconnell@wpi.edu>

* Update localization stack to use random filter to cap NDT input (#767)

* Update localization stack to use random filter to cap NDT input

* Add release build to carma-platform

Co-authored-by: Larry 1 <support@autonomoustuff.com>

* Fix/carma rviz (#762)

* Update caram exec

* update carma script

* update script

* update with --gui flag

* add help info

Co-authored-by: itabrl <itabrl@leidos.com>
Co-authored-by: Kyle Rush <kyle.rush@leidos.com>

* Merge implementation for new geofence spec into develop (#792)

* first commit

* schedule

* integration testing

* remove headers

* clean old dependencies

* remove old header

* try running on github

* enable cpp_message

* add unit test

* change half checkout

* unit test

* test

* remove unnecessary condition

* fix checkout

* first

* fix typo

* address comments

Co-authored-by: saina-ramyar <55258852+saina-ramyar@users.noreply.github.com>
Co-authored-by: Jerry Sun <Jerry.Sun@leidos.com>
Co-authored-by: Larry 1 <support@autonomoustuff.com>
Co-authored-by: Michael McConnell <msmcconnell@wpi.edu>
Co-authored-by: jsun6065 <52839028+jsun6065@users.noreply.github.com>
Co-authored-by: Ily <49001908+itabrl@users.noreply.github.com>
Co-authored-by: itabrl <itabrl@leidos.com>
Co-authored-by: Kyle Rush <kyle.rush@leidos.com>

* plugin_type_fix (#779)

* plugin_type_fix

* fix

* header

* fix1

* fix3

* fix3

* launch

* yaml-update

* pure pursuit

* PP

* final

* active

* active

* fix routing publisher

* spin

* spinfix

* spincallback

* spinfix1

* test

Co-authored-by: mishkamn <bayartsem@gmail.com>

* merge develop

* first commit

* Feature/report current geofences (#846)

* Commit edits to feature/report_current_geofences

* Commit error fixes to feature/report_current_geofences branch

* Commit testing changes

* Commit changes to feature/report_current_geofences branch

* Commit test case changes to feature/report_current_geofences

* Commit test changes to feature/report_current_geofences

* Commit test case changes to feature/report_current_geofences branch

* Commit integration/testing changes

* Commit integration testing changes to feature/report_current_geofences

* Commit new topic

* Commit msg changes to feature/report_current_geofences

* Commit PR changes

* Commit PR Changes

* Commit PR comment changes

* Commit Checkout change

* Commit  PR changes

* Integration/Routing Branch - Route State and Merge Issues (#818)

* update test for route package and remove route_generator package

* remove .osm file

* add ROS error logging and route files

* add route files

* Revert "add route files"

This reverts commit eada5ce.

* Revert "add ROS error logging and route files"

This reverts commit 4db1040.

* update maps and routes

* update map and route in opt_carna_setup.bash

* update .gitignore

* update laneletMap to laneletSubmap

* update state field in route_state topic and launch parameters

* update current_loc

* fix code coverage issue

* fix coverage issue for convertRouteStateToInt()

* fix coverage issue for convertRouteStateToInt() again

* fix coverage issue

* remove default case

* add back default case

* update unit test and add const keyword due to coverage issue

* fix coverage issue

* remove convert function

* remove convert function test

* fix motion_computation

* change arbitrator

* sdsd

* Revert "change arbitrator"

This reverts commit 1b43360.

* revert accidental changes

* first commit (#867)

* Fix/routing integration (#873)

* first

* resolve default controller

* fixy (#874)

* Feature/autoware waypoint generation (#612)

* Initial take at WorldModel Curvature calculation

* Remove empty files left after merge resolution

* Implement new getLocalCurvatures implementation

* Implement much of waypoint_generator mathematics

* Add orientation calculation to waypoints

* Implement accel/decel limiters

* Refactor curvature calculations to static methods

* Initial implementation of waypoint_generator_node

* Properly prefix member variables

* Implement parameter refactor

* Add launch file

* Add proper name to launch file

* Use speed limits from WM

* Update sonar scanner properties

* Fix route test issue

* Address Sonarscanner bugs

* Update to address PR comments

* Fix install and launch issues

* Add topic mapping and include to guidance launch file

* Resolve build and test errors after merge

* Fix usage of geometry namespace in waypoint_generator

* Add using namespace to unit tests

* Add new line at end of file

* Add header to unit test

* Put unit test in anonymous namespace

* Try extra bracket

Co-authored-by: Michael McConnell <Michael.McConnell-2@leidos.com>

* Remove mis-merged java code

* Cleanup default route files

* Fix launch and build errors

* add mpc parameters to vehicle calibration (#875) (#876)

* add mpc parameters to vehicle calibration

* updates

* Update mpc_follower_wrapper.launch

Co-authored-by: Michael McConnell <msmcconnell@wpi.edu>

Co-authored-by: saina-ramyar <55258852+saina-ramyar@users.noreply.github.com>

* Remove autoware_plugin

* remove autoware plugin from launch

* Add prints to waypoint generator

* Set world model for worker and add prints

* Refactor to support unit testing

* Update existing unit tests

* add unit test for lane_array, getspeedlimits (#881)

* add unit test for lane_array, getspeedlimits

* remove comment

* turned on tests and added comment

* check w

* Add unit test and fix issues in compute_orientations

* Remove unneeded print

* Fix curvature calculation

* add clarification on unit test

* Add unit test for loading actual maps

* Fix topic remapping for waypoints

* Protect end time

* Add prints

* Fix nan from getAngleBetweenVectors

* Disable test

* remove nodes

* Fix potential source of error in acos call in waypoint_generator

* fix header

* remap topics to accomodate removed nodes

* add logic for successing lanelets (#887)

* add logic for successing lanelets

* update sonar scanner

* address code smells

* change error to exception

* Fix bug in lanechange test

* moving

* Rework inlane-cruising to support operation without velocity set node

* fix install path on inlanecruising

* fix bug

* add prints

* fix existing unit tests

* add extra unit test

* WIP

* working on orientations

* Add orientation computation to mpc_follower_node

* Update waypoint visualization

* Use first point for from vehicle orientation

* Fix bug

* refactored compute_orientation (#897)

* Add rpyFromQuaternion

* change trajectory points to use time

* add bug fix

* fixes

* Add orientation extractor

* fix script

* Use atan2 for orientation calc

* Fix/add lane array to lane converter (#900)

* added lane_select node

* add to sonar

* fix build error

* connect visualizer

* removed viz and renamed

* sonar fix

* Fix index

* Fixes

* Fix/first point dedupe (#904)

* Add epsilon-based dedupe on linestring concat

* Add waypoint dedupe during lanelet concatenation

* Update epsilon value

* Properly convert string

* Update checkout.sh

* Feature/waypoint downsample (#905)

* Add epsilon-based dedupe on linestring concat

* Add waypoint dedupe during lanelet concatenation

* Update epsilon value

* Properly convert string

* Initial build of waypoint downsample

* Update to iterator style

* Fix iterator usage error

* remove map from commit history

* cleanup geometry and add comments

* fix empty maneuver

* Fix/testing cleanup (#947)

* WIP InLaneCruising waypoint generation

* use downtrack for sampling points

* Add sub_curve calc wip

* calc orientation and curvature (#910)

* calc orientation and curvature

* fix

* updates

* fix indexes

* add more points to fit curve

* Curve fit

* Fixes

* fix bugs

* add safety check

* bug fix

* more prints

* Add curvature regions computation

* Use max not min for curvature regions

* fix param

* fixes

* Add guard for maneuver planning

* fixes

* spline temp

* apply tf

* smooth trajectory (#919)

* No curvature regions

* lookahead speed change (#921)

* wm improvements

* wip

* Use maneuvers and look ahead

* Add orientation to vehicle state for plan delegator

* refactor inlanecruising for unit testing

* use config values in code

* Passing basic straight line unit test

* add time boundary

* Fixes

* add execution timer

* moving average applied to speed and curvature

* Runs whole route

* remove unused nodes

* More general boost dep

* WIP cleanup

* WIP

* Building

* Docker build fixes

* Delete waypoint generator

* pure pursuit tests passing

* fixing carma_wm unit tests

* carma_wm unit tests passing again

* Add unit tests for carma_wm

* test

* fix unit test after merge

* Fix unit tests after merge

* Revert checkout.bash after merge

* fix checkout.sh

* Add more tests and fix abs errors

* Fix time boundary

* Fixes

* WIP

* WIP

* Unit test compute_sub_curves

* populate longitudinal velocity

* Rework pute_pursuit wrapper

* Updated pure_pursuit_wrapper tests

* Probably will work

* reset pure pursuit

* Add comments

* Remove commented out block in launch file

* Fix CI

* Fix last minute change bug

* Change some logs to DEBUG

* Resolve comments

Co-authored-by: saina-ramyar <55258852+saina-ramyar@users.noreply.github.com>
Co-authored-by: Michael McConnell <mmcconnell3@gatech.edu>
Co-authored-by: Larry 1 <support@autonomoustuff.com>

* Feature/add traj viz (#959)

* Cleanup display

* Add traj viz to guidance.launch and rviz

* More stable max speed value

* fix build error

* Fix exception from visualization code (#964)

Co-authored-by: jtbaird <jtbaird95@gmail.com>

* fix checkout.sh

* fix (#967)

* Remove unused package

* remove unused package from sonar scanner

* Fix sonar cloud issues

* fix enum access

* fix const in route generator after sonar changes

* remove unsupported const

* Add inlanecruising to sonar scanner

* enable inlanecruising unit test

* Feature/speed update test (#965)

* speed lookup using traffic rules

* check implementation

* speed limit update with unit tests

* using sample vector map for testing

* speed_comment_out

* correcting config-limit topic sub

* speed limit associated with vector map

* adding node to read config_limit

Co-authored-by: Larry 1 <support@autonomoustuff.com>

* Fix/unit test route following (#977)

* add resource folder to cmake install

* adding condition for unset config_lim in unit test

Co-authored-by: Kyle Rush <kyle.rush@leidos.com>
Co-authored-by: Saina Ramyar <saina.ramyar@leidos.com>
Co-authored-by: saina-ramyar <55258852+saina-ramyar@users.noreply.github.com>
Co-authored-by: maefromm <MaeMargarette.M.Fromm@leidos.com>
Co-authored-by: mishkamn <bayartsem@gmail.com>
Co-authored-by: Michael McConnell <msmcconnell@wpi.edu>
Co-authored-by: afrye51 <32797997+afrye51@users.noreply.github.com>
Co-authored-by: Michael McConnell <Michael.McConnell-2@leidos.com>
Co-authored-by: afrye-leidos <alexander.frye@leidos.com>
Co-authored-by: lewisid <66792727+lewisid@users.noreply.github.com>
Co-authored-by: Misheel Bayartsengel <misheel.bayart@gmail.com>
Co-authored-by: Jerry Sun <Jerry.Sun@leidos.com>
Co-authored-by: Larry 1 <support@autonomoustuff.com>
Co-authored-by: jsun6065 <52839028+jsun6065@users.noreply.github.com>
Co-authored-by: Ily <49001908+itabrl@users.noreply.github.com>
Co-authored-by: itabrl <itabrl@leidos.com>
Co-authored-by: Aswath <32398753+aswath1@users.noreply.github.com>
Co-authored-by: dan-du-car <62157949+dan-du-car@users.noreply.github.com>
Co-authored-by: qswawrq <shuwei.qiang@leidos.com>
Co-authored-by: mishkamn <misheel.bayar@ucla.edu>
Co-authored-by: Michael McConnell <mmcconnell3@gatech.edu>
Co-authored-by: jtbaird <jtbaird95@gmail.com>
Co-authored-by: Anish_deva <51463994+adev4a@users.noreply.github.com>
msmcconnell added a commit that referenced this pull request Jan 27, 2021
* Add routing mechanism (#502)

* initial route manager implementation

* update sonarqube file

* add route state pub

* add more unit test

* address comments

* Update carma msg branch

* Update Autoware image version

* initial implementation

* add unit test

* add doc

* update sonarcloud file

* Fix CI test

* Update test_route_generator.cpp

* address comments

* add in lane cruising tactical plugin

* update planning stack to use route manager

* updated email info

* update route and guidance

* update test cases

* update names

* update unit tests

* Update guidance.launch

* Update route_generator_worker.cpp

* fix defects

* handle async issue

* add logic to handle route.msg

* update unit test

* Update WMListenerWorker.cpp

* fix compile error

* fixed unused args bug

* Feature/ui information (#636)

* Add logic to populate speed limit

* Fix syntax errors in route_generator_worker speed limit changes

* update unit tests

* Updated unit test to use the lanelet2_extension libraries (#725)

* Updated unit test to use the lanelet2_extension libraries for the DigitalSpeedLimit regulatory element, and changed the test to use lanelet IDs instead of starting/ending points because that was not working correctly

* Updated code to fix sonarcloud issues

* Made file links relative

Co-authored-by: Michael McConnell <Michael.McConnell-2@leidos.com>
Co-authored-by: afrye-leidos <alexander.frye@leidos.com>

* first commit

* route package builds, but seg error

* checkpoint

* fix te

* correct the msg branch

* Squashed commits

* clean

* Fix checkout.bash for integration/routing branch (#757)

* Fix checkout.bash for integration/routing branch

* Add seg fault fix to WMListenerWorker

* Remove debug prints from listener worker test

* Feature/control request (#771)


Co-authored-by: mishkamn <bayartsem@gmail.com>

* Merge implementation for new geofence spec into routing (#797)

* Feature/platoon strategic plugin (#739)

* add platooning strategic plugin

* removed readme

* updated code

* add to sonar scanner

* update sonar scanner

* removed files

* removed fules

* updated files

* updates

* addressed code smells

* fix code smells

* Created Mobility messages

* Created mobility messages

* addressed comments

* removed extra files

* addressed comments

* update code

* update

* add comments

Co-authored-by: Jerry Sun <Jerry.Sun@leidos.com>

* Add missing dependancies for cav_srvs

* updated package.xml Files (#765)

Co-authored-by: Jerry Sun <Jerry.Sun@leidos.com>
Co-authored-by: Michael McConnell <msmcconnell@wpi.edu>

* Update localization stack to use random filter to cap NDT input (#767)

* Update localization stack to use random filter to cap NDT input

* Add release build to carma-platform

Co-authored-by: Larry 1 <support@autonomoustuff.com>

* Fix/carma rviz (#762)

* Update caram exec

* update carma script

* update script

* update with --gui flag

* add help info

Co-authored-by: itabrl <itabrl@leidos.com>
Co-authored-by: Kyle Rush <kyle.rush@leidos.com>

* Merge implementation for new geofence spec into develop (#792)

* first commit

* schedule

* integration testing

* remove headers

* clean old dependencies

* remove old header

* try running on github

* enable cpp_message

* add unit test

* change half checkout

* unit test

* test

* remove unnecessary condition

* fix checkout

* first

* fix typo

* address comments

Co-authored-by: saina-ramyar <55258852+saina-ramyar@users.noreply.github.com>
Co-authored-by: Jerry Sun <Jerry.Sun@leidos.com>
Co-authored-by: Larry 1 <support@autonomoustuff.com>
Co-authored-by: Michael McConnell <msmcconnell@wpi.edu>
Co-authored-by: jsun6065 <52839028+jsun6065@users.noreply.github.com>
Co-authored-by: Ily <49001908+itabrl@users.noreply.github.com>
Co-authored-by: itabrl <itabrl@leidos.com>
Co-authored-by: Kyle Rush <kyle.rush@leidos.com>

* plugin_type_fix (#779)

* plugin_type_fix

* fix

* header

* fix1

* fix3

* fix3

* launch

* yaml-update

* pure pursuit

* PP

* final

* active

* active

* fix routing publisher

* spin

* spinfix

* spincallback

* spinfix1

* test

Co-authored-by: mishkamn <bayartsem@gmail.com>

* merge develop

* first commit

* Feature/report current geofences (#846)

* Commit edits to feature/report_current_geofences

* Commit error fixes to feature/report_current_geofences branch

* Commit testing changes

* Commit changes to feature/report_current_geofences branch

* Commit test case changes to feature/report_current_geofences

* Commit test changes to feature/report_current_geofences

* Commit test case changes to feature/report_current_geofences branch

* Commit integration/testing changes

* Commit integration testing changes to feature/report_current_geofences

* Commit new topic

* Commit msg changes to feature/report_current_geofences

* Commit PR changes

* Commit PR Changes

* Commit PR comment changes

* Commit Checkout change

* Commit  PR changes

* Integration/Routing Branch - Route State and Merge Issues (#818)

* update test for route package and remove route_generator package

* remove .osm file

* add ROS error logging and route files

* add route files

* Revert "add route files"

This reverts commit eada5ce.

* Revert "add ROS error logging and route files"

This reverts commit 4db1040.

* update maps and routes

* update map and route in opt_carna_setup.bash

* update .gitignore

* update laneletMap to laneletSubmap

* update state field in route_state topic and launch parameters

* update current_loc

* fix code coverage issue

* fix coverage issue for convertRouteStateToInt()

* fix coverage issue for convertRouteStateToInt() again

* fix coverage issue

* remove default case

* add back default case

* update unit test and add const keyword due to coverage issue

* fix coverage issue

* remove convert function

* remove convert function test

* fix motion_computation

* change arbitrator

* sdsd

* Revert "change arbitrator"

This reverts commit 1b43360.

* revert accidental changes

* first commit (#867)

* Fix/routing integration (#873)

* first

* resolve default controller

* fixy (#874)

* Feature/autoware waypoint generation (#612)

* Initial take at WorldModel Curvature calculation

* Remove empty files left after merge resolution

* Implement new getLocalCurvatures implementation

* Implement much of waypoint_generator mathematics

* Add orientation calculation to waypoints

* Implement accel/decel limiters

* Refactor curvature calculations to static methods

* Initial implementation of waypoint_generator_node

* Properly prefix member variables

* Implement parameter refactor

* Add launch file

* Add proper name to launch file

* Use speed limits from WM

* Update sonar scanner properties

* Fix route test issue

* Address Sonarscanner bugs

* Update to address PR comments

* Fix install and launch issues

* Add topic mapping and include to guidance launch file

* Resolve build and test errors after merge

* Fix usage of geometry namespace in waypoint_generator

* Add using namespace to unit tests

* Add new line at end of file

* Add header to unit test

* Put unit test in anonymous namespace

* Try extra bracket

Co-authored-by: Michael McConnell <Michael.McConnell-2@leidos.com>

* Remove mis-merged java code

* Cleanup default route files

* Fix launch and build errors

* add mpc parameters to vehicle calibration (#875) (#876)

* add mpc parameters to vehicle calibration

* updates

* Update mpc_follower_wrapper.launch

Co-authored-by: Michael McConnell <msmcconnell@wpi.edu>

Co-authored-by: saina-ramyar <55258852+saina-ramyar@users.noreply.github.com>

* Remove autoware_plugin

* remove autoware plugin from launch

* Add prints to waypoint generator

* Set world model for worker and add prints

* Refactor to support unit testing

* Update existing unit tests

* add unit test for lane_array, getspeedlimits (#881)

* add unit test for lane_array, getspeedlimits

* remove comment

* turned on tests and added comment

* check w

* Add unit test and fix issues in compute_orientations

* Remove unneeded print

* Fix curvature calculation

* add clarification on unit test

* Add unit test for loading actual maps

* Fix topic remapping for waypoints

* Protect end time

* Add prints

* Fix nan from getAngleBetweenVectors

* Disable test

* remove nodes

* Fix potential source of error in acos call in waypoint_generator

* fix header

* remap topics to accomodate removed nodes

* add logic for successing lanelets (#887)

* add logic for successing lanelets

* update sonar scanner

* address code smells

* change error to exception

* Fix bug in lanechange test

* moving

* Rework inlane-cruising to support operation without velocity set node

* fix install path on inlanecruising

* fix bug

* add prints

* fix existing unit tests

* add extra unit test

* WIP

* working on orientations

* Add orientation computation to mpc_follower_node

* Update waypoint visualization

* Use first point for from vehicle orientation

* Fix bug

* refactored compute_orientation (#897)

* Add rpyFromQuaternion

* change trajectory points to use time

* add bug fix

* fixes

* Add orientation extractor

* fix script

* Use atan2 for orientation calc

* Fix/add lane array to lane converter (#900)

* added lane_select node

* add to sonar

* fix build error

* connect visualizer

* removed viz and renamed

* sonar fix

* Fix index

* Fixes

* Fix/first point dedupe (#904)

* Add epsilon-based dedupe on linestring concat

* Add waypoint dedupe during lanelet concatenation

* Update epsilon value

* Properly convert string

* Update checkout.sh

* Feature/waypoint downsample (#905)

* Add epsilon-based dedupe on linestring concat

* Add waypoint dedupe during lanelet concatenation

* Update epsilon value

* Properly convert string

* Initial build of waypoint downsample

* Update to iterator style

* Fix iterator usage error

* remove map from commit history

* cleanup geometry and add comments

* fix empty maneuver

* Fix/testing cleanup (#947)

* WIP InLaneCruising waypoint generation

* use downtrack for sampling points

* Add sub_curve calc wip

* calc orientation and curvature (#910)

* calc orientation and curvature

* fix

* updates

* fix indexes

* add more points to fit curve

* Curve fit

* Fixes

* fix bugs

* add safety check

* bug fix

* more prints

* Add curvature regions computation

* Use max not min for curvature regions

* fix param

* fixes

* Add guard for maneuver planning

* fixes

* spline temp

* apply tf

* smooth trajectory (#919)

* No curvature regions

* lookahead speed change (#921)

* wm improvements

* wip

* Use maneuvers and look ahead

* Add orientation to vehicle state for plan delegator

* refactor inlanecruising for unit testing

* use config values in code

* Passing basic straight line unit test

* add time boundary

* Fixes

* add execution timer

* moving average applied to speed and curvature

* Runs whole route

* remove unused nodes

* More general boost dep

* WIP cleanup

* WIP

* Building

* Docker build fixes

* Delete waypoint generator

* pure pursuit tests passing

* fixing carma_wm unit tests

* carma_wm unit tests passing again

* Add unit tests for carma_wm

* test

* fix unit test after merge

* Fix unit tests after merge

* Revert checkout.bash after merge

* fix checkout.sh

* Add more tests and fix abs errors

* Fix time boundary

* Fixes

* WIP

* WIP

* Unit test compute_sub_curves

* populate longitudinal velocity

* Rework pute_pursuit wrapper

* Updated pure_pursuit_wrapper tests

* Probably will work

* reset pure pursuit

* Add comments

* Remove commented out block in launch file

* Fix CI

* Fix last minute change bug

* Change some logs to DEBUG

* Resolve comments

Co-authored-by: saina-ramyar <55258852+saina-ramyar@users.noreply.github.com>
Co-authored-by: Michael McConnell <mmcconnell3@gatech.edu>
Co-authored-by: Larry 1 <support@autonomoustuff.com>

* Feature/add traj viz (#959)

* Cleanup display

* Add traj viz to guidance.launch and rviz

* More stable max speed value

* fix build error

* Fix exception from visualization code (#964)

Co-authored-by: jtbaird <jtbaird95@gmail.com>

* fix checkout.sh

* fix (#967)

* Try optimized speed

* more conservative behavior

* unit test changes

* try

* wip unit tests

* better test

* Fix merge conflicts

* Remove unused files

* baseline

* first resolve conflict

* buildable baseline

* one file

* pic kcorrect speed

* first

* merge changes in develop

* merge develop 2

* add test

* first

* fix remaining error

* curvature unit test finally working

* bump unit test up

* disable map dependent unit test

* prep for integ test

* fix back_and_future bug

* with simulated unit testing

* final try

* Revert "final try"

This reverts commit 6d460aa.

* fix merge conflict

* add unit test for optimize speed first point

* parameterize

* fix bug

* last run parameters

* final clean

* resolve code smells before unit test bump

* bump up unit test

* address PR

Co-authored-by: qswawrq <qswawrq@gmail.com>
Co-authored-by: Kyle Rush <kyle.rush@leidos.com>
Co-authored-by: Saina Ramyar <saina.ramyar@leidos.com>
Co-authored-by: saina-ramyar <55258852+saina-ramyar@users.noreply.github.com>
Co-authored-by: maefromm <MaeMargarette.M.Fromm@leidos.com>
Co-authored-by: Michael McConnell <msmcconnell@wpi.edu>
Co-authored-by: afrye51 <32797997+afrye51@users.noreply.github.com>
Co-authored-by: Michael McConnell <Michael.McConnell-2@leidos.com>
Co-authored-by: afrye-leidos <alexander.frye@leidos.com>
Co-authored-by: lewisid <66792727+lewisid@users.noreply.github.com>
Co-authored-by: Jerry Sun <Jerry.Sun@leidos.com>
Co-authored-by: Larry 1 <support@autonomoustuff.com>
Co-authored-by: jsun6065 <52839028+jsun6065@users.noreply.github.com>
Co-authored-by: Ily <49001908+itabrl@users.noreply.github.com>
Co-authored-by: itabrl <itabrl@leidos.com>
Co-authored-by: Aswath <32398753+aswath1@users.noreply.github.com>
Co-authored-by: dan-du-car <62157949+dan-du-car@users.noreply.github.com>
Co-authored-by: qswawrq <shuwei.qiang@leidos.com>
Co-authored-by: Michael McConnell <mmcconnell3@gatech.edu>
Co-authored-by: jtbaird <jtbaird95@gmail.com>
Co-authored-by: snallamothu <sknallamothu@gmail.com>
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

None yet

2 participants