[Feature] Pure Pursuit Controller + Nav2 Integration

data/navigator_default.rviz

@@ -4,12 +4,15 @@ Panels:
     Name: Displays
     Property Tree Widget:
       Expanded:
+        - /Global Options1
         - /TF1/Frames1
         - /Routes Paths1
-        - /Cameras1
+        - /Routes Paths1/Path1/Topic1
         - /Grids1
+        - /Nav21/Map1
+        - /Pure Pursuit Controller1
       Splitter Ratio: 0.5
-    Tree Height: 960
+    Tree Height: 603
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -55,34 +58,20 @@ Visualization Manager:
         History Policy: Keep Last
         Reliability Policy: Reliable
         Value: /robot_description
-      Enabled: true
+      Enabled: false
       Links:
         All Links Enabled: true
         Expand Joint Details: false
         Expand Link Details: false
         Expand Tree: false
         Link Tree Style: Links in Alphabetic Order
-        base_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        hero:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-        model_link:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
       Mass Properties:
         Inertia: false
         Mass: false
       Name: RobotModel
       TF Prefix: ""
       Update Interval: 0
-      Value: true
+      Value: false
       Visual Enabled: true
     - Class: rviz_default_plugins/TF
       Enabled: false
@@ -181,7 +170,7 @@ Visualization Manager:
             Durability Policy: Volatile
             Filter size: 10
             History Policy: Keep Last
-            Reliability Policy: Reliable
+            Reliability Policy: Best Effort
             Value: /planning/path
           Value: true
       Enabled: true
@@ -237,9 +226,9 @@ Visualization Manager:
           Enabled: true
           Invert Rainbow: false
           Max Color: 255; 255; 255
-          Max Intensity: 0.9825243353843689
+          Max Intensity: 0.9850215315818787
           Min Color: 0; 73; 112
-          Min Intensity: 0.7125484347343445
+          Min Intensity: 0.716873288154602
           Name: Filtered LiDAR
           Position Transformer: XYZ
           Selectable: true
@@ -336,7 +325,7 @@ Visualization Manager:
       Displays:
         - Alpha: 0.699999988079071
           Class: rviz_default_plugins/Map
-          Color Scheme: raw
+          Color Scheme: map
           Draw Behind: true
           Enabled: false
           Name: Drivable Area
@@ -537,6 +526,143 @@ Visualization Manager:
         Reliability Policy: Reliable
         Value: /radar/radar_viz
       Value: false
+    - Class: rviz_default_plugins/Marker
+      Enabled: true
+      Name: Planning Goal
+      Namespaces:
+        "": true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /planning/goal_marker
+      Value: true
+    - Class: rviz_default_plugins/Marker
+      Enabled: true
+      Name: Planning Look Ahead
+      Namespaces:
+        "": true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /planning/look_ahead_marker
+      Value: true
+    - Class: rviz_common/Group
+      Displays:
+        - Alpha: 0.699999988079071
+          Class: rviz_default_plugins/Map
+          Color Scheme: map
+          Draw Behind: false
+          Enabled: true
+          Name: Map
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /nav2/map
+          Update Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /nav2/map_updates
+          Use Timestamp: false
+          Value: true
+        - Alpha: 1
+          Class: rviz_default_plugins/Polygon
+          Color: 25; 255; 0
+          Enabled: true
+          Name: Polygon
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /nav2/global_costmap/published_footprint
+          Value: true
+        - Alpha: 1
+          Buffer Length: 1
+          Class: rviz_default_plugins/Path
+          Color: 25; 255; 0
+          Enabled: true
+          Head Diameter: 0.30000001192092896
+          Head Length: 0.20000000298023224
+          Length: 0.30000001192092896
+          Line Style: Lines
+          Line Width: 0.029999999329447746
+          Name: Path
+          Offset:
+            X: 0
+            Y: 0
+            Z: 0
+          Pose Color: 255; 85; 255
+          Pose Style: None
+          Radius: 0.029999999329447746
+          Shaft Diameter: 0.10000000149011612
+          Shaft Length: 0.10000000149011612
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /nav2/plan
+          Value: true
+      Enabled: true
+      Name: Nav2
+    - Class: rviz_common/Group
+      Displays:
+        - Alpha: 1
+          Buffer Length: 1
+          Class: rviz_default_plugins/Path
+          Color: 255; 0; 0
+          Enabled: true
+          Head Diameter: 0.30000001192092896
+          Head Length: 0.20000000298023224
+          Length: 0.30000001192092896
+          Line Style: Billboards
+          Line Width: 1
+          Name: Pure Pursuit Path
+          Offset:
+            X: 0
+            Y: 0
+            Z: 0
+          Pose Color: 255; 85; 255
+          Pose Style: None
+          Radius: 0.029999999329447746
+          Shaft Diameter: 0.10000000149011612
+          Shaft Length: 0.10000000149011612
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /ppc/path
+          Value: true
+        - Class: rviz_default_plugins/Marker
+          Enabled: true
+          Name: Pure Pursuit Lookahead
+          Namespaces:
+            lookahead: true
+          Topic:
+            Depth: 5
+            Durability Policy: Volatile
+            Filter size: 10
+            History Policy: Keep Last
+            Reliability Policy: Reliable
+            Value: /planning/barrier_marker
+          Value: true
+      Enabled: true
+      Name: Pure Pursuit Controller
   Enabled: true
   Global Options:
     Background Color: 222; 222; 222
