File hesai_hw_interface.hpp

File List > include > nebula_hw_interfaces > nebula_hw_interfaces_hesai > hesai_hw_interface.hpp

Go to the documentation of this file

// Copyright 2024 TIER IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef NEBULA_HESAI_HW_INTERFACE_H
#define NEBULA_HESAI_HW_INTERFACE_H
// Have to define macros to silence warnings about deprecated headers being used by
// boost/property_tree/ in some versions of boost.
// See: https://github.com/boostorg/property_tree/issues/51
#include <nebula_common/nebula_status.hpp>

#include <boost/version.hpp>

#include <cstddef>
#if (BOOST_VERSION / 100 >= 1073 && BOOST_VERSION / 100 <= 1076)  // Boost 1.73 - 1.76
#define BOOST_BIND_GLOBAL_PLACEHOLDERS
#endif
#if (BOOST_VERSION / 100 == 1074)  // Boost 1.74
#define BOOST_ALLOW_DEPRECATED_HEADERS
#endif
#include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp"

#include <boost_tcp_driver/http_client_driver.hpp>
#include <boost_tcp_driver/tcp_driver.hpp>
#include <boost_udp_driver/udp_driver.hpp>
#include <nebula_common/hesai/hesai_common.hpp>
#include <nebula_common/hesai/hesai_status.hpp>
#include <nebula_common/loggers/logger.hpp>
#include <nebula_common/util/expected.hpp>

#include <boost/algorithm/string.hpp>
#include <boost/property_tree/json_parser.hpp>
#include <boost/property_tree/ptree.hpp>

#include <memory>
#include <mutex>
#include <string>
#include <utility>
#include <vector>

namespace nebula::drivers
{
const int g_pandar_tcp_command_port = 9347;
const uint8_t g_ptc_command_dummy_byte = 0x00;
const uint8_t g_ptc_command_header_high = 0x47;
const uint8_t g_ptc_command_header_low = 0x74;
const uint8_t g_ptc_command_get_lidar_calibration = 0x05;
const uint8_t g_ptc_command_ptp_diagnostics = 0x06;
const uint8_t g_ptc_command_ptp_status = 0x01;
const uint8_t g_ptc_command_ptp_port_data_set = 0x02;
const uint8_t g_ptc_command_ptp_time_status_np = 0x03;
const uint8_t g_ptc_command_ptp_grandmaster_settings_np = 0x04;
const uint8_t g_ptc_command_get_inventory_info = 0x07;
const uint8_t g_ptc_command_get_config_info = 0x08;
const uint8_t g_ptc_command_get_lidar_status = 0x09;
const uint8_t g_ptc_command_set_spin_rate = 0x17;
const uint8_t g_ptc_command_set_sync_angle = 0x18;
const uint8_t g_ptc_command_set_trigger_method = 0x1b;
const uint8_t g_ptc_command_set_standby_mode = 0x1c;
const uint8_t g_ptc_command_set_return_mode = 0x1e;
const uint8_t g_ptc_command_set_clock_source = 0x1f;
const uint8_t g_ptc_command_set_destination_ip = 0x20;
const uint8_t g_ptc_command_set_control_port = 0x21;
const uint8_t g_ptc_command_set_lidar_range = 0x22;
const uint8_t g_ptc_command_get_lidar_range = 0x23;
const uint8_t g_ptc_command_set_ptp_config = 0x24;
const uint8_t g_ptc_command_get_ptp_config = 0x26;
const uint8_t g_ptp_command_set_ptp_lock_offset = 0x39;
const uint8_t g_ptp_command_get_ptp_lock_offset = 0x3a;
const uint8_t g_ptc_command_reset = 0x25;
const uint8_t g_ptc_command_set_rotate_direction = 0x2a;
const uint8_t g_ptc_command_lidar_monitor = 0x27;

const uint8_t g_ptc_error_code_no_error = 0x00;
const uint8_t g_ptc_error_code_invalid_input_param = 0x01;
const uint8_t g_ptc_error_code_server_conn_failed = 0x02;
const uint8_t g_ptc_error_code_invalid_data = 0x03;
const uint8_t g_ptc_error_code_out_of_memory = 0x04;
const uint8_t g_ptc_error_code_unsupported_cmd = 0x05;
const uint8_t g_ptc_error_code_fpga_comm_failed = 0x06;
const uint8_t g_ptc_error_code_other = 0x07;

const uint8_t g_tcp_error_unrelated_response = 1;
const uint8_t g_tcp_error_unexpected_payload = 2;
const uint8_t g_tcp_error_timeout = 4;
const uint8_t g_tcp_error_incomplete_response = 8;

const uint16_t g_mtu_size = 1500;

const size_t g_udp_socket_buffer_size = g_mtu_size * 3600;

// Time interval between Announce messages, in units of log seconds (default: 1)
const int g_ptp_log_announce_interval = 1;
// Time interval between Sync messages, in units of log seconds (default: 1)
const int g_ptp_sync_interval = 1;
// Minimum permitted mean time between Delay_Req messages, in units of log seconds (default: 0)
const int g_ptp_log_min_delay_interval = 0;

const int g_hesai_lidar_gps_clock_source = 0;
const int g_hesai_lidar_ptp_clock_source = 1;

class HesaiHwInterface
{
private:
  struct ptc_error_t
  {
    uint8_t error_flags = 0;
    uint8_t ptc_error_code = 0;

