Program Listing for File gps.hpp

↰ Return to documentation for file (/tmp/ws/src/ublox/ublox_gps/include/ublox_gps/gps.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_GPS_HPP
#define UBLOX_GPS_GPS_HPP
// STL
#include <atomic>
#include <chrono>
#include <locale>
#include <map>
#include <stdexcept>
#include <string>
#include <vector>

#include <rclcpp/rclcpp.hpp>

// Other u-blox packages
#include <ublox_msgs/serialization.hpp>
// u-blox gps
#include <ublox_gps/async_worker.hpp>
#include <ublox_gps/callback.hpp>
#include <ublox_gps/rtcm.hpp>

namespace ublox_gps {
const std::vector<unsigned int> kBaudrates{ 4800,
                                            9600,
                                            19200,
                                            38400,
                                            57600,
                                            115200,
                                            230400,
                                            460800 };
class Gps final {
 public:
  const int kSetBaudrateSleepMs = 500;
  constexpr static double kDefaultAckTimeout = 1.0;
  constexpr static int kWriterSize = 2056;

  explicit Gps(int debug, const rclcpp::Logger & logger);
  ~Gps();

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

  void setSaveOnShutdown(bool save_on_shutdown) {
    save_on_shutdown_ = save_on_shutdown;
  }

  void setConfigOnStartup(bool config_on_startup) { config_on_startup_flag_ = config_on_startup; }

  void initializeTcp(const std::string & host, const std::string & port);

  void initializeUdp(const std::string & host, const std::string & port);

  void initializeSerial(const std::string & port, unsigned int baudrate,
                        uint16_t uart_in, uint16_t uart_out);

  void resetSerial(const std::string & port);

  bool sendRtcm(const std::vector<uint8_t> &message);

  void close();

  void reset(const std::chrono::milliseconds& wait);

  bool configReset(uint16_t nav_bbr_mask, uint16_t reset_mode);

  bool configGnss(ublox_msgs::msg::CfgGNSS gnss,
                  const std::chrono::milliseconds& wait);

  bool clearBbr();

  bool configUart1(unsigned int baudrate, uint16_t in_proto_mask,
                   uint16_t out_proto_mask);

  bool disableUart1(ublox_msgs::msg::CfgPRT& prev_config);

  bool configUsb(uint16_t tx_ready, uint16_t in_proto_mask,
                 uint16_t out_proto_mask);

  bool configRate(uint16_t meas_rate, uint16_t nav_rate);

  bool configRtcm(const std::vector<Rtcm> & rtcms);

  bool configSbas(bool enable, uint8_t usage, uint8_t max_sbas);

  bool configTmode3Fixed(bool lla_flag,
                         std::vector<double> arp_position,
                         std::vector<int8_t> arp_position_hp,
                         float fixed_pos_acc);

  bool configTmode3SurveyIn(unsigned int svin_min_dur, float svin_acc_limit);

  bool disableTmode3();

  bool setRate(uint8_t class_id, uint8_t message_id, uint8_t rate);

  bool setDynamicModel(uint8_t model);

  bool setFixMode(uint8_t mode);

  bool setDeadReckonLimit(uint8_t limit);

  bool setPpp(bool enable);

  bool setDgnss(uint8_t mode);

  bool setUseAdr(bool enable);

  bool setUTCtime();

  bool setTimtm2(uint8_t rate);

  template <typename T>
  void subscribe(typename CallbackHandler_<T>::Callback callback,
                 unsigned int rate);
  template <typename T>
  void subscribe(typename CallbackHandler_<T>::Callback callback);

  void subscribe_nmea(std::function<void(const std::string &)> callback);

  template <typename T>
  void subscribeId(typename CallbackHandler_<T>::Callback callback,
                   unsigned int message_id);

  template <typename T>
  bool read(T& message,
            const std::chrono::milliseconds& timeout = default_timeout_);

  bool isInitialized() const { return worker_ != nullptr; }
  bool isConfigured() const { return isInitialized() && configured_; }
  bool isOpen() const { return worker_->isOpen(); }

  template <typename ConfigT>
  bool poll(ConfigT& message,
            const std::vector<uint8_t>& payload = std::vector<uint8_t>(),
            const std::chrono::milliseconds& timeout = default_timeout_);
  bool poll(uint8_t class_id, uint8_t message_id,
            const std::vector<uint8_t>& payload = std::vector<uint8_t>());

  template <typename ConfigT>
  bool configure(const ConfigT& message, bool wait = true);

  bool waitForAcknowledge(const std::chrono::milliseconds& timeout,
                          uint8_t class_id, uint8_t msg_id);

  void setRawDataCallback(const Worker::WorkerRawCallback& callback);

 private:
  enum AckType {
    NACK,
    ACK,
    WAIT
  };

  struct Ack {
    AckType type;
    uint8_t class_id;
    uint8_t msg_id;
  };

  void setWorker(const std::shared_ptr<Worker>& worker);

  void subscribeAcks();

  void processAck(const ublox_msgs::msg::Ack &m);

  void processNack(const ublox_msgs::msg::Ack &m);

  void processUpdSosAck(const ublox_msgs::msg::UpdSOSAck &m);

  bool saveOnShutdown();

  std::shared_ptr<Worker> worker_;
  bool configured_;
  bool save_on_shutdown_;
  bool config_on_startup_flag_;


  static const std::chrono::milliseconds default_timeout_;
  std::atomic<Ack> ack_{};

  int debug_;

  CallbackHandlers callbacks_;

  std::string host_, port_;

  rclcpp::Logger logger_;
};

template <typename T>
void Gps::subscribe(
    typename CallbackHandler_<T>::Callback callback, unsigned int rate) {
  if (!setRate(T::CLASS_ID, T::MESSAGE_ID, rate)) {
    return;
  }
  subscribe<T>(callback);
}

template <typename T>
void Gps::subscribe(typename CallbackHandler_<T>::Callback callback) {
  callbacks_.insert<T>(callback);
}

template <typename T>
void Gps::subscribeId(typename CallbackHandler_<T>::Callback callback,
                      unsigned int message_id) {
  callbacks_.insert<T>(callback, message_id);
}

template <typename ConfigT>
bool Gps::poll(ConfigT& message,
               const std::vector<uint8_t>& payload,
               const std::chrono::milliseconds& timeout) {
  if (!poll(ConfigT::CLASS_ID, ConfigT::MESSAGE_ID, payload)) {
    return false;
  }
  return read(message, timeout);
}

template <typename T>
bool Gps::read(T& message, const std::chrono::milliseconds& timeout) {
  if (!worker_) {
    return false;
  }
  return callbacks_.read(message, timeout);
}

template <typename ConfigT>
bool Gps::configure(const ConfigT& message, bool wait) {
  if (!worker_) {
    return false;
  }

  // Reset ack
  Ack ack{};
  ack.type = WAIT;
  ack.class_id = 0;
  ack.msg_id = 0;
  ack_.store(ack, std::memory_order_seq_cst);

  // Encode the message
  std::vector<unsigned char> out(kWriterSize);
  ublox::Writer writer(out.data(), out.size());
  if (!writer.write(message)) {
    RCLCPP_ERROR(logger_, "Failed to encode config message 0x%02x / 0x%02x",
                 message.CLASS_ID, message.MESSAGE_ID);
    return false;
  }
  // Send the message to the device
  worker_->send(out.data(), writer.end() - out.data());

  if (!wait) {
    return true;
  }

  // Wait for an acknowledgment and return whether or not it was received
  return waitForAcknowledge(default_timeout_,
                            message.CLASS_ID,
                            message.MESSAGE_ID);
}

}  // namespace ublox_gps

#endif  // UBLOX_GPS_GPS_HPP