diff --git a/.bash_history b/.bash_history deleted file mode 100644 index 2b013755c..000000000 --- a/.bash_history +++ /dev/null @@ -1,10 +0,0 @@ -pip3 install transforms3d -bash start_simulator.txt -ls -exit -ls -exit -exit -exit -tmux -exit diff --git a/.dbus/session-bus/8aece318a1a6496da0c6a5453e8ec298-0 b/.dbus/session-bus/8aece318a1a6496da0c6a5453e8ec298-0 deleted file mode 100644 index 3d56be237..000000000 --- a/.dbus/session-bus/8aece318a1a6496da0c6a5453e8ec298-0 +++ /dev/null @@ -1,8 +0,0 @@ -# This file allows processes on the machine with id 8aece318a1a6496da0c6a5453e8ec298 using -# display :0 to find the D-Bus session bus with the below address. -# If the DBUS_SESSION_BUS_ADDRESS environment variable is set, it will -# be used rather than this file. -# See "man dbus-launch" for more details. -DBUS_SESSION_BUS_ADDRESS='unix:abstract=/tmp/dbus-tWCJCTb6oM,guid=4e283cd2cbec42fdb53437c864db1743' -DBUS_SESSION_BUS_PID=33 -DBUS_SESSION_BUS_WINDOWID=90177537 diff --git a/.gitignore b/.gitignore index a93cba067..84697b2e8 100644 --- a/.gitignore +++ b/.gitignore @@ -1,9 +1,11 @@ # IDE stuff **/.vscode/ **/.idea/ +**/.dbus/ ./CMakeLists.txt # top level cmake files can help with autocompletion, but should not be in repo ./driverless_ws/CMakeLists.txt **/CMakeCache.txt + # ROS Stuff **/build/ **/install/ diff --git a/__pycache__/test.cpython-38.pyc b/__pycache__/test.cpython-38.pyc new file mode 100644 index 000000000..03103834f Binary files /dev/null and b/__pycache__/test.cpython-38.pyc differ diff --git a/docker/simulator/log/build_2023-10-04_15-10-56/events.log b/docker/simulator/log/build_2023-10-04_15-10-56/events.log new file mode 100644 index 000000000..c2c623756 --- /dev/null +++ b/docker/simulator/log/build_2023-10-04_15-10-56/events.log @@ -0,0 +1,2 @@ +[0.000000] (-) TimerEvent: {} +[0.000073] (-) EventReactorShutdown: {} diff --git a/docker/simulator/log/build_2023-10-04_15-10-56/logger_all.log b/docker/simulator/log/build_2023-10-04_15-10-56/logger_all.log new file mode 100644 index 000000000..d35f09cb3 --- /dev/null +++ b/docker/simulator/log/build_2023-10-04_15-10-56/logger_all.log @@ -0,0 +1,62 @@ +[0.876s] DEBUG:colcon:Command line arguments: ['/usr/bin/colcon', 'build'] +[0.876s] DEBUG:colcon:Parsed command line arguments: Namespace(allow_overriding=[], ament_cmake_args=None, base_paths=['.'], build_base='build', catkin_cmake_args=None, catkin_skip_building_tests=False, cmake_args=None, cmake_clean_cache=False, cmake_clean_first=False, cmake_force_configure=False, cmake_target=None, cmake_target_skip_unavailable=False, continue_on_error=False, event_handlers=None, executor='parallel', ignore_user_meta=False, install_base='install', log_base=None, log_level=None, main=>, merge_install=False, metas=['./colcon.meta'], packages_above=None, packages_above_and_dependencies=None, packages_above_depth=None, packages_end=None, packages_ignore=None, packages_ignore_regex=None, packages_select=None, packages_select_build_failed=False, packages_select_by_dep=None, packages_select_regex=None, packages_select_test_failures=False, packages_skip=None, packages_skip_build_finished=False, packages_skip_by_dep=None, packages_skip_regex=None, packages_skip_test_passed=False, packages_skip_up_to=None, packages_start=None, packages_up_to=None, packages_up_to_regex=None, parallel_workers=24, paths=None, symlink_install=False, test_result_base=None, verb_extension=, verb_name='build', verb_parser=) +[0.937s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) check parameters +[0.937s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) check parameters +[0.937s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) check parameters +[0.937s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) check parameters +[0.937s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) discover +[0.938s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) discover +[0.938s] INFO:colcon.colcon_core.package_discovery:Crawling recursively for packages in '/home/dale/driverless/docker/simulator' +[0.938s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ignore', 'ignore_ament_install'] +[0.938s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore' +[0.938s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore_ament_install' +[0.938s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_pkg'] +[0.938s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_pkg' +[0.938s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_meta'] +[0.938s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_meta' +[0.938s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ros'] +[0.938s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ros' +[0.947s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['cmake', 'python'] +[0.947s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'cmake' +[0.947s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python' +[0.947s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['python_setup_py'] +[0.947s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python_setup_py' +[0.947s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extensions ['ignore', 'ignore_ament_install'] +[0.948s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extension 'ignore' +[0.948s] Level 1:colcon.colcon_core.package_identification:_identify(build) ignored +[0.948s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extensions ['ignore', 'ignore_ament_install'] +[0.948s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extension 'ignore' +[0.948s] Level 1:colcon.colcon_core.package_identification:_identify(install) ignored +[0.948s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extensions ['ignore', 'ignore_ament_install'] +[0.948s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extension 'ignore' +[0.948s] Level 1:colcon.colcon_core.package_identification:_identify(log) ignored +[0.948s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) using defaults +[0.948s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) discover +[0.948s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) using defaults +[0.948s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) discover +[0.948s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) using defaults +[1.032s] Level 1:colcon.colcon_core.package_discovery:discover_packages(prefix_path) check parameters +[1.032s] Level 1:colcon.colcon_core.package_discovery:discover_packages(prefix_path) discover +[1.062s] DEBUG:colcon.colcon_installed_package_information.package_discovery:Found 268 installed packages in /opt/ros/foxy +[1.063s] Level 1:colcon.colcon_core.package_discovery:discover_packages(prefix_path) using defaults +[1.136s] INFO:colcon.colcon_core.executor:Executing jobs using 'parallel' executor +[1.150s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete +[1.150s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:closing loop +[1.150s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:loop closed +[1.150s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete finished with '0' +[1.150s] DEBUG:colcon.colcon_core.event_reactor:joining thread +[1.168s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.terminal_notifier': Not used on non-Darwin systems +[1.168s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.win32': Not used on non-Windows systems +[1.168s] INFO:colcon.colcon_notification.desktop_notification:Sending desktop notification using 'notify2' +[1.182s] DEBUG:colcon.colcon_core.event_reactor:joined thread +[1.202s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_core.shell.bat': Not used on non-Windows systems +[1.202s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/dale/driverless/docker/simulator/install/local_setup.ps1' +[1.202s] INFO:colcon.colcon_core.shell:Creating prefix util module '/home/dale/driverless/docker/simulator/install/_local_setup_util_ps1.py' +[1.203s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/dale/driverless/docker/simulator/install/setup.ps1' +[1.218s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/dale/driverless/docker/simulator/install/local_setup.sh' +[1.218s] INFO:colcon.colcon_core.shell:Creating prefix util module '/home/dale/driverless/docker/simulator/install/_local_setup_util_sh.py' +[1.219s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/dale/driverless/docker/simulator/install/setup.sh' +[1.233s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/dale/driverless/docker/simulator/install/local_setup.bash' +[1.234s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/dale/driverless/docker/simulator/install/setup.bash' +[1.248s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/dale/driverless/docker/simulator/install/local_setup.zsh' +[1.249s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/dale/driverless/docker/simulator/install/setup.zsh' diff --git a/docker/simulator/log/build_2023-10-23_21-33-42/events.log b/docker/simulator/log/build_2023-10-23_21-33-42/events.log new file mode 100644 index 000000000..8b8607151 --- /dev/null +++ b/docker/simulator/log/build_2023-10-23_21-33-42/events.log @@ -0,0 +1,2 @@ +[0.000000] (-) TimerEvent: {} +[0.000302] (-) EventReactorShutdown: {} diff --git a/docker/simulator/log/build_2023-10-23_21-33-42/logger_all.log b/docker/simulator/log/build_2023-10-23_21-33-42/logger_all.log new file mode 100644 index 000000000..74c823e08 --- /dev/null +++ b/docker/simulator/log/build_2023-10-23_21-33-42/logger_all.log @@ -0,0 +1,62 @@ +[1.102s] DEBUG:colcon:Command line arguments: ['/usr/bin/colcon', 'build'] +[1.102s] DEBUG:colcon:Parsed command line arguments: Namespace(allow_overriding=[], ament_cmake_args=None, base_paths=['.'], build_base='build', catkin_cmake_args=None, catkin_skip_building_tests=False, cmake_args=None, cmake_clean_cache=False, cmake_clean_first=False, cmake_force_configure=False, cmake_target=None, cmake_target_skip_unavailable=False, continue_on_error=False, event_handlers=None, executor='parallel', ignore_user_meta=False, install_base='install', log_base=None, log_level=None, main=>, merge_install=False, metas=['./colcon.meta'], packages_above=None, packages_above_and_dependencies=None, packages_above_depth=None, packages_end=None, packages_ignore=None, packages_ignore_regex=None, packages_select=None, packages_select_build_failed=False, packages_select_by_dep=None, packages_select_regex=None, packages_select_test_failures=False, packages_skip=None, packages_skip_build_finished=False, packages_skip_by_dep=None, packages_skip_regex=None, packages_skip_test_passed=False, packages_skip_up_to=None, packages_start=None, packages_up_to=None, packages_up_to_regex=None, parallel_workers=24, paths=None, symlink_install=False, test_result_base=None, verb_extension=, verb_name='build', verb_parser=) +[1.150s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) check parameters +[1.150s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) check parameters +[1.150s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) check parameters +[1.150s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) check parameters +[1.150s] Level 1:colcon.colcon_core.package_discovery:discover_packages(colcon_meta) discover +[1.150s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) discover +[1.150s] INFO:colcon.colcon_core.package_discovery:Crawling recursively for packages in '/home/dale/driverless/docker/simulator' +[1.150s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ignore', 'ignore_ament_install'] +[1.150s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore' +[1.150s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ignore_ament_install' +[1.150s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_pkg'] +[1.150s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_pkg' +[1.150s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['colcon_meta'] +[1.150s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'colcon_meta' +[1.150s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['ros'] +[1.150s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'ros' +[1.155s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['cmake', 'python'] +[1.155s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'cmake' +[1.155s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python' +[1.155s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extensions ['python_setup_py'] +[1.155s] Level 1:colcon.colcon_core.package_identification:_identify(.) by extension 'python_setup_py' +[1.155s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extensions ['ignore', 'ignore_ament_install'] +[1.155s] Level 1:colcon.colcon_core.package_identification:_identify(build) by extension 'ignore' +[1.155s] Level 1:colcon.colcon_core.package_identification:_identify(build) ignored +[1.155s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extensions ['ignore', 'ignore_ament_install'] +[1.155s] Level 1:colcon.colcon_core.package_identification:_identify(install) by extension 'ignore' +[1.155s] Level 1:colcon.colcon_core.package_identification:_identify(install) ignored +[1.156s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extensions ['ignore', 'ignore_ament_install'] +[1.156s] Level 1:colcon.colcon_core.package_identification:_identify(log) by extension 'ignore' +[1.156s] Level 1:colcon.colcon_core.package_identification:_identify(log) ignored +[1.156s] Level 1:colcon.colcon_core.package_discovery:discover_packages(recursive) using defaults +[1.156s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) discover +[1.156s] Level 1:colcon.colcon_core.package_discovery:discover_packages(ignore) using defaults +[1.156s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) discover +[1.156s] Level 1:colcon.colcon_core.package_discovery:discover_packages(path) using defaults +[1.255s] Level 1:colcon.colcon_core.package_discovery:discover_packages(prefix_path) check parameters +[1.255s] Level 1:colcon.colcon_core.package_discovery:discover_packages(prefix_path) discover +[1.293s] DEBUG:colcon.colcon_installed_package_information.package_discovery:Found 268 installed packages in /opt/ros/foxy +[1.294s] Level 1:colcon.colcon_core.package_discovery:discover_packages(prefix_path) using defaults +[1.362s] INFO:colcon.colcon_core.executor:Executing jobs using 'parallel' executor +[1.381s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete +[1.381s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:closing loop +[1.381s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:loop closed +[1.381s] DEBUG:colcon.colcon_parallel_executor.executor.parallel:run_until_complete finished with '0' +[1.381s] DEBUG:colcon.colcon_core.event_reactor:joining thread +[1.402s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.terminal_notifier': Not used on non-Darwin systems +[1.402s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_notification.desktop_notification.win32': Not used on non-Windows systems +[1.402s] INFO:colcon.colcon_notification.desktop_notification:Sending desktop notification using 'notify2' +[1.410s] DEBUG:colcon.colcon_core.event_reactor:joined thread +[1.430s] INFO:colcon.colcon_core.plugin_system:Skipping extension 'colcon_core.shell.bat': Not used on non-Windows systems +[1.430s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/dale/driverless/docker/simulator/install/local_setup.ps1' +[1.431s] INFO:colcon.colcon_core.shell:Creating prefix util module '/home/dale/driverless/docker/simulator/install/_local_setup_util_ps1.py' +[1.431s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/dale/driverless/docker/simulator/install/setup.ps1' +[1.450s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/dale/driverless/docker/simulator/install/local_setup.sh' +[1.450s] INFO:colcon.colcon_core.shell:Creating prefix util module '/home/dale/driverless/docker/simulator/install/_local_setup_util_sh.py' +[1.450s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/dale/driverless/docker/simulator/install/setup.sh' +[1.469s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/dale/driverless/docker/simulator/install/local_setup.bash' +[1.469s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/dale/driverless/docker/simulator/install/setup.bash' +[1.488s] INFO:colcon.colcon_core.shell:Creating prefix script '/home/dale/driverless/docker/simulator/install/local_setup.zsh' +[1.488s] INFO:colcon.colcon_core.shell:Creating prefix chain script '/home/dale/driverless/docker/simulator/install/setup.zsh' diff --git a/docker/simulator/start_docker_ubuntu.txt b/docker/simulator/start_docker_ubuntu.txt index 06986afcc..113f254a7 100644 --- a/docker/simulator/start_docker_ubuntu.txt +++ b/docker/simulator/start_docker_ubuntu.txt @@ -1,2 +1,6 @@ xhost + +<<<<<<< HEAD +sudo docker run -it --rm --privileged --net=host -v $PWD/../..:/driverless -e DISPLAY=$DISPLAY akhandelwal2025/eufs:latest +======= sudo docker run -it --rm --privileged --net=host -v $PWD/../..:/driverless -e DISPLAY=$DISPLAY eufs:latest +>>>>>>> main diff --git a/driverless_ws/TEST.TXT b/driverless_ws/TEST.TXT new file mode 100644 index 000000000..df31b2106 --- /dev/null +++ b/driverless_ws/TEST.TXT @@ -0,0 +1,74 @@ +Landmark 0: (-4.594687, 5.430464) +Landmark 1: (1.967950, 4.239662) +Landmark 2: (1.538826, 7.457808) +Landmark 3: (-2.009286, 4.823530) +Landmark 4: (-2.100775, 9.195071) +Landmark 5: (2.734748, 2.223263) +Landmark 6: (0.807712, 6.122287) +Landmark 7: (-3.377982, 7.109918) +Landmark 7: (-3.377982, 7.109918) +Landmark 8: (-1.733143, 2.628232) +Landmark 9: (-6.942954, 8.613847) +Landmark 11: (3.021747, 10.243058) +Landmark 12: (3.422997, 16.249179) ├─────────────────────────────────────────────────── +Landmark 13: (3.787509, 18.715111) +Landmark 14: (4.611504, 20.834904) +Landmark 15: (4.637874, 5.291833) +Landmark 16: (7.567601, 12.024079) +Landmark 17: (10.199362, 14.983701) +Landmark 18: (2.994543, -1.285342) +Landmark 19: (7.635023, 4.431190) +Landmark 20: (5.260285, 2.464374) +Landmark 21: (15.104269, 13.125831) +Landmark 22: (8.460316, 5.769965) +Landmark 23: (4.444549, 0.574193) +Landmark 24: (11.669410, 3.583154) +Landmark 25: (13.911226, 9.267782) +Landmark 26: (14.627269, 5.707978) +Landmark 27: (18.918588, 3.273233) +Landmark 28: (7.119548, 0.042108) +Landmark 29: (13.514128, 3.794054) +Landmark 30: (9.457868, -0.894370) +Landmark 31: (10.858415, -0.624106) +Landmark 32: (18.145516, -0.861109) +Landmark 33: (5.523495, -2.197246) +Landmark 34: (8.049446, -0.667351) +Landmark 35: (17.276359, -2.324643) +Landmark 36: (12.931200, -2.857532) +Landmark 37: (16.209311, -3.148373) +Landmark 38: (4.706798, -1.712573) +Landmark 39: (19.173193, 14.564768) +Landmark 40: (17.262786, 14.670033) +Landmark 41: (11.338709, -2.888651) +Landmark 42: (20.448955, 4.933934) +Landmark 43: (14.424963, -1.349045) +Landmark 44: (2.319717, -3.058686) +Landmark 45: (21.757419, -3.442369) +Landmark 46: (20.155840, -2.998067) +Landmark 47: (5.862567, -6.448751) +Landmark 48: (4.049399, -6.381755) +Landmark 49: (6.464423, -9.064069) +Landmark 50: (11.812272, -9.818149) +Landmark 51: (9.391889, -10.243822) +Landmark 52: (0.514438, -4.829087) +Landmark 53: (8.161301, -9.727599) +Landmark 54: (5.984112, -7.935561) +Landmark 55: (1.742093, -5.845147) +Landmark 56: (-1.646198, -2.972010) +Landmark 58: (8.771079, -16.269117) +Landmark 59: (9.851890, -20.904494) -0.00032782551716081803 +Landmark 60: (1.282086, -19.855977) ar_velocity_covariance: +Landmark 61: (-1.066061, -12.305357) +Landmark 62: (-1.025112, -9.380614) +Landmark 63: (0.308752, -16.631303) +Landmark 64: (-1.463143, -7.863209) +Landmark 65: (-2.665747, -10.371793) +Landmark 66: (-4.015659, -14.380410) +Landmark 67: (10.506240, -22.862874) +Landmark 68: (-2.550809, -6.381250) +Landmark 69: (-5.448959, -12.589348) +Landmark 70: (-9.430758, -17.238599) r_acceleration: +Landmark 71: (-2.632866, -3.567939) -0.2648930549621582 +Landmark 72: (-3.362047, -4.227224) -9.755420684814453 +Landmark 73: (-5.837891, -6.611940) -0.11772461980581285 +Landmark 74: (1.922412, -20.777298) \ No newline at end of file diff --git a/driverless_ws/src/.vscode/settings.json b/driverless_ws/src/.vscode/settings.json new file mode 100644 index 000000000..659a9c893 --- /dev/null +++ b/driverless_ws/src/.vscode/settings.json @@ -0,0 +1,73 @@ +{ + "files.associations": { + "cctype": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "array": "cpp", + "atomic": "cpp", + "strstream": "cpp", + "*.tcc": "cpp", + "bitset": "cpp", + "chrono": "cpp", + "complex": "cpp", + "condition_variable": "cpp", + "cstdint": "cpp", + "deque": "cpp", + "list": "cpp", + "unordered_map": "cpp", + "vector": "cpp", + "exception": "cpp", + "algorithm": "cpp", + "functional": "cpp", + "iterator": "cpp", + "map": "cpp", + "memory": "cpp", + "memory_resource": "cpp", + "numeric": "cpp", + "optional": "cpp", + "random": "cpp", + "ratio": "cpp", + "regex": "cpp", + "set": "cpp", + "string": "cpp", + "string_view": "cpp", + "system_error": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "utility": "cpp", + "fstream": "cpp", + "future": "cpp", + "initializer_list": "cpp", + "iomanip": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "istream": "cpp", + "limits": "cpp", + "mutex": "cpp", + "new": "cpp", + "ostream": "cpp", + "shared_mutex": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "streambuf": "cpp", + "thread": "cpp", + "cfenv": "cpp", + "cinttypes": "cpp", + "typeindex": "cpp", + "typeinfo": "cpp", + "variant": "cpp", + "bit": "cpp", + "core": "cpp", + "codecvt": "cpp", + "forward_list": "cpp", + "unordered_set": "cpp" + } +} \ No newline at end of file diff --git a/driverless_ws/src/HesaiLidar_ROS_2.0 b/driverless_ws/src/HesaiLidar_ROS_2.0 new file mode 160000 index 000000000..941b3c550 --- /dev/null +++ b/driverless_ws/src/HesaiLidar_ROS_2.0 @@ -0,0 +1 @@ +Subproject commit 941b3c550fe81c6cfdbef88a12f87b30820c00d5 diff --git a/driverless_ws/src/common/launch/__pycache__/pipeline.launch.cpython-38.pyc b/driverless_ws/src/common/launch/__pycache__/pipeline.launch.cpython-38.pyc new file mode 100644 index 000000000..03b44570d Binary files /dev/null and b/driverless_ws/src/common/launch/__pycache__/pipeline.launch.cpython-38.pyc differ diff --git a/driverless_ws/src/eufs_msgs b/driverless_ws/src/eufs_msgs new file mode 160000 index 000000000..09bcd4786 --- /dev/null +++ b/driverless_ws/src/eufs_msgs @@ -0,0 +1 @@ +Subproject commit 09bcd4786ef3282332be7146f557c583700aac05 diff --git a/driverless_ws/src/interfaces/CMakeLists.txt b/driverless_ws/src/interfaces/CMakeLists.txt index 80fd3eeb3..fbd263c66 100644 --- a/driverless_ws/src/interfaces/CMakeLists.txt +++ b/driverless_ws/src/interfaces/CMakeLists.txt @@ -37,17 +37,25 @@ endif() find_package(geometry_msgs REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(sensor_msgs REQUIRED) +find_package(eufs_msgs REQUIRED) +find_package(sbg_driver REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} + "msg/Brakes.msg" "msg/CarROT.msg" "msg/ConeList.msg" "msg/ConePositions.msg" + "msg/ControlAction.msg" + "msg/DataFrame.msg" + "msg/Heartbeat.msg" "msg/PairROT.msg" "msg/Points.msg" + "msg/SlamOutput.msg" + "msg/SlamFrame.msg" "msg/Spline.msg" "msg/SplineList.msg" - "msg/ControlAction.msg" - DEPENDENCIES geometry_msgs sensor_msgs # Add packages that above messages depend on, in this case geometry_msgs for Sphere.msg + "msg/VehicleState.msg" + DEPENDENCIES geometry_msgs sensor_msgs eufs_msgs sbg_driver # Add packages that above messages depend on, in this case geometry_msgs for Sphere.msg ) ament_package() diff --git a/driverless_ws/src/interfaces/msg/SimDataFrame.msg b/driverless_ws/src/interfaces/msg/SimDataFrame.msg deleted file mode 100644 index b7ee3b265..000000000 --- a/driverless_ws/src/interfaces/msg/SimDataFrame.msg +++ /dev/null @@ -1,4 +0,0 @@ -eufs_msgs/ConeArrayWithCovariance gt_cones -sensor_msgs/Image zed_left_img -sensor_msgs/PointCloud2 vlp16_pts -sensor_msgs/PointCloud2 zed_pts \ No newline at end of file diff --git a/driverless_ws/src/interfaces/msg/SlamFrame.msg b/driverless_ws/src/interfaces/msg/SlamFrame.msg new file mode 100644 index 000000000..b52ecd774 --- /dev/null +++ b/driverless_ws/src/interfaces/msg/SlamFrame.msg @@ -0,0 +1,3 @@ +eufs_msgs/ConeArray stereo_cones +geometry_msgs/Vector3Stamped imu_linear_velocity +sensor_msgs/Imu imu_data \ No newline at end of file diff --git a/driverless_ws/src/interfaces/msg/SlamOutput.msg b/driverless_ws/src/interfaces/msg/SlamOutput.msg new file mode 100644 index 000000000..ac8279aa2 --- /dev/null +++ b/driverless_ws/src/interfaces/msg/SlamOutput.msg @@ -0,0 +1,5 @@ +std_msgs/Header header +float64 car_x +float64 car_y +float64 car_heading +geometry_msgs/Point[] landmarks \ No newline at end of file diff --git a/driverless_ws/src/interfaces/msg/SplineFrame.msg b/driverless_ws/src/interfaces/msg/SplineFrame.msg new file mode 100644 index 000000000..cfb4af8ba --- /dev/null +++ b/driverless_ws/src/interfaces/msg/SplineFrame.msg @@ -0,0 +1,6 @@ +std_msgs/Header header + +geometry_msgs/Point point +float64 progress + +float64 curvature #not needed, but just in case diff --git a/driverless_ws/src/interfaces/msg/SplineFrameList.msg b/driverless_ws/src/interfaces/msg/SplineFrameList.msg new file mode 100644 index 000000000..ec250c05a --- /dev/null +++ b/driverless_ws/src/interfaces/msg/SplineFrameList.msg @@ -0,0 +1,3 @@ +std_msgs/Header header + +SplineFrame[] frames \ No newline at end of file diff --git a/driverless_ws/src/interfaces/package.xml b/driverless_ws/src/interfaces/package.xml index 17a773e36..1a7793e83 100644 --- a/driverless_ws/src/interfaces/package.xml +++ b/driverless_ws/src/interfaces/package.xml @@ -13,6 +13,8 @@ ament_lint_common geometry_msgs sensor_msgs + eufs_msgs + sbg_driver rosidl_default_generators rosidl_default_runtime diff --git a/driverless_ws/src/perceptions/package.xml b/driverless_ws/src/perceptions/package.xml index 5c9364b91..b2ec4f3e2 100644 --- a/driverless_ws/src/perceptions/package.xml +++ b/driverless_ws/src/perceptions/package.xml @@ -14,7 +14,7 @@ pandas sensor_msgs geometry_msgs - ros2_numpy + ros2_numpy_dv ament_copyright ament_flake8 diff --git a/driverless_ws/src/perceptions/perceptions/__pycache__/DataNode.cpython-38.pyc b/driverless_ws/src/perceptions/perceptions/__pycache__/DataNode.cpython-38.pyc new file mode 100644 index 000000000..808f6f3a6 Binary files /dev/null and b/driverless_ws/src/perceptions/perceptions/__pycache__/DataNode.cpython-38.pyc differ diff --git a/driverless_ws/src/perceptions/perceptions/__pycache__/SyncNode.cpython-38.pyc b/driverless_ws/src/perceptions/perceptions/__pycache__/SyncNode.cpython-38.pyc new file mode 100644 index 000000000..1c41f5e51 Binary files /dev/null and b/driverless_ws/src/perceptions/perceptions/__pycache__/SyncNode.cpython-38.pyc differ diff --git a/driverless_ws/src/perceptions/perceptions/__pycache__/ZEDNode.cpython-38.pyc b/driverless_ws/src/perceptions/perceptions/__pycache__/ZEDNode.cpython-38.pyc new file mode 100644 index 000000000..74ac9df46 Binary files /dev/null and b/driverless_ws/src/perceptions/perceptions/__pycache__/ZEDNode.cpython-38.pyc differ diff --git a/driverless_ws/src/perceptions/perceptions/__pycache__/__init__.cpython-38.pyc b/driverless_ws/src/perceptions/perceptions/__pycache__/__init__.cpython-38.pyc new file mode 100644 index 000000000..d9cc238d7 Binary files /dev/null and b/driverless_ws/src/perceptions/perceptions/__pycache__/__init__.cpython-38.pyc differ diff --git a/driverless_ws/src/perceptions/perceptions/__pycache__/conversions.cpython-38.pyc b/driverless_ws/src/perceptions/perceptions/__pycache__/conversions.cpython-38.pyc new file mode 100644 index 000000000..de9ff41e0 Binary files /dev/null and b/driverless_ws/src/perceptions/perceptions/__pycache__/conversions.cpython-38.pyc differ diff --git a/driverless_ws/src/perceptions/perceptions/__pycache__/topics.cpython-38.pyc b/driverless_ws/src/perceptions/perceptions/__pycache__/topics.cpython-38.pyc new file mode 100644 index 000000000..fc0f5dc5e Binary files /dev/null and b/driverless_ws/src/perceptions/perceptions/__pycache__/topics.cpython-38.pyc differ diff --git a/driverless_ws/src/perceptions/perceptions/__pycache__/zed.cpython-38.pyc b/driverless_ws/src/perceptions/perceptions/__pycache__/zed.cpython-38.pyc new file mode 100644 index 000000000..a15863a3d Binary files /dev/null and b/driverless_ws/src/perceptions/perceptions/__pycache__/zed.cpython-38.pyc differ diff --git a/driverless_ws/src/perceptions/perceptions/ros/predictors/__pycache__/StereoNode.cpython-38.pyc b/driverless_ws/src/perceptions/perceptions/ros/predictors/__pycache__/StereoNode.cpython-38.pyc new file mode 100644 index 000000000..44ce30d9c Binary files /dev/null and b/driverless_ws/src/perceptions/perceptions/ros/predictors/__pycache__/StereoNode.cpython-38.pyc differ diff --git a/driverless_ws/src/perceptions/perceptions/ros/utils/DataNode.py b/driverless_ws/src/perceptions/perceptions/ros/utils/DataNode.py index 5a2441aa2..d5b61fcef 100644 --- a/driverless_ws/src/perceptions/perceptions/ros/utils/DataNode.py +++ b/driverless_ws/src/perceptions/perceptions/ros/utils/DataNode.py @@ -3,9 +3,10 @@ from rclpy.node import Node from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy, QoSDurabilityPolicy +from eufs_msgs.msg import DataFrame + # ROS2 message types from sensor_msgs.msg import Image, PointCloud2 -from eufs_msgs.msg import DataFrame # ROS2 msg to python datatype conversions import perceptions.ros.utils.conversions as conv @@ -23,6 +24,11 @@ durability = QoSDurabilityPolicy.VOLATILE, depth = 5) +RELIABLE_QOS_PROFILE = QoSProfile(reliability = QoSReliabilityPolicy.RELIABLE, + history = QoSHistoryPolicy.KEEP_LAST, + durability = QoSDurabilityPolicy.VOLATILE, + depth = 5) + # setup the topic names that we are reading from LEFT_IMAGE_TOPIC = "/zedsdk_left_color_image" RIGHT_IMAGE_TOPIC = "/zedsdk_right_color_image" @@ -31,14 +37,7 @@ POINT_TOPIC = "/lidar_points" DATAFRAME_TOPIC = "/DataFrame" -DEBUG = True - -RELIABLE_QOS_PROFILE = QoSProfile( -depth=10, -reliability=QoSReliabilityPolicy.RELIABLE, -durability=QoSDurabilityPolicy.VOLATILE, -history=QoSHistoryPolicy.KEEP_LAST, -) +DEBUG = False class DataNode(Node): @@ -51,20 +50,15 @@ def __init__(self, name="data_node"): self.xyz_image_window = vis.init_visualizer_window() # subscribe to each piece of data that we want to collect on - self.left_color_subscriber = self.create_subscription(Image, LEFT_IMAGE_TOPIC, self.left_color_callback, qos_profile=BEST_EFFORT_QOS_PROFILE) -<<<<<<< HEAD:driverless_ws/src/perceptions/perceptions/DataNode.py + + # self.left_color_subscriber = self.create_subscription(Image, LEFT_IMAGE_TOPIC, self.left_color_callback, qos_profile=BEST_EFFORT_QOS_PROFILE) # self.right_color_subscriber = self.create_subscription(Image, RIGHT_IMAGE_TOPIC, self.right_color_callback, qos_profile=BEST_EFFORT_QOS_PROFILE) - # self.xyz_image_subscriber = self.create_subscription(Image, XYZ_IMAGE_TOPIC, self.xyz_callback, qos_profile=BEST_EFFORT_QOS_PROFILE) + # self.xyz_image_subscriber = self.create_subscription(Image, XYZ_IMAGE_TOPIC, self.xyz_image_callback, qos_profile=BEST_EFFORT_QOS_PROFILE) # self.depth_subscriber = self.create_subscription(Image, DEPTH_IMAGE_TOPIC, self.depth_image_callback, qos_profile=BEST_EFFORT_QOS_PROFILE) - # self.point_subscriber = self.create_subscription(PointCloud2, POINT_TOPIC, self.point_callback, qos_profile=BEST_EFFORT_QOS_PROFILE) - # self.dataframe_subscriber = self.create_subscription(DataFrame, DATAFRAME_TOPIC, self.dataframe_callback, qos_profile=RELIABLE_QOS_PROFILE) - # define varaibles to store the data - self.left_color = None - self.right_color = None - self.xyz_image = None - self.depth_image = None - self.points = None -======= + # self.point_subscriber = self.create_subscription(PointCloud2, POINT_TOPIC, self.points_callback, qos_profile=BEST_EFFORT_QOS_PROFILE) + self.dataframe_subscriber = self.create_subscription(DataFrame, DATAFRAME_TOPIC, self.dataframe_callback, qos_profile=RELIABLE_QOS_PROFILE) + + self.left_color_subscriber = self.create_subscription(Image, LEFT_IMAGE_TOPIC, self.left_color_callback, qos_profile=BEST_EFFORT_QOS_PROFILE) self.right_color_subscriber = self.create_subscription(Image, RIGHT_IMAGE_TOPIC, self.right_color_callback, qos_profile=BEST_EFFORT_QOS_PROFILE) self.xyz_image_subscriber = self.create_subscription(Image, XYZ_IMAGE_TOPIC, self.xyz_image_callback, qos_profile=BEST_EFFORT_QOS_PROFILE) self.depth_subscriber = self.create_subscription(Image, DEPTH_IMAGE_TOPIC, self.depth_image_callback, qos_profile=BEST_EFFORT_QOS_PROFILE) @@ -79,7 +73,20 @@ def __init__(self, name="data_node"): self.xyz_image_str = "xyz_image" self.depth_image_str = "depth_image" self.points_str = "points" ->>>>>>> main:driverless_ws/src/perceptions/perceptions/ros/utils/DataNode.py + self.imu_data = "imu_data" + self.lin_vel_str = "lin_vel" + + def got_all_data(self): + # returns whether data node has all pieces of data + # return self.left_color_str in self.data and \ + # self.right_color_str in self.data and \ + # self.xyz_image_str in self.data and \ + # self.depth_image_str in self.data and \ + # self.points_str in self.data + return self.left_color_str in self.data and \ + self.imu_data in self.data and \ + self.lin_vel_str in self.data and \ + self.xyz_image_str in self.data def got_all_data(self): @@ -105,7 +112,7 @@ def right_color_callback(self, msg): cv2.waitKey(1) def xyz_image_callback(self, msg): - self.data[self.xyz_image_str] =conv.img_to_npy(msg) + self.data[self.xyz_image_str] = conv.img_to_npy(msg) if DEBUG: # display xyz_image as unstructured point cloud @@ -130,8 +137,15 @@ def points_callback(self, msg): points = self.data[self.points_str][:, :3] points = points[:, [1, 0, 2]] points[:, 0] *= -1 - vis.update_visualizer_window(self.window, points[:,:3]) + vis.update_visualizer_window(None, points[:,:3]) + # vis.update_visualizer_window(self.window, points[:,:3]) + + def dataframe_callback(self, msg): + self.data[self.left_color_str] = conv.img_to_npy(msg.image_msg) + self.data[self.xyz_image_str] = conv.img_to_npy(msg.xyz_msg) + self.data[self.imu_data] = msg.imu_data + self.data[self.lin_vel_str] = msg.imu_linear_velocity def main(args=None): rclpy.init(args=args) diff --git a/driverless_ws/src/perceptions/perceptions/ros/utils/FileNode.py b/driverless_ws/src/perceptions/perceptions/ros/utils/FileNode.py index abcecd959..277b2205d 100644 --- a/driverless_ws/src/perceptions/perceptions/ros/utils/FileNode.py +++ b/driverless_ws/src/perceptions/perceptions/ros/utils/FileNode.py @@ -14,7 +14,7 @@ # set to determine what folder to create (find in ~/driverless/driverless_ws/) # DO NOT MAKE "src", "build", "install", or "log" -FOLDER_NAME = "tt-09-29-1" +FOLDER_NAME = "ecg-12-02-full" # define path to data directory WS_DIR = Path(__file__).parents[3] diff --git a/driverless_ws/src/perceptions/perceptions/ros/utils/PredictNode.py b/driverless_ws/src/perceptions/perceptions/ros/utils/PredictNode.py index 35e3f0004..654e4e0fb 100644 --- a/driverless_ws/src/perceptions/perceptions/ros/utils/PredictNode.py +++ b/driverless_ws/src/perceptions/perceptions/ros/utils/PredictNode.py @@ -4,7 +4,7 @@ from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy, QoSDurabilityPolicy # for converting predictor output to cone message type -from eufs_msgs.msg import ConeArray +from eufs_msgs.msg import ConeArray, SlamFrame import perceptions.ros.utils.conversions as conversions # for collecting data from sensors @@ -13,7 +13,12 @@ import time # configure QOS profile -BEST_EFFORT_QOS_PROFILE = QoSProfile(reliability = QoSReliabilityPolicy.RELIABLE, +BEST_EFFORT_QOS_PROFILE = QoSProfile(reliability = QoSReliabilityPolicy.BEST_EFFORT, + history = QoSHistoryPolicy.KEEP_LAST, + durability = QoSDurabilityPolicy.VOLATILE, + depth = 5) + +RELIABLE_QOS_PROFILE = QoSProfile(reliability = QoSReliabilityPolicy.RELIABLE, history = QoSHistoryPolicy.KEEP_LAST, durability = QoSDurabilityPolicy.VOLATILE, depth = 5) @@ -35,8 +40,9 @@ def __init__(self, name, debug_flag=False, time_flag=True): # initialize published cone topic based on name self.cone_topic = f"/{name}_cones" - self.qos_profile = BEST_EFFORT_QOS_PROFILE - self.cone_publisher = self.create_publisher(ConeArray, self.cone_topic, self.qos_profile) + self.slam_frame_topic = f"/SLAMFrame" + self.qos_profile = RELIABLE_QOS_PROFILE + self.cone_publisher = self.create_publisher(SlamFrame, self.slam_frame_topic, self.qos_profile) # create predictor, any subclass of PredictNode needs to fill this component self.predictor = self.init_predictor() @@ -62,7 +68,11 @@ def predict_callback(self): print(cones) # publish message - msg = conversions.cones_to_msg(cones) + msg = SlamFrame() + msg.stereo_cones = conversions.cones_to_msg(cones) + msg.imu_linear_velocity = self.data[self.lin_vel_str] + msg.imu_data = self.data[self.imu_data] + self.cone_publisher.publish(msg) if self.time: diff --git a/driverless_ws/src/perceptions/perceptions/ros/utils/__pycache__/AggregateNode.cpython-38.pyc b/driverless_ws/src/perceptions/perceptions/ros/utils/__pycache__/AggregateNode.cpython-38.pyc new file mode 100644 index 000000000..85127275c Binary files /dev/null and b/driverless_ws/src/perceptions/perceptions/ros/utils/__pycache__/AggregateNode.cpython-38.pyc differ diff --git a/driverless_ws/src/perceptions/perceptions/ros/utils/__pycache__/DataNode.cpython-38.pyc b/driverless_ws/src/perceptions/perceptions/ros/utils/__pycache__/DataNode.cpython-38.pyc new file mode 100644 index 000000000..4cf45217b Binary files /dev/null and b/driverless_ws/src/perceptions/perceptions/ros/utils/__pycache__/DataNode.cpython-38.pyc differ diff --git a/driverless_ws/src/perceptions/perceptions/ros/utils/__pycache__/FileNode.cpython-38.pyc b/driverless_ws/src/perceptions/perceptions/ros/utils/__pycache__/FileNode.cpython-38.pyc new file mode 100644 index 000000000..e4af7710e Binary files /dev/null and b/driverless_ws/src/perceptions/perceptions/ros/utils/__pycache__/FileNode.cpython-38.pyc differ diff --git a/driverless_ws/src/perceptions/perceptions/ros/utils/__pycache__/LidarTestNode.cpython-38.pyc b/driverless_ws/src/perceptions/perceptions/ros/utils/__pycache__/LidarTestNode.cpython-38.pyc new file mode 100644 index 000000000..84963886e Binary files /dev/null and b/driverless_ws/src/perceptions/perceptions/ros/utils/__pycache__/LidarTestNode.cpython-38.pyc differ diff --git a/driverless_ws/src/perceptions/perceptions/ros/utils/__pycache__/PredictNode.cpython-38.pyc b/driverless_ws/src/perceptions/perceptions/ros/utils/__pycache__/PredictNode.cpython-38.pyc new file mode 100644 index 000000000..567e96823 Binary files /dev/null and b/driverless_ws/src/perceptions/perceptions/ros/utils/__pycache__/PredictNode.cpython-38.pyc differ diff --git a/driverless_ws/src/perceptions/perceptions/ros/utils/__pycache__/ZEDNode.cpython-38.pyc b/driverless_ws/src/perceptions/perceptions/ros/utils/__pycache__/ZEDNode.cpython-38.pyc new file mode 100644 index 000000000..79a0b36c1 Binary files /dev/null and b/driverless_ws/src/perceptions/perceptions/ros/utils/__pycache__/ZEDNode.cpython-38.pyc differ diff --git a/driverless_ws/src/perceptions/perceptions/ros/utils/__pycache__/conversions.cpython-38.pyc b/driverless_ws/src/perceptions/perceptions/ros/utils/__pycache__/conversions.cpython-38.pyc new file mode 100644 index 000000000..b5274daad Binary files /dev/null and b/driverless_ws/src/perceptions/perceptions/ros/utils/__pycache__/conversions.cpython-38.pyc differ diff --git a/driverless_ws/src/perceptions/perceptions/ros/utils/__pycache__/topics.cpython-38.pyc b/driverless_ws/src/perceptions/perceptions/ros/utils/__pycache__/topics.cpython-38.pyc new file mode 100644 index 000000000..1f52e65d2 Binary files /dev/null and b/driverless_ws/src/perceptions/perceptions/ros/utils/__pycache__/topics.cpython-38.pyc differ diff --git a/driverless_ws/src/perceptions/perceptions/ros/utils/__pycache__/zed.cpython-38.pyc b/driverless_ws/src/perceptions/perceptions/ros/utils/__pycache__/zed.cpython-38.pyc new file mode 100644 index 000000000..9161f80d0 Binary files /dev/null and b/driverless_ws/src/perceptions/perceptions/ros/utils/__pycache__/zed.cpython-38.pyc differ diff --git a/driverless_ws/src/perceptions/setup.py b/driverless_ws/src/perceptions/setup.py index 44b9929f4..cce1188cf 100644 --- a/driverless_ws/src/perceptions/setup.py +++ b/driverless_ws/src/perceptions/setup.py @@ -21,11 +21,10 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'zed_node = perceptions.ZEDNode:main', - 'data_node = perceptions.DataNode:main', - 'stereo_node = perceptions.StereoNode:main', - 'lidar_node = perceptions.LidarNode:main', - 'sync_node = perceptions.SyncNode:main' + 'data_node = perceptions.ros.utils.DataNode:main', + 'file_node = perceptions.ros.utils.FileNode:main', + 'stereo_node = perceptions.ros.predictors.StereoNode:main', + 'lidar_node = perceptions.ros.predictors.LidarNode:main' ], }, ) diff --git a/driverless_ws/src/planning/CMakeLists.txt b/driverless_ws/src/planning/CMakeLists.txt index 6af36bc30..4a9ab76d0 100644 --- a/driverless_ws/src/planning/CMakeLists.txt +++ b/driverless_ws/src/planning/CMakeLists.txt @@ -14,33 +14,47 @@ find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(geometry_msgs REQUIRED) + find_package(message_filters REQUIRED) find_package(eufs_msgs REQUIRED) find_package(Eigen3 3.3 REQUIRED) find_package(GSL REQUIRED) find_package(interfaces REQUIRED) + # -------- PLANNING LIBRARY EXPORT -------- add_library(planning_export src/planning_codebase/raceline/raceline.cpp) ament_target_dependencies(planning_export interfaces rclcpp std_msgs geometry_msgs Eigen3 eufs_msgs GSL) - - add_executable(midpoint_test src/nodes/midpoint.cpp src/planning_codebase/midline/generator.cpp src/planning_codebase/raceline/raceline.cpp) - # add_executable(midline_test src/generator.cpp src/raceline/raceline.cpp) - ament_target_dependencies(midpoint_test interfaces rclcpp std_msgs geometry_msgs Eigen3 eufs_msgs GSL) - ament_export_targets(planning_exportTargets HAS_LIBRARY_TARGET) ament_export_dependencies(interfaces rclcpp std_msgs geometry_msgs Eigen3 eufs_msgs GSL) - # ament_target_dependencies(midline_test rclcpp std_msgs geometry_msgs eufs_msgs GSL) - install( - TARGETS midpoint_test + TARGETS planning_export + EXPORT planning_exportTargets DESTINATION lib/${PROJECT_NAME} ) + # -------- MIDLINE SETUP -------- + add_executable(midpoint_test src/nodes/midpoint.cpp src/planning_codebase/midline/generator.cpp src/planning_codebase/raceline/raceline.cpp) + ament_target_dependencies(midpoint_test interfaces rclcpp std_msgs geometry_msgs Eigen3 eufs_msgs GSL) + install( - TARGETS planning_export - EXPORT planning_exportTargets + TARGETS midpoint_test DESTINATION lib/${PROJECT_NAME} ) + # -------- EKF SLAM SETUP -------- + add_executable(ekf_sim_validation src/nodes/SLAMSimNode.cpp) + add_executable(ekf_slam src/nodes/SLAMNode.cpp) + + ament_target_dependencies(ekf_sim_validation rclcpp message_filters eufs_msgs GSL Eigen3 interfaces) + ament_target_dependencies(ekf_slam rclcpp message_filters eufs_msgs GSL Eigen3 interfaces) + + install(TARGETS + ekf_sim_validation + DESTINATION lib/${PROJECT_NAME}) + + install(TARGETS + ekf_slam + DESTINATION lib/${PROJECT_NAME}) + ament_package() diff --git a/driverless_ws/src/planning/package.xml b/driverless_ws/src/planning/package.xml index 4a7fe0cec..c94e75156 100644 --- a/driverless_ws/src/planning/package.xml +++ b/driverless_ws/src/planning/package.xml @@ -20,7 +20,7 @@ eufs_msgs GSL Eigen3 - + ament_cmake diff --git a/driverless_ws/src/planning/src/nodes/SLAMNode.cpp b/driverless_ws/src/planning/src/nodes/SLAMNode.cpp new file mode 100644 index 000000000..b1fe54d5b --- /dev/null +++ b/driverless_ws/src/planning/src/nodes/SLAMNode.cpp @@ -0,0 +1,214 @@ +#include + +#include "rclcpp/rclcpp.hpp" + +#include +#include +#include + +#include "eufs_msgs/msg/cone_array_with_covariance.hpp" +#include "eufs_msgs/msg/cone_array.hpp" +#include "eufs_msgs/msg/car_state.hpp" +#include "interfaces/msg/slam_frame.hpp" + +#include "../planning_codebase/ekf_slam/ekf_slam.cpp" + +#include +#include +#include +#include + +#define CONE_DATA_TOPIC "/cones" +#define VEHICLE_DATA_TOPIC "/ground_truth/state" + +using namespace std; +using std::placeholders::_1; + +struct Cone{ + double x; + double y; + +}; + +struct VehiclePosition{ + double dx; + double dz; + double dyaw; +}; + +class SLAMValidation : public rclcpp::Node +{ + public: + SLAMValidation(): Node("slam_validation"){ + // gsl_matrix_set_identity(pEst); + + slamframe_sub = this->create_subscription( + "/SLAMFrame", 10, std::bind(&SLAMValidation::slamframe_callback, this, _1)); + + timer = this->create_wall_timer(100ms, std::bind(&SLAMValidation::timer_callback, this)); + } + private: + void slamframe_callback(const interfaces::msg::SlamFrame::SharedPtr slamframe){ + eufs_msgs::msg::ConeArray cone_data = slamframe->stereo_cones; + geometry_msgs::msg::Vector3Stamped imu_linear_velocity = slamframe->imu_linear_velocity; + sensor_msgs::msg::Imu imu_data = slamframe->imu_data; + + RCLCPP_INFO(this->get_logger(), "CONECALLBACK: B: %i| Y: %i| O: %i", cone_data.blue_cones.size(), cone_data.yellow_cones.size(), cone_data.orange_cones.size()); + blue_cones.clear(); + yellow_cones.clear(); + orange_cones.clear(); + for(int i = 0; i < cone_data.blue_cones.size(); i++){ + Cone to_add; + to_add.x = cone_data.blue_cones[i].x; + to_add.y = cone_data.blue_cones[i].y; + blue_cones.push_back(to_add); + } + for(int i = 0; i < cone_data.yellow_cones.size(); i++){ + Cone to_add; + to_add.x = cone_data.yellow_cones[i].x; + to_add.y = cone_data.yellow_cones[i].y; + yellow_cones.push_back(to_add); + } + for(int i = 0; i < cone_data.orange_cones.size(); i++){ + Cone to_add = {cone_data.orange_cones[i].x, cone_data.orange_cones[i].y}; + orange_cones.push_back(to_add); + } + + // Generate rotation matrix from GPS world to sensor frame + float q0 = imu_data.orientation.w; + float q0_2 = std::pow(q0, 2); + + float q1 = imu_data.orientation.x; + float q1_2 = std::pow(q1, 2); + + float q2 = imu_data.orientation.y; + float q2_2 = std::pow(q2, 2); + + float q3 = imu_data.orientation.z; + float q3_2 = std::pow(q3, 2); + + Eigen::MatrixXd world_to_sensor(3, 3); + world_to_sensor << (q0_2 + q1_2 - q2_2 - q3_2), 2*(q1*q2 + q0*q3), 2*(q1*q3 - q0*q2), + 2*(q1*q2 - q0*q3), (q0_2 - q1_2 + q2_2 - q3_2), 2*(q2*q3 + q0*q1), + 2*(q1*q3 + q0*q2), 2*(q2*q3 - q0*q1), (q0_2 - q1_2 - q2_2 + q3_2); + + Eigen::MatrixXd lin_vel_world(3, 1); + lin_vel_world(0, 0) = imu_linear_velocity.vector.x; + lin_vel_world(1, 0) = imu_linear_velocity.vector.y; + lin_vel_world(2, 0) = imu_linear_velocity.vector.z; + + Eigen::MatrixXd lin_vel_sensor = world_to_sensor * lin_vel_world; + vpos_is_jacob.dx = lin_vel_sensor(0, 0); + vpos_is_jacob.dz = lin_vel_sensor(2, 0);; + vpos_is_jacob.dyaw = imu_data.angular_velocity.y; + + } + void timer_callback(){ + RCLCPP_INFO(this->get_logger(), "hello\n"); + RCLCPP_ERROR(this->get_logger(), "ERRRO!\n"); + run_slam(); + } + + void run_slam(){ + // dt = std::chrono::duration_cast(curr_time - prev_time).count() / 1000000.0; + // prev_time = curr_time; + // RCLCPP_INFO(this->get_logger(), "curr_time: %d | prev_time: %d | dt: %d\n", curr_time.count(), prev_time.count(), dt); + // Time difference from one callback to another is so close to 0.1 seconds, gonna assume 0.1 everywhere + // Cuz elapsed time stuff not working rn + + + // gsl_matrix* u = gsl_matrix_calloc(2, 1); + // gsl_matrix_set(u, 0, 0, hypot(vpos_is_jacob.dx, vpos_is_jacob.dy)); + // gsl_matrix_set(u, 1, 0, `aw); + + // Eigen::MatrixXd u = calcInput(); + Eigen::MatrixXd u(2, 1); + u << hypot(vpos_is_jacob.dx, vpos_is_jacob.dz), + vpos_is_jacob.dyaw; + + int n = blue_cones.size() + yellow_cones.size() + orange_cones.size(); + int idx = 0; + + // Declaring z matrix dimensions + Eigen::MatrixXd z(n, 2); + + // gsl_matrix* z = gsl_matrix_calloc(n, 3); + RCLCPP_INFO(this->get_logger(), "RUNSLAM: B: %i | Y: %i | O: %i\n", blue_cones.size(), yellow_cones.size(), orange_cones.size()); + for(int i = 0; i < blue_cones.size(); i++){ + Cone c = blue_cones[i]; + double dist = hypot(c.x, c.y); + double angle = atan2(c.y, c.x); + double corrected_angle = std::fmod(angle + M_PI, 2 * M_PI) - M_PI; + // gsl_matrix_set(z, idx, 0, dist); + // gsl_matrix_set(z, idx, 1, angle); + z(idx, 0) = dist; + z(idx, 1) = angle; + idx++; + } + + for(int i = 0; i < yellow_cones.size(); i++){ + Cone c = yellow_cones[i]; + double dist = hypot(c.x, c.y); + double angle = atan2(c.y, c.x); + double corrected_angle = std::fmod(angle + M_PI, 2 * M_PI) - M_PI; + // gsl_matrix_set(z, idx, 0, dist); + // gsl_matrix_set(z, idx, 1, angle); + z(idx, 0) = dist; + z(idx, 1) = angle; + idx++; + } + + for(int i = 0; i < orange_cones.size(); i++){ + Cone c = orange_cones[i]; + double dist = hypot(c.x, c.y); + double angle = atan2(c.y, c.x); + double corrected_angle = std::fmod(angle + M_PI, 2 * M_PI) - M_PI; + // gsl_matrix_set(z, idx, 0, dist); + // gsl_matrix_set(z, idx, 1, angle); + z(idx, 0) = dist; + z(idx, 1) = angle; + idx++; + } + RCLCPP_INFO(this->get_logger(), "before ekf slam"); + // slam_output = ekf_slam(xEst, pEst, u, z, 0.1, this->get_logger()); + if (z.rows() != 0){ + RCLCPP_INFO(this->get_logger(), "in herebb\n"); + slam_output = ekf_slam(this->get_logger(), xEst, pEst, u, z, 0.1); + } + RCLCPP_INFO(this->get_logger(), "got output\n"); + RCLCPP_INFO(this->get_logger(), "NUM_LANDMARKS: %i\n", (xEst.rows()-3)/2); + // RCLCPP_INFO(this->get_logger(), "Position: %f, %f, %f", xEst(0, 0), xEst(1, 0), xEst(2, 0)); + xEst = slam_output.x; + pEst = slam_output.p; + + } + rclcpp::Subscription::SharedPtr slamframe_sub; + vector blue_cones; + vector yellow_cones; + vector orange_cones; + + VehiclePosition vpos_is_jacob; + rclcpp::TimerBase::SharedPtr timer; + + // gsl_matrix* xEst = gsl_matrix_calloc(STATE_SIZE, 1); + // gsl_matrix* pEst = gsl_matrix_calloc(STATE_SIZE, STATE_SIZE); + + // Initialize xEst matrix with zeros + Eigen::MatrixXd xEst = Eigen::MatrixXd::Zero(STATE_SIZE, 1); + // Initialize PEst matrix as an identity matrix + Eigen::MatrixXd pEst = Eigen::MatrixXd::Identity(STATE_SIZE, STATE_SIZE); + + double dt; + + ekfPackage slam_output; +}; + +int main(int argc, char * argv[]){ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + RCLCPP_INFO(node->get_logger(), "got output\n"); + rclcpp::spin(node); + rclcpp::shutdown(); + + return 0; +} \ No newline at end of file diff --git a/driverless_ws/src/planning/src/nodes/SLAMSimNode.cpp b/driverless_ws/src/planning/src/nodes/SLAMSimNode.cpp new file mode 100644 index 000000000..9dddd327c --- /dev/null +++ b/driverless_ws/src/planning/src/nodes/SLAMSimNode.cpp @@ -0,0 +1,214 @@ +#include + +#include "rclcpp/rclcpp.hpp" + +#include +#include +#include + +#include "eufs_msgs/msg/cone_array_with_covariance.hpp" +#include "eufs_msgs/msg/car_state.hpp" +#include "interfaces/msg/slam_output.hpp" + +// #include "new_slam.cpp" +// Adding new_slam_correct.cpp +#include "../planning_codebase/ekf_slam/ekf_slam.cpp" + +#include +#include +#include +#include +// #include + +#define CONE_DATA_TOPIC "/cones" +#define VEHICLE_DATA_TOPIC "/ground_truth/state" + +using namespace std; +using std::placeholders::_1; + +struct Cone{ + double x; + double y; + +}; + +struct VehiclePosition{ + double x; + double y; + double yaw; + double dx; + double dy; + double dyaw; +}; + + + +class SLAMSimNode : public rclcpp::Node +{ + public: + SLAMSimNode(): Node("sim_validation_node"){ + + // Sim Cone Subscriber + cone_sub = this->create_subscription( + "/cones", 10, std::bind(&SLAMSimNode::cone_callback, this, _1)); + + // Sim Vehicle State Subscriber + vehicle_state_sub = this->create_subscription( + "/ground_truth/state", 10, std::bind(&SLAMSimNode::vehicle_state_callback, this, _1)); + + slam_output_pub = this->create_publisher("/slam_output",10); + + // Timer to execute slam callback + timer = this->create_wall_timer(100ms, std::bind(&SLAMSimNode::run_slam, this)); + } + private: + // Callback to store most recent cones output by sim + void cone_callback(const eufs_msgs::msg::ConeArrayWithCovariance::SharedPtr cone_data){ + RCLCPP_INFO(this->get_logger(), "CONECALLBACK: B: %i| Y: %i| O: %i", cone_data->blue_cones.size(), cone_data->yellow_cones.size(), cone_data->orange_cones.size()); + + // Clear cones from previous frame + blue_cones.clear(); + yellow_cones.clear(); + orange_cones.clear(); + + // Store all blue cones from current sim frame + for(int i = 0; i < cone_data->blue_cones.size(); i++){ + Cone to_add; + to_add.x = cone_data->blue_cones[i].point.x; + to_add.y = cone_data->blue_cones[i].point.y; + blue_cones.push_back(to_add); + } + + // Store all yellow cones from current sim frame + for(int i = 0; i < cone_data->yellow_cones.size(); i++){ + Cone to_add; + to_add.x = cone_data->yellow_cones[i].point.x; + to_add.y = cone_data->yellow_cones[i].point.y; + yellow_cones.push_back(to_add); + } + + // Store all orange cones from current sim frame + for(int i = 0; i < cone_data->orange_cones.size(); i++){ + Cone to_add; + to_add.x = cone_data->orange_cones[i].point.x; + to_add.y = cone_data->orange_cones[i].point.y; + orange_cones.push_back(to_add); + } + } + + // Callback to store most recent vehicle state output by sim + void vehicle_state_callback(const eufs_msgs::msg::CarState::SharedPtr vehicle_state_data){ + double q1 = vehicle_state_data->pose.pose.orientation.x; + double q2 = vehicle_state_data->pose.pose.orientation.y; + double q3 = vehicle_state_data->pose.pose.orientation.z; + double q0 = vehicle_state_data->pose.pose.orientation.w; + double yaw = atan2(2*(q0*q3+q1*q2), pow(q0, 2)+pow(q1, 2)-pow(q2, 2)-pow(q3, 2)); + + vehicle_pos.x = vehicle_state_data->pose.pose.position.x; + vehicle_pos.y = vehicle_state_data->pose.pose.position.y; + vehicle_pos.yaw = yaw; + vehicle_pos.dx = vehicle_state_data->twist.twist.linear.x; + vehicle_pos.dy = vehicle_state_data->twist.twist.linear.y; + vehicle_pos.dyaw = vehicle_state_data->twist.twist.angular.z; + } + + void run_slam(){ + // Generate input velocity matrix, u + Eigen::MatrixXd u(2, 1); + u << hypot(vehicle_pos.dx, vehicle_pos.dy), // linear velocity + vehicle_pos.dyaw; // angular velocity + + // Create single matrix called z to store all blue, yellow, orange cones + int n = blue_cones.size() + yellow_cones.size() + orange_cones.size(); + Eigen::MatrixXd z(n, 2); + int idx = 0; + + // Iterate through all blue cones and store in z + for(int i = 0; i < blue_cones.size(); i++){ + Cone c = blue_cones[i]; + double dist = hypot(c.x, c.y); + double angle = atan2(c.y, c.x); + z(idx, 0) = dist; + z(idx, 1) = angle; + idx++; + } + + // Iterate through all yellow cones and store in z + for(int i = 0; i < yellow_cones.size(); i++){ + Cone c = yellow_cones[i]; + double dist = hypot(c.x, c.y); + double angle = atan2(c.y, c.x); + z(idx, 0) = dist; + z(idx, 1) = angle; + idx++; + } + + // Iterate through all orange cones and store in z + for(int i = 0; i < orange_cones.size(); i++){ + Cone c = orange_cones[i]; + double dist = hypot(c.x, c.y); + double angle = atan2(c.y, c.x); + z(idx, 0) = dist; + z(idx, 1) = angle; + idx++; + } + + // Only run ekf_slam if the number of cones is greater than 0 + if (z.rows() != 0){ + slam_output = ekf_slam(this->get_logger(), xEst, pEst, u, z, 0.1); + + xEst = slam_output.x; + pEst = slam_output.p; + int num_landmarks = (xEst.rows()-3)/2; + RCLCPP_INFO(this->get_logger(), "NUM_LANDMARKS: %i\n", num_landmarks); + + // Construct SLAMOutput message + interfaces::msg::SlamOutput slam_output_msg = interfaces::msg::SlamOutput(); + slam_output_msg.car_x = xEst(0, 0); + slam_output_msg.car_y = xEst(1, 0); + slam_output_msg.car_heading = xEst(2, 0); + + std::vector landmarks(num_landmarks); + for(int i = 0; i < num_landmarks; i++){ + geometry_msgs::msg::Point curr_landmark = geometry_msgs::msg::Point(); + curr_landmark.x = xEst(2*i+3, 0); + curr_landmark.y = xEst(2*i+4, 0); + landmarks[i] = curr_landmark; + } + slam_output_msg.landmarks = landmarks; + slam_output_pub->publish(slam_output_msg); + } + } + + + // ------ TOPIC SUBSCRIBERS + PUBLISHERS ------ + rclcpp::Subscription::SharedPtr cone_sub; + rclcpp::Subscription::SharedPtr vehicle_state_sub; + rclcpp::Publisher::SharedPtr slam_output_pub; + + // ------ CONE ARRAYS ------ + vector blue_cones; + vector yellow_cones; + vector orange_cones; + + VehiclePosition vehicle_pos; + rclcpp::TimerBase::SharedPtr timer; + + // ------ STATE MATRIX - Initialized to all zeros ------ + Eigen::MatrixXd xEst = Eigen::MatrixXd::Zero(STATE_SIZE, 1); + + // ------ COVARIANCE MATRIX - Initialized to identity ------ + Eigen::MatrixXd pEst = Eigen::MatrixXd::Identity(STATE_SIZE, STATE_SIZE); + + double dt; + ekfPackage slam_output; +}; + +int main(int argc, char * argv[]){ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + + return 0; +} \ No newline at end of file diff --git a/driverless_ws/src/planning/src/nodes/midpoint.cpp b/driverless_ws/src/planning/src/nodes/midpoint.cpp index 736095846..714c91e40 100644 --- a/driverless_ws/src/planning/src/nodes/midpoint.cpp +++ b/driverless_ws/src/planning/src/nodes/midpoint.cpp @@ -12,7 +12,7 @@ #include "interfaces/msg/spline.hpp" // #include "frenet.hpp" // #include "runpy.hpp" -#include +#include //publish topic example diff --git a/driverless_ws/src/planning/src/planning_codebase/ekf_slam/ekf_slam.cpp b/driverless_ws/src/planning/src/planning_codebase/ekf_slam/ekf_slam.cpp new file mode 100755 index 000000000..5f9b76880 --- /dev/null +++ b/driverless_ws/src/planning/src/planning_codebase/ekf_slam/ekf_slam.cpp @@ -0,0 +1,320 @@ +#include +#include +#include +#include +#include + +// Maximum threshold for adding new landmark +double M_DIST_TH = 2; + +// State size [x,y,yaw] +int STATE_SIZE = 3; + +// Landmark state size [x,y] +int LM_SIZE = 2; + +// Create Cx matrix +Eigen::Matrix3d Cx = (Eigen::Matrix3d() << 0.5 * 0.5, 0.0, 0.0, + 0.0, 0.5 * 0.5, 0.0, + 0.0, 0.0, (std::cos(30.0) * std::cos(30.0))).finished(); // Convert 30.0 degrees to radians + +// Function to calculate the motion model +Eigen::MatrixXd motion_model(auto logger, const Eigen::MatrixXd& x, const Eigen::MatrixXd& u, double dt) { + Eigen::MatrixXd F(3, 3); + F << 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0; + + Eigen::MatrixXd B(3, 2); + B << dt * std::cos(x(2, 0)), 0.0, + dt * std::sin(x(2, 0)), 0.0, + 0.0, dt; + return (F * x) + (B * u); +} + +// Function to calculate the number of landmarks +int calc_n_lm(const Eigen::MatrixXd& x) { + int n = static_cast((x.rows() - STATE_SIZE) / LM_SIZE); + return n; +} + +// Storing G and Fx matrices in the jacob_motion package +struct jacob_motion_package { + Eigen::MatrixXd Fx; + Eigen::MatrixXd G; +}; + +// Function to calculate jacobian motion +jacob_motion_package jacob_motion(auto logger, const Eigen::MatrixXd& x, const Eigen::MatrixXd& u, double dt) { + + // Creating Identity Matrix of Size 3 x 3 + Eigen::MatrixXd identityMatrix = Eigen::MatrixXd::Identity(STATE_SIZE, STATE_SIZE); + + + // Creating Zeroes Matrix + int n_lm = calc_n_lm(x); + Eigen::MatrixXd zeroesMatrix(STATE_SIZE, LM_SIZE * n_lm); + zeroesMatrix.setZero(); + + // Stacking Identity and Zeroes Matrix + Eigen::MatrixXd Fx(STATE_SIZE, STATE_SIZE + (LM_SIZE * n_lm)); + Fx << identityMatrix, zeroesMatrix; + + Eigen::MatrixXd jF(3, 3); + + // Creating jF matrix + jF << 0.0, 0.0, -dt * u(0, 0) * std::sin(x(2, 0)), + 0.0, 0.0, dt * u(0, 0) * std::cos(x(2, 0)), + 0.0, 0.0, 0.0; + + // Creating Fx transpose matrix + Eigen::MatrixXd FxT = Fx.transpose().eval(); + + // Creating G matrix + Eigen::MatrixXd G = identityMatrix + (FxT * jF * Fx); + + // Constructing jacob motion package result + jacob_motion_package result; + result.Fx = Fx; + result.G = G; + + return result; +} + + +// Function to return (x, y) position of landmark stored as 2 x 1 matrix +Eigen::MatrixXd calc_landmark_position(const Eigen::MatrixXd& x, const Eigen::MatrixXd& z) { + + Eigen::MatrixXd zp(2, 1); + zp.setZero(); + + zp(0,0) = x(0, 0) + (z(0, 0) * std::cos(x(2, 0) + z(0, 1))); + zp(1,0) = x(1, 0) + (z(0, 0) * std::sin(x(2, 0) + z(0, 1))); + + return zp; +} + +Eigen::MatrixXd get_landmark_position_from_state(const Eigen::MatrixXd& x, int ind) { + + // Calculate the starting and ending row indices for the landmark position + int start_row = STATE_SIZE + LM_SIZE * ind; + int end_row = STATE_SIZE + LM_SIZE * (ind + 1); + + // Extract the landmark position from the state vector + Eigen::MatrixXd lm = x.block(start_row, 0, LM_SIZE, 1); + + return lm; +} + +// Function Calculates Jacobian Matrix H +Eigen::MatrixXd jacob_h(auto logger, double q, const Eigen::MatrixXd& delta, const Eigen::MatrixXd& x, int i) { + double sq = std::sqrt(q); + + Eigen::MatrixXd G(2, 5); + G << -sq * delta(0, 0), -sq * delta(1, 0), 0.0, sq * delta(0, 0), sq * delta(1, 0), + delta(1, 0), -1 * delta(0,0), -q, -1 * delta(1, 0), delta(0, 0); + G /= q; + + // Calculate the number of landmarks + int nLM = calc_n_lm(x); + + // Construct the F1 matrix + Eigen::MatrixXd F1(3, 3 + 2 * nLM); + F1 << Eigen::MatrixXd::Identity(3, 3), Eigen::MatrixXd::Zero(3, 2 * nLM); + + // Construct the F2 matrix + Eigen::MatrixXd F2(2, 3 + 2 * nLM); + F2 << Eigen::MatrixXd::Zero(2, 3), Eigen::MatrixXd::Zero(2, 2 * (i - 1)), Eigen::MatrixXd::Identity(2, 2), Eigen::MatrixXd::Zero(2, 2 * nLM - 2 * i); + + // Concatenate F1 and F2 to create F matrix + Eigen::MatrixXd F(F1.rows() + F2.rows(), F1.cols()); + F << F1, F2; + + // Calculate the Jacobian H by multiplying G and F + Eigen::MatrixXd H = G * F; + + return H; + +} + +double pi_2_pi(double angle) { + return fmod(angle + M_PI, 2.0 * M_PI) - M_PI; +} + +struct innovation_package { + Eigen::MatrixXd y; + Eigen::MatrixXd S; + Eigen::MatrixXd H; +}; + +struct innovation_package calc_innovation(auto logger, const Eigen::MatrixXd& lm, const Eigen::MatrixXd& xEst, const Eigen::MatrixXd& PEst, const Eigen::MatrixXd& z, int LMid) { + Eigen::MatrixXd delta = lm - xEst.topRows(2); + + double q = delta.squaredNorm(); + double z_angle = std::atan2(delta(1, 0), delta(0, 0)) - xEst(2, 0); + + Eigen::MatrixXd zp(2, 1); + zp(0, 0) = std::sqrt(q); + zp(1, 0) = pi_2_pi(z_angle); + + Eigen::MatrixXd y = (z.transpose().eval() - zp); + y(1, 0) = pi_2_pi(y(1, 0)); + + Eigen::MatrixXd H = jacob_h(logger, q, delta, xEst, LMid + 1); + Eigen::MatrixXd S = H * PEst * H.transpose().eval() + Cx.block(0, 0, 2, 2); + + innovation_package result; + result.y = y; + result.S = S; + result.H = H; + + return result; + +} + +int search_correspond_landmark_id(auto logger, const Eigen::MatrixXd& xAug, const Eigen::MatrixXd& PAug, const Eigen::MatrixXd& zi) { + + // Calculate the number of landmarks + int nLM = calc_n_lm(xAug); + + + // Vector that will store mahalanobis distances + std::vector min_dist; + double r = zi(0, 0); + double theta = zi(0, 1); + + double meas_x = r*std::cos(theta); + double meas_y = r*std::sin(theta); + + for (int i = 0; i < nLM; ++i) { + Eigen::MatrixXd lm = get_landmark_position_from_state(xAug, i); + + // Calculating y, S, and H matrices from innovation package + struct innovation_package i_p = calc_innovation(logger, lm, xAug, PAug, zi, i); + Eigen::MatrixXd y = i_p.y; + Eigen::MatrixXd S = i_p.S; + Eigen::MatrixXd H = i_p.H; + + // Calculating mahalanobis distance + // double mahalanobis = y.transpose().eval() * S.ldlt().solve(y); + Eigen::MatrixXd temp = (y.transpose().eval() * S.inverse() * y); + double mahalanobis = (y.transpose().eval() * S.inverse() * y)(0, 0); + Eigen::MatrixXd lm_car_frame = lm - xAug.topRows(2); + double euclidean = std::sqrt(pow(lm_car_frame(0, 0)-meas_x, 2) + pow(lm_car_frame(1, 0)-meas_y, 2)); + // RCLCPP_INFO(logger, " Landmark %i (Car Frame): (%f, %f) | Mahalanobis: %f | Euclidean: %f", i, lm_car_frame(0,0), lm_car_frame(1,0), mahalanobis, euclidean); + + // Adding mahalanobis distance to minimum distance vector + min_dist.push_back(mahalanobis); + } + + + min_dist.push_back(M_DIST_TH); // Add M_DIST_TH for new landmark + + // Find the index of the minimum element in 'min_dist' + int min_id = std::distance(min_dist.begin(), std::min_element(min_dist.begin(), min_dist.end())); + + return min_id; + +} + +struct ekfPackage { + Eigen::MatrixXd x; + Eigen::MatrixXd p; + std::vector cone; +}; + +struct ekfPackage ekf_slam(auto logger, Eigen::MatrixXd& xEst, Eigen::MatrixXd& PEst, Eigen::MatrixXd& u, Eigen::MatrixXd& z, double dt) { + + // Ensuring that z is a 2 x n matrix where every landmark is 2 x 1 matrix + int nLM = (xEst.rows()-3)/2; + for(int i = 0; i < nLM; i++){ + Eigen::MatrixXd lm = xEst.block(i*2+3, 0, LM_SIZE, 1); + RCLCPP_INFO(logger, "Landmark %i: (%f, %f)", i, lm(0, 0), lm(1, 0)); + } + + std::vector cones; + int S = STATE_SIZE; + + struct jacob_motion_package j_m_p = jacob_motion(logger, xEst.topRows(S), u, dt); + Eigen::MatrixXd G = j_m_p.G; + Eigen::MatrixXd Fx = j_m_p.Fx; + + Eigen::MatrixXd M_t(2, 2); + + // Calculate the elements of M_t + double element_1 = std::pow(0.11 * std::abs(u(0, 0)) + 0.01 * std::abs(u(1, 0)), 2); + double element_2 = std::pow(0.18 * std::abs(u(0, 0)) + 0.08 * std::abs(u(1, 0)), 2); + + // Assign the elements to M_t + M_t << element_1, 0, + 0, element_2; + Eigen::MatrixXd x = xEst.topRows(S); + + Eigen::MatrixXd V_t(3, 2); + + double cos_x2 = std::cos(x(2, 0)); + double sin_x2 = std::sin(x(2, 0)); + + // Calculate the elements of V_t + V_t << cos_x2, -0.5 * sin_x2, + sin_x2, 0.5 * cos_x2, + 0, 1; + xEst.topRows(S) = motion_model(logger, xEst.topRows(S), u, dt); + + PEst.block(0, 0, S, S) = G.transpose().eval() * PEst.block(0, 0, S, S) * G + Fx.transpose().eval() * Cx * Fx; + + Eigen::MatrixXd initP = Eigen::MatrixXd::Identity(2, 2); + + // Initializing landmark position + Eigen::MatrixXd lm; + + for (int iz = 0; iz < z.rows(); ++iz) { + int min_id = search_correspond_landmark_id(logger, xEst, PEst, z.row(iz)); + int nLM = calc_n_lm(xEst); + if (min_id == nLM) { + + // Extend state and covariance matrix + Eigen::MatrixXd xAug(xEst.rows() + LM_SIZE, xEst.cols()); + xAug << xEst, calc_landmark_position(xEst, z.row(iz)); + + Eigen::MatrixXd m1(PEst.rows(), PEst.cols() + LM_SIZE); + Eigen::MatrixXd m1_zerosMatrix(xEst.rows(), LM_SIZE); + m1_zerosMatrix.setZero(); + + m1 << PEst, m1_zerosMatrix; + + Eigen::MatrixXd m2(LM_SIZE, xEst.rows() + initP.rows()); + Eigen::MatrixXd m2_zerosMatrix(LM_SIZE, xEst.rows()); + m2_zerosMatrix.setZero(); + + m2 << m2_zerosMatrix, initP; + + Eigen::MatrixXd PAug(m1.rows() + m2.rows(), m1.cols()); + PAug << m1, m2; + + xEst = xAug; + PEst = PAug; + } + + lm = get_landmark_position_from_state(xEst, min_id); + innovation_package i_p = calc_innovation(logger, lm, xEst, PEst, z.row(iz), min_id); + Eigen::MatrixXd y = i_p.y; + Eigen::MatrixXd S = i_p.S; + Eigen::MatrixXd H = i_p.H; + Eigen::MatrixXd K = PEst * H.transpose() * S.inverse(); + xEst.block(3, 0, xEst.rows() - 3, xEst.cols()) = xEst.block(3, 0, xEst.rows() - 3, xEst.cols()) + (K.block(3, 0, K.rows() - 3, K.cols()) * y); + PEst = (Eigen::MatrixXd::Identity(PEst.rows(), PEst.cols()) - K * H) * PEst; + } + + + // xEst.row(2) = pi_2_pi(xEst.row(2)); + xEst(2, 0) = pi_2_pi(xEst(2, 0)); + + // Constructing EKF SLAM Package Result + ekfPackage result; + result.x = xEst; + result.p = PEst; + result.cone = cones; + + return result; +} \ No newline at end of file diff --git a/driverless_ws/src/planning/src/planning_codebase/midline/generator.cpp b/driverless_ws/src/planning/src/planning_codebase/midline/generator.cpp index dd738527c..9baf0e51f 100644 --- a/driverless_ws/src/planning/src/planning_codebase/midline/generator.cpp +++ b/driverless_ws/src/planning/src/planning_codebase/midline/generator.cpp @@ -1,5 +1,5 @@ #include "generator.hpp" -#include +#include MidpointGenerator::MidpointGenerator(int interpolation_num){ interpolation_number=interpolation_num; diff --git a/driverless_ws/src/planning/src/planning_codebase/midline/generator.hpp b/driverless_ws/src/planning/src/planning_codebase/midline/generator.hpp index a6a1eb588..f3dbd78d5 100644 --- a/driverless_ws/src/planning/src/planning_codebase/midline/generator.hpp +++ b/driverless_ws/src/planning/src/planning_codebase/midline/generator.hpp @@ -3,7 +3,7 @@ #include #include #include -#include +#include #include #ifndef MIDPOINTGENERATOR #define MIDPOINTGENERATOR diff --git a/driverless_ws/src/planning/src/planning_codebase/raceline/frenetEigen.cpp b/driverless_ws/src/planning/src/planning_codebase/raceline/frenetEigen.cpp index 68679074b..578a309cc 100644 --- a/driverless_ws/src/planning/src/planning_codebase/raceline/frenetEigen.cpp +++ b/driverless_ws/src/planning/src/planning_codebase/raceline/frenetEigen.cpp @@ -7,8 +7,8 @@ #include #include #include -#include -#include +#include +#include #include "frenetEigen.hpp" #include "racelineEigen.hpp" diff --git a/driverless_ws/src/planning/src/planning_codebase/raceline/raceline.cpp b/driverless_ws/src/planning/src/planning_codebase/raceline/raceline.cpp index 819877703..95f32ba7f 100644 --- a/driverless_ws/src/planning/src/planning_codebase/raceline/raceline.cpp +++ b/driverless_ws/src/planning/src/planning_codebase/raceline/raceline.cpp @@ -1,5 +1,5 @@ #include "raceline.hpp" -#include +#include // #include "random.h" polynomial poly(int deg = 3){ diff --git a/driverless_ws/src/planning/src/planning_codebase/raceline/raceline.hpp b/driverless_ws/src/planning/src/planning_codebase/raceline/raceline.hpp index fd1a82ef6..0a8d29863 100644 --- a/driverless_ws/src/planning/src/planning_codebase/raceline/raceline.hpp +++ b/driverless_ws/src/planning/src/planning_codebase/raceline/raceline.hpp @@ -1,7 +1,7 @@ #include #include -#include -#include +#include +#include #include #include #include diff --git a/driverless_ws/src/ros2_numpy b/driverless_ws/src/ros2_numpy new file mode 160000 index 000000000..a41620034 --- /dev/null +++ b/driverless_ws/src/ros2_numpy @@ -0,0 +1 @@ +Subproject commit a416200346a35fae7ce110132a98eac381838fe2 diff --git a/driverless_ws/vis.py b/driverless_ws/vis.py new file mode 100644 index 000000000..c49028b95 --- /dev/null +++ b/driverless_ws/vis.py @@ -0,0 +1,122 @@ +import matplotlib.pyplot as plt +import re +import numpy as np +input = """Landmark 0: (2.129901, 2.205865) +Landmark 1: (0.523999, 3.927027) +Landmark 2: (-0.468952, 6.349779) +Landmark 3: (-5.928563, 8.595943) +Landmark 4: (-1.245529, 7.966677) +Landmark 5: (-2.553666, 11.802902) +Landmark 6: (-6.566005, 11.201945) +Landmark 7: (-4.606813, 7.453072) +Landmark 8: (-7.676098, 14.080339) +Landmark 9: (0.006841, 4.830301) +Landmark 10: (-0.512391, 2.222208) +Landmark 11: (-2.175734, 4.417270) +Landmark 12: (-1.747342, 9.961391) +Landmark 13: (-7.008097, 12.795580) +Landmark 14: (-2.410997, 6.272010) +Landmark 15: (1.376774, 2.833958) +Landmark 16: (-0.841064, 9.061395) +Landmark 17: (2.100315, 3.945045) +Landmark 18: (2.901135, 2.098022) +Landmark 19: (2.535140, 6.609478) +Landmark 20: (3.844637, 3.034387) +Landmark 21: (4.264339, 1.777372) +Landmark 22: (5.432917, 5.021804) +Landmark 23: (5.619909, 3.035109) +Landmark 24: (5.697473, 6.002658) +Landmark 25: (5.920960, 0.931273) +Landmark 26: (7.842798, 1.816840) +Landmark 27: (7.187929, 3.564434) +Landmark 28: (7.808201, 0.004718) +Landmark 29: (9.056057, -0.695020) +Landmark 30: (8.745105, -3.408502) +Landmark 31: (10.569989, -2.178791) +Landmark 32: (11.772617, -1.860644) +Landmark 33: (17.673637, 0.187580) +Landmark 34: (15.193674, -0.954982) +Landmark 35: (20.474212, 1.325021) +Landmark 36: (13.311998, -2.053602) +Landmark 37: (7.332384, -4.670448) +Landmark 38: (21.565972, -6.223838) +Landmark 39: (16.683399, -6.872555) +Landmark 40: (23.526384, -5.678956) +Landmark 41: (18.327590, -6.021619) +Landmark 42: (31.946370, -5.237320) +Landmark 43: (9.195679, -5.358219) +Landmark 44: (10.119221, -6.062894) +Landmark 45: (5.970833, -4.934991) +Landmark 46: (20.894037, -12.051444) +Landmark 47: (26.955190, -12.826948) +Landmark 48: (8.980995, -8.484803) +Landmark 49: (19.203145, -16.590359) +Landmark 50: (15.175811, -7.604598) +Landmark 51: (13.270050, -6.783250) +Landmark 52: (6.529149, -6.404448) +Landmark 53: (9.373762, -9.581655) +Landmark 54: (13.258733, -8.867169) +Landmark 55: (11.194578, -16.337348) +Landmark 56: (9.421012, -13.566578) +Landmark 57: (12.324096, -18.686143) +Landmark 58: (6.546754, -7.780066) +Landmark 59: (7.259822, -11.782836) +Landmark 60: (10.249393, -18.277154) +Landmark 61: (9.109203, -15.620197) +Landmark 62: (8.176176, -12.847238) +Landmark 63: (17.661160, -19.599510) +Landmark 64: (4.493823, -5.347015) +Landmark 65: (11.919947, -20.420983) +Landmark 66: (11.554966, -13.359834) +Landmark 67: (6.227170, -9.051277) +Landmark 68: (4.226945, -6.274415) +Landmark 69: (3.017956, -5.167364) +Landmark 70: (1.712976, -10.462305) +Landmark 71: (1.943705, -8.462368) +Landmark 72: (1.721949, -5.110186) +Landmark 73: (0.018740, -11.246074) +Landmark 74: (-1.628637, -13.016495) +Landmark 75: (-2.843279, -14.951093) +Landmark 76: (-3.662257, -21.591300) +Landmark 77: (-5.565102, -24.592640) +Landmark 78: (-3.540340, -16.276053) +Landmark 79: (0.100094, -5.769790) +Landmark 80: (-4.727813, -8.759857) +Landmark 81: (-1.634968, -7.350431) +Landmark 82: (-8.018344, -17.035909) +Landmark 83: (-4.151267, -4.743113) +Landmark 84: (-1.829468, -5.192279) +Landmark 85: (-11.073952, -9.773135) +Landmark 86: (-14.208149, -10.698259) +Landmark 87: (-0.327683, -3.196381) +Landmark 88: (1.396725, -2.814225) +Landmark 89: (-9.171774, -5.973722) +Landmark 90: (-7.628968, -5.547021) +Landmark 91: (-4.650450, -6.713457) +Landmark 92: (-9.611591, -14.064956) +Landmark 93: (2.373882, -4.184158) +Landmark 94: (0.442169, -1.650560) +Landmark 95: (-11.941525, 0.506180) +Landmark 96: (-16.640320, 0.280447) +Landmark 97: (-1.581557, -2.500204) +Landmark 98: (-3.570190, -1.434067) +Landmark 99: (-10.507244, -5.233756) +Landmark 100: (0.899846, -0.106313) +Landmark 101: (-17.794587, -5.848648) """ + +landmarks = input.split("\n") +x = [] +y = [] +for i in range(len(landmarks)): + tokens = re.split(r'[( ,)]', landmarks[i].strip()) + x.append(float(tokens[3])) + y.append(float(tokens[5])) + +print(x) +print(y) +colors = np.linspace(0, 1, 102) +colormap = plt.cm.RdYlGn + +plt.scatter(x, y, c=colors, cmap=colormap, edgecolors='k', marker='o') +plt.show() +# print(landmarks) \ No newline at end of file