Skip to content

Commit

Permalink
feat: hesai ptp setup parameters (#110)
Browse files Browse the repository at this point in the history 
* add initial support for ot128/128e4x

Signed-off-by: amc-nu <[email protected]>

* Initial support for QT128

Signed-off-by: amc-nu <[email protected]>

* hesai. automotive ptp support

Signed-off-by: amc-nu <[email protected]>

* nebula_decoder. hesai tokenize the calibration file to avoid errors at load

Signed-off-by: amc-nu <[email protected]>

* hesai_w. fix spellcheck

Signed-off-by: amc-nu <[email protected]>

* nebula_ros. add xml based component launcher

Signed-off-by: amc-nu <[email protected]>

* launch. launch typo

Signed-off-by: amc-nu <[email protected]>

* launch. match target container

Signed-off-by: amc-nu <[email protected]>

* hesai_hw. replace publisher with move operator

Signed-off-by: amc-nu <[email protected]>

* launch. add ptp support to all launch systems

Signed-off-by: amc-nu <[email protected]>

* launch. component_launch reset namespace

Signed-off-by: amc-nu <[email protected]>

* velodyne launch. revert commented out hw

Signed-off-by: amc-nu <[email protected]>

* hesai_hw_interface. add explanation to sleep time between cmds

Signed-off-by: amc-nu <[email protected]>

* hesai_common. reduce cout verbosity

Signed-off-by: amc-nu <[email protected]>

---------

Signed-off-by: amc-nu <[email protected]>
  • Loading branch information
@amc-nu
amc-nu authored Mar 8, 2024
1 parent 910d12b commit 45c01cf
Show file tree
Hide file tree
Showing 9 changed files with 466 additions and 189 deletions.
96 changes: 36 additions & 60 deletions nebula_common/include/nebula_common/hesai/hesai_common.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,9 @@ struct HesaiSensorConfiguration : SensorConfigurationBase
uint16_t rotation_speed;
uint16_t cloud_min_angle;
uint16_t cloud_max_angle;
PtpProfile ptp_profile;
uint8_t ptp_domain;
PtpTransportType ptp_transport_type;
};
/// @brief Convert HesaiSensorConfiguration to string (Overloading the << operator)
/// @param os
Expand All @@ -32,7 +35,9 @@ inline std::ostream & operator<<(std::ostream & os, HesaiSensorConfiguration con
os << (SensorConfigurationBase)(arg) << ", GnssPort: " << arg.gnss_port
<< ", ScanPhase:" << arg.scan_phase << ", RotationSpeed:" << arg.rotation_speed
<< ", FOV(Start):" << arg.cloud_min_angle << ", FOV(End):" << arg.cloud_max_angle
<< ", DualReturnDistanceThreshold:" << arg.dual_return_distance_threshold;
<< ", DualReturnDistanceThreshold:" << arg.dual_return_distance_threshold
<< ", PtpProfile:" << arg.ptp_profile << ", PtpDomain:" << std::to_string(arg.ptp_domain)
<< ", PtpTransportType:" << arg.ptp_transport_type;
return os;
}

Expand All @@ -48,21 +53,10 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase
if (!ifs) {
return Status::INVALID_CALIBRATION_FILE;
}

std::string header;
std::getline(ifs, header);

char sep;
int laser_id;
float elevation;
float azimuth;
while (!ifs.eof()) {
ifs >> laser_id >> sep >> elevation >> sep >> azimuth;
elev_angle_map[laser_id - 1] = elevation;
azimuth_offset_map[laser_id - 1] = azimuth;
}
std::ostringstream ss;
ss << ifs.rdbuf(); // reading data
ifs.close();
return Status::OK;
return LoadFromString(ss.str());
}

