Feat/odometry #16
Feat/odometry #16
Conversation
also added commentary for future us, also turned a few lists that should've been tuples into tuples (a list is arbitrarily many things of a common type; what we want is a single thing with components, which a tuple fits better)
ok i think i understand the architecture now? danger zone does odometry in the autonav_filters node, and it's structured as a main script that runs the show and one or more distinct filters (dead reckoning and particle filter). also there's c++ for some reason and it seems less organized than and unrelated to the python stuff. somehow. this autonav_odometry could be similar but python only and with just a particle filter to start
update type names, remove dead reckoning, commentate, may need to change the filter terminology too
also it has debug output now comes in very handy when testing the odometer in rqt with messages that make no temporal sense (e.g. latitude changes by 1 deg between two gps readings)
| # find dependencies | ||
| find_package(ament_cmake_python REQUIRED) | ||
| find_package(rclpy REQUIRED) | ||
| find_package(autonav_shared REQUIRED) |
There was a problem hiding this comment.
you might need a find_package(autonav_msgs REQUIRED) in here too, I think, can't remember
| <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> | ||
| <package format="3"> | ||
| <name>autonav_odometry</name> | ||
| <version>0.1.0</version> |
There was a problem hiding this comment.
2025.0.1 (or 2025.1.0, whichever you prefer) please
| <buildtool_depend>ament_cmake_python</buildtool_depend> | ||
|
|
||
| <depend>rclpy</depend> | ||
| <depend>autonav_shared</depend> |
There was a problem hiding this comment.
might need a <depend>autonav_msgs</depend> here, can't remember
| from autonav_msgs.msg import MotorFeedback, GPSFeedback, Position | ||
| from autonav_shared.types import DeviceState, SystemState, LogLevel # i am goig to beat pylance with a rock | ||
| from particlefilter import ParticleFilter | ||
| # from scr_msgs.msg import SystemState |
There was a problem hiding this comment.
this was from last year's Core, you can safely delete this line
| self.gps_noise = 0.45 | ||
| self.odom_noise = (0, 0, 0.1) | ||
| # TODO should these parameters be in a config file in autonav_odometry/resource? | ||
| self.init_particles() |
There was a problem hiding this comment.
bit of a redundant call (as you call onReset() in init for FiltersNode line 34) but it's not that big of a deal.
what's a bigger deal is using the seed heading, which I think. . . actually, nevermind, because init_particles is called in FiltersNode after initialization when we would actually be able to send it a seed heading, so that's a problem for FiltersNode not ParticleFilter
| for particle in self.particles: | ||
| # update particles | ||
| # MotorFeedback gives robot-relative data (x is forward, y is left (???)) so we convert it to track-relative with trig magic | ||
| # TODO why are we scaling down delta_x and delta_theta again? |
There was a problem hiding this comment.
that would be a Tony question but basically because magic number
we might not need it with the swerve drive (or we might need a different magic number), that remains to be seen
| def onReset(self): | ||
| self.first_gps = None | ||
| self.last_gps = None | ||
| self.pf.init_particles() |
There was a problem hiding this comment.
I think we should use the seed heading parameter of init_particles, or at least make whether we use it or not controlled by the config. this would've been handy last year, as we only ever started facing one direction, whereas in 2023 you could start facing north or south.
| self.pf.init_particles() | ||
| # INIT NEW FILTERS HERE | ||
|
|
||
| def system_state_transition(self, old: SystemState, updated: SystemState): |
There was a problem hiding this comment.
doesn't currently exist this year in the Core but this is probably a good argument for implementing it this year so leave this here for now, and if it breaks when building because this function doesn't exist/get called then nudge Dylan or I and we can add it
| 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 |
There was a problem hiding this comment.
this might be more of an in-person conversation (maybe probably with Tony) but I think lat and long length should be part of the config instead of being launch file parameters.
It builds, runs, and seems to return numbers that make sense without crashing when I poke it in rqt. More meaningful testing could be done in the future when we have a robot.