From 032351775dfb9de884258a76634e98fac3bc7f5e Mon Sep 17 00:00:00 2001 From: dorukayhan Date: Thu, 10 Oct 2024 19:15:03 -0500 Subject: [PATCH 1/5] particlefilter.py from last year 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) --- .../autonav_odometry/__init__.py | 0 autonav_ws/src/autonav_odometry/package.xml | 18 ++++ .../resource/autonav_odometry | 0 autonav_ws/src/autonav_odometry/setup.cfg | 4 + autonav_ws/src/autonav_odometry/setup.py | 25 +++++ .../autonav_odometry/src/particlefilter.py | 101 ++++++++++++++++++ .../autonav_odometry/test/test_copyright.py | 25 +++++ .../src/autonav_odometry/test/test_flake8.py | 25 +++++ .../src/autonav_odometry/test/test_pep257.py | 23 ++++ 9 files changed, 221 insertions(+) create mode 100644 autonav_ws/src/autonav_odometry/autonav_odometry/__init__.py create mode 100644 autonav_ws/src/autonav_odometry/package.xml create mode 100644 autonav_ws/src/autonav_odometry/resource/autonav_odometry create mode 100644 autonav_ws/src/autonav_odometry/setup.cfg create mode 100644 autonav_ws/src/autonav_odometry/setup.py create mode 100644 autonav_ws/src/autonav_odometry/src/particlefilter.py create mode 100644 autonav_ws/src/autonav_odometry/test/test_copyright.py create mode 100644 autonav_ws/src/autonav_odometry/test/test_flake8.py create mode 100644 autonav_ws/src/autonav_odometry/test/test_pep257.py diff --git a/autonav_ws/src/autonav_odometry/autonav_odometry/__init__.py b/autonav_ws/src/autonav_odometry/autonav_odometry/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/autonav_ws/src/autonav_odometry/package.xml b/autonav_ws/src/autonav_odometry/package.xml new file mode 100644 index 0000000..4c0c776 --- /dev/null +++ b/autonav_ws/src/autonav_odometry/package.xml @@ -0,0 +1,18 @@ + + + + autonav_odometry + 0.1.0 + Thingy to track the robot's position with both GPS and motor odometry + Doruk Ayhan + MIT + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/autonav_ws/src/autonav_odometry/resource/autonav_odometry b/autonav_ws/src/autonav_odometry/resource/autonav_odometry new file mode 100644 index 0000000..e69de29 diff --git a/autonav_ws/src/autonav_odometry/setup.cfg b/autonav_ws/src/autonav_odometry/setup.cfg new file mode 100644 index 0000000..1f4a775 --- /dev/null +++ b/autonav_ws/src/autonav_odometry/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/autonav_odometry +[install] +install_scripts=$base/lib/autonav_odometry diff --git a/autonav_ws/src/autonav_odometry/setup.py b/autonav_ws/src/autonav_odometry/setup.py new file mode 100644 index 0000000..c25f805 --- /dev/null +++ b/autonav_ws/src/autonav_odometry/setup.py @@ -0,0 +1,25 @@ +from setuptools import find_packages, setup + +package_name = 'autonav_odometry' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='lunarsmurf', + maintainer_email='lunarsmurf@todo.todo', + description='TODO: Package description', + license='MIT', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + ], + }, +) diff --git a/autonav_ws/src/autonav_odometry/src/particlefilter.py b/autonav_ws/src/autonav_odometry/src/particlefilter.py new file mode 100644 index 0000000..8ddafc8 --- /dev/null +++ b/autonav_ws/src/autonav_odometry/src/particlefilter.py @@ -0,0 +1,101 @@ +### particlefilter.py from last year ### +# TODO could all the for-each-particle math in this code be done ✨🚀faster🚀✨ with numpy than vanilla python? +from autonav_msgs.msg import MotorFeedback, GPSFeedback, Position +import numpy as np +import math +import random + + +class Particle: + 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, 0, 0.1) + # TODO should these parameters be in a config file in autonav_odometry/resource? + self.init_particles() + # self.first_gps = None + + self.latitude_length = latitudeLength + self.longitude_length = longitudeLength + + def init_particles(self, seedHeading: float = 0.0, useSeedHeading: bool = False): + self.first_gps = None + if useSeedHeading: # particles generally point in an initial direction + self.particles = [Particle(0, 0, seedHeading + np.random.normal(0, 0.1)) for i in range(self.num_particles)] + else: # particles point literally everywhere + self.particles = [Particle(0, 0, i / self.num_particles * 2 * math.pi) for i in range(self.num_particles)] + + def feedback(self, feedback: MotorFeedback) -> tuple[float, float, float]: + sum_x = 0 + sum_y = 0 + sum_theta_x = 0 + sum_theta_y = 0 + sum_weight = 0 + + 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? + particle.x += feedback.delta_x * 0.95 * math.cos(particle.theta) + feedback.delta_y * math.sin(particle.theta) + particle.y += feedback.delta_x * 0.95 * math.sin(particle.theta) + feedback.delta_y * math.cos(particle.theta) + particle.theta += feedback.delta_theta * 0.8 + particle.theta = particle.theta % (2 * math.pi) + # weighted sum of particles + weight = particle.weight ** 2 + sum_x += particle.x * weight + sum_y += particle.y * weight + 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 + + # now weighted average + 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) -> tuple[float, float]: + if self.first_gps is None: + self.first_gps = gps + + # convert latlon to distance offset from first gps reading + gps_x = (gps.latitude - self.first_gps.latitude) * self.latitude_length + gps_y = (self.first_gps.longitude - gps.longitude) * self.longitude_length + + # adjust particle weights - closer to gps reading is better + for particle in self.particles: + dist_sqrt = np.sqrt((particle.x - gps_x) ** 2 + (particle.y - gps_y) ** 2) # root mean square error + particle.weight = math.exp(-dist_sqrt / (2 * self.gps_noise ** 2)) # less accurate gps = weights decrease smoother further from reading + + 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) + 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)) \ No newline at end of file diff --git a/autonav_ws/src/autonav_odometry/test/test_copyright.py b/autonav_ws/src/autonav_odometry/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/autonav_ws/src/autonav_odometry/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/autonav_ws/src/autonav_odometry/test/test_flake8.py b/autonav_ws/src/autonav_odometry/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/autonav_ws/src/autonav_odometry/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/autonav_ws/src/autonav_odometry/test/test_pep257.py b/autonav_ws/src/autonav_odometry/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/autonav_ws/src/autonav_odometry/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' From 7bb5aab88abd69d0887a4e4ed158af0d33c54d86 Mon Sep 17 00:00:00 2001 From: dorukayhan Date: Tue, 15 Oct 2024 18:46:58 -0500 Subject: [PATCH 2/5] filters.py from last year 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 --- autonav_ws/src/autonav_odometry/src/main.py | 116 ++++++++++++++++++++ 1 file changed, 116 insertions(+) create mode 100644 autonav_ws/src/autonav_odometry/src/main.py diff --git a/autonav_ws/src/autonav_odometry/src/main.py b/autonav_ws/src/autonav_odometry/src/main.py new file mode 100644 index 0000000..42b11ac --- /dev/null +++ b/autonav_ws/src/autonav_odometry/src/main.py @@ -0,0 +1,116 @@ +#!/usr/bin/env python3 +# filters.py from last year + +import json +from types import SimpleNamespace +from autonav_msgs.msg import MotorFeedback, GPSFeedback, Position, IMUData +from scr.states import DeviceStateEnum, SystemStateEnum, SystemModeEnum +from particlefilter import ParticleFilter +from scr_msgs.msg import SystemState +from autonav_shared import Node +from enum import IntEnum +import rclpy +import math + +class FilterType(IntEnum): + DEAD_RECKONING = 0, + PARTICLE_FILTER = 1 + + +class FiltersNodeConfig: + def __init__(self): + self.filter_type = FilterType.PARTICLE_FILTER + + +class FiltersNode(Node): + def __init__(self): + super().__init__("autonav_filters") + + 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): + self.config = json.loads(self.jdump(jsonObject), object_hook=lambda d: SimpleNamespace(**d)) + self.onReset() + + def get_default_config(self): + return FiltersNodeConfig() + + def init(self): + self.create_subscription(GPSFeedback, "/autonav/gps", self.onGPSReceived, 20) + self.create_subscription(MotorFeedback, "/autonav/MotorFeedback", self.onMotorFeedbackReceived, 20) + self.position_publisher = self.create_publisher(Position, "/autonav/position", 20) + + self.set_device_state(DeviceStateEnum.OPERATING) + + def onReset(self): + self.first_gps = None + self.last_gps = None + 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.state != SystemStateEnum.MANUAL and updated.state == SystemStateEnum.MANUAL: + 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.first_gps is None: + self.first_gps = msg + + self.last_gps = msg + if self.config.filter_type == FilterType.PARTICLE_FILTER: + self.pf.gps(msg) + else: + self.dr.gps(msg) + + def onMotorFeedbackReceived(self, msg: MotorFeedback): + averages = None + if self.config.filter_type == FilterType.PARTICLE_FILTER: + averages = self.pf.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 = averages[2] + + 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 + + self.position_publisher.publish(position) + + +def main(): + rclpy.init() + node = FiltersNode() + Node.run_node(node) + rclpy.shutdown() + + +if __name__ == "__main__": + main() From 60ac2bf3f3f83502ccdd0dccdc7525d4fb8ab44e Mon Sep 17 00:00:00 2001 From: dorukayhan Date: Tue, 15 Oct 2024 19:44:06 -0500 Subject: [PATCH 3/5] clean up main.py update type names, remove dead reckoning, commentate, may need to change the filter terminology too --- autonav_ws/src/autonav_odometry/src/main.py | 51 ++++++++++----------- 1 file changed, 23 insertions(+), 28 deletions(-) diff --git a/autonav_ws/src/autonav_odometry/src/main.py b/autonav_ws/src/autonav_odometry/src/main.py index 42b11ac..7153539 100644 --- a/autonav_ws/src/autonav_odometry/src/main.py +++ b/autonav_ws/src/autonav_odometry/src/main.py @@ -1,70 +1,65 @@ #!/usr/bin/env python3 -# filters.py from last year +# filters.py from last year for the most part import json from types import SimpleNamespace -from autonav_msgs.msg import MotorFeedback, GPSFeedback, Position, IMUData -from scr.states import DeviceStateEnum, SystemStateEnum, SystemModeEnum +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 -from autonav_shared import Node +from autonav_shared.node import Node from enum import IntEnum import rclpy -import math class FilterType(IntEnum): - DEAD_RECKONING = 0, PARTICLE_FILTER = 1 - + # ADD NEW FILTERS HERE class FiltersNodeConfig: def __init__(self): self.filter_type = FilterType.PARTICLE_FILTER - class FiltersNode(Node): def __init__(self): super().__init__("autonav_filters") - + # init gps and position 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 - + # init filters + # IF WE IMPLEMENT MORE FILTERS ADD THEM HERE (AND ANYWHERE THAT COMES UP WHEN YOU CTRL-F "NEW FILTERS")! self.pf = ParticleFilter(self.latitude_length, self.longitude_length) - self.dr = DeadReckoningFilter() - self.config = self.get_default_config() - + self.config = FiltersNodeConfig() self.onReset() def config_updated(self, jsonObject): self.config = json.loads(self.jdump(jsonObject), object_hook=lambda d: SimpleNamespace(**d)) + # reinit filters after getting new config self.onReset() - def get_default_config(self): - return FiltersNodeConfig() - def init(self): + # [melee announcer voice] ready... self.create_subscription(GPSFeedback, "/autonav/gps", self.onGPSReceived, 20) self.create_subscription(MotorFeedback, "/autonav/MotorFeedback", self.onMotorFeedbackReceived, 20) self.position_publisher = self.create_publisher(Position, "/autonav/position", 20) - - self.set_device_state(DeviceStateEnum.OPERATING) + # go! + self.set_device_state(DeviceState.OPERATING) def onReset(self): self.first_gps = None self.last_gps = None - self.dr.reset() self.pf.init_particles() + # INIT NEW FILTERS HERE def system_state_transition(self, old: SystemState, updated: SystemState): - if old.state != SystemStateEnum.AUTONOMOUS and updated.state == SystemStateEnum.AUTONOMOUS: + # reinit filters when robot changes modes + if old.state != SystemState.AUTONOMOUS and updated.state == SystemState.AUTONOMOUS: self.onReset() - if old.state != SystemStateEnum.MANUAL and updated.state == SystemStateEnum.MANUAL: + if old.state != SystemState.MANUAL and updated.state == SystemState.MANUAL: self.onReset() - + # or gets going if old.mobility == False and updated.mobility == True: self.onReset() @@ -76,21 +71,23 @@ def onGPSReceived(self, msg: GPSFeedback): self.first_gps = msg self.last_gps = msg + # USE NEW FILTERS HERE if self.config.filter_type == FilterType.PARTICLE_FILTER: self.pf.gps(msg) else: - self.dr.gps(msg) + self.log(f"{self.config.filter_type} isn't a valid FilterType! Did you implement a new filter and forget to use it?", LogLevel.FATAL) def onMotorFeedbackReceived(self, msg: MotorFeedback): averages = None + # USE NEW FILTERS HERE if self.config.filter_type == FilterType.PARTICLE_FILTER: averages = self.pf.feedback(msg) else: - averages = self.dr.feedback(msg) + self.log(f"{self.config.filter_type} isn't a valid FilterType! Did you implement a new filter and forget to use it?", LogLevel.FATAL) if averages is None: return - + # unpack what the filter just returned position = Position() position.x = averages[0] position.y = averages[1] @@ -104,13 +101,11 @@ def onMotorFeedbackReceived(self, msg: MotorFeedback): self.position_publisher.publish(position) - def main(): rclpy.init() node = FiltersNode() Node.run_node(node) rclpy.shutdown() - if __name__ == "__main__": main() From 2ca1921d225efd9fea2ffea7f16a06bf541592cb Mon Sep 17 00:00:00 2001 From: dorukayhan Date: Tue, 12 Nov 2024 18:41:40 -0600 Subject: [PATCH 4/5] having a working build is important innit? --- autonav_ws/src/autonav_launch/launch/test.xml | 1 + .../src/autonav_odometry/CMakeLists.txt | 24 +++++++++++++++++ autonav_ws/src/autonav_odometry/package.xml | 26 +++++++++---------- autonav_ws/src/autonav_odometry/setup.cfg | 4 --- autonav_ws/src/autonav_odometry/setup.py | 25 ------------------ autonav_ws/src/autonav_odometry/src/main.py | 6 ++--- .../autonav_odometry/test/test_copyright.py | 25 ------------------ .../src/autonav_odometry/test/test_flake8.py | 25 ------------------ .../src/autonav_odometry/test/test_pep257.py | 23 ---------------- 9 files changed, 41 insertions(+), 118 deletions(-) create mode 100644 autonav_ws/src/autonav_odometry/CMakeLists.txt delete mode 100644 autonav_ws/src/autonav_odometry/setup.cfg delete mode 100644 autonav_ws/src/autonav_odometry/setup.py delete mode 100644 autonav_ws/src/autonav_odometry/test/test_copyright.py delete mode 100644 autonav_ws/src/autonav_odometry/test/test_flake8.py delete mode 100644 autonav_ws/src/autonav_odometry/test/test_pep257.py diff --git a/autonav_ws/src/autonav_launch/launch/test.xml b/autonav_ws/src/autonav_launch/launch/test.xml index a7b130c..fff7967 100644 --- a/autonav_ws/src/autonav_launch/launch/test.xml +++ b/autonav_ws/src/autonav_launch/launch/test.xml @@ -1,4 +1,5 @@ + \ No newline at end of file diff --git a/autonav_ws/src/autonav_odometry/CMakeLists.txt b/autonav_ws/src/autonav_odometry/CMakeLists.txt new file mode 100644 index 0000000..7764169 --- /dev/null +++ b/autonav_ws/src/autonav_odometry/CMakeLists.txt @@ -0,0 +1,24 @@ +cmake_minimum_required(VERSION 3.8) +project(autonav_odometry) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake_python REQUIRED) +find_package(rclpy REQUIRED) +find_package(autonav_shared REQUIRED) + +# Install Python modules +ament_python_install_package(${PROJECT_NAME}) + +# Install Python programs +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 + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() diff --git a/autonav_ws/src/autonav_odometry/package.xml b/autonav_ws/src/autonav_odometry/package.xml index 4c0c776..0f7ed09 100644 --- a/autonav_ws/src/autonav_odometry/package.xml +++ b/autonav_ws/src/autonav_odometry/package.xml @@ -1,18 +1,18 @@ - autonav_odometry - 0.1.0 - Thingy to track the robot's position with both GPS and motor odometry - Doruk Ayhan - MIT + autonav_odometry + 0.1.0 + Thingies to track the robot's position + Doruk Ayhan + MIT - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest + ament_cmake_python - - ament_python - - + rclpy + autonav_shared + + + ament_cmake + + \ No newline at end of file diff --git a/autonav_ws/src/autonav_odometry/setup.cfg b/autonav_ws/src/autonav_odometry/setup.cfg deleted file mode 100644 index 1f4a775..0000000 --- a/autonav_ws/src/autonav_odometry/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/autonav_odometry -[install] -install_scripts=$base/lib/autonav_odometry diff --git a/autonav_ws/src/autonav_odometry/setup.py b/autonav_ws/src/autonav_odometry/setup.py deleted file mode 100644 index c25f805..0000000 --- a/autonav_ws/src/autonav_odometry/setup.py +++ /dev/null @@ -1,25 +0,0 @@ -from setuptools import find_packages, setup - -package_name = 'autonav_odometry' - -setup( - name=package_name, - version='0.0.0', - packages=find_packages(exclude=['test']), - data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - ], - install_requires=['setuptools'], - zip_safe=True, - maintainer='lunarsmurf', - maintainer_email='lunarsmurf@todo.todo', - description='TODO: Package description', - license='MIT', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - ], - }, -) diff --git a/autonav_ws/src/autonav_odometry/src/main.py b/autonav_ws/src/autonav_odometry/src/main.py index 7153539..d951eef 100644 --- a/autonav_ws/src/autonav_odometry/src/main.py +++ b/autonav_ws/src/autonav_odometry/src/main.py @@ -6,7 +6,7 @@ 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 +# from scr_msgs.msg import SystemState from autonav_shared.node import Node from enum import IntEnum import rclpy @@ -21,7 +21,7 @@ def __init__(self): class FiltersNode(Node): def __init__(self): - super().__init__("autonav_filters") + super().__init__("autonav_odometry") # init gps and position self.first_gps = None self.last_gps = None @@ -104,7 +104,7 @@ def onMotorFeedbackReceived(self, msg: MotorFeedback): def main(): rclpy.init() node = FiltersNode() - Node.run_node(node) + rclpy.spin(node) rclpy.shutdown() if __name__ == "__main__": diff --git a/autonav_ws/src/autonav_odometry/test/test_copyright.py b/autonav_ws/src/autonav_odometry/test/test_copyright.py deleted file mode 100644 index 97a3919..0000000 --- a/autonav_ws/src/autonav_odometry/test/test_copyright.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_copyright.main import main -import pytest - - -# Remove the `skip` decorator once the source file(s) have a copyright header -@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found errors' diff --git a/autonav_ws/src/autonav_odometry/test/test_flake8.py b/autonav_ws/src/autonav_odometry/test/test_flake8.py deleted file mode 100644 index 27ee107..0000000 --- a/autonav_ws/src/autonav_odometry/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_flake8.main import main_with_errors -import pytest - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ - '\n'.join(errors) diff --git a/autonav_ws/src/autonav_odometry/test/test_pep257.py b/autonav_ws/src/autonav_odometry/test/test_pep257.py deleted file mode 100644 index b234a38..0000000 --- a/autonav_ws/src/autonav_odometry/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' From 06c2344c6945adfa755c4382249a76882b88fa4f Mon Sep 17 00:00:00 2001 From: dorukayhan Date: Tue, 12 Nov 2024 20:06:35 -0600 Subject: [PATCH 5/5] we can now teleport across state lines without crashing particlefilter 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) --- autonav_ws/src/autonav_odometry/src/main.py | 6 ++++-- autonav_ws/src/autonav_odometry/src/particlefilter.py | 3 ++- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/autonav_ws/src/autonav_odometry/src/main.py b/autonav_ws/src/autonav_odometry/src/main.py index d951eef..5b5c972 100644 --- a/autonav_ws/src/autonav_odometry/src/main.py +++ b/autonav_ws/src/autonav_odometry/src/main.py @@ -73,7 +73,8 @@ def onGPSReceived(self, msg: GPSFeedback): self.last_gps = msg # USE NEW FILTERS HERE if self.config.filter_type == FilterType.PARTICLE_FILTER: - self.pf.gps(msg) + gps_x, gps_y = self.pf.gps(msg) + self.log(f"PF back from GPS ping w/ X={gps_x}, Y={gps_y}", LogLevel.DEBUG) else: self.log(f"{self.config.filter_type} isn't a valid FilterType! Did you implement a new filter and forget to use it?", LogLevel.FATAL) @@ -81,7 +82,8 @@ def onMotorFeedbackReceived(self, msg: MotorFeedback): averages = None # USE NEW FILTERS HERE if self.config.filter_type == FilterType.PARTICLE_FILTER: - averages = self.pf.feedback(msg) + avg_x, avg_y, avg_theta = self.pf.feedback(msg) + self.log(f"PF back from motor odom w/ X={avg_x}, Y={avg_y}, θ={avg_theta}", LogLevel.DEBUG) else: self.log(f"{self.config.filter_type} isn't a valid FilterType! Did you implement a new filter and forget to use it?", LogLevel.FATAL) diff --git a/autonav_ws/src/autonav_odometry/src/particlefilter.py b/autonav_ws/src/autonav_odometry/src/particlefilter.py index 8ddafc8..2013f48 100644 --- a/autonav_ws/src/autonav_odometry/src/particlefilter.py +++ b/autonav_ws/src/autonav_odometry/src/particlefilter.py @@ -78,7 +78,8 @@ def gps(self, gps: GPSFeedback) -> tuple[float, float]: for particle in self.particles: dist_sqrt = np.sqrt((particle.x - gps_x) ** 2 + (particle.y - gps_y) ** 2) # root mean square error particle.weight = math.exp(-dist_sqrt / (2 * self.gps_noise ** 2)) # less accurate gps = weights decrease smoother further from reading - + if particle.weight <= 0.00001 / len(self.particles): # prevent resample() from blowing up if bot teleports 100 km away + particle.weight = 0.00001 / len(self.particles) self.resample() return (gps_x, gps_y)