Skip to content

Commit

Permalink
feat(sync_angle): hesai. Enable Clock Sync Features (#39)
Browse files Browse the repository at this point in the history 
* hesai. Enable Sync Phase

Signed-off-by: amc-nu <abraham.monrroy@gmail.com>

* cspell fixes

Signed-off-by: amc-nu <abraham.monrroy@gmail.com>

* cspell fixes

Signed-off-by: amc-nu <abraham.monrroy@gmail.com>

* hesai. enable ptp clock source

Signed-off-by: amc-nu <abraham.monrroy@gmail.com>

* at128. enably clock features sync angle/ptp

Signed-off-by: amc-nu <abraham.monrroy@gmail.com>

---------

Signed-off-by: amc-nu <abraham.monrroy@gmail.com>
Co-authored-by: David Wong <33114676+drwnz@users.noreply.github.com>
  • Loading branch information
@amc-nu @drwnz
amc-nu and drwnz authored Jul 30, 2023
1 parent 9b742bd commit 2249891
Show file tree
Hide file tree
Showing 11 changed files with 292 additions and 58 deletions.
9 changes: 8 additions & 1 deletion .cspell.json
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,13 @@
"AT",
"XT",
"QT",
"XTM"
"XTM",
"Pdelay",
"gprmc",
"Vccint",
"vccint",
"adctp",
"Adctp",

]
}
3 changes: 3 additions & 0 deletions nebula_common/include/nebula_common/nebula_status.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -70,6 +70,9 @@ struct Status
case Status::HTTP_CONNECTION_ERROR:
os << "Http Connection Error";
break;
case Status::WAITING_FOR_SENSOR_RESPONSE:
os << "Waiting for Sensor Response";
break;
case Status::ERROR_1:
default:
os << "Generic Error";
Expand Down
44 changes: 43 additions & 1 deletion ...interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,6 +52,7 @@ 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;
Expand All @@ -76,6 +77,16 @@ const uint16_t PANDAR128_E4X_EXTENDED_PACKET_SIZE = 1117;

const uint16_t MTU_SIZE = 1500;

const int PTP_PROFILE = 0; // Fixed: IEEE 1588v2
const int PTP_DOMAIN_ID = 0; // 0-127, Default: 0
const int PTP_NETWORK_TRANSPORT = 0; // 0: UDP/IP, 1: L2
const int PTP_LOG_ANNOUNCE_INTERVAL = 1; // Time interval between Announce messages, in units of log seconds (default: 1)
const int PTP_SYNC_INTERVAL = 1; //Time interval between Sync messages, in units of log seconds (default: 1)
const int PTP_LOG_MIN_DELAY_INTERVAL = 0; //Minimum permitted mean time between Delay_Req messages, in units of log seconds (default: 0)

const int HESAI_LIDAR_GPS_CLOCK_SOURCE = 0;
const int HESAI_LIDAR_PTP_CLOCK_SOURCE = 1;

/// @brief Hardware interface of hesai driver
class HesaiHwInterface : NebulaHwInterfaceBase
{
Expand Down Expand Up @@ -104,7 +115,7 @@ class HesaiHwInterface : NebulaHwInterfaceBase
int tm_fail_cnt_max = 0;
std::timed_mutex tms_;
int tms_fail_cnt = 0;
int tms_fail_cnt_max = 0;
int tms_fail_cnt_max = 3;
bool wl = true;
bool is_solid_state = false;
int target_model_no;
Expand Down Expand Up @@ -159,6 +170,9 @@ class HesaiHwInterface : NebulaHwInterfaceBase
/// @param setup_sensor Whether to also initialize tcp_driver for sensor configuration
/// @return Resulting status
Status InitializeTcpDriver(bool setup_sensor = true);
/// @brief Closes the TcpDriver and related resources
/// @return Status result
Status FinalizeTcpDriver();
/// @brief Parsing json string to property_tree
/// @param str JSON string
/// @return Parsed property_tree
Expand Down Expand Up @@ -724,6 +738,11 @@ class HesaiHwInterface : NebulaHwInterfaceBase
/// @param with_run Automatically executes run() of TcpDriver
/// @return Resulting status
Status GetLidarRange(bool with_run = true);

Status SetClockSource(std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int clock_source, bool with_run);
Status SetClockSource(std::shared_ptr<boost::asio::io_context> ctx, int clock_source, bool with_run);
Status SetClockSource(int clock_source, bool with_run = true);

/// @brief Setting values with PTC_COMMAND_SET_PTP_CONFIG
/// @param target_tcp_driver TcpDriver used
/// @param profile IEEE timing and synchronization standard
Expand Down Expand Up @@ -864,6 +883,29 @@ class HesaiHwInterface : NebulaHwInterfaceBase
/// @param rpm spin_speed (300, 600, 1200)
/// @return Resulting status
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 logMinDelatReqInterval);
HesaiStatus SetPtpConfigSyncHttp(int profile,
int domain,
int network,
int logAnnounceInterval,
int logSyncInterval,
int logMinDelatReqInterval);
HesaiStatus SetSyncAngleSyncHttp(
std::shared_ptr<boost::asio::io_context> ctx,
int enable,
int angle);
HesaiStatus SetSyncAngleSyncHttp(int enable, int angle);



