let me into main plz

autonav_ws/src/autonav_filters/src/filters.py

@@ -12,6 +12,7 @@
 import rclpy
 import math
 
+
 class FilterType(IntEnum):
     DEAD_RECKONING = 0,
     PARTICLE_FILTER = 1
@@ -20,25 +21,22 @@ class FilterType(IntEnum):
 class FiltersNodeConfig:
     def __init__(self):
         self.filter_type = FilterType.PARTICLE_FILTER
-        self.degree_offset = 107.0
-        self.seed_heading = False
 
 
 class FiltersNode(Node):
     def __init__(self):
         super().__init__("autonav_filters")
 
-        self.lastIMUReceived = None
-        self.firstGps = None
-        self.lastGps = None
-
-        self.latitudeLength = self.declare_parameter("latitude_length", 111086.2).get_parameter_value().double_value
-        self.longitudeLength = self.declare_parameter("longitude_length", 81978.2).get_parameter_value().double_value
-
-        self.pf = ParticleFilter(self.latitudeLength, self.longitudeLength)
-        self.reckoning = DeadReckoningFilter()
+        self.first_gps = None
+        self.last_gps = None
+
+        self.latitude_length = self.declare_parameter("latitude_length", 111086.2).get_parameter_value().double_value
+        self.longitude_length = self.declare_parameter("longitude_length", 81978.2).get_parameter_value().double_value
+
+        self.pf = ParticleFilter(self.latitude_length, self.longitude_length)
+        self.dr = DeadReckoningFilter()
         self.config = self.get_default_config()
-        
+
         self.onReset()
 
     def config_updated(self, jsonObject):
@@ -49,80 +47,61 @@ def get_default_config(self):
 
     def init(self):
         self.create_subscription(GPSFeedback, "/autonav/gps", self.onGPSReceived, 20)
-        self.create_subscription(IMUData, "/autonav/imu", self.onIMUReceived, 20);
         self.create_subscription(MotorFeedback, "/autonav/MotorFeedback", self.onMotorFeedbackReceived, 20)
-        self.positionPublisher = self.create_publisher(Position, "/autonav/position", 20)
+        self.position_publisher = self.create_publisher(Position, "/autonav/position", 20)
 
         self.set_device_state(DeviceStateEnum.OPERATING)
-
-    def onIMUReceived(self, msg: IMUData):
-        self.lastIMUReceived = msg
-
-    def getRealHeading(self, heading: float):
-        if heading < 0:
-            heading = 360 + -heading
-
-        heading += self.config.degree_offset
-        return heading
 
     def onReset(self):
-        if self.lastIMUReceived is not None and self.config.seed_heading:
-            self.reckoning.reset(self.getRealHeading(self.lastIMUReceived.heading))
-            self.pf.init_particles(self.getRealHeading(self.lastIMUReceived.heading), True)
-        else:
-            self.reckoning.reset()
-            self.pf.init_particles()
+        self.dr.reset()
+        self.pf.init_particles()
 
     def system_state_transition(self, old: SystemState, updated: SystemState):
         if old.state != SystemStateEnum.AUTONOMOUS and updated.state == SystemStateEnum.AUTONOMOUS:
             self.onReset()
 
         if old.mobility == False and updated.mobility == True:
             self.onReset()
-            
+
     def onGPSReceived(self, msg: GPSFeedback):
         if msg.gps_fix == 0 and msg.is_locked == False:
             return
-
-        if self.firstGps is None:
-            self.firstGps = msg
 
-        self.lastGps = msg
+        if self.first_gps is None:
+            self.first_gps = msg
 
-        filterType = self.config.filter_type
-        if filterType == FilterType.PARTICLE_FILTER:
+        self.last_gps = msg
+        if self.config.filter_type == FilterType.PARTICLE_FILTER:
             self.pf.gps(msg)
-        elif filterType == FilterType.DEAD_RECKONING:
-            self.reckoning.gps(msg)
+        else:
+            self.dr.gps(msg)
 
     def onMotorFeedbackReceived(self, msg: MotorFeedback):
