Created path segmenter controller

.clangd

@@ -0,0 +1,6 @@
+---
+Diagnostics:
+  UnusedIncludes: None 
+  MissingIncludes: None
+  ClangTidy:
+    FastCheckFilter: Loose

.gitignore

@@ -1,3 +1,4 @@
+.cache
 build
 install
 log

colcon_defaults.yaml

@@ -0,0 +1,6 @@
+{
+    "build": {
+        "cmake-args": ["-DCMAKE_EXPORT_COMPILE_COMMANDS=1"],
+        "symlink-install": true
+    }
+}

toid_bot_description/src/toid_bot_description.urdf

@@ -3,8 +3,8 @@
 
 <xacro:arg name="use_mock" default="true" />
 
-<xacro:property name="base_width" value="0.31"/>
+<xacro:property name="base_width" value="0.30"/>
-<xacro:property name="base_length" value="0.25"/>
+<xacro:property name="base_length" value="0.30"/>
 <xacro:property name="base_height" value="0.34"/>
 
 <xacro:property name="wheel_radius" value="0.04"/>
@@ -53,7 +53,7 @@
 <joint name="base_joint" type="fixed">
     <child link="base_link"/>
     <parent link="base_footprint"/>
-    <origin xyz="0.0 0.0 ${base_zoff}" rpy="0 0 0"/>
+    <origin xyz="${-wheel_xoff} 0.0 ${base_zoff}" rpy="0 0 0"/>
 </joint>
 
 <joint name="lidar_joint" type="fixed">

toid_navigation/CMakeLists.txt

@@ -16,6 +16,7 @@ install(
     launch
     maps
     params
+    behaviors
     rviz
   DESTINATION share/${PROJECT_NAME}
 )

toid_navigation/behaviors/navigate_to_pose_w_backtracking.xml

@@ -0,0 +1,51 @@
+
+<!--
+  This Behavior Tree replans the global path periodically at 1 Hz and it also has
+  recovery actions specific to planning / control as well as general system issues.
+  This will be continuous if a kinematically valid planner is selected.
+-->
+<root BTCPP_format="4" main_tree_to_execute="MainTree">
+  <BehaviorTree ID="MainTree">
+    <RecoveryNode number_of_retries="6" name="NavigateRecovery">
+      <PipelineSequence name="NavigateWithReplanning">
+        <ControllerSelector selected_controller="{selected_controller}" default_controller="FollowPath" topic_name="controller_selector"/>
+        <PlannerSelector selected_planner="{selected_planner}" default_planner="GridBased" topic_name="planner_selector"/>
+        <RateController hz="1.0">
+          <RecoveryNode number_of_retries="1" name="ComputePathToPose">
+            <ComputePathToPose goal="{goal}" path="{path}" planner_id="{selected_planner}" error_code_id="{compute_path_error_code}"/>
+            <Sequence>
+              <WouldAPlannerRecoveryHelp error_code="{compute_path_error_code}"/>
+              <ClearEntireCostmap name="ClearGlobalCostmap-Context" service_name="global_costmap/clear_entirely_global_costmap"/>
+            </Sequence>
+          </RecoveryNode>
+        </RateController>
+        <RecoveryNode number_of_retries="1" name="FollowPath">
+          <FollowPath path="{path}" controller_id="{selected_controller}" error_code_id="{follow_path_error_code}"/>
+          <Sequence>
+            <WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
+            <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
+          </Sequence>
+        </RecoveryNode>
+      </PipelineSequence>
+      <Sequence>
+        <Fallback>
+          <WouldAControllerRecoveryHelp error_code="{follow_path_error_code}"/>
+          <WouldAPlannerRecoveryHelp error_code="{compute_path_error_code}"/>
+        </Fallback>
+        <ReactiveFallback name="RecoveryFallback">
+          <GoalUpdated/>
+          <RoundRobin name="RecoveryActions">
+            <Sequence name="ClearingActions">
+              <ClearEntireCostmap name="ClearLocalCostmap-Subtree" service_name="local_costmap/clear_entirely_local_costmap"/>
+              <ClearEntireCostmap name="ClearGlobalCostmap-Subtree" service_name="global_costmap/clear_entirely_global_costmap"/>
+            </Sequence>
+            <BackUp backup_dist="0.04" backup_speed="0.05" error_code_id="{backup_code_id}"/>
+            <BackUp backup_dist="-0.08" backup_speed="0.05" error_code_id="{backup_code_id}"/>
+            <BackUp backup_dist="0.012" backup_speed="0.05" error_code_id="{backup_code_id}"/>
+            <BackUp backup_dist="-0.16" backup_speed="0.05" error_code_id="{backup_code_id}"/>
+          </RoundRobin>
+        </ReactiveFallback>
+      </Sequence>
+    </RecoveryNode>
+  </BehaviorTree>
+</root>

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',

