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 "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 <utility>
#include <vector>
namespace nebula::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 = {});
static std::pair<HesaiStatus, std::string> unwrap_http_response(const std::string & response);
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();
std::shared_ptr<HesaiInventoryBase> GetInventory();
std::shared_ptr<HesaiConfigBase> GetConfig();
std::shared_ptr<HesaiLidarStatusBase> 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,
std::shared_ptr<HesaiConfigBase> 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 nebula::drivers
#endif // NEBULA_HESAI_HW_INTERFACE_H