-        filterType = self.config.filter_type
         averages = None
-        if filterType == FilterType.PARTICLE_FILTER:
+        if self.config.filter_type == FilterType.PARTICLE_FILTER:
             averages = self.pf.feedback(msg)
-        if filterType == FilterType.DEAD_RECKONING:
-            averages = self.reckoning.feedback(msg)
-            
+        else:
+            averages = self.dr.feedback(msg)
+
         if averages is None:
             return
-            
+
         position = Position()
         position.x = averages[0]
         position.y = averages[1]
         position.theta = (-1 * math.pi * 2 + averages[2]) * 1
 
-
-        if self.firstGps is not None:
-            gps_x = self.firstGps.latitude + position.x / self.latitudeLength
-            gps_y = self.firstGps.longitude - position.y / self.longitudeLength
+        if self.first_gps is not None:
+            gps_x = self.first_gps.latitude + position.x / self.latitude_length
+            gps_y = self.first_gps.longitude - position.y / self.longitude_length
             position.latitude = gps_x
             position.longitude = gps_y
 
-        if self.system_mode == SystemModeEnum.SIMULATION and self.lastGps is not None:
-            position.latitude = self.lastGps.latitude
-            position.longitude = self.lastGps.longitude
-        
-        self.positionPublisher.publish(position)
+        if self.system_mode == SystemModeEnum.SIMULATION and self.last_gps is not None:
+            position.latitude = self.last_gps.latitude
+            position.longitude = self.last_gps.longitude
+
+        self.position_publisher.publish(position)
 
 
 def main():

autonav_ws/src/autonav_filters/src/particlefilter.py

@@ -3,23 +3,25 @@
 import math
 import random
 
+
 class Particle:
-    def __init__(self, x = 0, y = 0, theta = 0, weight = 1) -> None:
+    def __init__(self, x=0, y=0, theta=0, weight=1) -> None:
         self.x = x
         self.y = y
         self.theta = theta
         self.weight = weight
 
+
 class ParticleFilter:
     def __init__(self, latitudeLength, longitudeLength) -> None:
         self.num_particles = 750
         self.gps_noise = [0.45]
         self.odom_noise = [0.05, 0.05, 0.1]
         self.init_particles()
         self.first_gps = None
-        
-        self.latitudeLength = latitudeLength
-        self.longitudeLength = longitudeLength
+
+        self.latitude_length = latitudeLength
+        self.longitude_length = longitudeLength
 
     def init_particles(self, seedHeading: float = 0.0, useSeedHeading: bool = False):
         if useSeedHeading:
@@ -33,7 +35,7 @@ def feedback(self, feedback: MotorFeedback) -> list[float]:
         sum_theta_x = 0
         sum_theta_y = 0
         sum_weight = 0
-        
+
         for particle in self.particles:
             particle.x += feedback.delta_x * 1.2 * math.cos(particle.theta) + feedback.delta_y * math.sin(particle.theta)
             particle.y += feedback.delta_x * 1.2 * math.sin(particle.theta) + feedback.delta_y * math.cos(particle.theta)
@@ -45,44 +47,44 @@ def feedback(self, feedback: MotorFeedback) -> list[float]:
             sum_theta_x += math.cos(particle.theta) * weight
             sum_theta_y += math.sin(particle.theta) * weight
             sum_weight += weight
-            
+
         if sum_weight < 0.000001:
             sum_weight = 0.000001
-            
+
         avg_x = sum_x / sum_weight
         avg_y = sum_y / sum_weight
         avg_theta = math.atan2(sum_theta_y / sum_weight, sum_theta_x / sum_weight) % (2 * math.pi)
-        
+
         return [avg_x, avg_y, avg_theta]
-    
+
     def gps(self, gps: GPSFeedback) -> list[float]:
         if self.first_gps is None:
             self.first_gps = gps