toid_navigation/package.xml

@@ -14,7 +14,7 @@
   <depend>nav2_planner</depend>
   <depend>nav2_controller</depend>
   <depend>nav2_bt_navigator</depend>
-  <depend>nav2_lifecycle_manger</depend>
+  <depend>nav2_lifecycle_manager</depend>
   <depend>nav2_bringup</depend>
   <depend>ros2_control</depend>
   <depend>rviz2</depend>

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
+        footprint_clearing_enabled: true
-        cost_scaling_factor: 12.0
+        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: "[[-0.08, 0.15], [0.22, 0.15], [0.22, -0.15], [-0.08, -0.15]]"
-      footprint_padding: 0.02
+      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
+        cost_scaling_factor: 3.0
-        inflation_radius: 0.45
+        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"
+      plugin: "toid::SpinnerController"
-#      debug_trajectory_details: True
+      primary_controller:
-#      min_vel_x: 0.0
+        plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
-#      min_vel_y: 0.0
+        desired_linear_vel: 0.50
-#      max_vel_x: 0.26
+        lookahead_dist: 0.1
-#      max_vel_y: 0.0
+        min_lookahead_dist: 0.08
-#      max_vel_theta: 1.0
+        max_lookahead_dist: 0.15
-#      min_speed_xy: 0.0
+        lookahead_time: 0.2
-#      max_speed_xy: 0.26
+        rotate_to_heading_angular_vel: 0.8
-#      min_speed_theta: 0.0
+        use_velocity_scaled_lookahead_dist: true
-#      acc_lim_x: 2.5
+        min_approach_linear_velocity: 0.1
-#      acc_lim_y: 0.0
+        approach_velocity_scaling_dist: 0.6
-#      acc_lim_theta: 3.2
+        use_collision_detection: true
-#      decel_lim_x: -2.5
+        max_allowed_time_to_collision_up_to_carrot: 0.3
-#      decel_lim_y: 0.0
+        use_regulated_linear_velocity_scaling: true
-#      decel_lim_theta: -3.2
+        use_fixed_curvature_lookahead: false
-#      vx_samples: 20
+        curvature_lookahead_dist: 0.1
-#      vtheta_samples: 20
+        use_cost_regulated_linear_velocity_scaling: true
-#      sim_time: 1.7
+        cost_scaling_dist: 0.1
-#      linear_granularity: 0.05
+        cost_scaling_gain: 1.5
-#      angular_granularity: 0.025
+        regulated_linear_scaling_min_radius: 0.9
-#      transform_tolerance: 0.2
+        regulated_linear_scaling_min_speed: 0.25
-#      xy_goal_tolerance: 0.10
+        use_rotate_to_heading: false
-#      trans_stopped_velocity: 0.25
+        allow_reversing: true
-#      short_circuit_trajectory_evaluation: True
+        max_angular_accel: 3.2
-#      limit_vel_cmd_in_traj: False
+        min_distance_to_obstacle: 0.0
-#      stateful: True
+        max_robot_pose_search_dist: 0.5
-#      critics: ["RotateToGoal", "Oscillation", "ObstacleFootprint", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
+        use_cancel_deceleration: true
-#      ObstacleFootprint.scale: 1.00
+        stateful: true
-#      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
 
 lifecycle_manager:
   ros__parameters:

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: <Fixed 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 Diameter: 0.029999999329447746
-      Head Length: 0.20000000298023224
+      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 Diameter: 0.009999999776482582
-      Shaft Length: 0.10000000149011612
+      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
+        X: 0.1778966784477234
-        Y: -0.15811485052108765
+        Y: -1.1747734546661377
-        Z: -8.940696716308594e-07
+        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:

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()

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.

