Program Listing for File node.hpp

↰ Return to documentation for file (/tmp/ws/src/ublox/ublox_gps/include/ublox_gps/node.hpp)

//==============================================================================
// Copyright (c) 2012, Johannes Meyer, TU Darmstadt
// All rights reserved.

// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//     * Redistributions of source code must retain the above copyright
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above copyright
//       notice, this list of conditions and the following disclaimer in the
//       documentation and/or other materials provided with the distribution.
//     * Neither the name of the Flight Systems and Automatic Control group,
//       TU Darmstadt, nor the names of its contributors may be used to
//       endorse or promote products derived from this software without
//       specific prior written permission.

// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND
// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
// DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
//==============================================================================

#ifndef UBLOX_GPS_NODE_HPP
#define UBLOX_GPS_NODE_HPP

// STL
#include <memory>
#include <string>
#include <vector>
// ROS includes
#include <diagnostic_updater/diagnostic_updater.hpp>
#include <rclcpp/rclcpp.hpp>
// U-Blox msgs nicludes
#include <ublox_msgs/msg/cfg_cfg.hpp>
#include <ublox_msgs/msg/cfg_dat.hpp>
#include <ublox_msgs/msg/inf.h>
#include <rtcm_msgs/msg/message.hpp>
#include <nmea_msgs/msg/sentence.hpp>
// Ublox GPS includes
#include <ublox_gps/component_interface.hpp>
#include <ublox_gps/fix_diagnostic.hpp>
#include <ublox_gps/gnss.hpp>
#include <ublox_gps/gps.hpp>
#include <ublox_gps/rtcm.hpp>
#include <ublox_gps/raw_data_pa.hpp>

// This file also declares UbloxNode which is the main class and ros node. It
// implements functionality which applies to any u-blox device, regardless of
// the firmware version or product type.  The class is designed in compositional
// style; it contains ComponentInterfaces which implement features specific to
// the device based on its firmware version and product category. UbloxNode
// calls the public methods of each component.

namespace ublox_node {

class UbloxNode final : public rclcpp::Node {
 public:
  constexpr static int kResetWait = 10;
  constexpr static double kKeepAlivePeriod = 10.0;
  constexpr static double kPollDuration = 1.0;
  // Constants used for diagnostic frequency updater
  const float kDiagnosticPeriod = 0.2;
  const double kFixFreqTol = 0.15;
  const double kFixFreqWindow = 10;
  const double kTimeStampStatusMin = 0;

  explicit UbloxNode(const rclcpp::NodeOptions & options);

  ~UbloxNode() override;

  UbloxNode(UbloxNode &&c) = delete;
  UbloxNode &operator=(UbloxNode &&c) = delete;
  UbloxNode(const UbloxNode &c) = delete;
  UbloxNode &operator=(const UbloxNode &c) = delete;

  void getRosParams();

  bool configureUblox();

  void subscribe();

  void initializeRosDiagnostics();

  void printInf(const ublox_msgs::msg::Inf &m, uint8_t id);

 private:

  void rtcmCallback(const rtcm_msgs::msg::Message::SharedPtr msg);

  rclcpp::Subscription<rtcm_msgs::msg::Message>::SharedPtr subscription_;

  void initializeIo();

  void initialize();

  void shutdown();

  bool resetDevice();

  void processMonVer();

  void addFirmwareInterface();

  void addProductInterface(const std::string & product_category,
                           const std::string & ref_rov = "");

  void keepAlive();

  void pollMessages();

  void configureInf();


  std::vector<std::shared_ptr<ComponentInterface> > components_;

  float protocol_version_ = 0.0;
  // Variables set from parameter server
  std::string device_;
  std::string dynamic_model_;
  std::string fix_mode_;
  uint8_t dmodel_{0};
  uint8_t fmode_{0};
  uint32_t baudrate_{0};
  uint16_t uart_in_{0};
  uint16_t uart_out_{0};
  uint16_t usb_tx_{0};

  bool set_usb_{false};
  uint16_t usb_in_{0};
  uint16_t usb_out_{0};
  double rate_{0.0};
  ublox_msgs::msg::CfgDAT cfg_dat_;
  uint8_t sbas_usage_{0};
  uint8_t max_sbas_{0};
  uint8_t dr_limit_{0};
  ublox_msgs::msg::CfgCFG load_;
  ublox_msgs::msg::CfgCFG save_;
  uint8_t tim_rate_{0};

  std::shared_ptr<RawDataStreamPa> raw_data_stream_pa_;

  rclcpp::Publisher<ublox_msgs::msg::NavSTATUS>::SharedPtr nav_status_pub_;
  rclcpp::Publisher<ublox_msgs::msg::NavPOSECEF>::SharedPtr nav_posecef_pub_;
  rclcpp::Publisher<ublox_msgs::msg::NavCLOCK>::SharedPtr nav_clock_pub_;
  rclcpp::Publisher<ublox_msgs::msg::NavCOV>::SharedPtr nav_cov_pub_;
  rclcpp::Publisher<ublox_msgs::msg::AidALM>::SharedPtr aid_alm_pub_;
  rclcpp::Publisher<ublox_msgs::msg::AidEPH>::SharedPtr aid_eph_pub_;
  rclcpp::Publisher<ublox_msgs::msg::AidHUI>::SharedPtr aid_hui_pub_;
  rclcpp::Publisher<nmea_msgs::msg::Sentence>::SharedPtr nmea_pub_;

  void publish_nmea(const std::string & sentence, const std::string & topic);

  uint16_t nav_rate_{0};

  uint16_t meas_rate_{0};

  std::string frame_id_;

  std::shared_ptr<diagnostic_updater::Updater> updater_;

  std::shared_ptr<FixDiagnostic> freq_diag_;

  std::vector<ublox_gps::Rtcm> rtcms_;

  std::shared_ptr<Gnss> gnss_;

  std::shared_ptr<ublox_gps::Gps> gps_;

  rclcpp::TimerBase::SharedPtr keep_alive_;
  rclcpp::TimerBase::SharedPtr poller_;
};

}  // namespace ublox_node

#endif  // UBLOX_GPS_NODE_HPP