Erik may 16 changes for autonomy

src/HW-Devices/science_sensors/science_sensors/esp_serial_bridge.py

@@ -71,7 +71,9 @@ def __init__(self):
         self.declare_parameter("read_poll_hz", 100.0)
         self.declare_parameter("reconnect_period_s", 2.0)
 
-        configured_port = self.get_parameter("esp_port").get_parameter_value().string_value
+        configured_port = (
+            self.get_parameter("esp_port").get_parameter_value().string_value
+        )
         self._port_config = self._normalize_port(configured_port)
         self._port = self._resolve_port_path(self._port_config)
         if configured_port != self._port_config:

src/HW-Devices/science_sensors/science_sensors/panoramic.py

@@ -23,7 +23,9 @@ def __init__(self):
         self.video_cli = self.create_client(
             VideoCapture, "/capture_frame", callback_group=self.video_callback_group
         )
-        self.set_cam_cli = self.create_client(VideoOut, "/start_video", callback_group=self.video_callback_group)
+        self.set_cam_cli = self.create_client(
+            VideoOut, "/start_video", callback_group=self.video_callback_group
+        )
         self.pan_srv = self.create_service(
             VideoCapture,
             "/capture_panoramic",
@@ -113,7 +115,7 @@ def start(self, request, response):
         source.width = 100
         source.height = 100
         video_start_req.sources = [source]
-        
+
         # Use an Event to wait for the async call to complete
         # Using ros2 executor blocking calls creates deadlock after first call
         event = Event()

src/Nav/gps/gps/gps_base_pub_node.py

@@ -146,8 +146,8 @@ def _configure_module(self):
                 "CFG",
                 "CFG-MSG",
                 SET,
-                # msgClass=UBX_NAV_CLASS,
-                # msgID=UBX_NAV_SVIN_ID,
+                msgClass=1,
+                msgID=0x3B,
                 rateI2C=0,
                 rateUART1=0,
                 rateUART2=0,

src/Nav/gps/gps/heading_pub_node.py

@@ -30,10 +30,12 @@ def __init__(self):
         queue_depth = (
             self.get_parameter("QueueDepth").get_parameter_value().integer_value
         )
-        self.heading_pub = self.create_publisher(Imu, "/heading", queue_depth)
+        self.heading_pub = self.create_publisher(Imu, "heading", queue_depth)
+        self.get_logger().info(f"Initializing HeadingNode with device: {self.dev}, baudrate: {self.baudrate}, frequency: {self.freq} Hz, persistent: {self.persistent}")
         self.serial_conn = UbxIoManager(
             port=self.dev, baud=self.baudrate, msg_filter=UBX_PROTOCOL
         )
+        self.get_logger().info(f"Successfully opened serial port: {self.dev} and baud rate: {self.baudrate}")
         self.layers = SET_LAYER_RAM | SET_LAYER_BBR
         if self.persistent:
             self.layers |= SET_LAYER_FLASH

src/Nav/gps/gps/ubx_io_manager.py

@@ -30,7 +30,13 @@ def __init__(self, port="/dev/ttyACM0", baud=9600, msg_filter=RTCM3_PROTOCOL):
         """
         self.lock = threading.Lock()
         try:
-            self.worker = serial.Serial(port, baud, timeout=1)
+            self.worker = serial.Serial()
+            self.worker.port = port
+            self.worker.baudrate = baud
+            self.worker.timeout = 1
+            self.worker.dtr = False
+            self.worker.rts = False
+            self.worker.open()
         except serial.SerialException as e:
             raise RuntimeError(f"Failed to open serial port {port}: {e}") from e
         self.ubr = UBXReader(

src/Nav/gps/launch/basestation.launch.py

@@ -29,22 +29,22 @@ def generate_launch_description():
                     },
                     {"Baudrate": 115200},
                     {"Freq": 5.0},  # Publish rate (hz)
-                    {"SvinMindur": 300},  # Survey in time (s)
-                    {"SvinMinAccDur": 10_000},  # Survey in accuracy (mm)
+                    {"SvinMinDur": 600},  # Survey in time (s)
+                    {"SvinAccLimit": 10_000},  # Survey in accuracy (mm)
                     {"QueueDepth": 10},
                 ],
             ),
             # Optionally run the standalone basestation config script when requested
-            ExecuteProcess(
-                cmd=[
-                    "python3",
-                    os.path.join(
-                        get_package_share_directory("gps"),
-                        "config",
-                        "fr_basestation.py",
-                    ),
-                ],
-                condition=IfCondition(load_config),
-            ),
+            # ExecuteProcess(
+            #     cmd=[
+            #         "python3",
+            #         os.path.join(
+            #             get_package_share_directory("gps"),
+            #             "config",
+            #             "fr_basestation.py",
+            #         ),
+            #     ],
+            #     condition=IfCondition(load_config),
+            # ),
         ]
     )

src/Nav/gps/launch/heading.launch.py

@@ -22,25 +22,26 @@ def generate_launch_description():
                 package="gps",
                 executable="heading_pub_node",
                 name="gps_heading_node",
+                output="screen",
                 parameters=[
                     {"frame_id": "gps_link"},
                     {"Freq": 5.0},  # Publish rate (hz)
-                    {"Baudrate": 115200},
+                    {"Baudrate": 38400},
                     {
                         "Device": "/dev/serial/by-id/usb-FTDI_FT230X_Basic_UART_D30EFLJN-if00-port0"
                     },
                 ],
                 remappings=[("heading", "gps/heading")],
             ),
             # Optionally run the standalone config script when requested
-            ExecuteProcess(
-                cmd=[
-                    "python3",
-                    os.path.join(
-                        get_package_share_directory("gps"), "config", "fr_heading.py"
-                    ),
-                ],
-                condition=IfCondition(load_config),
-            ),
+            # ExecuteProcess(
+            #     cmd=[
+            #         "python3",
+            #         os.path.join(
+            #             get_package_share_directory("gps"), "config", "fr_heading.py"
+            #         ),
+            #     ],
+            #     condition=IfCondition(load_config),
+            # ),
         ]
     )

src/Nav/gps/launch/rover.launch.py

@@ -49,15 +49,15 @@ def generate_launch_description():
                 ],
             ),
             # Optionally run the rover config script when requested
-            ExecuteProcess(
-                cmd=[
-                    "python3",
-                    os.path.join(
-                        get_package_share_directory("gps"), "config", "fr_rover.py"
-                    ),
-                ],
-                condition=IfCondition(load_config),
-            ),
+            # ExecuteProcess(
+            #     cmd=[
+            #         "python3",
+            #         os.path.join(
+            #             get_package_share_directory("gps"), "config", "fr_rover.py"
+            #         ),
+            #     ],
+            #     condition=IfCondition(load_config),
+            # ),
             heading_cmd,
         ]
     )

src/Nav/navigation/behavior_trees/bt_swerve_dynamic_replanning.xml

@@ -22,7 +22,7 @@
               </ReactiveSequence>
 
               <Sequence name="PlanAndSmooth">
-                <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>
+                <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBasedTransit"/>
                 <!-- <SmoothPath unsmoothed_path="{path}" smoothed_path="{smoothed_path}" smoother_id="Smoother"/> -->
               </Sequence>
             </Fallback>
@@ -32,7 +32,7 @@
         </RateController>
 
         <RecoveryNode number_of_retries="1" name="FollowPath">
-          <FollowPath path="{path}" controller_id="FollowPath"/>
+          <FollowPath path="{path}" controller_id="FollowPathTransit"/>
           <!-- <FollowPath path="{smoothed_path}" controller_id="FollowPathTransit"/> -->
           <ClearEntireCostmap name="ClearLocalCostmap-Context" service_name="local_costmap/clear_entirely_local_costmap"/>
         </RecoveryNode>

src/Nav/navigation/params/nav2.yaml

@@ -157,18 +157,18 @@ planner_server:
       lookup_table_size: 25.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.
       debug_visualizations: False         # For Hybrid nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning) and the path's footprints on the /planned_footprints topic. WARNING: heavy to compute and to display, for debug only as it degrades the performance.
-      use_quadratic_cost_penalty: False
+      use_quadratic_cost_penalty: True
       downsample_obstacle_heuristic: True
       allow_primitive_interpolation: False
       smooth_path: True                   # If true, does a simple and quick smoothing post-processing to the path
 
       smoother:
-        max_iterations: 2000
+        max_iterations: 800
         w_smooth: 0.7
         w_data: 0.2
-        tolerance: 1.0e-10
+        tolerance: 1.0e-4
         do_refinement: true
-        refinement_num: 4
+        refinement_num: 2
 
     GridBasedSearch:
       plugin: "cprt_planner_plugins::EventHorizonPlanner"
@@ -204,18 +204,18 @@ planner_server:
       lookup_table_size: 25.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.
       debug_visualizations: False         # For Hybrid nodes: Whether to publish expansions on the /expansions topic as an array of poses (the orientation has no meaning) and the path's footprints on the /planned_footprints topic. WARNING: heavy to compute and to display, for debug only as it degrades the performance.
-      use_quadratic_cost_penalty: False
+      use_quadratic_cost_penalty: True
       downsample_obstacle_heuristic: True
       allow_primitive_interpolation: False
       smooth_path: True                   # If true, does a simple and quick smoothing post-processing to the path
 
       smoother:
-        max_iterations: 2000
+        max_iterations: 800
         w_smooth: 0.7
         w_data: 0.2
-        tolerance: 1.0e-10
+        tolerance: 1.0e-4
         do_refinement: true
-        refinement_num: 4
+        refinement_num: 2
 
 smoother_server:
   ros__parameters: