vehicle_adapter.hpp

Go to the documentation of this file.

#pragma once
 
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>
#include <message_filters/time_synchronizer.h>
 
#include "custom_interfaces/msg/imu_data.hpp"
#include "custom_interfaces/msg/operational_status.hpp"
#include "custom_interfaces/msg/steering_angle.hpp"
#include "custom_interfaces/msg/wheel_rpm.hpp"
#include "geometry_msgs/msg/vector3_stamped.hpp"
#include "node/node.hpp"
#include "std_srvs/srv/empty.hpp"
#include "std_srvs/srv/trigger.hpp"
 
class VehicleAdapter : public VENode {
  message_filters::Subscriber<custom_interfaces::msg::WheelRPM>
      _fl_wheel_rpm_subscription_;  
  message_filters::Subscriber<custom_interfaces::msg::WheelRPM>
      _fr_wheel_rpm_subscription_;  
 
  message_filters::Subscriber<geometry_msgs::msg::Vector3Stamped> _free_acceleration_subscription_;
  message_filters::Subscriber<geometry_msgs::msg::Vector3Stamped> _angular_velocity_subscription_;
  using XsensImuPolicy = message_filters::sync_policies::ApproximateTime<
      geometry_msgs::msg::Vector3Stamped,
      geometry_msgs::msg::Vector3Stamped>;  
  using WheelSSPolicy = message_filters::sync_policies::ApproximateTime<
      custom_interfaces::msg::WheelRPM,
      custom_interfaces::msg::WheelRPM>;  
  std::shared_ptr<message_filters::Synchronizer<WheelSSPolicy>>
      _wss_sync_;  
  std::shared_ptr<message_filters::Synchronizer<XsensImuPolicy>> _xsens_imu_sync_;
 
  rclcpp::Subscription<custom_interfaces::msg::SteeringAngle>::SharedPtr _steering_angle_sub_;
 
  rclcpp::Subscription<custom_interfaces::msg::WheelRPM>::SharedPtr _resolver_sub_;
 
  double last_decent_fl_wss_reading = 0;
  double average_imu_bias = 0.0;
  int number_of_imu_readings = 0;
 
public:
  explicit VehicleAdapter(const VEParameters& parameters);
  void imu_callback(const geometry_msgs::msg::Vector3Stamped::SharedPtr& free_acceleration_msg,
                    const geometry_msgs::msg::Vector3Stamped::SharedPtr& angular_velocity_msg);
  void wss_callback(const custom_interfaces::msg::WheelRPM& rl_wheel_rpm_msg,
                    const custom_interfaces::msg::WheelRPM& rr_wheel_rpm_msg);
 
  void steering_angle_callback(const custom_interfaces::msg::SteeringAngle msg);
 
  void resolver_callback(custom_interfaces::msg::WheelRPM msg);
};