dvl_odometry_component.hpp

Go to the documentation of this file.

 
#ifndef _DVL_ODOMETRY_HPP
#define _DVL_ODOMETRY_HPP
 
#include <driver_msgs/msg/dvl.hpp>
#include <driver_msgs/msg/imu.hpp>
#include <localization_msgs/msg/odometry.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp/service.hpp>
#include <std_srvs/srv/trigger.hpp>
 
#include "std_srvs/srv/trigger.hpp"
 
namespace localization::dvl
{
 
const double RADIAN_SCALE = std::numbers::pi / 180.0;
 
class DVLOdometry : public rclcpp::Node
{
private:
  rclcpp::Publisher<localization_msgs::msg::Odometry>::SharedPtr pub_;
  rclcpp::Subscription<localization_msgs::msg::Odometry>::SharedPtr sub_imu_;
  rclcpp::Subscription<driver_msgs::msg::DVL>::SharedPtr sub_dvl_;
  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr srv_;
 
  rclcpp::Time pre_time;
 
  std::array<float, 3> offset;
  std::shared_ptr<localization_msgs::msg::Odometry> imu_msg_;
 
  double pos_x = 0.0;
  double pos_y = 0.0;
 
  void update_callback(const driver_msgs::msg::DVL::UniquePtr msg);
 
  void update_imu_callback(const localization_msgs::msg::Odometry::UniquePtr msg);
 
  void reset_callback(
    const std_srvs::srv::Trigger::Request::SharedPtr request,
    const std_srvs::srv::Trigger::Response::SharedPtr response);
 
public:
  explicit DVLOdometry(const rclcpp::NodeOptions & options);
 
  void reset();
};
}  // namespace localization::dvl
 
#endif