/// @brief Getting lidar_monitor via HTTP API
/// @param ctx IO Context
/// @param str_callback Callback function for received string
Expand Down
207 changes: 189 additions & 18 deletions nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -84,15 +84,11 @@ Status HesaiHwInterface::CloudInterfaceStart()
std::cout << "Starting UDP server on: " << *sensor_configuration_ << std::endl;
cloud_udp_driver_->init_receiver(
sensor_configuration_->host_ip, sensor_configuration_->data_port);
PrintError("init ok");
cloud_udp_driver_->receiver()->open();
PrintError("open ok");
cloud_udp_driver_->receiver()->bind();
PrintError("bind ok");

cloud_udp_driver_->receiver()->asyncReceive(
std::bind(&HesaiHwInterface::ReceiveCloudPacketCallback, this, std::placeholders::_1));
PrintError("async receive set");
} catch (const std::exception & ex) {
Status status = Status::UDP_CONNECTION_ERROR;
std::cerr << status << sensor_configuration_->sensor_ip << ","
Expand Down Expand Up @@ -201,6 +197,17 @@ Status HesaiHwInterface::InitializeTcpDriver(bool setup_sensor)
return Status::OK;
}

Status HesaiHwInterface::FinalizeTcpDriver() {
try {
tcp_driver_->close();
}
catch(std::exception &e) {
PrintError("Error while finalizing the TcpDriver");
return Status::UDP_CONNECTION_ERROR;
}
return Status::OK;
}

boost::property_tree::ptree HesaiHwInterface::ParseJson(const std::string & str)
{
boost::property_tree::ptree tree;
Expand Down Expand Up @@ -2042,6 +2049,62 @@ Status HesaiHwInterface::GetLidarRange(bool with_run)
[this](HesaiLidarRangeAll & result) { std::cout << result << std::endl; }, with_run);
}

Status HesaiHwInterface::SetClockSource(
std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int clock_source,
bool with_run)
{
std::vector<unsigned char> buf_vec;
int len = 1;
buf_vec.emplace_back(PTC_COMMAND_HEADER_HIGH);
buf_vec.emplace_back(PTC_COMMAND_HEADER_LOW);
buf_vec.emplace_back(PTC_COMMAND_SET_CLOCK_SOURCE);
buf_vec.emplace_back(PTC_COMMAND_DUMMY_BYTE);
buf_vec.emplace_back((len >> 24) & 0xff);
buf_vec.emplace_back((len >> 16) & 0xff);
buf_vec.emplace_back((len >> 8) & 0xff);
buf_vec.emplace_back((len >> 0) & 0xff);

buf_vec.emplace_back((clock_source >> 0) & 0xff);

if (!CheckLock(tms_, tms_fail_cnt, tms_fail_cnt_max, "SetClockSource")) {
return SetClockSource(target_tcp_driver, clock_source, with_run);
}
PrintDebug("SetClockSource: start");

target_tcp_driver->asyncSend(buf_vec, [this]() { CheckUnlock(tms_, "SetClockSource"); });
if (with_run) {
boost::system::error_code ec = target_tcp_driver->run();
if (ec) {
PrintError("HesaiHwInterface::SetClockSource: " + ec.message());
}
#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE
std::cout << "ctx->run(): SetReturnMode" << std::endl;
#endif
}

return Status::WAITING_FOR_SENSOR_RESPONSE;
}
Status HesaiHwInterface::SetClockSource(
std::shared_ptr<boost::asio::io_context> ctx, int clock_source, bool with_run)
{
auto tcp_driver_local = std::make_shared<::drivers::tcp_driver::TcpDriver>(ctx);
tcp_driver_local->init_socket(
sensor_configuration_->sensor_ip, PandarTcpCommandPort, sensor_configuration_->host_ip,
PandarTcpCommandPort);
return SetClockSource(tcp_driver_local, clock_source, with_run);
}
Status HesaiHwInterface::SetClockSource(int clock_source, bool with_run)
{
//*
if (with_run) {
if (tcp_driver_s_->GetIOContext()->stopped()) {
tcp_driver_s_->GetIOContext()->restart();
}
}
//*/
return SetClockSource(tcp_driver_s_, clock_source, with_run);
}