toid_spinner_controller/include/toid_spinner_controller/path_handler.hpp

@@ -0,0 +1,49 @@
+#pragma once
+
+#include <memory>
+#include <rclcpp/logger.hpp>
+#include <tf2_ros/buffer.hpp>
+
+#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<tf2_ros::Buffer> tf,
+              std::shared_ptr<nav2_costmap_2d::Costmap2DROS> 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<tf2_ros::Buffer> tf_;
+  std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
+  nav_msgs::msg::Path global_plan_;
+};
+
+} // namespace toid

toid_spinner_controller/include/toid_spinner_controller/toid_spinner_controller.hpp

@@ -0,0 +1,82 @@
+#pragma once
+#include <memory>
+
+#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<tf2_ros::Buffer> tf,
+                 std::shared_ptr<nav2_costmap_2d::Costmap2DROS>) 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<PathHandler> path_handler_;
+  std::unique_ptr<nav2_controller::PositionGoalChecker> position_goal_checker_;
+  std::unique_ptr<
+      nav2_costmap_2d::FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>>
+      collision_checker_;
+  rclcpp_lifecycle::LifecyclePublisher<
+      geometry_msgs::msg::PointStamped>::SharedPtr closest_pt_pub_;
+
+  std::shared_ptr<tf2_ros::Buffer> tf_;
+  std::string plugin_name_;
+  bool new_path_recieved = false;
+  double last_angular_vel_;
+
+  pluginlib::ClassLoader<nav2_core::Controller> 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

toid_spinner_controller/package.xml

@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>toid_spinner_controller</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="82343504+pimpest@users.noreply.github.com">petar</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>angles</depend>
+  <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
+  <depend>nav2_costmap_2d</depend>
+  <depend>nav2_msgs</depend>
+  <depend>nav2_util</depend>
+  <depend>nav2_core</depend>
+  <depend>nav2_controller</depend>
+  <depend>pluginlib</depend>
+  <depend>tf2</depend>
+  <depend>tf2_ros</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+    <nav2_core plugin="${prefix}/toid_spinner_controller.xml" />
+  </export>
+</package>

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 <algorithm>
+#include <cmath>
+#include <cstdint>
+#include <iterator>
+#include <memory>
+#include <numeric>
+
+namespace toid {
+PathHandler::PathHandler(
+    tf2::Duration tranform_tolerance, std::shared_ptr<tf2_ros::Buffer> tf,
+    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> 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

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 <algorithm>
+#include <angles/angles.h>
+#include <cmath>
+#include <limits>
+#include <memory>
+#include <rclcpp/duration.hpp>
+#include <tf2/time.hpp>
+#include <tf2/utils.hpp>
+
+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<tf2_ros::Buffer> tf,
+    std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) {
+
+  position_goal_checker_ =
+      std::make_unique<nav2_controller::PositionGoalChecker>();
+  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<PathHandler>(tf2::durationFromSec(0.1), tf_,
+                                                costmap_ros);
+
+  closest_pt_pub_ = node->create_publisher<geometry_msgs::msg::PointStamped>(
+      "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<double>::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<double>::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)

toid_spinner_controller/toid_spinner_controller.xml

@@ -0,0 +1,9 @@
+<class_libraries>
+    <library path="toid_spinner_controller">
+        <class type="toid::SpinnerController" base_class_type="nav2_core::Controller">
+            <description>
+                toid_SpinnerController
+            </description>
+        </class>
+    </library>
+</class_libraries>