diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..18b14d6 --- /dev/null +++ b/.gitignore @@ -0,0 +1,36 @@ +# ROS2 build directories +build/ +install/ +log/ + +# Python cache +_pycache_/ +*.pyc +*.pyo +*.pyd + +# CMake +CMakeCache.txt +CMakeFiles/ +cmake_install.cmake +Makefile + +# Colcon +.colcon/ + +# Editor / IDE +.vscode/ +.idea/ +*.swp +*~ + +# OS-specific +.DS_Store +Thumbs.db + +# Python venvs +*env*/ +.env/ + +# ROS2 launch logs +*.launch.pyc diff --git a/LICENSE b/LICENSE deleted file mode 100644 index 6e0a232..0000000 --- a/LICENSE +++ /dev/null @@ -1,21 +0,0 @@ -MIT License - -Copyright (c) 2025 Wisconsin Robotics - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in all -copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -SOFTWARE. diff --git a/README.md b/README.md deleted file mode 100644 index 2576d5d..0000000 --- a/README.md +++ /dev/null @@ -1,2 +0,0 @@ -# WRoverSoftware_25-26 -Official Wisconsin Robotics software repository for the 2026 University Rover Challenge, containing autonomy, control, and base station code. diff --git a/localization_workspace/src/wr_compass/CMakeLists.txt b/localization_workspace/src/wr_compass/CMakeLists.txt new file mode 100644 index 0000000..aa9d913 --- /dev/null +++ b/localization_workspace/src/wr_compass/CMakeLists.txt @@ -0,0 +1,47 @@ +cmake_minimum_required(VERSION 3.8) +project(wr_compass) + +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 REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(Threads REQUIRED) +find_package(SDL2 REQUIRED) +find_package(phoenix6 REQUIRED) # Ensure this is the correct package name +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +add_executable(compass src/compass.cpp) +ament_target_dependencies(compass rclcpp std_msgs sensor_msgs phoenix6) + + +target_include_directories(compass PUBLIC + $ + $) +target_compile_features(compass PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 + +install(TARGETS compass + DESTINATION lib/${PROJECT_NAME}) + + # Link against the correct libraries +target_link_libraries(compass phoenix6 Threads::Threads ${SDL2_LIBRARIES}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/localization_workspace/src/wr_compass/package.xml b/localization_workspace/src/wr_compass/package.xml new file mode 100644 index 0000000..f4c1ae5 --- /dev/null +++ b/localization_workspace/src/wr_compass/package.xml @@ -0,0 +1,18 @@ + + + + wr_compass + 0.0.0 + TODO: Package description + wiscrobo + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/localization_workspace/src/wr_compass/src/compass.cpp b/localization_workspace/src/wr_compass/src/compass.cpp new file mode 100644 index 0000000..5323ef5 --- /dev/null +++ b/localization_workspace/src/wr_compass/src/compass.cpp @@ -0,0 +1,97 @@ +#include +#include "rclcpp/rclcpp.hpp" +#include // Include this for std::bind4 +#include +#include +#include +#include "ctre/phoenix6/Pigeon2.hpp" +#include "sensor_msgs/msg/imu.hpp" +#include +#include + + +using namespace ctre::phoenix6; +using namespace std::chrono_literals; + + +class CompassDataPublisher : public rclcpp::Node { + public: + CompassDataPublisher() + : + Node("compass"), + pigeon2imu(10, "can0") + { + publisher_quat = this->create_publisher("imu_quat_data", 10); + publisher_euler = this->create_publisher("imu_euler_data", 10); + timer_ = this->create_wall_timer( + 500ms, std::bind(&CompassDataPublisher::timer_callback, this)); + } + + private: + void timer_callback() + { + sensor_msgs::msg::Imu quat_message; + + double qx = pigeon2imu.GetQuatX().GetValue().value(); + double qy = pigeon2imu.GetQuatY().GetValue().value(); + double qz = pigeon2imu.GetQuatZ().GetValue().value(); + double qw = pigeon2imu.GetQuatW().GetValue().value(); + //normalizing values + double n = std::sqrt(qx*qx + qy*qy + qz*qz + qw*qw); + if (n > 0.0) { + qx /= n; qy /= n; qz /= n; qw /= n; + } + + double gx = pigeon2imu.GetAngularVelocityX().GetValue().value(); + double gy = pigeon2imu.GetAngularVelocityY().GetValue().value(); + double gz = pigeon2imu.GetAngularVelocityZ().GetValue().value(); + + constexpr double deg2rad = std::numbers::pi / 180.0; + gx *= deg2rad; + gy *= deg2rad; + gz *= deg2rad; + + double ax = pigeon2imu.GetAccelerationX().GetValue().value(); + double ay = pigeon2imu.GetAccelerationY().GetValue().value(); + double az = pigeon2imu.GetAccelerationZ().GetValue().value(); + + quat_message.orientation.x = qx; + quat_message.orientation.y = qy; + quat_message.orientation.z = qz; + quat_message.orientation.w = qw; + + quat_message.angular_velocity.x = gx; + quat_message.angular_velocity.y = gy; + quat_message.angular_velocity.z = gz; + + quat_message.linear_acceleration.x = ax; + quat_message.linear_acceleration.y = ay; + quat_message.linear_acceleration.z = az; + + publisher_quat->publish(quat_message); + + //euler + sensor_msgs::msg::Imu euler_message; + + euler_message.orientation.x = pigeon2imu.GetRoll().GetValue().value() * deg2rad; + euler_message.orientation.y = pigeon2imu.GetPitch().GetValue().value() * deg2rad; + euler_message.orientation.z = pigeon2imu.GetYaw().GetValue().value() * deg2rad; + + euler_message.angular_velocity = quat_message.angular_velocity; + euler_message.linear_acceleration = quat_message.linear_acceleration; + + publisher_euler->publish(euler_message); + } + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr publisher_quat; + rclcpp::Publisher::SharedPtr publisher_euler; + hardware::Pigeon2 pigeon2imu; +}; + + +int main(int argc, char *argv[]) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/localization_workspace/src/wr_fusion/package.xml b/localization_workspace/src/wr_fusion/package.xml new file mode 100644 index 0000000..23facec --- /dev/null +++ b/localization_workspace/src/wr_fusion/package.xml @@ -0,0 +1,18 @@ + + + + wr_fusion + 0.0.0 + TODO: Package description + wiscrobo + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/docs/.gitkeep b/localization_workspace/src/wr_fusion/resource/wr_fusion similarity index 100% rename from docs/.gitkeep rename to localization_workspace/src/wr_fusion/resource/wr_fusion diff --git a/localization_workspace/src/wr_fusion/setup.cfg b/localization_workspace/src/wr_fusion/setup.cfg new file mode 100644 index 0000000..63a76ed --- /dev/null +++ b/localization_workspace/src/wr_fusion/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/wr_fusion +[install] +install_scripts=$base/lib/wr_fusion diff --git a/localization_workspace/src/wr_fusion/setup.py b/localization_workspace/src/wr_fusion/setup.py new file mode 100644 index 0000000..c0839f8 --- /dev/null +++ b/localization_workspace/src/wr_fusion/setup.py @@ -0,0 +1,26 @@ +from setuptools import find_packages, setup + +package_name = 'wr_fusion' + +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='wiscrobo', + maintainer_email='nicolasdittmarg1@gmail.com', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'fusion = wr_fusion.fusion:main' + ], + }, +) diff --git a/localization_workspace/src/wr_fusion/test/test_copyright.py b/localization_workspace/src/wr_fusion/test/test_copyright.py new file mode 100644 index 0000000..97a3919 --- /dev/null +++ b/localization_workspace/src/wr_fusion/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/localization_workspace/src/wr_fusion/test/test_flake8.py b/localization_workspace/src/wr_fusion/test/test_flake8.py new file mode 100644 index 0000000..27ee107 --- /dev/null +++ b/localization_workspace/src/wr_fusion/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/localization_workspace/src/wr_fusion/test/test_pep257.py b/localization_workspace/src/wr_fusion/test/test_pep257.py new file mode 100644 index 0000000..b234a38 --- /dev/null +++ b/localization_workspace/src/wr_fusion/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' diff --git a/src/.gitkeep b/localization_workspace/src/wr_fusion/wr_fusion/__init__.py similarity index 100% rename from src/.gitkeep rename to localization_workspace/src/wr_fusion/wr_fusion/__init__.py diff --git a/localization_workspace/src/wr_fusion/wr_fusion/fusion.py b/localization_workspace/src/wr_fusion/wr_fusion/fusion.py new file mode 100644 index 0000000..8afcc56 --- /dev/null +++ b/localization_workspace/src/wr_fusion/wr_fusion/fusion.py @@ -0,0 +1,285 @@ +import rclpy +import numpy as np +from rclpy.node import Node +from sensor_msgs.msg import Imu, NavSatFix +from nav_msgs.msg import Odometry +from pyproj import CRS, Transformer + +class FusionNode(Node): + def __init__(self): + super().__init__('fusion_node') + + self.imu_state = { + 'quaternion' : np.array([0.0, 0.0, 0.0, 1.0]), + 'angular_velocity' : np.zeros(3), + 'linear_acceleration' : np.zeros(3) + } + + self.gnss_ref = None # will store initial lat/lon for later conversions + self.gnss_state = np.zeros(2) + + self.state_vector = np.zeros(10) + self.H = np.zeros((3,10)) #Observation + self.H[0,0] = 1 + self.H[1,1] = 1 + self.H[2,2] = 1 + self.P = np.eye(10) * 0.1 #covariance + self.Q = np.eye(10) * 0.01 #process noise, need to measure from the robot + self.R = np.eye(3) * 5.0 # GNSS measurement noise, 5.0 is placeholder for how many meters^2 accurate the gnss is, need to measure + self.R[2,2] = 1e6 #makes it so z is largely ignored as we arent measuring it too closely for now + self.last_time = None + self.initialized = False + + self.imu_sub = self.create_subscription(Imu, '/imu_quat_data', self.imu_callback, 10) + self.gnss_sub = self.create_subscription(NavSatFix, 'fix', self.gnss_callback, 10) + self.pub = self.create_publisher(Odometry, '/fused_data', 10) + + self.timer = self.create_timer(0.02, self.fusion_timer_callback) + + self.utm_transformer = None + self.utm_crs = None + self.utm_zone = None + self.initial_yaw = None + + + def fusion_timer_callback(self): + now = self.get_clock().now().nanoseconds() * 1e-9 + dt = 0 if self.last_time is None else now - self.last_time + self.last_time = now + + self.fusion(dt) + + def imu_callback(self, msg: Imu): + self.imu_state['quaternion'] = np.array([ + msg.orientation.x, + msg.orientation.y, + msg.orientation.z, + msg.orientation.w + ]) + + self.imu_state['angular_velocity'] = np.array([ + msg.angular_velocity.x, + msg.angular_velocity.y, + msg.angular_velocity.z + ]) + + self.imu_state['linear_acceleration'] = np.array([ + msg.linear_acceleration.x, + msg.linear_acceleration.y, + msg.linear_acceleration.z + ]) + + def gnss_callback(self, msg: NavSatFix): + if self.gnss_ref is None: + self.gnss_ref = np.array([msg.latitude, msg.longitude]) + self.gnss_state = np.array([msg.latitude, msg.longitude]) + + def initialize_state(self): + if self.initialized: + return + + if np.linalg.norm(self.imu_state['quaternion']) == 0: + return + + if self.gnss_ref is None and np.linalg.norm(self.gnss_state) != 0: + self.gnss_ref = self.gnss_state.copy() + + if self.gnss_ref is not None: + position = self.to_cartesian(self.gnss_state) + else: + position = np.zeros(3) + + self.state_vector[0:3] = position + self.state_vector[3:6] = np.zeros(3) + self.state_vector[6:10] = self.imu_state['quaternion'] + + self.initialized = True + self.last_time = 0 + + + def fusion(self, dt): + self.initialize_state() + if not self.initialized: + return + + self.state_vector[6:10] = self.imu_state['quaternion'] + self.update_position_state(dt) + self.update_covariance(dt) + + self.gnss_correction() + + self.publish_fused_state() + + def update_position_state(self, dt): + px, py, pz = self.state_vector[0:3] + vx, vy, vz = self.state_vector[3:6] + q = self.state_vector[6:10] + + acceleration_body = self.imu_state['linear_acceleration'] + + rotation = self.quaternion_to_rotation(q) + acceleration_world = rotation @ acceleration_body + + gravity = np.array([0, 0, 9.8]) + acceleration_world -= gravity + + vx_updated = vx + acceleration_world[0] * dt + vy_updated = vy + acceleration_world[1] * dt + vz_updated = vz + acceleration_world[2] * dt + + px_updated = px + vx * dt + 0.5 * acceleration_world[0] * dt * dt + py_updated = py + vy * dt + 0.5 * acceleration_world[1] * dt * dt + pz_updated = pz + vz * dt + 0.5 * acceleration_world[2] * dt * dt + + self.state_vector[0:3] = [px_updated, py_updated, pz_updated] + self.state_vector[3:6] = [vx_updated, vy_updated, vz_updated] + + def update_covariance(self, dt): + F = np.eye(10) #Jacobian of IMU state + F[0, 3] = dt + F[1, 4] = dt + F[2, 5] = dt + + self.P = F @ self.P @ F.transpose() + self.Q + + def compute_kalman_gain(self): + S = self.H @ self.P @ self.H.transpose() + self.R + return self.P @ self.H.transpose() @ np.linalg.inv(S) + + def gnss_correction(self): + if self.gnss_ref is None or np.linalg.norm(self.gnss_state) == 0: + return + + current = self.to_cartesian(self.gnss_state) + expected_current = self.H @ self.state_vector + error = current - expected_current + + K = self.compute_kalman_gain() + + self.state_vector = self.state_vector + (K @ error) + + I = np.eye(self.P.shape[0]) + self.P = (I - K @ self.H) @ self.P + + #renormalize quartinion + q = self.state_vector[6:10] + qn = np.linalg.norm(q) + if qn > 1e-12: + self.state_vector[6:10] = q / qn + + def to_cartesian(self, latlon): + #converts gnss to local cartesian frame + #x-> East + #y-> North + lat, lon = latlon + lat0, lon0 = self.gnss_ref + + R = 6378137.0 #radius of earth in meters + + lat_rad = np.deg2rad(lat) + lon_rad = np.deg2rad(lon) + lat0_rad = np.deg2rad(lat0) + lon0_rad = np.deg2rad(lon0) + + dlat = lat_rad - lat0_rad + dlon = lon_rad - lon0_rad + + x = dlon * np.cos(lat0_rad) * R + y = dlat * R + z = 0 + + return np.array([x, y, z]) + + def quaternion_to_rotation(self, q): + qx, qy, qz, qw = q + + xx = qx * qx + yy = qy * qy + zz = qz * qz + xy = qx * qy + xz = qx * qz + yz = qy * qz + wx = qw * qx + wy = qw * qy + wz = qw * qz + + rotation = np.array([ + [1 - 2 * (yy + zz), 2 * (xy - wz), 2 * (xz + wy)], + [2 * (xy + wz), 1 - 2 * (xx + zz), 2 * (yz - wx)], + [2 * (xz - wy), 2 * (yz + wx), 1 - 2 * (xx + yy)] + ]) + + return rotation + + def publish_fused_state(self): + if not self.initialized or self.gnss_ref is None: + return + + R = 6378137.0 + + x_local, y_local, _ = self.state_vector[0:3] + + lat0, lon0 = self.gnss_ref + lat0_rad = np.deg2rad(lat0) + + lat = lat0 + (y_local / R) * (180.0 / np.pi) + lon = lon0 + (x_local / (R * np.cos(lat0_rad))) * (180.0 / np.pi) + + # Ensure UTM transformer exists (based on start zone) + self.ensure_utm(lat0, lon0) + + # Convert to UTM (meters) + easting, northing = self.utm_transformer.transform(lon, lat) + + # Yaw from quaternion + qx, qy, qz, qw = self.state_vector[6:10] + yaw = np.arctan2( + 2.0 * (qw * qz + qx * qy), + 1.0 - 2.0 * (qy * qy + qz * qz) + ) + + if self.initial_yaw is None: + self.initial_yaw = yaw + yaw_rel = yaw - self.initial_yaw + yaw_rel = (yaw_rel + np.pi) % (2.0 * np.pi) - np.pi # wrap [-pi, pi] + + + msg = Odometry() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = f"utm_zone_{self.utm_zone}" + msg.child_frame_id = "base_link" + + msg.pose.pose.position.x = float(easting) + msg.pose.pose.position.y = float(northing) + msg.pose.pose.position.z = 0.0 + + # Quaternion for yaw_rel (roll=pitch=0) + msg.pose.pose.orientation.z = float(np.sin(yaw_rel / 2.0)) + msg.pose.pose.orientation.w = float(np.cos(yaw_rel / 2.0)) + + self.pub.publish(msg) + + + def ensure_utm(self, lat, lon): + # Build transformer once (based on start position) + if self.utm_transformer is not None: + return + + zone = int(np.floor((lon + 180.0) / 6.0) + 1) + north = lat >= 0.0 + epsg = 32600 + zone if north else 32700 + zone # WGS84 UTM + + self.utm_zone = zone + self.utm_crs = CRS.from_epsg(epsg) + self.utm_transformer = Transformer.from_crs( + CRS.from_epsg(4326), # WGS84 lat/lon + self.utm_crs, + always_xy=True # expects lon, lat + ) + + +def main(args=None): + rclpy.init(args=args) + node = FusionNode() + rclpy.spin(node) + rclpy.shutdown() diff --git a/localization_workspace/src/wr_imu/CMakeLists.txt b/localization_workspace/src/wr_imu/CMakeLists.txt new file mode 100644 index 0000000..e3412f5 --- /dev/null +++ b/localization_workspace/src/wr_imu/CMakeLists.txt @@ -0,0 +1,55 @@ +cmake_minimum_required(VERSION 3.8) +project(wr_imu) + +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 REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) # Swapped std_msgs for sensor_msgs +find_package(phoenix6 REQUIRED) +find_package(Threads REQUIRED) + +add_executable(compass src/imu_publisher.cpp) + +target_include_directories(compass PUBLIC + $ + $) + +target_compile_features(compass PUBLIC c_std_99 cxx_std_17) + +target_link_libraries(compass phoenix6 Threads::Threads) + +ament_target_dependencies(compass + rclcpp + sensor_msgs +) + +install(TARGETS compass + DESTINATION lib/${PROJECT_NAME}) + +# Install config directory +install(DIRECTORY config + DESTINATION share/${PROJECT_NAME} +) + +# Install launch directory +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/localization_workspace/src/wr_imu/config/ekf.yaml b/localization_workspace/src/wr_imu/config/ekf.yaml new file mode 100644 index 0000000..cb3b2d8 --- /dev/null +++ b/localization_workspace/src/wr_imu/config/ekf.yaml @@ -0,0 +1,69 @@ +### ekf.yaml ### + +# 1. LOCAL EKF (Odom frame) - Fuses IMU only for smooth local movement +ekf_filter_node_odom: + ros__parameters: + frequency: 30.0 + sensor_timeout: 0.1 + two_d_mode: true + publish_tf: true + map_frame: map + odom_frame: odom + base_link_frame: base_link + world_frame: odom + + # IMU0: Orientation, Angular Velocity, Linear Acceleration + imu0: imu/data + imu0_config: [false, false, false, + true, true, true, + false, false, false, + true, true, true, + true, true, true] + imu0_differential: false + imu0_remove_gravitational_acceleration: true + +# 2. GLOBAL EKF (Map frame) - Fuses IMU + GPS +ekf_filter_node_map: + ros__parameters: + frequency: 30.0 + sensor_timeout: 0.1 + two_d_mode: true + publish_tf: true + map_frame: map + odom_frame: odom + base_link_frame: base_link + world_frame: map + + # IMU0 (Same as above) + imu0: imu/data + imu0_config: [false, false, false, + true, true, true, + false, false, false, + true, true, true, + true, true, true] + imu0_differential: false + imu0_remove_gravitational_acceleration: true + + # GPS (Input from NavSat Node) + odom0: odometry/gps + # Fuse X, Y, Z position from GPS + odom0_config: [true, true, false, + false, false, false, + false, false, false, + false, false, false, + false, false, false] + odom0_differential: false + +# 3. NAVSAT TRANSFORM - Converts GPS Lat/Long to X/Y +navsat_transform: + ros__parameters: + frequency: 30.0 + # MAGNETIC DECLINATION: You MUST search Google for your city's value! + # Example: New York is roughly -0.22 radians. 0.0 assumes True North = Magnetic North. + magnetic_declination_radians: 0.04712 + yaw_offset: 0.0 + zero_altitude: true + broadcast_utm_transform: true + publish_filtered_gps: true + use_odometry_yaw: false + wait_for_datum: false \ No newline at end of file diff --git a/localization_workspace/src/wr_imu/launch/gps_fusion_launch.py b/localization_workspace/src/wr_imu/launch/gps_fusion_launch.py new file mode 100644 index 0000000..2916fcf --- /dev/null +++ b/localization_workspace/src/wr_imu/launch/gps_fusion_launch.py @@ -0,0 +1,66 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +import os +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + + + package_name = 'wr_imu' + + # Locate the config file + config_file = os.path.join( + get_package_share_directory(package_name), + 'config', + 'ekf.yaml' + ) + + return LaunchDescription([ + + # 1. STATIC TRANSFORM (The "Link Stuff") + # Connects 'base_link' (robot center) to 'imu_link' (sensor) + # Arguments: x y z yaw pitch roll parent child + Node( + package='tf2_ros', + executable='static_transform_publisher', + name='static_tf_pub_imu', + arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'imu_link'] + ), + + # 2. Local EKF (Odom Frame) + Node( + package='robot_localization', + executable='ekf_node', + name='ekf_filter_node_odom', + output='screen', + parameters=[config_file], + remappings=[('odometry/filtered', 'odometry/local')] + ), + + # 3. Global EKF (Map Frame) + Node( + package='robot_localization', + executable='ekf_node', + name='ekf_filter_node_map', + output='screen', + parameters=[config_file], + remappings=[('odometry/filtered', 'odometry/global')] + ), + + # 4. Navsat Transform (GPS -> X/Y) + Node( + package='robot_localization', + executable='navsat_transform_node', + name='navsat_transform', + output='screen', + parameters=[config_file], + remappings=[ + ('imu/data', 'imu/data'), + ('gps/fix', '/fix'), + ('odometry/gps', 'odometry/gps'), + # Navsat needs to know the robot's current heading to align the GPS + # So it listens to the Global EKF output + ('odometry/filtered', 'odometry/global') + ] + ) + ]) \ No newline at end of file diff --git a/localization_workspace/src/wr_imu/package.xml b/localization_workspace/src/wr_imu/package.xml new file mode 100644 index 0000000..8ea3882 --- /dev/null +++ b/localization_workspace/src/wr_imu/package.xml @@ -0,0 +1,21 @@ + + + + wr_imu + 0.0.0 + IMU Driver for Pigeon2 + aarav + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + + rclcpp + sensor_msgs + phoenix6 + + ament_cmake + + diff --git a/localization_workspace/src/wr_imu/src/imu_publisher.cpp b/localization_workspace/src/wr_imu/src/imu_publisher.cpp new file mode 100644 index 0000000..9e63ad6 --- /dev/null +++ b/localization_workspace/src/wr_imu/src/imu_publisher.cpp @@ -0,0 +1,92 @@ +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/imu.hpp" +#include "ctre/phoenix6/Pigeon2.hpp" + +using namespace ctre::phoenix6; +using namespace std::chrono_literals; + +class CompassDataPublisher : public rclcpp::Node { + public: + CompassDataPublisher() + : Node("compass"), + pigeon2imu(10, "can0") + { + // Publisher set to "imu/data" + publisher_ = this->create_publisher("imu/data", 10); + + // 50Hz Timer + timer_ = this->create_wall_timer( + 20ms, std::bind(&CompassDataPublisher::timer_callback, this)); + } + + private: + void timer_callback() + { + auto message = sensor_msgs::msg::Imu(); + + message.header.stamp = this->get_clock()->now(); + + message.header.frame_id = "imu_link"; + //Get Signals + auto &q_w = pigeon2imu.GetQuatW(); + auto &q_x = pigeon2imu.GetQuatX(); + auto &q_y = pigeon2imu.GetQuatY(); + auto &q_z = pigeon2imu.GetQuatZ(); + + auto &gyro_x = pigeon2imu.GetAngularVelocityXWorld(); + auto &gyro_y = pigeon2imu.GetAngularVelocityYWorld(); + auto &gyro_z = pigeon2imu.GetAngularVelocityZWorld(); + + auto &accel_x = pigeon2imu.GetAccelerationX(); + auto &accel_y = pigeon2imu.GetAccelerationY(); + auto &accel_z = pigeon2imu.GetAccelerationZ(); + + // Synchronous Refresh + BaseStatusSignal::RefreshAll( + q_w, q_x, q_y, q_z, + gyro_x, gyro_y, gyro_z, + accel_x, accel_y, accel_z + ); + + // Populate Orientation + message.orientation.w = q_w.GetValue().value(); + message.orientation.x = q_x.GetValue().value(); + message.orientation.y = q_y.GetValue().value(); + message.orientation.z = q_z.GetValue().value(); + + // Populate Angular Velocity (Deg -> Rad) + double deg_to_rad = M_PI / 180.0; + message.angular_velocity.x = gyro_x.GetValue().value() * deg_to_rad; + message.angular_velocity.y = gyro_y.GetValue().value() * deg_to_rad; + message.angular_velocity.z = gyro_z.GetValue().value() * deg_to_rad; + + // Populate Linear Acceleration (G -> m/s^2) + double g_to_mps2 = 9.80665; + message.linear_acceleration.x = accel_x.GetValue().value() * g_to_mps2; + message.linear_acceleration.y = accel_y.GetValue().value() * g_to_mps2; + message.linear_acceleration.z = accel_z.GetValue().value() * g_to_mps2; + + // Covariance + message.orientation_covariance = {0.001, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.001}; + message.angular_velocity_covariance = {0.02, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, 0.02}; + message.linear_acceleration_covariance = {0.04, 0.0, 0.0, 0.0, 0.04, 0.0, 0.0, 0.0, 0.04}; + + publisher_->publish(message); + } + + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr publisher_; + hardware::Pigeon2 pigeon2imu; +}; + +int main(int argc, char *argv[]) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file