-            
-        gps_x = (gps.latitude - self.first_gps.latitude) * self.latitudeLength
-        gps_y = (self.first_gps.longitude - gps.longitude) * self.longitudeLength
-    
+
+        gps_x = (gps.latitude - self.first_gps.latitude) * self.latitude_length
+        gps_y = (self.first_gps.longitude - gps.longitude) * self.longitude_length
+
         for particle in self.particles:
             dist_sqrt = np.sqrt((particle.x - gps_x) ** 2 + (particle.y - gps_y) ** 2)
             particle.weight = math.exp(-dist_sqrt / (2 * self.gps_noise[0] ** 2))
-            
+
         self.resample()
         return [gps_x, gps_y]
-    
+
     def resample(self) -> None:
         weights = [particle.weight for particle in self.particles]
         weights_sum = sum(weights)
         if weights_sum <= 0.00001:
             weights_sum = 0.00001
         weights = [weight / weights_sum for weight in weights]
-        
-        new_particles = random.choices(self.particles, weights, k = self.num_particles)
+
+        new_particles = random.choices(self.particles, weights, k=self.num_particles)
         self.particles = []
-        
+
         for particle in new_particles:
             rand_x = np.random.normal(0, self.odom_noise[0])
             rand_y = np.random.normal(0, self.odom_noise[1])
             x = particle.x + rand_x * math.cos(particle.theta) + rand_y * math.sin(particle.theta)
             y = particle.y + rand_x * math.sin(particle.theta) + rand_y * math.cos(particle.theta)
             theta = np.random.normal(particle.theta, self.odom_noise[2]) % (2 * math.pi)
-            self.particles.append(Particle(x, y, theta, particle.weight))
+            self.particles.append(Particle(x, y, theta, particle.weight))

autonav_ws/src/autonav_launch/launch/competition.xml

@@ -1,22 +1,18 @@
 <launch>
     <!-- Core -->
-    <node pkg="scr_controller" exec="core">
-        <param name="mode" value="1" /> 
-        <param name="state" value="1" /> 
-        <param name="mobility" value="false" /> 
-    </node>
+    <node pkg="scr_controller" exec="core" />
     <node pkg="scr_controller" exec="logging" />
 
     <!-- Filtering -->
     <node pkg="autonav_filters" exec="filters.py">
         <param name="latitude_length" value="111086.2" />
         <param name="longitude_length" value="81978.2" />
-        <param name="default_type" value="1" />
     </node>
 
     <!-- Vision Nodes -->
     <node pkg="autonav_vision" exec="transformations.py" />
     <node pkg="autonav_vision" exec="combination.py" />
+    <node pkg="autonav_vision" exec="expandify" />
 
     <!-- Manual Nodes -->
     <node pkg="autonav_manual" exec="steam.py" />
@@ -25,7 +21,12 @@
     <!-- Serial Nodes -->
     <node pkg="autonav_serial" exec="serial_node.py" />
     <node pkg="autonav_serial" exec="camera.py" />
+    <node pkg="autonav_serial" exec="vectornav_node" />
 
     <!-- Other -->
     <node pkg="autonav_display" exec="display.py" />
+
+    <!-- Robot -->
+    <node pkg="autonav_nav" exec="path_resolver.py" />
+    <node pkg="autonav_nav" exec="astar.py" />
 </launch>

autonav_ws/src/autonav_launch/launch/managed_steam.xml

@@ -1,17 +1,12 @@
 <launch>
     <!-- Core -->
-    <node pkg="scr_controller" exec="core">
-        <param name="mode" value="0" /> 
-        <param name="state" value="0" /> 
-        <param name="mobility" value="false" /> 
-    </node>
+    <node pkg="scr_controller" exec="core" />
     <node pkg="scr_controller" exec="logging" />
 
     <!-- Filtering -->
     <node pkg="autonav_filters" exec="filters.py">
-        <param name="latitude_length" value="111086.2" />
-        <param name="longitude_length" value="81978.2" />
-        <param name="default_type" value="1" />
+        <param name="latitude_length" value="110944.12" />
+        <param name="longitude_length" value="91071.17" />
     </node>
 
     <!-- Vision Nodes -->
