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)
danielbrownmsm
left a comment
danielbrownmsm
left a comment
There was a problem hiding this comment.
just a few notes and things, if you have any questions feel free to ask (and Tony is probably the better person to ask because he wrote this, I don't know how the particle filter actually works.
| 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
There was a problem hiding this comment.
noise values should absolutely be in the config
| 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
There was a problem hiding this comment.
These are fudge factors and they should probably go in the config. Last year I used 1.2 for example. Just depends on the robot/ simulator.
| 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.
| install(PROGRAMS | ||
| # enumerate every file in src by hand here like a bozo because globs don't just work i guess | ||
| src/main.py | ||
| src/particlefilter.py |
There was a problem hiding this comment.
particlefilter.py just creates a class that the node uses. It doesn't need to be enumerated here since it's not a node.
| <?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.x.y, follow example in autonav_example_py
antonioc76
left a comment
antonioc76
left a comment
There was a problem hiding this comment.
Good way to test is to just instantiate a particlefilter object and run it on last years data, which is all time stamped. Data is available in autonav_software_2024 on the pftest-tony branch, in autonav_filters/tests.
I believe ros2 has support for pytest with colcon test. I think it would be great for the team to learn how to use that and round out our testing framework knowledge.
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.