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 <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 "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/util/expected.hpp"
#include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_cmd_response.hpp"

#include <rclcpp/rclcpp.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 <vector>

namespace nebula
{
namespace drivers
{
const int PandarTcpCommandPort = 9347;
const uint8_t PTC_COMMAND_DUMMY_BYTE = 0x00;
const uint8_t PTC_COMMAND_HEADER_HIGH = 0x47;
const uint8_t PTC_COMMAND_HEADER_LOW = 0x74;
const uint8_t PTC_COMMAND_GET_LIDAR_CALIBRATION = 0x05;
const uint8_t PTC_COMMAND_PTP_DIAGNOSTICS = 0x06;
const uint8_t PTC_COMMAND_PTP_STATUS = 0x01;
const uint8_t PTC_COMMAND_PTP_PORT_DATA_SET = 0x02;
const uint8_t PTC_COMMAND_PTP_TIME_STATUS_NP = 0x03;
const uint8_t PTC_COMMAND_PTP_GRANDMASTER_SETTINGS_NP = 0x04;
const uint8_t PTC_COMMAND_GET_INVENTORY_INFO = 0x07;
const uint8_t PTC_COMMAND_GET_CONFIG_INFO = 0x08;
const uint8_t PTC_COMMAND_GET_LIDAR_STATUS = 0x09;
const uint8_t PTC_COMMAND_SET_SPIN_RATE = 0x17;
const uint8_t PTC_COMMAND_SET_SYNC_ANGLE = 0x18;
const uint8_t PTC_COMMAND_SET_TRIGGER_METHOD = 0x1b;
const uint8_t PTC_COMMAND_SET_STANDBY_MODE = 0x1c;
const uint8_t PTC_COMMAND_SET_RETURN_MODE = 0x1e;
const uint8_t PTC_COMMAND_SET_CLOCK_SOURCE = 0x1f;
const uint8_t PTC_COMMAND_SET_DESTINATION_IP = 0x20;
const uint8_t PTC_COMMAND_SET_CONTROL_PORT = 0x21;
const uint8_t PTC_COMMAND_SET_LIDAR_RANGE = 0x22;
const uint8_t PTC_COMMAND_GET_LIDAR_RANGE = 0x23;
const uint8_t PTC_COMMAND_SET_PTP_CONFIG = 0x24;
const uint8_t PTC_COMMAND_GET_PTP_CONFIG = 0x26;
const uint8_t PTC_COMMAND_RESET = 0x25;
const uint8_t PTC_COMMAND_SET_ROTATE_DIRECTION = 0x2a;
const uint8_t PTC_COMMAND_LIDAR_MONITOR = 0x27;

const uint8_t PTC_ERROR_CODE_NO_ERROR = 0x00;
const uint8_t PTC_ERROR_CODE_INVALID_INPUT_PARAM = 0x01;
const uint8_t PTC_ERROR_CODE_SERVER_CONN_FAILED = 0x02;
const uint8_t PTC_ERROR_CODE_INVALID_DATA = 0x03;
const uint8_t PTC_ERROR_CODE_OUT_OF_MEMORY = 0x04;
const uint8_t PTC_ERROR_CODE_UNSUPPORTED_CMD = 0x05;
const uint8_t PTC_ERROR_CODE_FPGA_COMM_FAILED = 0x06;
const uint8_t PTC_ERROR_CODE_OTHER = 0x07;

const uint8_t TCP_ERROR_UNRELATED_RESPONSE = 1;
const uint8_t TCP_ERROR_UNEXPECTED_PAYLOAD = 2;
const uint8_t TCP_ERROR_TIMEOUT = 4;
const uint8_t TCP_ERROR_INCOMPLETE_RESPONSE = 8;

const uint16_t PANDARQT64_PACKET_SIZE = 1072;
const uint16_t PANDARQT128_PACKET_SIZE = 1127;
const uint16_t PANDARXT32_PACKET_SIZE = 1080;
const uint16_t PANDARXT32M_PACKET_SIZE = 820;
const uint16_t PANDARAT128_PACKET_SIZE = 1118;
const uint16_t PANDAR64_PACKET_SIZE = 1194;
const uint16_t PANDAR64_EXTENDED_PACKET_SIZE = 1198;
const uint16_t PANDAR40_PACKET_SIZE = 1262;
const uint16_t PANDAR40P_EXTENDED_PACKET_SIZE = 1266;
const uint16_t PANDAR128_E4X_PACKET_SIZE = 861;
const uint16_t PANDAR128_E4X_EXTENDED_PACKET_SIZE = 1117;
const uint16_t MTU_SIZE = 1500;

const size_t UDP_SOCKET_BUFFER_SIZE = MTU_SIZE * 3600;

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

const int HESAI_LIDAR_GPS_CLOCK_SOURCE = 0;
const int 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::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 GetHttpClientDriverOnce(
    std::shared_ptr<boost::asio::io_context> ctx,
    std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> & hcd);
  HesaiStatus GetHttpClientDriverOnce(
    std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> & hcd);
  void str_cb(const std::string & str);