    [[nodiscard]] bool ok() const { return !error_flags && !ptc_error_code; }
  };

  using ptc_cmd_result_t = nebula::util::expected<std::vector<uint8_t>, ptc_error_t>;

  std::shared_ptr<loggers::Logger> logger_;
  std::unique_ptr<::drivers::common::IoContext> cloud_io_context_;
  std::shared_ptr<boost::asio::io_context> m_owned_ctx_;
  std::unique_ptr<::drivers::udp_driver::UdpDriver> cloud_udp_driver_;
  std::shared_ptr<::drivers::tcp_driver::TcpDriver> tcp_driver_;
  std::shared_ptr<const HesaiSensorConfiguration> sensor_configuration_;
  std::function<void(std::vector<uint8_t> & buffer)>
    cloud_packet_callback_; 
  std::mutex mtx_inflight_tcp_request_;

  int target_model_no_;

  HesaiStatus get_http_client_driver_once(
    std::shared_ptr<boost::asio::io_context> ctx,
    std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> & hcd);
  HesaiStatus get_http_client_driver_once(
    std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> & hcd);
  void str_cb(const std::string & str);

  std::string pretty_print_ptc_error(ptc_error_t error_code);

  template <typename T>
  T check_size_and_parse(const std::vector<uint8_t> & data);

  ptc_cmd_result_t send_receive(
    const uint8_t command_id, const std::vector<uint8_t> & payload = {});

  static std::pair<HesaiStatus, std::string> unwrap_http_response(const std::string & response);

public:
  explicit HesaiHwInterface(const std::shared_ptr<loggers::Logger> & logger);
  ~HesaiHwInterface();
  Status initialize_tcp_driver();
  Status finalize_tcp_driver();
  boost::property_tree::ptree parse_json(const std::string & str);