@@ -25,6 +20,8 @@
     <!-- Serial Nodes -->
     <node pkg="autonav_serial" exec="serial_node.py" />
     <node pkg="autonav_serial" exec="camera.py" />
+    <node pkg="autonav_serial" exec="vectornav_node" />
+     <node pkg="autonav_serial" exec="vectornav_node" />
 
     <!-- Other -->
     <node pkg="autonav_display" exec="display.py" />

autonav_ws/src/autonav_launch/launch/practice.xml

@@ -1,17 +1,12 @@
 <launch>
     <!-- Core -->
-    <node pkg="scr_controller" exec="core">
-        <param name="mode" value="0" /> 
-        <param name="state" value="0" /> 
-        <param name="mobility" value="false" /> 
-    </node>
+    <node pkg="scr_controller" exec="core" />
     <node pkg="scr_controller" exec="logging" />
 
     <!-- Filtering -->
     <node pkg="autonav_filters" exec="filters.py">
         <param name="latitude_length" value="111086.2" />
         <param name="longitude_length" value="81978.2" />
-        <param name="default_type" value="1" />
     </node>
 
     <!-- Vision Nodes -->
@@ -25,6 +20,7 @@
     <!-- Serial Nodes -->
     <node pkg="autonav_serial" exec="serial_node.py" />
     <node pkg="autonav_serial" exec="camera.py" />
+     <node pkg="autonav_serial" exec="vectornav_node" />
 
     <!-- Other -->
     <node pkg="autonav_display" exec="display.py" />

autonav_ws/src/autonav_launch/launch/simulation.xml

@@ -3,22 +3,27 @@
     <include file="$(find-pkg-share rosbridge_server)/launch/rosbridge_websocket_launch.xml" />
     <node pkg="scr_controller" exec="core">
         <param name="mode" value="1" /> 
-        <param name="state" value="1" /> 
-        <param name="mobility" value="true" /> 
+        <param name="state" value="0" /> 
+        <param name="mobility" value="false" /> 
     </node>
     <node pkg="scr_controller" exec="logging" />
 
     <!-- Filtering -->
     <node pkg="autonav_filters" exec="filters.py">
-        <param name="latitude_length" value="111086.2" />
-        <param name="longitude_length" value="81978.2" />
-        <param name="default_type" value="1" />
+        <param name="latitude_length" value="110944.12" />
+        <param name="longitude_length" value="91071.17" />
     </node>
 
     <!-- Vision Nodes -->
     <node pkg="autonav_vision" exec="transformations.py" />
     <node pkg="autonav_vision" exec="combination.py" />
+    <node pkg="autonav_vision" exec="expandify" />
+
+    <!-- Pathing -->
+    <node pkg="autonav_nav" exec="path_resolver.py" />
+    <node pkg="autonav_nav" exec="astar.py" />
 
     <!-- Other -->
     <node pkg="autonav_display" exec="display.py" />
+    <node pkg="autonav_playback" exec="playback.py" />
 </launch>

autonav_ws/src/autonav_manual/src/steam.py

@@ -90,6 +90,7 @@ def onButtonReleased(self, button: SteamControllerButton, msTime: float):
             self.safetyLightsPublisher.publish(toSafetyLights(False, False, 2, 100, "#FF6F00"))
 
         if button == SteamControllerButton.STEAM and self.system_state != SystemStateEnum.AUTONOMOUS:
+            self.safetyLightsPublisher.publish(toSafetyLights(True, False, 2, 100, "#FF0000"))
             self.set_system_state(SystemStateEnum.AUTONOMOUS)
 
         if button == SteamControllerButton.BACK and self.system_state != SystemStateEnum.DISABLED:

autonav_ws/src/autonav_msgs/CMakeLists.txt

@@ -20,7 +20,6 @@ set(msg_files
   "msg/Position.msg"
   "msg/GoalPoint.msg"
   "msg/Waypoint.msg"
-  "msg/ObjectDetection.msg"
   "msg/Path.msg"
   "msg/Obstacle.msg"
   "msg/Obstacles.msg"