merged with main frfr

SoonerRobotics · May 23, 2024 · 1be4df2 · 1be4df2
2 parents ccd7b04 + 8d8da8b
commit 1be4df2
Show file tree

Hide file tree

Showing 53 changed files with 2,545 additions and 1,245 deletions.
diff --git a/autonav_ws/src/autonav_display/src/display.py b/autonav_ws/src/autonav_display/src/display.py
diff --git a/autonav_ws/src/autonav_filters/src/filters.cpp b/autonav_ws/src/autonav_filters/src/filters.cpp
@@ -61,6 +61,8 @@ void FiltersNode::on_reset() {
 
 void FiltersNode::config_updated(json newConfig) {
         config = newConfig.template get<ParticleFilterConfig>();
+        ParticleFilter particle_filter{config.num_particles, config.latitudeLength, config.longitudeLength, config.gps_noise, config.odom_noise_x, config.odom_noise_y, config.odom_noise_theta};
+        this->particle_filter = particle_filter;
 }
 
 json FiltersNode::get_default_config() {

diff --git a/autonav_ws/src/autonav_filters/src/filters.py b/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,97 +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):
         #self.get_logger().info("RECEIVED GPS")
         if msg.gps_fix == 0 and msg.is_locked == False:
             return
-
-        if self.firstGps is None:
-            self.firstGps = msg
-
-        #self.get_logger().info(f"firstGps.latitude {self.firstGps.latitude}\n")
-        #self.get_logger().info(f"firstGps.longitude {self.firstGps.longitude}\n")
 
-        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]
         #self.get_logger().info(f"position.x {position.x}\n")
         #self.get_logger().info(f"position.y {position.y}\n")
         position.theta = (-1 * math.pi * 2 + averages[2]) * 1
-        #self.get_logger().info(f"position.theta {position.theta}\n")
-
-        if self.firstGps is not None:
-            gps_x = self.firstGps.latitude + position.x / self.latitudeLength
-            gps_y = self.firstGps.longitude - position.y / self.longitudeLength
-            #self.get_logger().info(f"firstGps.latitude {self.firstGps.latitude}\n")
-            #self.get_logger().info(f"firstGps.longitude {self.firstGps.longitude}\n")
-            #self.get_logger().info(f"gps_x {gps_x}\n")
-            #self.get_logger().info(f"gps_y {gps_y}\n")
-
+
+        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
             #self.get_logger().info(f"position.latitude {position.latitude}\n")
             position.longitude = gps_y
-            #self.get_logger().info(f"position.longitude {position.longitude}\n")
-
-        if self.system_mode == SystemModeEnum.SIMULATION and self.lastGps is not None:
-            #position.latitude = self.lastGps.latitude
-            #position.longitude = self.lastGps.longitude
-            print("do nothing")
-
-        #self.get_logger().info(f"averages 0: {averages[0]}, averages 1: {averages[1]}, averages 2: {averages[2]}")
-        #self.get_logger().info(f"publishing position x: {position.x}, y: {position.y}, {position.theta}")
-        self.get_logger().info(f"position latitude: {position.latitude}, longitude {position.longitude}")
-        self.positionPublisher.publish(position)
+
+        self.position_publisher.publish(position)
 
 
 def main():

diff --git a/autonav_ws/src/autonav_filters/src/particlefilter.py b/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))
diff --git a/autonav_ws/src/autonav_launch/launch/competition.xml b/autonav_ws/src/autonav_launch/launch/competition.xml
@@ -1,22 +1,20 @@
 <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" />
+        <!-- <param name="latitude_length" value="111086.2" /> -->
+        <!-- <param name="longitude_length" value="81978.2" /> -->
+        <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" />
 
     <!-- Manual Nodes -->
     <node pkg="autonav_manual" exec="steam.py" />
@@ -25,7 +23,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>
diff --git a/autonav_ws/src/autonav_launch/launch/managed_steam.xml b/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" />

diff --git a/autonav_ws/src/autonav_launch/launch/practice.xml b/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" />