  std::shared_ptr<rclcpp::Logger> parent_node_logger;
  void PrintInfo(std::string info);
  void PrintError(std::string error);
  void PrintDebug(std::string debug);
  void PrintDebug(const std::vector<uint8_t> & bytes);

  std::string PrettyPrintPTCError(ptc_error_t error_code);

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

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

public:
  HesaiHwInterface();
  ~HesaiHwInterface();
  Status InitializeTcpDriver();
  Status FinalizeTcpDriver();
  boost::property_tree::ptree ParseJson(const std::string & str);

  void ReceiveSensorPacketCallback(std::vector<uint8_t> & buffer);
  Status SensorInterfaceStart();
  Status SensorInterfaceStop();
  Status GetSensorConfiguration(const SensorConfigurationBase & sensor_configuration);
  Status GetCalibrationConfiguration(CalibrationConfigurationBase & calibration_configuration);
  Status SetSensorConfiguration(
    std::shared_ptr<const SensorConfigurationBase> sensor_configuration);
  Status RegisterScanCallback(std::function<void(std::vector<uint8_t> &)> scan_callback);
  std::string GetLidarCalibrationString();
  std::vector<uint8_t> GetLidarCalibrationBytes();
  HesaiPtpDiagStatus GetPtpDiagStatus();
  HesaiPtpDiagPort GetPtpDiagPort();
  HesaiPtpDiagTime GetPtpDiagTime();
  HesaiPtpDiagGrandmaster GetPtpDiagGrandmaster();
  HesaiInventory GetInventory();
  HesaiConfig GetConfig();
  HesaiLidarStatus GetLidarStatus();
  Status SetSpinRate(uint16_t rpm);
  Status SetSyncAngle(int sync_angle, int angle);
  Status SetTriggerMethod(int trigger_method);
  Status SetStandbyMode(int standby_mode);
  Status SetReturnMode(int return_mode);
  Status SetDestinationIp(
    int dest_ip_1, int dest_ip_2, int dest_ip_3, int dest_ip_4, int port, int gps_port);
  Status SetControlPort(
    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 SetLidarRange(int method, std::vector<unsigned char> data);
  Status SetLidarRange(int start, int end);
  HesaiLidarRangeAll GetLidarRange();

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

  Status SetClockSource(int clock_source);

  Status SetPtpConfig(
    int profile, int domain, int network, int switch_type, int logAnnounceInterval = 1,
    int logSyncInterval = 1, int logMinDelayReqInterval = 0);
  HesaiPtpConfig GetPtpConfig();
  Status SendReset();
  Status SetRotDir(int mode);
  HesaiLidarMonitor GetLidarMonitor();

  void IOContextRun();
  std::shared_ptr<boost::asio::io_context> GetIOContext();

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

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

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

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

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

  void SetTargetModel(int model);

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

  bool UseHttpSetSpinRate(int model);
  bool UseHttpSetSpinRate();
  bool UseHttpGetLidarMonitor(int model);
  bool UseHttpGetLidarMonitor();

  void SetLogger(std::shared_ptr<rclcpp::Logger> node);
};
}  // namespace drivers
}  // namespace nebula

#endif  // NEBULA_HESAI_HW_INTERFACE_H