Status HesaiHwInterface::SetPtpConfig(
std::shared_ptr<::drivers::tcp_driver::TcpDriver> target_tcp_driver, int profile, int domain,
int network, int logAnnounceInterval = 1, int logSyncInterval = 1, int logMinDelayReqInterval = 0,
Expand Down Expand Up @@ -2521,14 +2584,90 @@ HesaiStatus HesaiHwInterface::SetSpinSpeedAsyncHttp(uint16_t rpm)
return SetSpinSpeedAsyncHttp(std::make_shared<boost::asio::io_context>(), rpm);
}

HesaiStatus HesaiHwInterface::SetPtpConfigSyncHttp(
std::shared_ptr<boost::asio::io_context> ctx,
int profile,
int domain,
int network,
int logAnnounceInterval,
int logSyncInterval,
int logMinDelatReqInterval)
{
std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd;
auto st = GetHttpClientDriverOnce(ctx, hcd);
if (st != Status::OK) {
return st;
}

auto response = hcd->get(
(boost::format("/pandar.cgi?action=set&object=lidar&key=ptp_configuration&value={" \
"\"Profile\": %d," \
"\"Domain\": %d," \
"\"Network\": %d," \
"\"LogAnnounceInterval\": %d," \
"\"LogSyncInterval\": %d," \
"\"LogMinDelayReqInterval\": %d," \
"\"tsn_switch\": %d" \
"}")
% profile % domain % network % logAnnounceInterval % logSyncInterval %logMinDelatReqInterval % 0
).str());
ctx->run();
PrintInfo(response);
return Status::OK;
}

HesaiStatus HesaiHwInterface::SetPtpConfigSyncHttp(int profile,
int domain,
int network,
int logAnnounceInterval,
int logSyncInterval,
int logMinDelatReqInterval)
{
return SetPtpConfigSyncHttp(std::make_shared<boost::asio::io_context>(),
profile,
domain,
network,
logAnnounceInterval,
logSyncInterval,
logMinDelatReqInterval);
}

HesaiStatus HesaiHwInterface::SetSyncAngleSyncHttp(
std::shared_ptr<boost::asio::io_context> ctx,
int enable,
int angle)
{
std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd;
auto st = GetHttpClientDriverOnce(ctx, hcd);
if (st != Status::OK) {
return st;
}
auto tmp_str = (boost::format("/pandar.cgi?action=set&object=lidar_sync&key=sync_angle&value={" \
"\"sync\": %d," \
"\"syncAngle\": %d" \
"}") % enable % angle).str();
PrintInfo(tmp_str);
auto response = hcd->get(tmp_str);
ctx->run();
PrintInfo(response);
return Status::OK;
}

HesaiStatus HesaiHwInterface::SetSyncAngleSyncHttp(int enable, int angle)
{
return SetSyncAngleSyncHttp(std::make_shared<boost::asio::io_context>(),
enable,
angle);
}

