vehicle.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 "adapter_ekf_state_est/adapter.hpp"
#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 "std_srvs/srv/empty.hpp"
#include "std_srvs/srv/trigger.hpp"
 
class SENode;
 
class VehicleAdapter : public Adapter {
  message_filters::Subscriber<custom_interfaces::msg::WheelRPM>
      _rl_wheel_rpm_subscription_;  
  message_filters::Subscriber<custom_interfaces::msg::WheelRPM>
      _rr_wheel_rpm_subscription_;  
  message_filters::Subscriber<custom_interfaces::msg::SteeringAngle>
      _steering_angle_subscription_;  
  rclcpp::Subscription<custom_interfaces::msg::OperationalStatus>::SharedPtr
      _operational_status_subscription_;  
 
  message_filters::Subscriber<geometry_msgs::msg::Vector3Stamped> _free_acceleration_subscription_;
  message_filters::Subscriber<geometry_msgs::msg::Vector3Stamped> _angular_velocity_subscription_;
 
  message_filters::Subscriber<custom_interfaces::msg::ImuData> _roll_accx_imu_subscription_;
  message_filters::Subscriber<custom_interfaces::msg::ImuData> _yaw_accy_imu_subscription_;
 
  using ImuPolicy = message_filters::sync_policies::ApproximateTime<
      custom_interfaces::msg::ImuData,
      custom_interfaces::msg::ImuData>;  
  using XsensImuPolicy = message_filters::sync_policies::ApproximateTime<
      geometry_msgs::msg::Vector3Stamped,
      geometry_msgs::msg::Vector3Stamped>;  
  using WheelSteerPolicy = message_filters::sync_policies::ApproximateTime<
      custom_interfaces::msg::WheelRPM, custom_interfaces::msg::WheelRPM,
      custom_interfaces::msg::SteeringAngle>;  
  std::shared_ptr<message_filters::Synchronizer<WheelSteerPolicy>>
      _sync_;  
 
  std::shared_ptr<message_filters::Synchronizer<ImuPolicy>> _imu_sync_;
 
  std::shared_ptr<message_filters::Synchronizer<XsensImuPolicy>> _xsens_imu_sync_;
 
  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr
      _finished_client_;  
 
  void wheel_speeds_subscription_callback(
      const custom_interfaces::msg::WheelRPM& rl_wheel_rpm_msg,
      const custom_interfaces::msg::WheelRPM& rr_wheel_rpm_msg,
      const custom_interfaces::msg::SteeringAngle& steering_angle_msg);
 
  void imu_subscription_callback(const custom_interfaces::msg::ImuData& roll_accx_data,
                                 const custom_interfaces::msg::ImuData& yaw_accy_data);
 
  void xsens_imu_subscription_callback(
      const geometry_msgs::msg::Vector3Stamped::SharedPtr& free_acceleration_msg,
      const geometry_msgs::msg::Vector3Stamped::SharedPtr& angular_velocity_msg);
 
public:
  explicit VehicleAdapter(std::shared_ptr<SENode> se_node);
 
  void finish() final;
};