From eca8d2532fc8f84efd88f0cc5eb54dc4905bf425 Mon Sep 17 00:00:00 2001 From: pimpest <82343504+pimpest@users.noreply.github.com> Date: Sun, 11 Jan 2026 22:57:26 +0100 Subject: [PATCH] Created path segmenter controller --- .clangd | 6 + .gitignore | 1 + colcon_defaults.yaml | 6 + .../src/toid_bot_description.urdf | 6 +- toid_navigation/CMakeLists.txt | 1 + .../navigate_to_pose_w_backtracking.xml | 51 + toid_navigation/launch/navigation_launch.py | 8 +- toid_navigation/package.xml | 2 +- toid_navigation/params/output.json | 3620 +++++++++++++++++ .../params/toid_general_params.yaml | 233 +- toid_navigation/rviz/nav2.rviz | 53 +- toid_spinner_controller/CMakeLists.txt | 68 + toid_spinner_controller/LICENSE | 17 + .../toid_spinner_controller/path_handler.hpp | 49 + .../toid_spinner_controller.hpp | 82 + toid_spinner_controller/package.xml | 31 + toid_spinner_controller/src/path_handler.cpp | 131 + .../src/toid_spinner_controller.cpp | 206 + .../toid_spinner_controller.xml | 9 + 19 files changed, 4411 insertions(+), 169 deletions(-) create mode 100644 .clangd create mode 100644 colcon_defaults.yaml create mode 100644 toid_navigation/behaviors/navigate_to_pose_w_backtracking.xml create mode 100644 toid_navigation/params/output.json create mode 100644 toid_spinner_controller/CMakeLists.txt create mode 100644 toid_spinner_controller/LICENSE create mode 100644 toid_spinner_controller/include/toid_spinner_controller/path_handler.hpp create mode 100644 toid_spinner_controller/include/toid_spinner_controller/toid_spinner_controller.hpp create mode 100644 toid_spinner_controller/package.xml create mode 100644 toid_spinner_controller/src/path_handler.cpp create mode 100644 toid_spinner_controller/src/toid_spinner_controller.cpp create mode 100644 toid_spinner_controller/toid_spinner_controller.xml diff --git a/.clangd b/.clangd new file mode 100644 index 0000000..68e449a --- /dev/null +++ b/.clangd @@ -0,0 +1,6 @@ +--- +Diagnostics: + UnusedIncludes: None + MissingIncludes: None + ClangTidy: + FastCheckFilter: Loose \ No newline at end of file diff --git a/.gitignore b/.gitignore index e8b072e..fe1e202 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,4 @@ +.cache build install log \ No newline at end of file diff --git a/colcon_defaults.yaml b/colcon_defaults.yaml new file mode 100644 index 0000000..ef401fa --- /dev/null +++ b/colcon_defaults.yaml @@ -0,0 +1,6 @@ +{ + "build": { + "cmake-args": ["-DCMAKE_EXPORT_COMPILE_COMMANDS=1"], + "symlink-install": true + } +} \ No newline at end of file diff --git a/toid_bot_description/src/toid_bot_description.urdf b/toid_bot_description/src/toid_bot_description.urdf index ceb860b..83de634 100644 --- a/toid_bot_description/src/toid_bot_description.urdf +++ b/toid_bot_description/src/toid_bot_description.urdf @@ -3,8 +3,8 @@ - - + + @@ -53,7 +53,7 @@ - + diff --git a/toid_navigation/CMakeLists.txt b/toid_navigation/CMakeLists.txt index e2d36e6..18fee8b 100644 --- a/toid_navigation/CMakeLists.txt +++ b/toid_navigation/CMakeLists.txt @@ -16,6 +16,7 @@ install( launch maps params + behaviors rviz DESTINATION share/${PROJECT_NAME} ) diff --git a/toid_navigation/behaviors/navigate_to_pose_w_backtracking.xml b/toid_navigation/behaviors/navigate_to_pose_w_backtracking.xml new file mode 100644 index 0000000..5fb81d7 --- /dev/null +++ b/toid_navigation/behaviors/navigate_to_pose_w_backtracking.xml @@ -0,0 +1,51 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/toid_navigation/launch/navigation_launch.py b/toid_navigation/launch/navigation_launch.py index c1c3289..8193379 100644 --- a/toid_navigation/launch/navigation_launch.py +++ b/toid_navigation/launch/navigation_launch.py @@ -13,6 +13,8 @@ def generate_launch_description(): map = os.path.join(pkg_share, 'maps', 'mapb2_5cm.yaml') launch_dir = os.path.join(pkg_share, 'launch') default_rviz_config_path = os.path.join(pkg_share, 'rviz', 'nav2.rviz') + bt_path = os.path.join(pkg_share, "behaviors", "navigate_to_pose_w_backtracking.xml") + lattice_path = os.path.join(pkg_share, "params", "output.json") position = "0.756 0.225 0 0 0 0".split(' ') #position = "0 0 0 0 0 0".split(' ') @@ -54,7 +56,7 @@ def generate_launch_description(): executable='planner_server', name='planner_server', output='screen', - parameters=[params] + parameters=[{'GridBased':{'lattice_filepath': lattice_path}}, params] ) controller_server = Node( @@ -70,9 +72,11 @@ def generate_launch_description(): executable='bt_navigator', name='bt_navigator', output='screen', - parameters=[params] + parameters=[{'default_nav_to_pose_bt_xml': bt_path}, params] ) +# default_nav_to_pose_bt_xml: "$(find-pkg-share toid_navigation)/behaviors/navigate_to_pose_w_backtracking.xml" + behavior_server = Node( package='nav2_behaviors', executable='behavior_server', diff --git a/toid_navigation/package.xml b/toid_navigation/package.xml index f378bae..2e2437a 100644 --- a/toid_navigation/package.xml +++ b/toid_navigation/package.xml @@ -14,7 +14,7 @@ nav2_planner nav2_controller nav2_bt_navigator - nav2_lifecycle_manger + nav2_lifecycle_manager nav2_bringup ros2_control rviz2 diff --git a/toid_navigation/params/output.json b/toid_navigation/params/output.json new file mode 100644 index 0000000..3811b9d --- /dev/null +++ b/toid_navigation/params/output.json @@ -0,0 +1,3620 @@ +{ + "version": 1.0, + "date_generated": "2026-01-07", + "lattice_metadata": { + "motion_model": "diff", + "turning_radius": 0.1, + "grid_resolution": 0.02, + "stopping_threshold": 5, + "num_of_headings": 16, + "heading_angles": [ + 0.0, + 0.4636476090008061, + 0.7853981633974483, + 1.1071487177940904, + 1.5707963267948966, + 2.0344439357957027, + 2.356194490192345, + 2.677945044588987, + 3.141592653589793, + 3.6052402625905993, + 3.9269908169872414, + 4.2487413713838835, + 4.71238898038469, + 5.176036589385496, + 5.497787143782138, + 5.81953769817878 + ], + "number_of_trajectories": 104 + }, + "primitives": [ + { + "trajectory_id": 0, + "start_angle_index": 0, + "end_angle_index": 13, + "left_turn": false, + "trajectory_radius": 0.10854, + "trajectory_length": 0.12309, + "arc_length": 0.12017, + "straight_length": 0.00292, + "poses": [ + [ + 0.02044, + -0.00142, + 6.121079291226373 + ], + [ + 0.04025, + -0.00662, + 5.932070750858197 + ], + [ + 0.05874, + -0.01545, + 5.743062210490022 + ], + [ + 0.07523, + -0.0276, + 5.554053670121847 + ], + [ + 0.08915, + -0.04262, + 5.365045129753671 + ], + [ + 0.1, + -0.06, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 1, + "start_angle_index": 0, + "end_angle_index": 14, + "left_turn": false, + "trajectory_radius": 0.14485, + "trajectory_length": 0.13862, + "arc_length": 0.11376, + "straight_length": 0.02485, + "poses": [ + [ + 0.01974, + -0.00135, + 6.146474682680838 + ], + [ + 0.03911, + -0.00538, + 6.009764058182089 + ], + [ + 0.05776, + -0.01201, + 5.87305343368334 + ], + [ + 0.07532, + -0.02112, + 5.736342809184592 + ], + [ + 0.09148, + -0.03254, + 5.599632184685843 + ], + [ + 0.106, + -0.046, + 5.497787143782138 + ], + [ + 0.12, + -0.06, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 2, + "start_angle_index": 0, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 6.128636104179318 + ], + [ + 0.0, + 0.0, + 5.9740869011790485 + ], + [ + 0.0, + 0.0, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 3, + "start_angle_index": 0, + "end_angle_index": 0, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.04, + "arc_length": 0.0, + "straight_length": 0.04, + "poses": [ + [ + 0.02, + 0.0, + 0.0 + ], + [ + 0.04, + 0.0, + 0.0 + ] + ] + }, + { + "trajectory_id": 4, + "start_angle_index": 0, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 0.0 + ], + [ + 0.0, + 0.0, + 0.1545492030002687 + ], + [ + 0.0, + 0.0, + 0.3090984060005374 + ], + [ + 0.0, + 0.0, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 5, + "start_angle_index": 0, + "end_angle_index": 2, + "left_turn": true, + "trajectory_radius": 0.14485, + "trajectory_length": 0.13862, + "arc_length": 0.11376, + "straight_length": 0.02485, + "poses": [ + [ + 0.01974, + 0.00135, + 0.13671062449874857 + ], + [ + 0.03911, + 0.00538, + 0.27342124899749715 + ], + [ + 0.05776, + 0.01201, + 0.4101318734962457 + ], + [ + 0.07532, + 0.02112, + 0.5468424979949943 + ], + [ + 0.09148, + 0.03254, + 0.6835531224937428 + ], + [ + 0.106, + 0.046, + 0.7853981633974483 + ], + [ + 0.12, + 0.06, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 6, + "start_angle_index": 0, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.10854, + "trajectory_length": 0.12309, + "arc_length": 0.12017, + "straight_length": 0.00292, + "poses": [ + [ + 0.02044, + 0.00142, + 0.16210601595321314 + ], + [ + 0.04025, + 0.00662, + 0.35111455632138866 + ], + [ + 0.05874, + 0.01545, + 0.540123096689564 + ], + [ + 0.07523, + 0.0276, + 0.7291316370577395 + ], + [ + 0.08915, + 0.04262, + 0.9181401774259149 + ], + [ + 0.1, + 0.06, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 7, + "start_angle_index": 1, + "end_angle_index": 15, + "left_turn": false, + "trajectory_radius": 0.1118, + "trajectory_length": 0.10367, + "arc_length": 0.10367, + "straight_length": 0.0, + "poses": [ + [ + 0.0193, + 0.0075, + 0.27818856540048365 + ], + [ + 0.03965, + 0.01132, + 0.0927295218001612 + ], + [ + 0.06035, + 0.01132, + 6.190455785379425 + ], + [ + 0.0807, + 0.0075, + 6.004996741779102 + ], + [ + 0.1, + 0.0, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 8, + "start_angle_index": 1, + "end_angle_index": 15, + "left_turn": false, + "trajectory_radius": 0.15652, + "trajectory_length": 0.14514, + "arc_length": 0.14514, + "straight_length": 0.0, + "poses": [ + [ + 0.01911, + 0.00801, + 0.3311768635720044 + ], + [ + 0.0391, + 0.01344, + 0.1987061181432026 + ], + [ + 0.05964, + 0.01618, + 0.06623537271440089 + ], + [ + 0.08036, + 0.01618, + 6.216949934465186 + ], + [ + 0.1009, + 0.01344, + 6.084479189036384 + ], + [ + 0.12089, + 0.00801, + 5.952008443607582 + ], + [ + 0.14, + 0.0, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 9, + "start_angle_index": 1, + "end_angle_index": 0, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 0.4636476090008061 + ], + [ + 0.0, + 0.0, + 0.23182380450040305 + ], + [ + 0.0, + 0.0, + 0.0 + ] + ] + }, + { + "trajectory_id": 10, + "start_angle_index": 1, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.04472, + "arc_length": 0.0, + "straight_length": 0.04472, + "poses": [ + [ + 0.02, + 0.01, + 0.4636476090008061 + ], + [ + 0.04, + 0.02, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 11, + "start_angle_index": 1, + "end_angle_index": 2, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 0.4636476090008061 + ], + [ + 0.0, + 0.0, + 0.6245228861991272 + ], + [ + 0.0, + 0.0, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 12, + "start_angle_index": 1, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.13416, + "trajectory_length": 0.08633, + "arc_length": 0.08633, + "straight_length": 0.0, + "poses": [ + [ + 0.01844, + 0.01116, + 0.6245228861991272 + ], + [ + 0.03487, + 0.02513, + 0.7853981633974483 + ], + [ + 0.04884, + 0.04156, + 0.9462734405957693 + ], + [ + 0.06, + 0.06, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 13, + "start_angle_index": 2, + "end_angle_index": 15, + "left_turn": false, + "trajectory_radius": 0.10342, + "trajectory_length": 0.13006, + "arc_length": 0.12918, + "straight_length": 0.00088, + "poses": [ + [ + 0.01414, + 0.01201, + 0.6142956219961762 + ], + [ + 0.0302, + 0.0213, + 0.4346384168300125 + ], + [ + 0.04766, + 0.02758, + 0.2549812116638488 + ], + [ + 0.06597, + 0.03063, + 0.075324006497685 + ], + [ + 0.08452, + 0.03036, + 6.178852108511108 + ], + [ + 0.10273, + 0.02678, + 5.999194903344944 + ], + [ + 0.12, + 0.02, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 14, + "start_angle_index": 2, + "end_angle_index": 1, + "left_turn": false, + "trajectory_radius": 0.17428, + "trajectory_length": 0.07252, + "arc_length": 0.05607, + "straight_length": 0.01644, + "poses": [ + [ + 0.01347, + 0.01212, + 0.6813758341424526 + ], + [ + 0.02812, + 0.02279, + 0.577353504887457 + ], + [ + 0.04379, + 0.03188, + 0.47333117563246124 + ], + [ + 0.06, + 0.04, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 15, + "start_angle_index": 2, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 0.7853981633974483 + ], + [ + 0.0, + 0.0, + 0.6245228861991272 + ], + [ + 0.0, + 0.0, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 16, + "start_angle_index": 2, + "end_angle_index": 2, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.05657, + "arc_length": 0.0, + "straight_length": 0.05657, + "poses": [ + [ + 0.01333, + 0.01333, + 0.7853981633974483 + ], + [ + 0.02667, + 0.02667, + 0.7853981633974483 + ], + [ + 0.04, + 0.04, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 17, + "start_angle_index": 2, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.17428, + "trajectory_length": 0.07252, + "arc_length": 0.05607, + "straight_length": 0.01644, + "poses": [ + [ + 0.01212, + 0.01347, + 0.8894204926524439 + ], + [ + 0.02279, + 0.02812, + 0.9934428219074396 + ], + [ + 0.03188, + 0.04379, + 1.0974651511624351 + ], + [ + 0.04, + 0.06, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 18, + "start_angle_index": 2, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 0.7853981633974483 + ], + [ + 0.0, + 0.0, + 0.9462734405957693 + ], + [ + 0.0, + 0.0, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 19, + "start_angle_index": 2, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.10342, + "trajectory_length": 0.13006, + "arc_length": 0.12918, + "straight_length": 0.00088, + "poses": [ + [ + 0.01201, + 0.01414, + 0.9565007047987204 + ], + [ + 0.0213, + 0.0302, + 1.1361579099648842 + ], + [ + 0.02758, + 0.04766, + 1.3158151151310478 + ], + [ + 0.03063, + 0.06597, + 1.4954723202972116 + ], + [ + 0.03036, + 0.08452, + 1.6751295254633751 + ], + [ + 0.02678, + 0.10273, + 1.8547867306295387 + ], + [ + 0.02, + 0.12, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 20, + "start_angle_index": 3, + "end_angle_index": 1, + "left_turn": false, + "trajectory_radius": 0.13416, + "trajectory_length": 0.08633, + "arc_length": 0.08633, + "straight_length": 0.0, + "poses": [ + [ + 0.01116, + 0.01844, + 0.9462734405957693 + ], + [ + 0.02513, + 0.03487, + 0.7853981633974483 + ], + [ + 0.04156, + 0.04884, + 0.6245228861991272 + ], + [ + 0.06, + 0.06, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 21, + "start_angle_index": 3, + "end_angle_index": 2, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 1.1071487177940904 + ], + [ + 0.0, + 0.0, + 0.9998985329952097 + ], + [ + 0.0, + 0.0, + 0.892648348196329 + ], + [ + 0.0, + 0.0, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 22, + "start_angle_index": 3, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.04472, + "arc_length": 0.0, + "straight_length": 0.04472, + "poses": [ + [ + 0.01, + 0.02, + 1.1071487177940904 + ], + [ + 0.02, + 0.04, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 23, + "start_angle_index": 3, + "end_angle_index": 4, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 1.1071487177940904 + ], + [ + 0.0, + 0.0, + 1.261697920794359 + ], + [ + 0.0, + 0.0, + 1.416247123794628 + ], + [ + 0.0, + 0.0, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 24, + "start_angle_index": 3, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.1118, + "trajectory_length": 0.10367, + "arc_length": 0.10367, + "straight_length": 0.0, + "poses": [ + [ + 0.0075, + 0.0193, + 1.292607761394413 + ], + [ + 0.01132, + 0.03965, + 1.4780668049947354 + ], + [ + 0.01132, + 0.06035, + 1.663525848595058 + ], + [ + 0.0075, + 0.0807, + 1.8489848921953804 + ], + [ + 0.0, + 0.1, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 25, + "start_angle_index": 3, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.15652, + "trajectory_length": 0.14514, + "arc_length": 0.14514, + "straight_length": 0.0, + "poses": [ + [ + 0.00801, + 0.01911, + 1.239619463222892 + ], + [ + 0.01344, + 0.0391, + 1.372090208651694 + ], + [ + 0.01618, + 0.05964, + 1.5045609540804956 + ], + [ + 0.01618, + 0.08036, + 1.6370316995092975 + ], + [ + 0.01344, + 0.1009, + 1.769502444938099 + ], + [ + 0.00801, + 0.12089, + 1.9019731903669008 + ], + [ + 0.0, + 0.14, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 26, + "start_angle_index": 4, + "end_angle_index": 1, + "left_turn": false, + "trajectory_radius": 0.10854, + "trajectory_length": 0.12309, + "arc_length": 0.12017, + "straight_length": 0.00292, + "poses": [ + [ + 0.00142, + 0.02044, + 1.4086903108416835 + ], + [ + 0.00662, + 0.04025, + 1.219681770473508 + ], + [ + 0.01545, + 0.05874, + 1.0306732301053325 + ], + [ + 0.0276, + 0.07523, + 0.841664689737157 + ], + [ + 0.04262, + 0.08915, + 0.6526561493689816 + ], + [ + 0.06, + 0.1, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 27, + "start_angle_index": 4, + "end_angle_index": 2, + "left_turn": false, + "trajectory_radius": 0.14485, + "trajectory_length": 0.13862, + "arc_length": 0.11376, + "straight_length": 0.02485, + "poses": [ + [ + 0.00135, + 0.01974, + 1.434085702296148 + ], + [ + 0.00538, + 0.03911, + 1.2973750777973994 + ], + [ + 0.01201, + 0.05776, + 1.160664453298651 + ], + [ + 0.02112, + 0.07532, + 1.0239538287999024 + ], + [ + 0.03254, + 0.09148, + 0.887243204301154 + ], + [ + 0.046, + 0.106, + 0.7853981633974483 + ], + [ + 0.06, + 0.12, + 0.7853981633974483 + ] + ] + }, + { + "trajectory_id": 28, + "start_angle_index": 4, + "end_angle_index": 3, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 1.5707963267948966 + ], + [ + 0.0, + 0.0, + 1.416247123794628 + ], + [ + 0.0, + 0.0, + 1.261697920794359 + ], + [ + 0.0, + 0.0, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 29, + "start_angle_index": 4, + "end_angle_index": 4, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.04, + "arc_length": 0.0, + "straight_length": 0.04, + "poses": [ + [ + 0.0, + 0.02, + 1.5707963267948966 + ], + [ + 0.0, + 0.04, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 30, + "start_angle_index": 4, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 1.5707963267948966 + ], + [ + 0.0, + 0.0, + 1.7253455297951652 + ], + [ + 0.0, + 0.0, + 1.879894732795434 + ], + [ + 0.0, + 0.0, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 31, + "start_angle_index": 4, + "end_angle_index": 6, + "left_turn": true, + "trajectory_radius": 0.14485, + "trajectory_length": 0.13862, + "arc_length": 0.11376, + "straight_length": 0.02485, + "poses": [ + [ + -0.00135, + 0.01974, + 1.7075069512936452 + ], + [ + -0.00538, + 0.03911, + 1.8442175757923938 + ], + [ + -0.01201, + 0.05776, + 1.9809282002911424 + ], + [ + -0.02112, + 0.07532, + 2.117638824789891 + ], + [ + -0.03254, + 0.09148, + 2.2543494492886396 + ], + [ + -0.046, + 0.106, + 2.356194490192345 + ], + [ + -0.06, + 0.12, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 32, + "start_angle_index": 4, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.10854, + "trajectory_length": 0.12309, + "arc_length": 0.12017, + "straight_length": 0.00292, + "poses": [ + [ + -0.00142, + 0.02044, + 1.7329023427481096 + ], + [ + -0.00662, + 0.04025, + 1.9219108831162852 + ], + [ + -0.01545, + 0.05874, + 2.110919423484461 + ], + [ + -0.0276, + 0.07523, + 2.299927963852636 + ], + [ + -0.04262, + 0.08915, + 2.4889365042208116 + ], + [ + -0.06, + 0.1, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 33, + "start_angle_index": 5, + "end_angle_index": 3, + "left_turn": false, + "trajectory_radius": 0.1118, + "trajectory_length": 0.10367, + "arc_length": 0.10367, + "straight_length": 0.0, + "poses": [ + [ + -0.0075, + 0.0193, + 1.8489848921953802 + ], + [ + -0.01132, + 0.03965, + 1.6635258485950577 + ], + [ + -0.01132, + 0.06035, + 1.4780668049947352 + ], + [ + -0.0075, + 0.0807, + 1.2926077613944127 + ], + [ + 0.0, + 0.1, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 34, + "start_angle_index": 5, + "end_angle_index": 3, + "left_turn": false, + "trajectory_radius": 0.15652, + "trajectory_length": 0.14514, + "arc_length": 0.14514, + "straight_length": 0.0, + "poses": [ + [ + -0.00801, + 0.01911, + 1.901973190366901 + ], + [ + -0.01344, + 0.0391, + 1.7695024449380992 + ], + [ + -0.01618, + 0.05964, + 1.6370316995092975 + ], + [ + -0.01618, + 0.08036, + 1.5045609540804956 + ], + [ + -0.01344, + 0.1009, + 1.3720902086516942 + ], + [ + -0.00801, + 0.12089, + 1.2396194632228923 + ], + [ + 0.0, + 0.14, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 35, + "start_angle_index": 5, + "end_angle_index": 4, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 2.0344439357957027 + ], + [ + 0.0, + 0.0, + 1.8026201312952996 + ], + [ + 0.0, + 0.0, + 1.5707963267948966 + ] + ] + }, + { + "trajectory_id": 36, + "start_angle_index": 5, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.04472, + "arc_length": 0.0, + "straight_length": 0.04472, + "poses": [ + [ + -0.01, + 0.02, + 2.0344439357957027 + ], + [ + -0.02, + 0.04, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 37, + "start_angle_index": 5, + "end_angle_index": 6, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 2.0344439357957027 + ], + [ + 0.0, + 0.0, + 2.1953192129940238 + ], + [ + 0.0, + 0.0, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 38, + "start_angle_index": 5, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.13416, + "trajectory_length": 0.08633, + "arc_length": 0.08633, + "straight_length": 0.0, + "poses": [ + [ + -0.01116, + 0.01844, + 2.1953192129940238 + ], + [ + -0.02513, + 0.03487, + 2.356194490192345 + ], + [ + -0.04156, + 0.04884, + 2.517069767390666 + ], + [ + -0.06, + 0.06, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 39, + "start_angle_index": 6, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.10342, + "trajectory_length": 0.13006, + "arc_length": 0.12918, + "straight_length": 0.00088, + "poses": [ + [ + -0.01414, + 0.01201, + 2.527297031593617 + ], + [ + -0.0302, + 0.0213, + 2.7069542367597808 + ], + [ + -0.04766, + 0.02758, + 2.886611441925944 + ], + [ + -0.06597, + 0.03063, + 3.066268647092108 + ], + [ + -0.08452, + 0.03036, + 3.2459258522582717 + ], + [ + -0.10273, + 0.02678, + 3.4255830574244355 + ], + [ + -0.12, + 0.02, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 40, + "start_angle_index": 6, + "end_angle_index": 3, + "left_turn": false, + "trajectory_radius": 0.10342, + "trajectory_length": 0.13006, + "arc_length": 0.12918, + "straight_length": 0.00088, + "poses": [ + [ + -0.01201, + 0.01414, + 2.1850919487910727 + ], + [ + -0.0213, + 0.0302, + 2.005434743624909 + ], + [ + -0.02758, + 0.04766, + 1.8257775384587454 + ], + [ + -0.03063, + 0.06597, + 1.6461203332925816 + ], + [ + -0.03036, + 0.08452, + 1.466463128126418 + ], + [ + -0.02678, + 0.10273, + 1.2868059229602544 + ], + [ + -0.02, + 0.12, + 1.1071487177940904 + ] + ] + }, + { + "trajectory_id": 41, + "start_angle_index": 6, + "end_angle_index": 5, + "left_turn": false, + "trajectory_radius": 0.17428, + "trajectory_length": 0.07252, + "arc_length": 0.05607, + "straight_length": 0.01644, + "poses": [ + [ + -0.01212, + 0.01347, + 2.252172160937349 + ], + [ + -0.02279, + 0.02812, + 2.1481498316823537 + ], + [ + -0.03188, + 0.04379, + 2.044127502427358 + ], + [ + -0.04, + 0.06, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 42, + "start_angle_index": 6, + "end_angle_index": 5, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 2.356194490192345 + ], + [ + 0.0, + 0.0, + 2.1953192129940238 + ], + [ + 0.0, + 0.0, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 43, + "start_angle_index": 6, + "end_angle_index": 6, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.05657, + "arc_length": 0.0, + "straight_length": 0.05657, + "poses": [ + [ + -0.01333, + 0.01333, + 2.356194490192345 + ], + [ + -0.02667, + 0.02667, + 2.356194490192345 + ], + [ + -0.04, + 0.04, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 44, + "start_angle_index": 6, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.17428, + "trajectory_length": 0.07252, + "arc_length": 0.05607, + "straight_length": 0.01644, + "poses": [ + [ + -0.01347, + 0.01212, + 2.4602168194473406 + ], + [ + -0.02812, + 0.02279, + 2.564239148702336 + ], + [ + -0.04379, + 0.03188, + 2.6682614779573317 + ], + [ + -0.06, + 0.04, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 45, + "start_angle_index": 6, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 2.356194490192345 + ], + [ + 0.0, + 0.0, + 2.517069767390666 + ], + [ + 0.0, + 0.0, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 46, + "start_angle_index": 7, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.1118, + "trajectory_length": 0.10367, + "arc_length": 0.10367, + "straight_length": 0.0, + "poses": [ + [ + -0.0193, + 0.0075, + 2.8634040881893092 + ], + [ + -0.03965, + 0.01132, + 3.048863131789632 + ], + [ + -0.06035, + 0.01132, + 3.2343221753899547 + ], + [ + -0.0807, + 0.0075, + 3.419781218990277 + ], + [ + -0.1, + 0.0, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 47, + "start_angle_index": 7, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.15652, + "trajectory_length": 0.14514, + "arc_length": 0.14514, + "straight_length": 0.0, + "poses": [ + [ + -0.01911, + 0.00801, + 2.810415790017789 + ], + [ + -0.0391, + 0.01344, + 2.9428865354465903 + ], + [ + -0.05964, + 0.01618, + 3.075357280875392 + ], + [ + -0.08036, + 0.01618, + 3.207828026304194 + ], + [ + -0.1009, + 0.01344, + 3.3402987717329955 + ], + [ + -0.12089, + 0.00801, + 3.4727695171617974 + ], + [ + -0.14, + 0.0, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 48, + "start_angle_index": 7, + "end_angle_index": 5, + "left_turn": false, + "trajectory_radius": 0.13416, + "trajectory_length": 0.08633, + "arc_length": 0.08633, + "straight_length": 0.0, + "poses": [ + [ + -0.01844, + 0.01116, + 2.517069767390666 + ], + [ + -0.03487, + 0.02513, + 2.356194490192345 + ], + [ + -0.04884, + 0.04156, + 2.1953192129940238 + ], + [ + -0.06, + 0.06, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 49, + "start_angle_index": 7, + "end_angle_index": 6, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 2.677945044588987 + ], + [ + 0.0, + 0.0, + 2.5706948597901063 + ], + [ + 0.0, + 0.0, + 2.4634446749912255 + ], + [ + 0.0, + 0.0, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 50, + "start_angle_index": 7, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.04472, + "arc_length": 0.0, + "straight_length": 0.04472, + "poses": [ + [ + -0.02, + 0.01, + 2.677945044588987 + ], + [ + -0.04, + 0.02, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 51, + "start_angle_index": 7, + "end_angle_index": 8, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 2.677945044588987 + ], + [ + 0.0, + 0.0, + 2.832494247589256 + ], + [ + 0.0, + 0.0, + 2.9870434505895243 + ], + [ + 0.0, + 0.0, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 52, + "start_angle_index": 8, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 3.141592653589793 + ], + [ + 0.0, + 0.0, + 3.296141856590062 + ], + [ + 0.0, + 0.0, + 3.4506910595903304 + ], + [ + 0.0, + 0.0, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 53, + "start_angle_index": 8, + "end_angle_index": 10, + "left_turn": true, + "trajectory_radius": 0.14485, + "trajectory_length": 0.13862, + "arc_length": 0.11376, + "straight_length": 0.02485, + "poses": [ + [ + -0.01974, + -0.00135, + 3.2783032780885417 + ], + [ + -0.03911, + -0.00538, + 3.4150139025872903 + ], + [ + -0.05776, + -0.01201, + 3.551724527086039 + ], + [ + -0.07532, + -0.02112, + 3.6884351515847875 + ], + [ + -0.09148, + -0.03254, + 3.825145776083536 + ], + [ + -0.106, + -0.046, + 3.9269908169872414 + ], + [ + -0.12, + -0.06, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 54, + "start_angle_index": 8, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.10854, + "trajectory_length": 0.12309, + "arc_length": 0.12017, + "straight_length": 0.00292, + "poses": [ + [ + -0.02044, + -0.00142, + 3.303698669543006 + ], + [ + -0.04025, + -0.00662, + 3.492707209911182 + ], + [ + -0.05874, + -0.01545, + 3.6817157502793574 + ], + [ + -0.07523, + -0.0276, + 3.8707242906475328 + ], + [ + -0.08915, + -0.04262, + 4.059732831015708 + ], + [ + -0.1, + -0.06, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 55, + "start_angle_index": 8, + "end_angle_index": 5, + "left_turn": false, + "trajectory_radius": 0.10854, + "trajectory_length": 0.12309, + "arc_length": 0.12017, + "straight_length": 0.00292, + "poses": [ + [ + -0.02044, + 0.00142, + 2.97948663763658 + ], + [ + -0.04025, + 0.00662, + 2.7904780972684042 + ], + [ + -0.05874, + 0.01545, + 2.601469556900229 + ], + [ + -0.07523, + 0.0276, + 2.4124610165320535 + ], + [ + -0.08915, + 0.04262, + 2.223452476163878 + ], + [ + -0.1, + 0.06, + 2.0344439357957027 + ] + ] + }, + { + "trajectory_id": 56, + "start_angle_index": 8, + "end_angle_index": 6, + "left_turn": false, + "trajectory_radius": 0.14485, + "trajectory_length": 0.13862, + "arc_length": 0.11376, + "straight_length": 0.02485, + "poses": [ + [ + -0.01974, + 0.00135, + 3.0048820290910445 + ], + [ + -0.03911, + 0.00538, + 2.868171404592296 + ], + [ + -0.05776, + 0.01201, + 2.7314607800935473 + ], + [ + -0.07532, + 0.02112, + 2.5947501555947987 + ], + [ + -0.09148, + 0.03254, + 2.45803953109605 + ], + [ + -0.106, + 0.046, + 2.356194490192345 + ], + [ + -0.12, + 0.06, + 2.356194490192345 + ] + ] + }, + { + "trajectory_id": 57, + "start_angle_index": 8, + "end_angle_index": 7, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 3.141592653589793 + ], + [ + 0.0, + 0.0, + 2.9870434505895243 + ], + [ + 0.0, + 0.0, + 2.832494247589256 + ], + [ + 0.0, + 0.0, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 58, + "start_angle_index": 8, + "end_angle_index": 8, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.04, + "arc_length": 0.0, + "straight_length": 0.04, + "poses": [ + [ + -0.02, + 0.0, + 3.141592653589793 + ], + [ + -0.04, + 0.0, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 59, + "start_angle_index": 9, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.04472, + "arc_length": 0.0, + "straight_length": 0.04472, + "poses": [ + [ + -0.02, + -0.01, + 3.6052402625905993 + ], + [ + -0.04, + -0.02, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 60, + "start_angle_index": 9, + "end_angle_index": 10, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 3.6052402625905993 + ], + [ + 0.0, + 0.0, + 3.7661155397889203 + ], + [ + 0.0, + 0.0, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 61, + "start_angle_index": 9, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.13416, + "trajectory_length": 0.08633, + "arc_length": 0.08633, + "straight_length": 0.0, + "poses": [ + [ + -0.01844, + -0.01116, + 3.7661155397889203 + ], + [ + -0.03487, + -0.02513, + 3.9269908169872414 + ], + [ + -0.04884, + -0.04156, + 4.0878660941855625 + ], + [ + -0.06, + -0.06, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 62, + "start_angle_index": 9, + "end_angle_index": 7, + "left_turn": false, + "trajectory_radius": 0.1118, + "trajectory_length": 0.10367, + "arc_length": 0.10367, + "straight_length": 0.0, + "poses": [ + [ + -0.0193, + -0.0075, + 3.419781218990277 + ], + [ + -0.03965, + -0.01132, + 3.2343221753899543 + ], + [ + -0.06035, + -0.01132, + 3.0488631317896315 + ], + [ + -0.0807, + -0.0075, + 2.8634040881893092 + ], + [ + -0.1, + 0.0, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 63, + "start_angle_index": 9, + "end_angle_index": 7, + "left_turn": false, + "trajectory_radius": 0.15652, + "trajectory_length": 0.14514, + "arc_length": 0.14514, + "straight_length": 0.0, + "poses": [ + [ + -0.01911, + -0.00801, + 3.4727695171617974 + ], + [ + -0.0391, + -0.01344, + 3.340298771732996 + ], + [ + -0.05964, + -0.01618, + 3.207828026304194 + ], + [ + -0.08036, + -0.01618, + 3.075357280875392 + ], + [ + -0.1009, + -0.01344, + 2.9428865354465907 + ], + [ + -0.12089, + -0.00801, + 2.810415790017789 + ], + [ + -0.14, + 0.0, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 64, + "start_angle_index": 9, + "end_angle_index": 8, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 3.6052402625905993 + ], + [ + 0.0, + 0.0, + 3.373416458090196 + ], + [ + 0.0, + 0.0, + 3.141592653589793 + ] + ] + }, + { + "trajectory_id": 65, + "start_angle_index": 10, + "end_angle_index": 9, + "left_turn": false, + "trajectory_radius": 0.17428, + "trajectory_length": 0.07252, + "arc_length": 0.05607, + "straight_length": 0.01644, + "poses": [ + [ + -0.01347, + -0.01212, + 3.8229684877322456 + ], + [ + -0.02812, + -0.02279, + 3.7189461584772503 + ], + [ + -0.04379, + -0.03188, + 3.6149238292222545 + ], + [ + -0.06, + -0.04, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 66, + "start_angle_index": 10, + "end_angle_index": 9, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 3.9269908169872414 + ], + [ + 0.0, + 0.0, + 3.7661155397889203 + ], + [ + 0.0, + 0.0, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 67, + "start_angle_index": 10, + "end_angle_index": 10, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.05657, + "arc_length": 0.0, + "straight_length": 0.05657, + "poses": [ + [ + -0.01333, + -0.01333, + 3.9269908169872414 + ], + [ + -0.02667, + -0.02667, + 3.9269908169872414 + ], + [ + -0.04, + -0.04, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 68, + "start_angle_index": 10, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.17428, + "trajectory_length": 0.07252, + "arc_length": 0.05607, + "straight_length": 0.01644, + "poses": [ + [ + -0.01212, + -0.01347, + 4.031013146242238 + ], + [ + -0.02279, + -0.02812, + 4.135035475497233 + ], + [ + -0.03188, + -0.04379, + 4.239057804752228 + ], + [ + -0.04, + -0.06, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 69, + "start_angle_index": 10, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 3.9269908169872414 + ], + [ + 0.0, + 0.0, + 4.0878660941855625 + ], + [ + 0.0, + 0.0, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 70, + "start_angle_index": 10, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.10342, + "trajectory_length": 0.13006, + "arc_length": 0.12918, + "straight_length": 0.00088, + "poses": [ + [ + -0.01201, + -0.01414, + 4.098093358388514 + ], + [ + -0.0213, + -0.0302, + 4.277750563554678 + ], + [ + -0.02758, + -0.04766, + 4.457407768720841 + ], + [ + -0.03063, + -0.06597, + 4.6370649738870044 + ], + [ + -0.03036, + -0.08452, + 4.816722179053168 + ], + [ + -0.02678, + -0.10273, + 4.996379384219332 + ], + [ + -0.02, + -0.12, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 71, + "start_angle_index": 10, + "end_angle_index": 7, + "left_turn": false, + "trajectory_radius": 0.10342, + "trajectory_length": 0.13006, + "arc_length": 0.12918, + "straight_length": 0.00088, + "poses": [ + [ + -0.01414, + -0.01201, + 3.7558882755859693 + ], + [ + -0.0302, + -0.0213, + 3.5762310704198055 + ], + [ + -0.04766, + -0.02758, + 3.396573865253642 + ], + [ + -0.06597, + -0.03063, + 3.2169166600874783 + ], + [ + -0.08452, + -0.03036, + 3.0372594549213145 + ], + [ + -0.10273, + -0.02678, + 2.8576022497551508 + ], + [ + -0.12, + -0.02, + 2.677945044588987 + ] + ] + }, + { + "trajectory_id": 72, + "start_angle_index": 11, + "end_angle_index": 9, + "left_turn": false, + "trajectory_radius": 0.13416, + "trajectory_length": 0.08633, + "arc_length": 0.08633, + "straight_length": 0.0, + "poses": [ + [ + -0.01116, + -0.01844, + 4.0878660941855625 + ], + [ + -0.02513, + -0.03487, + 3.9269908169872414 + ], + [ + -0.04156, + -0.04884, + 3.7661155397889203 + ], + [ + -0.06, + -0.06, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 73, + "start_angle_index": 11, + "end_angle_index": 10, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 4.2487413713838835 + ], + [ + 0.0, + 0.0, + 4.141491186585003 + ], + [ + 0.0, + 0.0, + 4.034241001786122 + ], + [ + 0.0, + 0.0, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 74, + "start_angle_index": 11, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.04472, + "arc_length": 0.0, + "straight_length": 0.04472, + "poses": [ + [ + -0.01, + -0.02, + 4.2487413713838835 + ], + [ + -0.02, + -0.04, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 75, + "start_angle_index": 11, + "end_angle_index": 12, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 4.2487413713838835 + ], + [ + 0.0, + 0.0, + 4.403290574384152 + ], + [ + 0.0, + 0.0, + 4.557839777384421 + ], + [ + 0.0, + 0.0, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 76, + "start_angle_index": 11, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.1118, + "trajectory_length": 0.10367, + "arc_length": 0.10367, + "straight_length": 0.0, + "poses": [ + [ + -0.0075, + -0.0193, + 4.434200414984206 + ], + [ + -0.01132, + -0.03965, + 4.619659458584529 + ], + [ + -0.01132, + -0.06035, + 4.805118502184851 + ], + [ + -0.0075, + -0.0807, + 4.9905775457851735 + ], + [ + 0.0, + -0.1, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 77, + "start_angle_index": 11, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.15652, + "trajectory_length": 0.14514, + "arc_length": 0.14514, + "straight_length": 0.0, + "poses": [ + [ + -0.00801, + -0.01911, + 4.381212116812685 + ], + [ + -0.01344, + -0.0391, + 4.513682862241487 + ], + [ + -0.01618, + -0.05964, + 4.646153607670289 + ], + [ + -0.01618, + -0.08036, + 4.77862435309909 + ], + [ + -0.01344, + -0.1009, + 4.911095098527892 + ], + [ + -0.00801, + -0.12089, + 5.043565843956694 + ], + [ + 0.0, + -0.14, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 78, + "start_angle_index": 12, + "end_angle_index": 9, + "left_turn": false, + "trajectory_radius": 0.10854, + "trajectory_length": 0.12309, + "arc_length": 0.12017, + "straight_length": 0.00292, + "poses": [ + [ + -0.00142, + -0.02044, + 4.550282964431476 + ], + [ + -0.00662, + -0.04025, + 4.361274424063301 + ], + [ + -0.01545, + -0.05874, + 4.172265883695125 + ], + [ + -0.0276, + -0.07523, + 3.98325734332695 + ], + [ + -0.04262, + -0.08915, + 3.7942488029587746 + ], + [ + -0.06, + -0.1, + 3.6052402625905993 + ] + ] + }, + { + "trajectory_id": 79, + "start_angle_index": 12, + "end_angle_index": 10, + "left_turn": false, + "trajectory_radius": 0.14485, + "trajectory_length": 0.13862, + "arc_length": 0.11376, + "straight_length": 0.02485, + "poses": [ + [ + -0.00135, + -0.01974, + 4.575678355885941 + ], + [ + -0.00538, + -0.03911, + 4.4389677313871925 + ], + [ + -0.01201, + -0.05776, + 4.302257106888444 + ], + [ + -0.02112, + -0.07532, + 4.165546482389695 + ], + [ + -0.03254, + -0.09148, + 4.028835857890947 + ], + [ + -0.046, + -0.106, + 3.9269908169872414 + ], + [ + -0.06, + -0.12, + 3.9269908169872414 + ] + ] + }, + { + "trajectory_id": 80, + "start_angle_index": 12, + "end_angle_index": 11, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 4.71238898038469 + ], + [ + 0.0, + 0.0, + 4.557839777384421 + ], + [ + 0.0, + 0.0, + 4.403290574384152 + ], + [ + 0.0, + 0.0, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 81, + "start_angle_index": 12, + "end_angle_index": 12, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.04, + "arc_length": 0.0, + "straight_length": 0.04, + "poses": [ + [ + 0.0, + -0.02, + 4.71238898038469 + ], + [ + 0.0, + -0.04, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 82, + "start_angle_index": 12, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 4.71238898038469 + ], + [ + 0.0, + 0.0, + 4.866938183384958 + ], + [ + 0.0, + 0.0, + 5.021487386385227 + ], + [ + 0.0, + 0.0, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 83, + "start_angle_index": 12, + "end_angle_index": 14, + "left_turn": true, + "trajectory_radius": 0.14485, + "trajectory_length": 0.13862, + "arc_length": 0.11376, + "straight_length": 0.02485, + "poses": [ + [ + 0.00135, + -0.01974, + 4.849099604883438 + ], + [ + 0.00538, + -0.03911, + 4.985810229382187 + ], + [ + 0.01201, + -0.05776, + 5.1225208538809355 + ], + [ + 0.02112, + -0.07532, + 5.259231478379684 + ], + [ + 0.03254, + -0.09148, + 5.395942102878433 + ], + [ + 0.046, + -0.106, + 5.497787143782138 + ], + [ + 0.06, + -0.12, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 84, + "start_angle_index": 12, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.10854, + "trajectory_length": 0.12309, + "arc_length": 0.12017, + "straight_length": 0.00292, + "poses": [ + [ + 0.00142, + -0.02044, + 4.874494996337903 + ], + [ + 0.00662, + -0.04025, + 5.063503536706079 + ], + [ + 0.01545, + -0.05874, + 5.252512077074254 + ], + [ + 0.0276, + -0.07523, + 5.441520617442429 + ], + [ + 0.04262, + -0.08915, + 5.630529157810605 + ], + [ + 0.06, + -0.1, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 85, + "start_angle_index": 13, + "end_angle_index": 11, + "left_turn": false, + "trajectory_radius": 0.1118, + "trajectory_length": 0.10367, + "arc_length": 0.10367, + "straight_length": 0.0, + "poses": [ + [ + 0.0075, + -0.0193, + 4.9905775457851735 + ], + [ + 0.01132, + -0.03965, + 4.80511850218485 + ], + [ + 0.01132, + -0.06035, + 4.619659458584528 + ], + [ + 0.0075, + -0.0807, + 4.434200414984206 + ], + [ + 0.0, + -0.1, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 86, + "start_angle_index": 13, + "end_angle_index": 11, + "left_turn": false, + "trajectory_radius": 0.15652, + "trajectory_length": 0.14514, + "arc_length": 0.14514, + "straight_length": 0.0, + "poses": [ + [ + 0.00801, + -0.01911, + 5.043565843956694 + ], + [ + 0.01344, + -0.0391, + 4.911095098527892 + ], + [ + 0.01618, + -0.05964, + 4.77862435309909 + ], + [ + 0.01618, + -0.08036, + 4.646153607670289 + ], + [ + 0.01344, + -0.1009, + 4.513682862241487 + ], + [ + 0.00801, + -0.12089, + 4.381212116812685 + ], + [ + 0.0, + -0.14, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 87, + "start_angle_index": 13, + "end_angle_index": 12, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 5.176036589385496 + ], + [ + 0.0, + 0.0, + 4.944212784885092 + ], + [ + 0.0, + 0.0, + 4.71238898038469 + ] + ] + }, + { + "trajectory_id": 88, + "start_angle_index": 13, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.04472, + "arc_length": 0.0, + "straight_length": 0.04472, + "poses": [ + [ + 0.01, + -0.02, + 5.176036589385496 + ], + [ + 0.02, + -0.04, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 89, + "start_angle_index": 13, + "end_angle_index": 14, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 5.176036589385496 + ], + [ + 0.0, + 0.0, + 5.336911866583817 + ], + [ + 0.0, + 0.0, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 90, + "start_angle_index": 13, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.13416, + "trajectory_length": 0.08633, + "arc_length": 0.08633, + "straight_length": 0.0, + "poses": [ + [ + 0.01116, + -0.01844, + 5.336911866583817 + ], + [ + 0.02513, + -0.03487, + 5.497787143782138 + ], + [ + 0.04156, + -0.04884, + 5.658662420980459 + ], + [ + 0.06, + -0.06, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 91, + "start_angle_index": 14, + "end_angle_index": 11, + "left_turn": false, + "trajectory_radius": 0.10342, + "trajectory_length": 0.13006, + "arc_length": 0.12918, + "straight_length": 0.00088, + "poses": [ + [ + 0.01201, + -0.01414, + 5.326684602380865 + ], + [ + 0.0213, + -0.0302, + 5.147027397214702 + ], + [ + 0.02758, + -0.04766, + 4.967370192048539 + ], + [ + 0.03063, + -0.06597, + 4.787712986882375 + ], + [ + 0.03036, + -0.08452, + 4.608055781716211 + ], + [ + 0.02678, + -0.10273, + 4.428398576550047 + ], + [ + 0.02, + -0.12, + 4.2487413713838835 + ] + ] + }, + { + "trajectory_id": 92, + "start_angle_index": 14, + "end_angle_index": 13, + "left_turn": false, + "trajectory_radius": 0.17428, + "trajectory_length": 0.07252, + "arc_length": 0.05607, + "straight_length": 0.01644, + "poses": [ + [ + 0.01212, + -0.01347, + 5.393764814527143 + ], + [ + 0.02279, + -0.02812, + 5.289742485272146 + ], + [ + 0.03188, + -0.04379, + 5.185720156017151 + ], + [ + 0.04, + -0.06, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 93, + "start_angle_index": 14, + "end_angle_index": 13, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 5.497787143782138 + ], + [ + 0.0, + 0.0, + 5.336911866583817 + ], + [ + 0.0, + 0.0, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 94, + "start_angle_index": 14, + "end_angle_index": 14, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.05657, + "arc_length": 0.0, + "straight_length": 0.05657, + "poses": [ + [ + 0.01333, + -0.01333, + 5.497787143782138 + ], + [ + 0.02667, + -0.02667, + 5.497787143782138 + ], + [ + 0.04, + -0.04, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 95, + "start_angle_index": 14, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.17428, + "trajectory_length": 0.07252, + "arc_length": 0.05607, + "straight_length": 0.01644, + "poses": [ + [ + 0.01347, + -0.01212, + 5.601809473037133 + ], + [ + 0.02812, + -0.02279, + 5.7058318022921295 + ], + [ + 0.04379, + -0.03188, + 5.809854131547125 + ], + [ + 0.06, + -0.04, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 96, + "start_angle_index": 14, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 5.497787143782138 + ], + [ + 0.0, + 0.0, + 5.658662420980459 + ], + [ + 0.0, + 0.0, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 97, + "start_angle_index": 14, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.10342, + "trajectory_length": 0.13006, + "arc_length": 0.12918, + "straight_length": 0.00088, + "poses": [ + [ + 0.01414, + -0.01201, + 5.6688896851834105 + ], + [ + 0.0302, + -0.0213, + 5.848546890349573 + ], + [ + 0.04766, + -0.02758, + 6.028204095515737 + ], + [ + 0.06597, + -0.03063, + 6.207861300681901 + ], + [ + 0.08452, + -0.03036, + 0.10433319866847846 + ], + [ + 0.10273, + -0.02678, + 0.28399040383464214 + ], + [ + 0.12, + -0.02, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 98, + "start_angle_index": 15, + "end_angle_index": 13, + "left_turn": false, + "trajectory_radius": 0.13416, + "trajectory_length": 0.08633, + "arc_length": 0.08633, + "straight_length": 0.0, + "poses": [ + [ + 0.01844, + -0.01116, + 5.658662420980459 + ], + [ + 0.03487, + -0.02513, + 5.497787143782138 + ], + [ + 0.04884, + -0.04156, + 5.336911866583817 + ], + [ + 0.06, + -0.06, + 5.176036589385496 + ] + ] + }, + { + "trajectory_id": 99, + "start_angle_index": 15, + "end_angle_index": 14, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 5.81953769817878 + ], + [ + 0.0, + 0.0, + 5.712287513379899 + ], + [ + 0.0, + 0.0, + 5.605037328581019 + ], + [ + 0.0, + 0.0, + 5.497787143782138 + ] + ] + }, + { + "trajectory_id": 100, + "start_angle_index": 15, + "end_angle_index": 15, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.04472, + "arc_length": 0.0, + "straight_length": 0.04472, + "poses": [ + [ + 0.02, + -0.01, + 5.81953769817878 + ], + [ + 0.04, + -0.02, + 5.81953769817878 + ] + ] + }, + { + "trajectory_id": 101, + "start_angle_index": 15, + "end_angle_index": 0, + "left_turn": true, + "trajectory_radius": 0.0, + "trajectory_length": 0.0, + "arc_length": 0.0, + "straight_length": 0.0, + "poses": [ + [ + 0.0, + 0.0, + 5.81953769817878 + ], + [ + 0.0, + 0.0, + 5.9740869011790485 + ], + [ + 0.0, + 0.0, + 6.128636104179318 + ], + [ + 0.0, + 0.0, + 0.0 + ] + ] + }, + { + "trajectory_id": 102, + "start_angle_index": 15, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.1118, + "trajectory_length": 0.10367, + "arc_length": 0.10367, + "straight_length": 0.0, + "poses": [ + [ + 0.0193, + -0.0075, + 6.004996741779102 + ], + [ + 0.03965, + -0.01132, + 6.190455785379425 + ], + [ + 0.06035, + -0.01132, + 0.09272952180016131 + ], + [ + 0.0807, + -0.0075, + 0.2781885654004837 + ], + [ + 0.1, + 0.0, + 0.4636476090008061 + ] + ] + }, + { + "trajectory_id": 103, + "start_angle_index": 15, + "end_angle_index": 1, + "left_turn": true, + "trajectory_radius": 0.15652, + "trajectory_length": 0.14514, + "arc_length": 0.14514, + "straight_length": 0.0, + "poses": [ + [ + 0.01911, + -0.00801, + 5.952008443607582 + ], + [ + 0.0391, + -0.01344, + 6.084479189036384 + ], + [ + 0.05964, + -0.01618, + 6.216949934465186 + ], + [ + 0.08036, + -0.01618, + 0.06623537271440089 + ], + [ + 0.1009, + -0.01344, + 0.19870611814320255 + ], + [ + 0.12089, + -0.00801, + 0.3311768635720042 + ], + [ + 0.14, + 0.0, + 0.4636476090008061 + ] + ] + } + ] +} \ No newline at end of file diff --git a/toid_navigation/params/toid_general_params.yaml b/toid_navigation/params/toid_general_params.yaml index 9fc95f6..7e91c2c 100644 --- a/toid_navigation/params/toid_general_params.yaml +++ b/toid_navigation/params/toid_general_params.yaml @@ -22,22 +22,25 @@ behavior_server: local_footprint_topic: local_costmap/published_footprint global_footprint_topic: global_costmap/published_footprint cycle_frequency: 10.0 - behavior_plugins: ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + behavior_plugins: ["backup", "spin", "drive_on_heading", "assisted_teleop", "wait"] spin: plugin: "nav2_behaviors::Spin" backup: plugin: "nav2_behaviors::BackUp" + enable_stamped_cmd_vel: true + simulate_ahead_time: 0.2 drive_on_heading: plugin: "nav2_behaviors::DriveOnHeading" wait: plugin: "nav2_behaviors::Wait" assisted_teleop: plugin: "nav2_behaviors::AssistedTeleop" - local_frame: odom + local_frame: map global_frame: map robot_base_frame: base_footprint + enable_stamped_cmd_vel: true transform_tolerance: 0.1 - simulate_ahead_time: 2.0 + simulate_ahead_time: 0.2 max_rotational_vel: 1.0 min_rotational_vel: 0.1 rotational_acc_lim: 3.2 @@ -45,22 +48,23 @@ behavior_server: global_costmap: global_costmap: ros__parameters: - update_frequency: 1.0 + update_frequency: 5.0 publish_frequency: 1.0 global_frame: map robot_base_frame: base_footprint - robot_radius: 0.18 + footprint: "[[-0.08, 0.15], [0.22, 0.15], [0.22, -0.15], [-0.08, -0.15]]" + footprint_padding: 0.02 track_unknown_space: false rolling_window: false plugins: ["static_layer", "inflation_layer"] static_layer: plugin: "nav2_costmap_2d::StaticLayer" - footprint_clearing_enabled: false + footprint_clearing_enabled: true map_subscribe_transient_local: True inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" - footprint_clearing_enabled: false - cost_scaling_factor: 12.0 + footprint_clearing_enabled: true + cost_scaling_factor: 6.0 inflation_radius: 0.40 always_send_full_costmap: True @@ -69,8 +73,8 @@ local_costmap: ros__parameters: update_frequency: 5.0 publish_frequency: 2.0 - footprint: "[[-0.13, 0.16], [0.13, 0.16], [0.13, -0.16], [-0.13, -0.16]]" - footprint_padding: 0.02 + footprint: "[[-0.08, 0.15], [0.22, 0.15], [0.22, -0.15], [-0.08, -0.15]]" + footprint_padding: 0.01 #robot_radius: 0.18 global_frame: map robot_base_frame: base_footprint @@ -82,25 +86,57 @@ local_costmap: plugins: ["static_layer", "inflation_layer"] static_layer: plugin: "nav2_costmap_2d::StaticLayer" + footprint_clearing_enabled: true map_subscribe_transient_local: True inflation_layer: plugin: "nav2_costmap_2d::InflationLayer" - cost_scaling_factor: 10.0 - inflation_radius: 0.45 + cost_scaling_factor: 3.0 + inflation_radius: 0.40 planner_server: ros__parameters: allow_partial_planning: false - expected_planner_frequency: 20.0 + expected_planner_frequency: 0.02 costmap_update_timeout: 1.0 introspection_mode: "disabled" planner_plugins: ['GridBased'] GridBased: - plugin: 'nav2_navfn_planner::NavfnPlanner' + plugin: "nav2_smac_planner::SmacPlannerLattice" # In Iron and older versions, "/" was used instead of "::" + allow_unknown: true # Allow traveling in unknown space + tolerance: 0.05 # dist-to-goal heuristic cost (distance) for valid tolerance endpoints if exact goal cannot be found. + max_iterations: 1000000 # Maximum total iterations to search for before failing (in case unreachable), set to -1 to disable + max_on_approach_iterations: 1000 # Maximum number of iterations after within tolerances to continue to try to find exact solution + max_planning_time: 5.0 # Max time in s for planner to plan, smooth + analytic_expansion_ratio: 3.5 # The ratio to attempt analytic expansions during search for final approach. + analytic_expansion_max_length: 0.1 # For Hybrid/Lattice nodes The maximum length of the analytic expansion to be considered valid to prevent unsafe shortcutting + analytic_expansion_max_cost: 50.0 # The maximum single cost for any part of an analytic expansion to contain and be valid, except when necessary on approach to goal + analytic_expansion_max_cost_override: false # Whether or not to override the maximum cost setting if within critical distance to goal (ie probably required) + downsample_obstacle_heuristic: true # Downsample the obstacle map dynamic programming distance expansion heuristic to speed up search at the cost of some path quality + use_quadratic_cost_penalty: false # Use quadratic cost penalty for traversal and heuristic cost computations rather than linear + reverse_penalty: 1.0 # Penalty to apply if motion is reversing, must be => 1 + change_penalty: 0.00 # Penalty to apply if motion is changing directions (L to R), must be >= 0 + non_straight_penalty: 1.2 # Penalty to apply if motion is non-straight, must be => 1 + cost_penalty: 2.5 # Penalty to apply to higher cost areas when adding into the obstacle map dynamic programming distance expansion heuristic. This drives the robot more towards the center of passages. A value between 1.3 - 3.5 is reasonable. + rotation_penalty: 4.0 # Penalty to apply to in-place rotations, if minimum control set contains them + retrospective_penalty: 0.015 + lookup_table_size: 1.0 # Size of the dubin/reeds-sheep distance window to cache, in meters. + cache_obstacle_heuristic: false # Cache the obstacle map dynamic programming distance expansion heuristic between subsequent replannings of the same goal location. Dramatically speeds up replanning performance (40x) if costmap is largely static. + allow_reverse_expansion: true # If true, allows the robot to use the primitives to expand in the mirrored opposite direction of the current robot's orientation (to reverse). + coarse_search_resolution: 1 # Number of bins to skip when doing a coarse search for the path. Only used for all_direction goal heading mode. + goal_heading_mode: "DEFAULT" # DEFAULT, BIDIRECTIONAL, ALL_DIRECTION + smooth_path: True # If true, does a simple and quick smoothing post-processing to the path + smoother: + max_iterations: 1000 + w_smooth: 0.3 + w_data: 0.2 + tolerance: 1.0e-10 + do_refinement: true + refinement_num: 2 + controller_server: ros__parameters: - controller_frequency: 20.0 + controller_frequency: 100.0 costmap_update_timeout: 0.3 failure_tolerance: 0.3 odom_topic: "odom" @@ -115,146 +151,39 @@ controller_server: movement_time_allowance: 5.0 goal_checker: plugin: "nav2_controller::SimpleGoalChecker" - xy_goal_tolerance: 0.10 + xy_goal_tolerance: 0.04 yaw_goal_tolerance: 0.10 stateful: True FollowPath: -# plugin: "dwb_core::DWBLocalPlanner" -# debug_trajectory_details: True -# min_vel_x: 0.0 -# min_vel_y: 0.0 -# max_vel_x: 0.26 -# max_vel_y: 0.0 -# max_vel_theta: 1.0 -# min_speed_xy: 0.0 -# max_speed_xy: 0.26 -# min_speed_theta: 0.0 -# acc_lim_x: 2.5 -# acc_lim_y: 0.0 -# acc_lim_theta: 3.2 -# decel_lim_x: -2.5 -# decel_lim_y: 0.0 -# decel_lim_theta: -3.2 -# vx_samples: 20 -# vtheta_samples: 20 -# sim_time: 1.7 -# linear_granularity: 0.05 -# angular_granularity: 0.025 -# transform_tolerance: 0.2 -# xy_goal_tolerance: 0.10 -# trans_stopped_velocity: 0.25 -# short_circuit_trajectory_evaluation: True -# limit_vel_cmd_in_traj: False -# stateful: True -# critics: ["RotateToGoal", "Oscillation", "ObstacleFootprint", "GoalAlign", "PathAlign", "PathDist", "GoalDist"] -# ObstacleFootprint.scale: 1.00 -# PathAlign.scale: 32.0 -# GoalAlign.scale: 24.0 -# PathAlign.forward_point_distance: 0.05 -# GoalAlign.forward_point_distance: 0.05 -# PathDist.scale: 32.0 -# GoalDist.scale: 24.0 -# RotateToGoal.scale: 32.0 -# RotateToGoal.slowing_factor: 5.0 -# RotateToGoal.lookahead_time: -1.0 - plugin: "nav2_mppi_controller::MPPIController" - time_steps: 56 - model_dt: 0.05 - batch_size: 2000 - ax_max: 3.0 - ax_min: -3.0 - ay_max: 3.0 - ay_min: -3.0 - az_max: 3.5 - vx_std: 0.2 - vy_std: 0.2 - wz_std: 0.4 - vx_max: 0.5 - vx_min: -0.35 - vy_max: 0.5 - wz_max: 1.9 - iteration_count: 1 - prune_distance: 1.7 - transform_tolerance: 0.1 - temperature: 0.3 - gamma: 0.015 - motion_model: "DiffDrive" - visualize: true - publish_optimal_trajectory: true - regenerate_noises: true - TrajectoryVisualizer: - trajectory_step: 5 - time_step: 3 - TrajectoryValidator: - # The validator can be configured with additional parameters if needed. - plugin: "mppi::DefaultOptimalTrajectoryValidator" - collision_lookahead_time: 1.0 # Time to look ahead to check for collisions - consider_footprint: true # Whether to consider the full robot's footprint (or circle) in trajectory validation collision checks - AckermannConstraints: - min_turning_r: 0.2 - critics: - [ - "ConstraintCritic", - "CostCritic", - "GoalCritic", - "GoalAngleCritic", - "PathAlignCritic", - "PathFollowCritic", - "PathAngleCritic", - "PreferForwardCritic", - ] - ConstraintCritic: - enabled: true - cost_power: 1 - cost_weight: 4.0 - GoalCritic: - enabled: true - cost_power: 1 - cost_weight: 5.0 - threshold_to_consider: 1.4 - GoalAngleCritic: - enabled: true - cost_power: 1 - cost_weight: 3.0 - threshold_to_consider: 0.5 - PreferForwardCritic: - enabled: true - cost_power: 1 - cost_weight: 5.0 - threshold_to_consider: 0.5 - CostCritic: - enabled: true - cost_power: 1 - cost_weight: 3.81 - near_collision_cost: 253 - critical_cost: 300.0 - consider_footprint: true - collision_cost: 1000000.0 - near_goal_distance: 1.0 - trajectory_point_step: 2 - PathAlignCritic: - enabled: true - cost_power: 1 - cost_weight: 14.0 - max_path_occupancy_ratio: 0.05 - trajectory_point_step: 4 - threshold_to_consider: 0.5 - offset_from_furthest: 20 - use_path_orientations: false - PathFollowCritic: - enabled: true - cost_power: 1 - cost_weight: 5.0 - offset_from_furthest: 5 - threshold_to_consider: 1.4 - PathAngleCritic: - enabled: true - cost_power: 1 - cost_weight: 2.0 - offset_from_furthest: 4 - threshold_to_consider: 0.5 - max_angle_to_furthest: 1.0 - mode: 0 + plugin: "toid::SpinnerController" + primary_controller: + plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController" + desired_linear_vel: 0.50 + lookahead_dist: 0.1 + min_lookahead_dist: 0.08 + max_lookahead_dist: 0.15 + lookahead_time: 0.2 + rotate_to_heading_angular_vel: 0.8 + use_velocity_scaled_lookahead_dist: true + min_approach_linear_velocity: 0.1 + approach_velocity_scaling_dist: 0.6 + use_collision_detection: true + max_allowed_time_to_collision_up_to_carrot: 0.3 + use_regulated_linear_velocity_scaling: true + use_fixed_curvature_lookahead: false + curvature_lookahead_dist: 0.1 + use_cost_regulated_linear_velocity_scaling: true + cost_scaling_dist: 0.1 + cost_scaling_gain: 1.5 + regulated_linear_scaling_min_radius: 0.9 + regulated_linear_scaling_min_speed: 0.25 + use_rotate_to_heading: false + allow_reversing: true + max_angular_accel: 3.2 + min_distance_to_obstacle: 0.0 + max_robot_pose_search_dist: 0.5 + use_cancel_deceleration: true + stateful: true lifecycle_manager: ros__parameters: diff --git a/toid_navigation/rviz/nav2.rviz b/toid_navigation/rviz/nav2.rviz index 0563bc8..5eaf224 100644 --- a/toid_navigation/rviz/nav2.rviz +++ b/toid_navigation/rviz/nav2.rviz @@ -4,6 +4,7 @@ Panels: Name: Displays Property Tree Widget: Expanded: + - /RobotModel1 - /GlobalCostMap1/Topic1 Splitter Ratio: 0.5 Tree Height: 592 @@ -46,7 +47,7 @@ Visualization Manager: Plane Cell Count: 64 Reference Frame: Value: true - - Alpha: 1 + - Alpha: 0.20000000298023224 Class: rviz_default_plugins/RobotModel Collision Enabled: false Description File: "" @@ -193,8 +194,8 @@ Visualization Manager: Class: rviz_default_plugins/Path Color: 25; 255; 0 Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 + Head Diameter: 0.029999999329447746 + Head Length: 0.019999999552965164 Length: 0.30000001192092896 Line Style: Lines Line Width: 0.029999999329447746 @@ -204,10 +205,10 @@ Visualization Manager: Y: 0 Z: 0 Pose Color: 255; 85; 255 - Pose Style: None + Pose Style: Arrows Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 + Shaft Diameter: 0.009999999776482582 + Shaft Length: 0.009999999776482582 Topic: Depth: 5 Durability Policy: Volatile @@ -216,6 +217,36 @@ Visualization Manager: Reliability Policy: Reliable Value: /plan Value: true + - Alpha: 0.20000000298023224 + Class: rviz_default_plugins/PointStamped + Color: 249; 240; 107 + Enabled: true + History Length: 1 + Name: PointStamped + Radius: 0.05000000074505806 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /lookahead_point + Value: true + - Alpha: 1 + Class: rviz_default_plugins/PointStamped + Color: 38; 162; 105 + Enabled: true + History Length: 1 + Name: PointStamped + Radius: 0.05000000074505806 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /start_point + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -262,16 +293,16 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/XYOrbit - Distance: 1.4671714305877686 + Distance: 3.863811731338501 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 0.8760442137718201 - Y: -0.15811485052108765 - Z: -8.940696716308594e-07 + X: 0.1778966784477234 + Y: -1.1747734546661377 + Z: 1.7285346984863281e-06 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false @@ -288,7 +319,7 @@ Window Geometry: Height: 896 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd000000040000000000000254000002defc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f000002de000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002defc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003f000002de000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005d30000003efc0100000002fb0000000800540069006d00650100000000000005d3000002de00fffffffb0000000800540069006d0065010000000000000450000000000000000000000264000002de00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd000000040000000000000254000002defc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f000002de000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002defc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003f000002de000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005d30000003efc0100000002fb0000000800540069006d00650100000000000005d30000027b00fffffffb0000000800540069006d0065010000000000000450000000000000000000000264000002de00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: diff --git a/toid_spinner_controller/CMakeLists.txt b/toid_spinner_controller/CMakeLists.txt new file mode 100644 index 0000000..209b612 --- /dev/null +++ b/toid_spinner_controller/CMakeLists.txt @@ -0,0 +1,68 @@ +cmake_minimum_required(VERSION 3.8) +project(toid_spinner_controller) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(library_name toid_spinner_controller) + + +set(PACKAGE_DEPS + rclcpp + geometry_msgs + nav2_costmap_2d + pluginlib + nav_msgs + nav2_util + nav2_core + tf2 + tf2_ros + angles + nav2_controller +) + +# find dependencies +find_package(ament_cmake REQUIRED) +foreach(PACKAGE ${PACKAGE_DEPS}) + find_package(${PACKAGE} REQUIRED) +endforeach() + +add_library(${library_name} SHARED + src/toid_spinner_controller.cpp + src/path_handler.cpp) + +target_include_directories(${library_name} + PRIVATE + include +) + +ament_target_dependencies(${library_name} + ${PACKAGE_DEPS} +) + +install(TARGETS ${library_name} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(DIRECTORY include/ + DESTINATION include/ +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_export_include_directories(include) +ament_export_libraries(${library_name}) +ament_export_dependencies(${PACKAGE_DEPS}) + +pluginlib_export_plugin_description_file(nav2_core toid_spinner_controller.xml) + +ament_package() diff --git a/toid_spinner_controller/LICENSE b/toid_spinner_controller/LICENSE new file mode 100644 index 0000000..30e8e2e --- /dev/null +++ b/toid_spinner_controller/LICENSE @@ -0,0 +1,17 @@ +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. diff --git a/toid_spinner_controller/include/toid_spinner_controller/path_handler.hpp b/toid_spinner_controller/include/toid_spinner_controller/path_handler.hpp new file mode 100644 index 0000000..a2969e9 --- /dev/null +++ b/toid_spinner_controller/include/toid_spinner_controller/path_handler.hpp @@ -0,0 +1,49 @@ +#pragma once + +#include +#include +#include + +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_core/controller_exceptions.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav_msgs/msg/path.hpp" +#include "tf2/time.hpp" + +namespace toid { + +class PathHandler { +public: + PathHandler(tf2::Duration tranform_tolerance, + std::shared_ptr tf, + std::shared_ptr costmap_ros); + + ~PathHandler() = default; + + nav_msgs::msg::Path + transformGlobalPlan(const geometry_msgs::msg::PoseStamped &pose, + double max_robot_pose_search_dist, + double angle_tolerance); + + bool tranformPose(const std::string frame, + const geometry_msgs::msg::PoseStamped &in_pose, + geometry_msgs::msg::PoseStamped &out_pose); + + void setPlan(const nav_msgs::msg::Path &path) { global_plan_ = path; } + + nav_msgs::msg::Path getPlan() { return global_plan_; } + +protected: + + double dist2(const geometry_msgs::msg::PoseStamped &p1, const geometry_msgs::msg::PoseStamped &p2); + + rclcpp::Logger logger_{rclcpp::get_logger("MultiPlexPathHandler")}; + tf2::Duration transform_tolerance_; + std::shared_ptr tf_; + std::shared_ptr costmap_ros_; + nav_msgs::msg::Path global_plan_; +}; + +} // namespace toid diff --git a/toid_spinner_controller/include/toid_spinner_controller/toid_spinner_controller.hpp b/toid_spinner_controller/include/toid_spinner_controller/toid_spinner_controller.hpp new file mode 100644 index 0000000..8e45298 --- /dev/null +++ b/toid_spinner_controller/include/toid_spinner_controller/toid_spinner_controller.hpp @@ -0,0 +1,82 @@ +#pragma once +#include + +#include "geometry_msgs/msg/point_stamped.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav2_controller/plugins/position_goal_checker.hpp" +#include "nav2_core/controller.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_costmap_2d/footprint_collision_checker.hpp" +#include "nav2_util/node_utils.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "pluginlib/class_loader.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/parameter_value.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "rclcpp_lifecycle/lifecycle_publisher.hpp" +#include "tf2_ros/buffer.hpp" +#include "toid_spinner_controller/path_handler.hpp" + +namespace toid { +class SpinnerController : public nav2_core::Controller { +public: + SpinnerController(); + ~SpinnerController() override = default; + + void configure(const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent, + std::string name, std::shared_ptr tf, + std::shared_ptr) override; + + void cleanup() override; + + void activate() override; + + void deactivate() override; + + void setPlan(const nav_msgs::msg::Path &path) override; + + bool cancel() override; + + geometry_msgs::msg::TwistStamped + computeVelocityCommands(const geometry_msgs::msg::PoseStamped &pose, + const geometry_msgs::msg::Twist &velocity, + nav2_core::GoalChecker *goal_checker) override; + + geometry_msgs::msg::TwistStamped + computeRotateToHeadingCommand(const double &angular_distance_to_heading, + const geometry_msgs::msg::PoseStamped &pose); + + void setSpeedLimit(const double &speed_limit, + const bool &percentage) override; + +protected: + rclcpp_lifecycle::LifecycleNode::WeakPtr node_; + rclcpp::Logger logger_{rclcpp::get_logger("SpinnerController")}; + rclcpp::Clock::SharedPtr clock_; + std::unique_ptr path_handler_; + std::unique_ptr position_goal_checker_; + std::unique_ptr< + nav2_costmap_2d::FootprintCollisionChecker> + collision_checker_; + rclcpp_lifecycle::LifecyclePublisher< + geometry_msgs::msg::PointStamped>::SharedPtr closest_pt_pub_; + + std::shared_ptr tf_; + std::string plugin_name_; + bool new_path_recieved = false; + double last_angular_vel_; + + pluginlib::ClassLoader lp_loader_; + nav2_core::Controller::Ptr primary_controller_; + + // Params + double angular_dist_threshold_; + double angular_disengage_threshold_; + double forward_sampling_distance_; + double max_angular_accel_; + double rotate_to_heading_; + + double control_duration_; +}; +} // namespace toid \ No newline at end of file diff --git a/toid_spinner_controller/package.xml b/toid_spinner_controller/package.xml new file mode 100644 index 0000000..8a7e95a --- /dev/null +++ b/toid_spinner_controller/package.xml @@ -0,0 +1,31 @@ + + + + toid_spinner_controller + 0.0.0 + TODO: Package description + petar + MIT + + ament_cmake + + angles + geometry_msgs + nav_msgs + nav2_costmap_2d + nav2_msgs + nav2_util + nav2_core + nav2_controller + pluginlib + tf2 + tf2_ros + + ament_lint_auto + ament_lint_common + + + ament_cmake + + + diff --git a/toid_spinner_controller/src/path_handler.cpp b/toid_spinner_controller/src/path_handler.cpp new file mode 100644 index 0000000..68ea8f9 --- /dev/null +++ b/toid_spinner_controller/src/path_handler.cpp @@ -0,0 +1,131 @@ +#include "toid_spinner_controller/path_handler.hpp" +#include "angles/angles.h" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav2_core/controller_exceptions.hpp" +#include "nav2_util/geometry_utils.hpp" +#include "nav_msgs/msg/path.hpp" +#include "tf2/LinearMath/Quaternion.hpp" +#include "tf2/LinearMath/Vector3.hpp" +#include "tf2/exceptions.hpp" + +#include +#include +#include +#include +#include +#include + +namespace toid { +PathHandler::PathHandler( + tf2::Duration tranform_tolerance, std::shared_ptr tf, + std::shared_ptr costmap_ros) + : transform_tolerance_(tranform_tolerance), tf_(tf), + costmap_ros_(costmap_ros) {} + +nav_msgs::msg::Path +PathHandler::transformGlobalPlan(const geometry_msgs::msg::PoseStamped &pose, + double max_robot_pose_search_dist, + double angle_tolerance) { + + if (global_plan_.poses.empty()) { + throw nav2_core::InvalidPath("Received plan with length of one"); + } + + geometry_msgs::msg::PoseStamped robot_pose; + if (!tranformPose(global_plan_.header.frame_id, pose, robot_pose)) { + throw nav2_core::ControllerTFError( + "Unable to transform robot pose into global plan's frame"); + } + + auto closest_pose_upper_bound = + nav2_util::geometry_utils::first_after_integrated_distance( + global_plan_.poses.begin(), global_plan_.poses.end(), + max_robot_pose_search_dist); + + const double robot_orientation = tf2::getYaw(robot_pose.pose.orientation); + + // Calculate the closeset pose and keep track of the next rotation point + auto closest_pose = global_plan_.poses.begin(); + auto last_rotation = global_plan_.poses.end(); + auto previous_it = global_plan_.poses.begin(); + auto it = global_plan_.poses.begin(); + double closest_dist = INFINITY; + + for (; it != closest_pose_upper_bound; it++) { + const double it_orientation = tf2::getYaw(it->pose.orientation); + const double angle_diff = std::abs( + angles::shortest_angular_distance(it_orientation, robot_orientation)); + + if (previous_it->pose.position == it->pose.position) { + last_rotation = it + 1; + } + + if (angle_diff <= angle_tolerance) { + double dist = dist2(robot_pose, *it); + if (dist <= closest_dist) { + closest_dist = dist; + closest_pose = it; + last_rotation = global_plan_.poses.end(); + } + } + previous_it = it; + } + if(last_rotation == closest_pose_upper_bound || last_rotation == global_plan_.poses.end()) { + for (; it != global_plan_.poses.end(); it++) { + if (previous_it->pose.position == it->pose.position) { + last_rotation = it + 1; + } else if (last_rotation != global_plan_.poses.end()) { + break; + } + previous_it = it; + } + } + + auto transformGlobalPoseToLocal = [&](const auto &global_plan_pose) { + geometry_msgs::msg::PoseStamped stamped_pose; + stamped_pose.header.frame_id = global_plan_.header.frame_id; + stamped_pose.header.stamp = robot_pose.header.stamp; + stamped_pose.pose = global_plan_pose.pose; + stamped_pose.pose.position.z = 0.0; + return stamped_pose; + }; + + nav_msgs::msg::Path out_path; + std::transform(closest_pose, last_rotation, + std::back_inserter(out_path.poses), + transformGlobalPoseToLocal); + + out_path.header.frame_id = global_plan_.header.frame_id; + out_path.header.stamp = robot_pose.header.stamp; + + global_plan_.poses.erase(begin(global_plan_.poses), closest_pose); + + return out_path; +} + +bool PathHandler::tranformPose(const std::string frame, + const geometry_msgs::msg::PoseStamped &in_pose, + geometry_msgs::msg::PoseStamped &out_pose) { + if (in_pose.header.frame_id == frame) { + out_pose = in_pose; + return true; + } + + try { + tf_->transform(in_pose, out_pose, frame, transform_tolerance_); + out_pose.header.frame_id = frame; + return true; + } catch (tf2::TransformException &ex) { + RCLCPP_ERROR(logger_, "Exception in tranformPose: %s", ex.what()); + } + return false; +} + +double PathHandler::dist2(const geometry_msgs::msg::PoseStamped &p1, + const geometry_msgs::msg::PoseStamped &p2) { + const double x = p1.pose.position.x - p2.pose.position.x; + const double y = p1.pose.position.y - p2.pose.position.y; + return x * x + y * y; +} + +} // namespace toid \ No newline at end of file diff --git a/toid_spinner_controller/src/toid_spinner_controller.cpp b/toid_spinner_controller/src/toid_spinner_controller.cpp new file mode 100644 index 0000000..72fa7f6 --- /dev/null +++ b/toid_spinner_controller/src/toid_spinner_controller.cpp @@ -0,0 +1,206 @@ +#include "toid_spinner_controller/toid_spinner_controller.hpp" +#include "geometry_msgs/msg/point_stamped.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "pluginlib/class_list_macros.hpp" +#include "pluginlib/exceptions.hpp" +#include "rclcpp/logging.hpp" +#include "toid_spinner_controller/path_handler.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace toid { +SpinnerController::SpinnerController() + : lp_loader_("nav2_core", "nav2_core::Controller") {} + +void SpinnerController::configure( + const rclcpp_lifecycle::LifecycleNode::WeakPtr &parent, std::string name, + std::shared_ptr tf, + std::shared_ptr costmap_ros) { + + position_goal_checker_ = + std::make_unique(); + position_goal_checker_->initialize(parent, ".position_checker", costmap_ros); + plugin_name_ = name; + node_ = parent; + auto node = node_.lock(); + + tf_ = tf; + logger_ = node->get_logger(); + clock_ = node->get_clock(); + + std::string primary_controller; + double control_frequency; + + nav2_util::declare_parameter_if_not_declared( + node, plugin_name_ + ".angular_dist_threshold", + rclcpp::ParameterValue(0.785)); + nav2_util::declare_parameter_if_not_declared( + node, plugin_name_ + ".angular_disengage_threshold", + rclcpp::ParameterValue(0.785 / 2.0)); + nav2_util::declare_parameter_if_not_declared( + node, plugin_name_ + ".forward_sampling_distance", + rclcpp::ParameterValue(0.5)); + nav2_util::declare_parameter_if_not_declared( + node, plugin_name_ + ".max_angular_accel", rclcpp::ParameterValue(3.2)); + nav2_util::declare_parameter_if_not_declared( + node, plugin_name_ + ".rotate_to_heading", rclcpp::ParameterValue(1.0)); + nav2_util::declare_parameter_if_not_declared( + node, plugin_name_ + ".primary_controller.plugin", + rclcpp::PARAMETER_STRING); + + node->get_parameter(plugin_name_ + ".angular_dist_threshold", + angular_dist_threshold_); + node->get_parameter(plugin_name_ + ".angular_disengage_threshold", + angular_disengage_threshold_); + node->get_parameter(plugin_name_ + ".forward_sampling_distance", + forward_sampling_distance_); + node->get_parameter(plugin_name_ + ".max_angular_accel", max_angular_accel_); + node->get_parameter(plugin_name_ + ".rotate_to_heading", rotate_to_heading_); + + primary_controller = + node->get_parameter(plugin_name_ + ".primary_controller.plugin") + .as_string(); + + node->get_parameter("controller_frequency", control_frequency); + control_duration_ = 1.0 / control_frequency; + + try { + primary_controller_ = lp_loader_.createUniqueInstance(primary_controller); + RCLCPP_INFO(logger_, + "Created internal controller for multiplexer: %s of type %s", + plugin_name_.c_str(), primary_controller.c_str()); + } catch (const pluginlib::PluginlibException &ex) { + RCLCPP_FATAL( + logger_, + "Failed to create primary controller for multiplexer. Exception: %s", + ex.what()); + return; + } + primary_controller_->configure(parent, name + ".primary_controller", tf, + costmap_ros); + + path_handler_ = std::make_unique(tf2::durationFromSec(0.1), tf_, + costmap_ros); + + closest_pt_pub_ = node->create_publisher( + "start_point", 1); +} + +void SpinnerController::cleanup() { + RCLCPP_INFO(logger_, "Cleaning SpinnerController"); + primary_controller_->cleanup(); + primary_controller_->reset(); + position_goal_checker_.reset(); + path_handler_.reset(); + closest_pt_pub_.reset(); +} + +void SpinnerController::activate() { + RCLCPP_INFO(logger_, "Activating SpinnerController"); + last_angular_vel_ = std::numeric_limits::max(); + new_path_recieved = false; + auto node = node_.lock(); + closest_pt_pub_->on_activate(); + primary_controller_->activate(); + position_goal_checker_->reset(); +} + +void SpinnerController::deactivate() { + RCLCPP_INFO(logger_, "Deactivating SpinnerController"); + closest_pt_pub_.reset(); + primary_controller_->deactivate(); + closest_pt_pub_->on_deactivate(); +} + +void SpinnerController::setPlan(const nav_msgs::msg::Path &path) { + path_handler_->setPlan(path); + new_path_recieved = true; + primary_controller_->setPlan(path); +} + +geometry_msgs::msg::TwistStamped SpinnerController::computeVelocityCommands( + const geometry_msgs::msg::PoseStamped &pose, + const geometry_msgs::msg::Twist &velocity, + nav2_core::GoalChecker *goal_checker) { + + auto path = path_handler_->transformGlobalPlan(pose, 1.0, M_PI / 8); + + if (path.poses.size() > 0) { + geometry_msgs::msg::PointStamped pt; + const auto &pose = path.poses.back(); + pt.point = pose.pose.position; + pt.header.frame_id = path.header.frame_id; + pt.header.stamp = path.header.stamp; + closest_pt_pub_->publish(pt); + } + + geometry_msgs::msg::PoseStamped robot_pose; + path_handler_->tranformPose(path.header.frame_id, pose, robot_pose); + const double robot_angle = tf2::getYaw(robot_pose.pose.orientation); + + if (path.poses.front().pose.position == path.poses.back().pose.position) { + const double path_angle = tf2::getYaw(path.poses.back().pose.orientation); + const double angle = + angles::shortest_angular_distance(robot_angle, path_angle); + return computeRotateToHeadingCommand(angle, pose); + } + + if (new_path_recieved) { + new_path_recieved = false; + primary_controller_->setPlan(path); + } + + auto twist = primary_controller_->computeVelocityCommands(pose, velocity, + goal_checker); + last_angular_vel_ = twist.twist.angular.z; + return twist; +} + +geometry_msgs::msg::TwistStamped +SpinnerController::computeRotateToHeadingCommand( + const double &angular_distance_to_heading, + const geometry_msgs::msg::PoseStamped &pose) { + geometry_msgs::msg::TwistStamped cmd; + + const double last_angular_vel = + (last_angular_vel_ == std::numeric_limits::max()) + ? 0 + : last_angular_vel_; + const double sign = angular_distance_to_heading > 0.0 ? 1.0 : -1.0; + const double speed_upper_bound = + last_angular_vel + control_duration_ * max_angular_accel_; + const double speed_lower_bound = + last_angular_vel - control_duration_ * max_angular_accel_; + + const double speed_until_overshoot = + 0.9 * std::sqrt(2.0 * max_angular_accel_ * + std::fabs(angular_distance_to_heading)); + const double requested_speed = sign * std::fmin(speed_until_overshoot,rotate_to_heading_); + const double speed = + std::clamp(requested_speed, speed_lower_bound, speed_upper_bound); + + cmd.header.frame_id = pose.header.frame_id; + cmd.header.stamp = pose.header.stamp; + cmd.twist.angular.z = speed; + last_angular_vel_ = speed; + return cmd; +} + +void SpinnerController::setSpeedLimit(const double &speed_limit, + const bool &percentage) { + primary_controller_->setSpeedLimit(speed_limit, percentage); +} + +bool SpinnerController::cancel() { return primary_controller_->cancel(); } + +} // namespace toid + +PLUGINLIB_EXPORT_CLASS(toid::SpinnerController, nav2_core::Controller) \ No newline at end of file diff --git a/toid_spinner_controller/toid_spinner_controller.xml b/toid_spinner_controller/toid_spinner_controller.xml new file mode 100644 index 0000000..f4c500b --- /dev/null +++ b/toid_spinner_controller/toid_spinner_controller.xml @@ -0,0 +1,9 @@ + + + + + toid_SpinnerController + + + + \ No newline at end of file