HesaiStatus HesaiHwInterface::GetLidarMonitorAsyncHttp(
std::shared_ptr<boost::asio::io_context> ctx,
std::function<void(const std::string & str)> str_callback)
{
std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> hcd;
auto st = GetHttpClientDriverOnce(ctx, hcd);
if (st != Status::OK) {
PrintError("HesaiHwInterface::GetLidarMonitorAsyncHttp: cannnot GetHttpClientDriverOnce");
PrintError("HesaiHwInterface::GetLidarMonitorAsyncHttp: cannot GetHttpClientDriverOnce");
return st;
}

Expand Down Expand Up @@ -2624,22 +2763,51 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig(
t.join();
}

set_flg = false;
auto sync_angle = static_cast<int>(hesai_config.sync_angle / 100);
auto scan_phase = static_cast<int>(sensor_configuration->scan_phase);
int sync_flg = 0 < scan_phase ? 1 : 0;
if (hesai_config.sync != sync_flg) {
set_flg = true;
} else if (0 < sync_flg && scan_phase != sync_angle) {
if (sensor_configuration->sensor_model != SensorModel::HESAI_PANDARAT128){
set_flg = true;
}
if (set_flg) {
PrintInfo("current lidar sync: " + std::to_string(hesai_config.sync));
PrintInfo("current lidar sync_angle: " + std::to_string(hesai_config.sync_angle));
PrintInfo("current configuration scan_phase: " + std::to_string(scan_phase));
std::thread t([this, sync_flg, scan_phase] { SetSyncAngle(sync_flg, scan_phase); });
auto sync_angle = static_cast<int>(hesai_config.sync_angle / 100);
auto scan_phase = static_cast<int>(sensor_configuration->scan_phase);
int sync_flg = 1;
if (scan_phase != sync_angle) {
set_flg = true;
}
if (sync_flg && set_flg) {
PrintInfo("current lidar sync: " + std::to_string(hesai_config.sync));
PrintInfo("current lidar sync_angle: " + std::to_string(sync_angle));
PrintInfo("current configuration scan_phase: " + std::to_string(scan_phase));
std::thread t([this, sync_flg, scan_phase] {
SetSyncAngle(sync_flg, scan_phase);
});
t.join();
}

std::thread t([this] {
PrintInfo("Trying to set Clock source to PTP");
SetClockSource(HESAI_LIDAR_PTP_CLOCK_SOURCE);
PrintInfo("Trying to set PTP Config: IEEE 1588 v2, Domain: 0, Transport: UDP/IP");
SetPtpConfig(PTP_PROFILE,
PTP_DOMAIN_ID,
PTP_NETWORK_TRANSPORT,
PTP_LOG_ANNOUNCE_INTERVAL,
PTP_SYNC_INTERVAL,
PTP_LOG_MIN_DELAY_INTERVAL
);
});
t.join();
}
else { //AT128 only supports PTP setup via HTTP
PrintInfo("Trying to set SyncAngle via HTTP");
SetSyncAngleSyncHttp(1,
static_cast<int>(sensor_configuration->scan_phase));
PrintInfo("Trying to set PTP Config: IEEE 1588 v2, Domain: 0, Transport: UDP/IP via HTTP");
SetPtpConfigSyncHttp(PTP_PROFILE,
PTP_DOMAIN_ID,
PTP_NETWORK_TRANSPORT,
PTP_LOG_ANNOUNCE_INTERVAL,
PTP_SYNC_INTERVAL,
PTP_LOG_MIN_DELAY_INTERVAL);

}

#ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE
std::cout << "End CheckAndSetConfig(HesaiConfig)!!" << std::endl;
Expand Down Expand Up @@ -2874,6 +3042,9 @@ bool HesaiHwInterface::CheckLock(
tcp_driver_s_->close();
tcp_driver_s_->open();
}
else {
return true;
}
return false;
}
fail_cnt = 0;
Expand Down
1 change: 1 addition & 0 deletions nebula_ros/include/nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,6 +64,7 @@ class HesaiHwInterfaceRosWrapper final : public rclcpp::Node, NebulaHwInterfaceW

public:
explicit HesaiHwInterfaceRosWrapper(const rclcpp::NodeOptions & options);
~HesaiHwInterfaceRosWrapper() noexcept override;
/// @brief Start point cloud streaming (Call CloudInterfaceStart of HwInterface)
/// @return Resulting status
Status StreamStart() override;
Expand Down
Loading

0 comments on commit 2249891

Please sign in to comment.