  void receive_sensor_packet_callback(std::vector<uint8_t> & buffer);
  Status sensor_interface_start();
  Status sensor_interface_stop();
  Status get_sensor_configuration(const SensorConfigurationBase & sensor_configuration);
  Status get_calibration_configuration(CalibrationConfigurationBase & calibration_configuration);
  Status set_sensor_configuration(
    std::shared_ptr<const SensorConfigurationBase> sensor_configuration);
  Status register_scan_callback(std::function<void(std::vector<uint8_t> &)> scan_callback);
  std::string get_lidar_calibration_string();
  std::vector<uint8_t> get_lidar_calibration_bytes();
  HesaiPtpDiagStatus get_ptp_diag_status();
  HesaiPtpDiagPort get_ptp_diag_port();
  HesaiPtpDiagTime get_ptp_diag_time();
  HesaiPtpDiagGrandmaster get_ptp_diag_grandmaster();
  std::shared_ptr<HesaiInventoryBase> get_inventory();
  std::shared_ptr<HesaiConfigBase> get_config();
  std::shared_ptr<HesaiLidarStatusBase> get_lidar_status();
  Status set_spin_rate(uint16_t rpm);
  Status set_sync_angle(int sync_angle, int angle);
  Status set_trigger_method(int trigger_method);
  Status set_standby_mode(int standby_mode);
  Status set_return_mode(int return_mode);
  Status set_destination_ip(
    int dest_ip_1, int dest_ip_2, int dest_ip_3, int dest_ip_4, int port, int gps_port);
  Status set_control_port(
    int ip_1, int ip_2, int ip_3, int ip_4, int mask_1, int mask_2, int mask_3, int mask_4,
    int gateway_1, int gateway_2, int gateway_3, int gateway_4, int vlan_flg, int vlan_id);
  Status set_lidar_range(int method, std::vector<unsigned char> data);
  Status set_lidar_range(int start, int end);
  HesaiLidarRangeAll get_lidar_range();

  [[nodiscard]] Status check_and_set_lidar_range(
    const HesaiCalibrationConfigurationBase & calibration);

  Status set_clock_source(int clock_source);

  Status set_ptp_config(
    int profile, int domain, int network, int switch_type, int logAnnounceInterval = 1,
    int logSyncInterval = 1, int logMinDelayReqInterval = 0);
  HesaiPtpConfig get_ptp_config();

  Status set_ptp_lock_offset(uint8_t lock_offset);

  uint8_t get_ptp_lock_offset();

  Status send_reset();
  Status set_rot_dir(int mode);
  HesaiLidarMonitor get_lidar_monitor();

  void io_context_run();
  std::shared_ptr<boost::asio::io_context> get_io_context();

  HesaiStatus set_spin_speed_async_http(std::shared_ptr<boost::asio::io_context> ctx, uint16_t rpm);
  HesaiStatus set_spin_speed_async_http(uint16_t rpm);

  HesaiStatus set_ptp_config_sync_http(
    std::shared_ptr<boost::asio::io_context> ctx, int profile, int domain, int network,
    int logAnnounceInterval, int logSyncInterval, int logMinDelayReqInterval);
  HesaiStatus set_ptp_config_sync_http(
    int profile, int domain, int network, int logAnnounceInterval, int logSyncInterval,
    int logMinDelayReqInterval);
  HesaiStatus set_sync_angle_sync_http(
    std::shared_ptr<boost::asio::io_context> ctx, int enable, int angle);
  HesaiStatus set_sync_angle_sync_http(int enable, int angle);

  HesaiStatus get_lidar_monitor_async_http(
    std::shared_ptr<boost::asio::io_context> ctx,
    std::function<void(const std::string & str)> str_callback);
  HesaiStatus get_lidar_monitor_async_http(
    std::function<void(const std::string & str)> str_callback);

  HesaiStatus check_and_set_config(
    std::shared_ptr<const HesaiSensorConfiguration> sensor_configuration,
    std::shared_ptr<HesaiConfigBase> hesai_config);
  HesaiStatus check_and_set_config(
    std::shared_ptr<const HesaiSensorConfiguration> sensor_configuration,
    HesaiLidarRangeAll hesai_lidar_range_all);
  HesaiStatus check_and_set_config();

  int nebula_model_to_hesai_model_no(nebula::drivers::SensorModel model);

  void set_target_model(int model);

  void set_target_model(nebula::drivers::SensorModel model);

  bool use_http_set_spin_rate(int model);
  bool use_http_set_spin_rate();
  bool use_http_get_lidar_monitor(int model);
  bool use_http_get_lidar_monitor();
};
}  // namespace nebula::drivers

#endif  // NEBULA_HESAI_HW_INTERFACE_H