localization_component.hpp

Go to the documentation of this file.

 
#ifndef _LOCALIZATIN_COMPONENT_HPP
#define _LOCALIZATIN_COMPONENT_HPP
 
#include <atomic>
#include <driver_msgs/msg/gnss.hpp>
#include <driver_msgs/msg/imu.hpp>
#include <geodetic_converter/geodetic_converter.hpp>
#include <localization_msgs/msg/odometry.hpp>
#include <localization_msgs/srv/reset.hpp>
#include <rclcpp/rclcpp.hpp>
#include <std_srvs/srv/trigger.hpp>
#include <visualization_msgs/msg/marker_array.hpp>
 
namespace localization
{
 
const std::array<std::array<double, 3>, 2> ANCHOR_POINTS_DMS[4] = {
  {{{33, 59, 14.8}, {132, 12, 5.4}}},  // I系: 33°00′00″N, 129°30′00″E
  {{{33, 59, 14.6}, {132, 12, 6.4}}},  // II系: 33°00′00″N, 131°00′00″E
  {{{33, 59, 15.2}, {132, 12, 6.4}}},  // III系: 36°00′00″N, 132°10′30″E
  {{{33, 59, 15.5}, {132, 12, 5.7}}},  // IV系: 33°00′00″N, 133°00′00″E
};
 
using FutureAndRequestId = rclcpp::Client<std_srvs::srv::Trigger>::SharedFuture;
 
class Localization : public rclcpp::Node
{
private:
  uint8_t coord_system_id;
  rclcpp::CallbackGroup::SharedPtr client_cb_group_;
  rclcpp::CallbackGroup::SharedPtr gnss_cb_group_;
  rclcpp::Publisher<localization_msgs::msg::Odometry>::SharedPtr pub_odom_;
  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr marker_pub_;
  rclcpp::Subscription<localization_msgs::msg::Odometry>::SharedPtr sub_depth_;
  rclcpp::Subscription<localization_msgs::msg::Odometry>::SharedPtr sub_imu_;
  rclcpp::Subscription<localization_msgs::msg::Odometry>::SharedPtr sub_dvl_;
  rclcpp::Subscription<driver_msgs::msg::Gnss>::SharedPtr sub_gnss_;
  rclcpp::Subscription<driver_msgs::msg::IMU>::SharedPtr sub_imu_raw_;
  rclcpp::Service<localization_msgs::srv::Reset>::SharedPtr srv_;
  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr client_depth_;
  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr client_imu_;
  rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr client_dvl_;
 
  rclcpp::TimerBase::SharedPtr timer_;
 
  std::shared_ptr<common::GeodeticConverter> geo_converter_;
  std::shared_ptr<localization_msgs::msg::Odometry> odom_msg_;
  std::shared_ptr<driver_msgs::msg::Gnss> gnss_msg_;
  std::shared_ptr<driver_msgs::msg::IMU> imu_raw_msg_;
  common::Geodetic origin_geodetic;
  common::Geodetic reference_geodetic;
  common::PlaneXY reference_plane;
  double azimuth;
 
  std::atomic<bool> gnss_updated{false};
 
  bool gnss_enable = false;
  uint8_t enabled_sensor = 0b11111000;
  uint8_t all_updated = 0b11111000;
 
  void depth_callback(const localization_msgs::msg::Odometry::UniquePtr msg);
 
  void imu_callback(const localization_msgs::msg::Odometry::UniquePtr msg);
 
  void dvl_callback(const localization_msgs::msg::Odometry::UniquePtr msg);
 
  void gnss_callback(const driver_msgs::msg::Gnss::UniquePtr msg);
 
  void imu_raw_callback(const driver_msgs::msg::IMU::UniquePtr msg);
 
  void _calc_global_pos(const localization_msgs::msg::Odometry::SharedPtr odom_);
 
  void publisher();
 
  void reset_callback(
    const localization_msgs::srv::Reset::Request::SharedPtr request,
    const localization_msgs::srv::Reset::Response::SharedPtr response);
 
  bool _is_server_active(const rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr client_);
 
  FutureAndRequestId _reset_request(
    const rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr client_);
 
  bool _reset_response(
    FutureAndRequestId future, std::chrono::duration<long, std::ratio<1, 1000>> dulation,
    std::string service_name);
 
public:
  explicit Localization(const rclcpp::NodeOptions & options);
 
  int reset();
};
 
}  // namespace localization
 
#endif