course correction (#75)

rae_description/urdf/rae_ros2_control.urdf.xacro

@@ -30,6 +30,11 @@
           <param name="PID_I_R">0.1</param>
           <param name="PID_D_R">0.0005</param>
 
+          <param name="PID_P_IMU">5.3</param>
+          <param name="PID_I_IMU">4.7</param>
+          <param name="PID_D_IMU">0.3</param>
+          <param name="static_correction">0</param>
+
           <param name="loop_rate">30</param>
           <param name="chip_name">gpiochip0</param>
         </hardware>

rae_hw/CMakeLists.txt

@@ -12,7 +12,7 @@ set(CMAKE_POSITION_INDEPENDENT_CODE ON)
 find_package(ament_cmake_auto REQUIRED)
 ament_auto_find_build_dependencies()
 find_package(ALSA REQUIRED)
-
+find_package(sensor_msgs REQUIRED)
 find_library(GPIOD_LIBRARY NAMES libgpiodcxx.so)
 if(NOT GPIOD_LIBRARY)
   message(FATAL_ERROR "gpiod library not found. Install apt install libgpiod-dev")
@@ -26,7 +26,7 @@ pkg_check_modules(MPG123 REQUIRED libmpg123)
 find_path(SNDFILE_INCLUDE_DIR sndfile.h)
 find_library(SNDFILE_LIBRARY NAMES sndfile)
 
-include_directories(include ${GST_INCLUDE_DIRS} ${MPG123_INCLUDE_DIRS} ${SNDFILE_INCLUDE_DIR})
+include_directories(include ${GST_INCLUDE_DIRS} ${MPG123_INCLUDE_DIRS} ${SNDFILE_INCLUDE_DIR} ${sensor_msgs_INCLUDE_DIRS})
 
 ament_auto_add_library(
   ${PROJECT_NAME}
@@ -35,7 +35,9 @@ ament_auto_add_library(
   src/rae_motors.cpp
 )
 
-ament_target_dependencies(${PROJECT_NAME} ${DEPENDENCIES} ALSA std_srvs)
+ament_target_dependencies(${PROJECT_NAME} ${DEPENDENCIES}  nav_msgs ALSA std_srvs)
+set(dependencies
+    sensor_msgs)
 
 target_link_libraries(
   ${PROJECT_NAME}

rae_hw/config/controller.yaml

@@ -29,7 +29,7 @@ diff_controller:
     # velocity computation filtering
     velocity_rolling_window_size: 1
 
-    open_loop: false
+    open_loop: true
     enable_odom_tf: false
 
     cmd_vel_timeout: 0.5

rae_hw/include/rae_hw/rae_hw.hpp

@@ -2,18 +2,22 @@
 #define RAE_HW__RAE_HW_HPP_
 
 #include <chrono>
+#include <memory>
 #include <string>
+#include <thread>
 #include <vector>
 
 #include "gpiod.hpp"
 #include "hardware_interface/handle.hpp"
 #include "hardware_interface/hardware_info.hpp"
 #include "hardware_interface/system_interface.hpp"
 #include "hardware_interface/types/hardware_interface_return_values.hpp"
+#include "nav_msgs/msg/odometry.hpp"
 #include "rae_hw/rae_motors.hpp"
 #include "rae_hw/visibility_control.h"
 #include "rclcpp/macros.hpp"
 #include "rclcpp/rclcpp.hpp"
+#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
 #include "rclcpp_lifecycle/state.hpp"
 
 namespace rae_hw {
@@ -48,11 +52,18 @@ class RaeHW : public hardware_interface::SystemInterface {
     hardware_interface::return_type write(const rclcpp::Time& time, const rclcpp::Duration& period) override;
 
    private:
+    void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg);
+    void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg);
     std::unique_ptr<RaeMotor> motorL, motorR;
     double leftMotorCMD, rightMotorCMD;
     int pwmA, pwmB, phA, phB;
+    double yaw_imu, yaw_odom, kp, ki, kd, sumerr, prev_yaw_imu, prev_yaw_odom, prev_err, static_err;
+    bool static_correction;
     double leftPos, rightPos, leftVel, rightVel;
     std::string leftWheelName, rightWheelName;
+    rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_subscriber_;
+    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_subscriber_;
+    rclcpp::Node::SharedPtr node_;
 };
 
 }  // namespace rae_hw

rae_hw/include/rae_hw/rae_motors.hpp

@@ -15,6 +15,8 @@
 #include <string>
 #include <thread>
 
+#include "sensor_msgs/msg/imu.hpp"
+
 namespace rae_hw {
 /// @brief Struct representing encoder GPIO states. Following states are used to calculate rotation:
 ///

rae_hw/package.xml

@@ -23,6 +23,7 @@
 
   <depend>sensor_msgs</depend>
   <depend>geometry_msgs</depend>
+  <depend>nav_msgs</depend>
   <depend>rae_msgs</depend>
   <depend>std_srvs</depend>
   <depend>cv_bridge</depend>

rae_hw/src/rae_hw.cpp

@@ -1,5 +1,6 @@
 #include "rae_hw/rae_hw.hpp"
 
+#include <cmath>
 #include <fstream>
 #include <limits>
 #include <vector>
@@ -37,13 +38,36 @@ hardware_interface::CallbackReturn RaeHW::on_init(const hardware_interface::Hard
     PID pidR{std::stof(info_.hardware_parameters["PID_P_R"]), std::stof(info_.hardware_parameters["PID_I_R"]), std::stof(info_.hardware_parameters["PID_D_R"])};
     motorR = std::make_unique<RaeMotor>(rightWheelName, chipName, pwmName, pwmR, phR, enRA, enRB, encTicsPerRevR, maxVelR, false, closedLoopR, pidR);
 
+    yaw_imu = 0.0;
+    yaw_odom = 0.0;
+    prev_yaw_imu = 0.0;
+    prev_yaw_odom = 0.0;
+    sumerr = 0.0;
+    prev_err = 0.0;
+    static_err = 0.0;
+    static_correction = static_cast<bool>(std::stoi(info_.hardware_parameters["static_correction"]));
+    ;
+    PID pidIMU{std::stof(info_.hardware_parameters["PID_P_IMU"]),
+               std::stof(info_.hardware_parameters["PID_I_IMU"]),
+               std::stof(info_.hardware_parameters["PID_D_IMU"])};
+    kp = pidIMU.P;
+    ki = pidIMU.I;
+    kd = pidIMU.D;
+    rclcpp::NodeOptions options;
+    options.arguments({"--ros-args", "-r", "__node:=topic_based_ros2_control_"});
+    node_ = rclcpp::Node::make_shared("_", options);
+
     return CallbackReturn::SUCCESS;
 }
 
 hardware_interface::CallbackReturn RaeHW::on_configure(const rclcpp_lifecycle::State& /*previous state*/) {
     motorL->run();
     motorR->run();
 
+    imu_subscriber_ = node_->create_subscription<sensor_msgs::msg::Imu>("/rae/imu/data", 10, std::bind(&RaeHW::imu_callback, this, std::placeholders::_1));
+    odom_subscriber_ =
+        node_->create_subscription<nav_msgs::msg::Odometry>("/diff_controller/odom", 10, std::bind(&RaeHW::odom_callback, this, std::placeholders::_1));
+
     return CallbackReturn::SUCCESS;
 }
 
@@ -84,8 +108,30 @@ hardware_interface::CallbackReturn RaeHW::on_shutdown(const rclcpp_lifecycle::St
 
     return CallbackReturn::SUCCESS;
 }
+void RaeHW::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) {
+    float q_x = msg->orientation.x;
+    float q_y = msg->orientation.y;
+    float q_z = msg->orientation.z;
+    float q_w = msg->orientation.w;
+
+    // Calculate yaw from the quaternion
+    yaw_imu = std::atan2(2.0 * (q_w * q_z + q_x * q_y), 1.0 - 2.0 * (std::pow(q_y, 2) + std::pow(q_z, 2)));
+}
+
+void RaeHW::odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) {
+    double q_x = msg->pose.pose.orientation.x;
+    double q_y = msg->pose.pose.orientation.y;
+    double q_z = msg->pose.pose.orientation.z;
+    double q_w = msg->pose.pose.orientation.w;
+
+    yaw_odom = std::atan2(2.0 * (q_w * q_z + q_x * q_y), 1.0 - 2.0 * (std::pow(q_y, 2) + std::pow(q_z, 2)));
+}
 
 hardware_interface::return_type RaeHW::read(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) {
+    if(rclcpp::ok())  // spin node
+    {
+        rclcpp::spin_some(node_);
+    }
     float currLeftPos = motorL->getPos();
     float currRightPos = motorR->getPos();
     leftVel = motorL->getSpeed();
@@ -96,10 +142,52 @@ hardware_interface::return_type RaeHW::read(const rclcpp::Time& /*time*/, const
     return hardware_interface::return_type::OK;
 }
 
-hardware_interface::return_type RaeHW::write(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) {
+hardware_interface::return_type RaeHW::write(const rclcpp::Time& /*time*/, const rclcpp::Duration& period) {
+    double dt = period.seconds();
+    auto adjustForWrap = [](double previousYaw, double currentYaw) {  // lambda for wraping, see nunmpy unwrap
+        double delta = currentYaw - previousYaw;
+        if(delta > M_PI) {
+            currentYaw -= 2 * M_PI;
+        } else if(delta < -M_PI) {
+            currentYaw += 2 * M_PI;
+        }
+        return currentYaw;
+    };
+
+    double yaw_imu_wraped = adjustForWrap(prev_yaw_imu, yaw_imu);  // to handle -pi pi discontinuity
+    double yaw_odom_wraped = adjustForWrap(prev_yaw_odom, yaw_odom - static_err);
+    double err = yaw_odom_wraped - yaw_imu_wraped;
+    double out = 0;
+
+    if(static_correction) {
+        sumerr += err * dt;
+        if(sumerr > 10) {  // anti windup
+            sumerr = 10;
+        }
+        if(sumerr < -10) {
+            sumerr = -10;
+        }
+        float delta_err = (err - prev_err) / dt;
+        if(yaw_imu) {  // prevent unwanted moves before initialization
+            if(leftMotorCMD || rightMotorCMD) {
+                out = err * kp + sumerr * ki + delta_err * kd;
+            } else {
+                out = (std::signbit(err) ? -3.95 : 3.95) + err * kp + sumerr * ki + delta_err * kd;
+            }  // feedforward controller to bypass deadzone
+        }
+    } else {
+        out = 0.0;
+        if(abs(err) > 0.01) static_err += err;
+    }
+    leftMotorCMD -= out;
+    rightMotorCMD += out;
+
+    prev_yaw_imu = yaw_imu_wraped;
+    prev_yaw_odom = yaw_odom_wraped;
+    prev_err = err;
+
     motorL->motorSet(leftMotorCMD);
     motorR->motorSet(rightMotorCMD);
-
     return hardware_interface::return_type::OK;
 }