dvl_driver.hpp

Go to the documentation of this file.

 
#ifndef _DVL_DRIVER_HPP
#define _DVL_DRIVER_HPP
 
#include <driver_msgs/msg/dvl.hpp>
#include <driver_msgs/srv/command.hpp>
#include <rclcpp/rclcpp.hpp>
 
#include "dvl_driver/path_finder.hpp"
#include "timer/timeout.hpp"
 
namespace driver::dvl_driver
{
 
class DVLDriver : public rclcpp::Node
{
public:
  explicit DVLDriver();
 
  ~DVLDriver();
 
private:
  std::string address;
  int listener_port;
  int sender_port;
  int64_t timeout_ms;
  bool turning;
 
  bool command_mode = false;
  bool break_cmd = false;
  std::shared_ptr<timer::Timeout> timeout_;
 
  // DVL Interface
  std::shared_ptr<path_finder::Sender> sender_;
  std::shared_ptr<path_finder::Listener> listener_;
 
  // ROS Interface
  rclcpp::Publisher<driver_msgs::msg::DVL>::SharedPtr pub_;
  rclcpp::Service<driver_msgs::srv::Command>::SharedPtr srv_;
  rclcpp::TimerBase::SharedPtr timer_;
 
  bool setup();
 
  bool _update();
 
  void update();
 
  template <typename T>
  void _convertVelocity(const T & raw_vel, driver_msgs::msg::DVL & msg);
 
  void _extractPd0Data(
    const std::shared_ptr<path_finder::pd0::Pd0Ensemble> & data, driver_msgs::msg::DVL & msg);
 
  void _extractPd5Data(
    const std::shared_ptr<path_finder::pd5::Pd5Ensemble> & data, driver_msgs::msg::DVL & msg);
 
  double _calculate_average_altitude(const float ranges[4]);
 
  void sendCommandCallback(
    const driver_msgs::srv::Command::Request::SharedPtr request,
    driver_msgs::srv::Command::Response::SharedPtr response);
};
 
}  // namespace driver::dvl_driver
 
#endif  // _DVL_DRIVER_HPP