depth_odometry_component.hpp

Go to the documentation of this file.

 
#ifndef _DEPTH_ODOMETRY_HPP
#define _DEPTH_ODOMETRY_HPP
 
#include <driver_msgs/msg/depth.hpp>
#include <localization_msgs/msg/odometry.hpp>
#include <rclcpp/rclcpp.hpp>
#include <std_srvs/srv/trigger.hpp>
 
namespace localization::depth
{
 
const float EMA_ALPHA = 0.4;
 
const int DEPTH_SMA_SUMPLE_NUM = 32;
 
const float sea2fresh_scale = 1029.0 / 1000.0;
 
class DepthOdometry : public rclcpp::Node
{
private:
  rclcpp::Publisher<localization_msgs::msg::Odometry>::SharedPtr pub_;
  rclcpp::Subscription<driver_msgs::msg::Depth>::SharedPtr sub_;
  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr srv_;
 
  bool fresh_water;
 
  rclcpp::Time pre_time;
  double pre_pos_z = 0.0;
 
  uint8_t idx = 0;
  std::array<double, DEPTH_SMA_SUMPLE_NUM> vel_z_list;  
  double offset_pos_z = 0.0;
  double pos_z = 0.0;
 
  void update_callback(const driver_msgs::msg::Depth::UniquePtr msg);
 
  void reset_callback(
    const std_srvs::srv::Trigger::Request::SharedPtr request,
    const std_srvs::srv::Trigger::Response::SharedPtr response);
 
public:
  explicit DepthOdometry(const rclcpp::NodeOptions & options);
 
  void reset();
};
 
}  // namespace localization::depth
 
#endif  // !_DEPTH_ODOMETRY_HPP