Dev/localization

.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

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(<dependency> REQUIRED)
+
+add_executable(compass src/compass.cpp)
+ament_target_dependencies(compass rclcpp std_msgs sensor_msgs phoenix6)
+
+
+target_include_directories(compass PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+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()

localization_workspace/src/wr_compass/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>wr_compass</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="nicolasdittmarg1@gmail.com">wiscrobo</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

localization_workspace/src/wr_compass/src/compass.cpp

@@ -0,0 +1,97 @@
+#include <memory>
+#include "rclcpp/rclcpp.hpp"
+#include <functional> // Include this for std::bind4
+#include <chrono>
+#include <functional>
+#include <memory>
+#include "ctre/phoenix6/Pigeon2.hpp"
+#include "sensor_msgs/msg/imu.hpp"
+#include <numbers>
+#include <cmath>
+
+
+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<sensor_msgs::msg::Imu>("imu_quat_data", 10);
+	    publisher_euler = this->create_publisher<sensor_msgs::msg::Imu>("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<sensor_msgs::msg::Imu>::SharedPtr publisher_quat;
+        rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr publisher_euler;
+        hardware::Pigeon2 pigeon2imu;
+};
+
+
+int main(int argc, char *argv[]) {
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<CompassDataPublisher>());
+    rclcpp::shutdown();
+    return 0;
+}

localization_workspace/src/wr_fusion/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>wr_fusion</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="nicolasdittmarg1@gmail.com">wiscrobo</maintainer>
+  <license>TODO: License declaration</license>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

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

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'
+        ],
+    },
+)

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'

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)

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'