/// @brief Loading calibration data
Expand All @@ -72,28 +66,31 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase
{
std::stringstream ss;
ss << calibration_content;

std::string header;
std::getline(ss, header);

char sep;
int laser_id;
float elevation;
float azimuth;

std::string line;
while (std::getline(ss, line)) {
if (line.empty()) {
constexpr size_t expected_cols = 3;
while(std::getline(ss, line)) {
boost::char_separator<char> sep(",");
boost::tokenizer<boost::char_separator<char>> tok(line, sep);

std::vector<std::string> actual_tokens(tok.begin(), tok.end());
if (actual_tokens.size() < expected_cols
|| actual_tokens.size() > expected_cols
) {
std::cerr << "Ignoring line with unexpected data:" << line << std::endl;
continue;
}
std::stringstream line_ss;
line_ss << line;
line_ss >> laser_id >> sep >> elevation >> sep >> azimuth;
elev_angle_map[laser_id - 1] = elevation;
azimuth_offset_map[laser_id - 1] = azimuth;
// std::cout << "laser_id=" << laser_id << ", elevation=" << elevation << ", azimuth=" << azimuth << std::endl;

try {
int laser_id = std::stoi(actual_tokens[0]);
float elevation = std::stof(actual_tokens[1]);
float azimuth = std::stof(actual_tokens[2]);
elev_angle_map[laser_id - 1] = elevation;
azimuth_offset_map[laser_id - 1] = azimuth;
} catch (const std::invalid_argument& ia) {
continue;
}

}
// std::cout << "LoadFromString fin" << std::endl;
return Status::OK;
}

Expand Down Expand Up @@ -129,9 +126,7 @@ struct HesaiCalibrationConfiguration : CalibrationConfigurationBase
return Status::CANNOT_SAVE_FILE;
}
ofs << calibration_string;
// std::cout << calibration_string << std::endl;
ofs.close();

return Status::OK;
}
};
Expand Down Expand Up @@ -169,21 +164,15 @@ struct HesaiCorrection
delimiter = (buf[index] & 0xff) << 8 | ((buf[index + 1] & 0xff));
versionMajor = buf[index + 2] & 0xff;
versionMinor = buf[index + 3] & 0xff;
std::cout << "versionMajor=" << static_cast<int>(versionMajor) << std::endl;
std::cout << "versionMinor=" << static_cast<int>(versionMinor) << std::endl;
channelNumber = buf[index + 4] & 0xff;
std::cout << "channelNumber=" << static_cast<int>(channelNumber) << std::endl;
mirrorNumber = buf[index + 5] & 0xff;
std::cout << "mirrorNumber=" << static_cast<int>(mirrorNumber) << std::endl;
frameNumber = buf[index + 6] & 0xff;
std::cout << "frameNumber=" << static_cast<int>(frameNumber) << std::endl;
index += 7;
for (uint8_t i = 0; i < 8; i++) {
frameConfig[i] = buf[index] & 0xff;
index++;
}
resolution = buf[index] & 0xff;
std::cout << "resolution=" << static_cast<int>(resolution) << std::endl;
index++;
switch (versionMinor) {
case 5:
Expand Down Expand Up @@ -216,14 +205,9 @@ struct HesaiCorrection
index++;
}

// 230328 add
for (uint8_t i = 0; i < mirrorNumber; i++) {
startFrame[i] *= resolution;
endFrame[i] *= resolution;
std::cout << "startFrame[" << static_cast<int>(i)
<< "]=" << static_cast<int>(startFrame[i]) << std::endl;
std::cout << "endFrame[" << static_cast<int>(i) << "]=" << static_cast<int>(endFrame[i])
<< std::endl;
}
for (uint8_t i = 0; i < channelNumber; i++) {
azimuth[i] *= resolution;
Expand Down Expand Up @@ -261,20 +245,6 @@ struct HesaiCorrection
index++;
}

for (uint8_t i = 0; i < mirrorNumber; i++) {
std::cout << "startFrame[" << static_cast<int>(i)
<< "]=" << static_cast<int>(startFrame[i]) << std::endl;
std::cout << "endFrame[" << static_cast<int>(i) << "]=" << static_cast<int>(endFrame[i])
<< std::endl;
/*
startFrame[i] *= 2.56;
endFrame[i] *= 2.56;
std::cout << "startFrame[" << static_cast<int>(i) << "]=" << static_cast<int>(startFrame[i])
<< std::endl; std::cout << "endFrame[" << static_cast<int>(i) << "]=" <<
static_cast<int>(endFrame[i]) << std::endl;
*/
}

break;