@@ -583,7 +709,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 40.92072296142578
+      Distance: 20.80792236328125
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -598,10 +724,10 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.649796724319458
+      Pitch: 0.9497964382171631
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz_default_plugins)
-      Yaw: 2.9573850631713867
+      Yaw: 2.19738507270813
     Saved: ~
 Window Geometry:
   Birds Eye:
@@ -616,16 +742,16 @@ Window Geometry:
     collapsed: false
   Displays:
     collapsed: false
-  Height: 1373
+  Height: 1016
   Hide Left Dock: false
   Hide Right Dock: true
-  QMainWindow State: 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
+  QMainWindow State: 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
   Selection:
     collapsed: false
   Tool Properties:
     collapsed: false
   Views:
     collapsed: true
-  Width: 2176
-  X: 74
+  Width: 1310
+  X: 610
   Y: 27

launches/launch.vehicle.py

@@ -29,44 +29,48 @@ def generate_launch_description():
 
     perception_launch_entities = perception_launch_description.describe_sub_entities()
 
-    return LaunchDescription(
-        [
-            map_manager_carla,
-            RegisterEventHandler(
-                OnProcessStart(
-                    target_action=map_manager_carla,
-                    on_start=[
-                        LogInfo(
-                            msg="Map Manager Started... launching the rest of Navigator..."
-                        ),
-                        # CONTROL
-                        # carla_controller,
-                        # INTERFACE
-                        # leaderboard_liaison,
-                        # web_bridge,
-                        ##joy_linux,
-                        ##joy_translation,
-                        # LOCALIZATION
-                        # gnss_averager,
-                        # mcl,
-                        # MAPPING
-                        # MISC
-                        # recorder,
-                        # rqt,
-                        # camera_streamer,
-                        # PERCEPTION
-                        *perception_launch_entities,
-                        # PLANNING
-                        routing_monitor,
-                        grid_summation,
-                        junction_manager,
-                        rtp,
-                        # SAFETY
-                        ##airbags,
-                        ##guardian,
-                        rviz,
-                    ],
-                )
-            ),
-        ]
-    )
+    return LaunchDescription([
+        map_manager_carla,
+        RegisterEventHandler(
+            OnProcessStart(
+                target_action=map_manager_carla,
+                on_start=[
+                    LogInfo(msg='Map Manager Started... launching the rest of Navigator...'),
+                    # CONTROL
+                    #carla_controller,
+
+                    # INTERFACE
+                    # leaderboard_liaison,
+                    # web_bridge,
+                    ##joy_linux,
+                    ##joy_translation,
+
+                    # LOCALIZATION
+                    # gnss_averager,
+                    # mcl,
+
+                    # MAPPING
+                    # MISC
+                    # recorder,
+                    # rqt,
+                    # camera_streamer,
+
+                    # PERCEPTION
+                    *perception_launch_entities,
+
+                    # PLANNING
+                    routing_monitor,
+                    grid_summation,
+                    junction_manager,
+                    rtp,
+                    pure_pursuit_controller,
+
+                    # SAFETY
+                    ##airbags,
+                    ##guardian,
+                    rviz,
+                ]
+            )
+        )
+    ])
+

launches/launch_node_definitions.py

@@ -4,6 +4,11 @@
 
 NAVIGATOR_DIR = "/navigator/"
 
+pure_pursuit_controller = Node(
+    package='pure_pursuit_controller',
+    executable='pure_pursuit_controller'
+)
+
 airbags = Node(
     package='airbags',
     executable='airbag_node'

src/control/pure_pursuit_controller/package.xml

@@ -0,0 +1,18 @@
+<?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>pure_pursuit_controller</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="root@todo.todo">root</maintainer>
+  <license>TODO: License declaration</license>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>