default:
Expand Down Expand Up @@ -392,6 +362,8 @@ inline ReturnMode ReturnModeFromStringHesai(
switch (sensor_model) {
case SensorModel::HESAI_PANDARXT32M:
case SensorModel::HESAI_PANDARAT128:
case SensorModel::HESAI_PANDAR128_E4X:
case SensorModel::HESAI_PANDARQT128:
if (return_mode == "Last") return ReturnMode::LAST;
if (return_mode == "Strongest") return ReturnMode::STRONGEST;
if (return_mode == "LastStrongest") return ReturnMode::DUAL_LAST_STRONGEST;
Expand Down Expand Up @@ -424,6 +396,8 @@ inline ReturnMode ReturnModeFromIntHesai(const int return_mode, const SensorMode
switch (sensor_model) {
case SensorModel::HESAI_PANDARXT32M:
case SensorModel::HESAI_PANDARAT128:
case SensorModel::HESAI_PANDAR128_E4X:
case SensorModel::HESAI_PANDARQT128:
if (return_mode == 0) return ReturnMode::LAST;
if (return_mode == 1) return ReturnMode::STRONGEST;
if (return_mode == 2) return ReturnMode::DUAL_LAST_STRONGEST;
Expand Down Expand Up @@ -455,6 +429,8 @@ inline int IntFromReturnModeHesai(const ReturnMode return_mode, const SensorMode
switch (sensor_model) {
case SensorModel::HESAI_PANDARXT32M:
case SensorModel::HESAI_PANDARAT128:
case SensorModel::HESAI_PANDAR128_E4X:
case SensorModel::HESAI_PANDARQT128:
if (return_mode == ReturnMode::LAST) return 0;
if (return_mode == ReturnMode::STRONGEST) return 1;
if (return_mode == ReturnMode::DUAL_LAST_STRONGEST
Expand Down
89 changes: 88 additions & 1 deletion nebula_common/include/nebula_common/nebula_common.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
#define NEBULA_COMMON_H

#include <nebula_common/point_types.hpp>

#include <boost/tokenizer.hpp>
#include <map>
#include <ostream>
#include <string>
Expand Down Expand Up @@ -341,6 +341,19 @@ enum class datatype {
FLOAT64 = 8
};

enum class PtpProfile {
IEEE_1588v2 = 0,
IEEE_802_1AS,
IEEE_802_1AS_AUTO,
PROFILE_UNKNOWN
};

enum class PtpTransportType {
UDP_IP = 0,
L2,
UNKNOWN_TRANSPORT
};

/// @brief not used?
struct PointField
{
Expand Down Expand Up @@ -556,6 +569,80 @@ inline ReturnMode ReturnModeFromString(const std::string & return_mode)
return ReturnMode::UNKNOWN;
}

/// @brief Converts String to PTP Profile
/// @param ptp_profile Profile as String
/// @return Corresponding PtpProfile
inline PtpProfile PtpProfileFromString(const std::string & ptp_profile)
{
// Hesai
auto tmp_str = ptp_profile;
std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(),
[](unsigned char c){ return std::tolower(c); });
if (tmp_str == "1588v2") return PtpProfile::IEEE_1588v2;
if (tmp_str == "802.1as") return PtpProfile::IEEE_802_1AS;
if (tmp_str == "automotive") return PtpProfile::IEEE_802_1AS_AUTO;

return PtpProfile::PROFILE_UNKNOWN;
}

/// @brief Convert PtpProfile enum to string (Overloading the << operator)
/// @param os
/// @param arg
/// @return stream
inline std::ostream & operator<<(std::ostream & os, nebula::drivers::PtpProfile const & arg)
{
switch (arg) {
case PtpProfile::IEEE_1588v2:
os << "IEEE_1588v2";
break;
case PtpProfile::IEEE_802_1AS:
os << "IEEE_802.1AS";
break;
case PtpProfile::IEEE_802_1AS_AUTO:
os << "IEEE_802.1AS Automotive";
break;
case PtpProfile::PROFILE_UNKNOWN:
os << "UNKNOWN";
break;
}
return os;
}

/// @brief Converts String to PTP TransportType
/// @param transport_type Transport as String
/// @return Corresponding PtpTransportType
inline PtpTransportType PtpTransportTypeFromString(const std::string & transport_type)
{
// Hesai
auto tmp_str = transport_type;
std::transform(tmp_str.begin(), tmp_str.end(), tmp_str.begin(),
[](unsigned char c){ return std::tolower(c); });
if (tmp_str == "udp") return PtpTransportType::UDP_IP;
if (tmp_str == "l2") return PtpTransportType::L2;

return PtpTransportType::UNKNOWN_TRANSPORT;
}

/// @brief Convert PtpTransportType enum to string (Overloading the << operator)
/// @param os
/// @param arg
/// @return stream
inline std::ostream & operator<<(std::ostream & os, nebula::drivers::PtpTransportType const & arg)
{
switch (arg) {
case PtpTransportType::UDP_IP:
os << "UDP/IP";
break;
case PtpTransportType::L2:
os << "L2";
break;
case PtpTransportType::UNKNOWN_TRANSPORT:
os << "UNKNOWN";
break;
}
return os;
}

[[maybe_unused]] pcl::PointCloud<PointXYZIR>::Ptr convertPointXYZIRADTToPointXYZIR(
const pcl::PointCloud<PointXYZIRADT>::ConstPtr & input_pointcloud);

Expand Down
5 changes: 1 addition & 4 deletions ...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 @@ -77,9 +77,6 @@ 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)
Expand Down Expand Up @@ -116,7 +113,7 @@ class HesaiHwInterface : NebulaHwInterfaceBase
std::timed_mutex tms_;
int tms_fail_cnt = 0;
int tms_fail_cnt_max = 3;
bool wl = true;
bool wl = false;
bool is_solid_state = false;
int target_model_no;

Expand Down
Loading

0 comments on commit 45c01cf

Please sign in to comment.