diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 9bc48e55e..ad9c1757e 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -87,7 +87,9 @@ repos: rev: 1.6.1 hooks: - id: cpplint - args: [--quiet] + # runtime/arrays uses the name of the variable to determine const-ness. + # This does not play well with our naming conventions + args: [--quiet, --filter=-runtime/arrays] exclude: .cu exclude: .svg diff --git a/nebula_common/include/nebula_common/continental/continental_ars548.hpp b/nebula_common/include/nebula_common/continental/continental_ars548.hpp index 971b3bad7..6d861384d 100644 --- a/nebula_common/include/nebula_common/continental/continental_ars548.hpp +++ b/nebula_common/include/nebula_common/continental/continental_ars548.hpp @@ -216,64 +216,64 @@ using boost::endian::big_uint16_buf_t; using boost::endian::big_uint32_buf_t; using boost::endian::big_uint64_buf_t; -constexpr int CONFIGURATION_SERVICE_ID = 0; -constexpr int CONFIGURATION_METHOD_ID = 390; -constexpr int CONFIGURATION_PAYLOAD_LENGTH = 56; -constexpr int CONFIGURATION_UDP_LENGTH = 64; +constexpr int configuration_service_id = 0; +constexpr int configuration_method_id = 390; +constexpr int configuration_payload_length = 56; +constexpr int configuration_udp_length = 64; -constexpr int DETECTION_LIST_METHOD_ID = 336; -constexpr int OBJECT_LIST_METHOD_ID = 329; -constexpr int SENSOR_STATUS_METHOD_ID = 380; -constexpr int FILTER_STATUS_METHOD_ID = 396; +constexpr int detection_list_method_id = 336; +constexpr int object_list_method_id = 329; +constexpr int sensor_status_method_id = 380; +constexpr int filter_status_method_id = 396; -constexpr int DETECTION_LIST_UDP_PAYLOAD = 35336; -constexpr int OBJECT_LIST_UDP_PAYLOAD = 9401; -constexpr int SENSOR_STATUS_UDP_PAYLOAD = 84; -constexpr int FILTER_STATUS_UDP_PAYLOAD = 330; +constexpr int detection_list_udp_payload = 35336; +constexpr int object_list_udp_payload = 9401; +constexpr int sensor_status_udp_payload = 84; +constexpr int filter_status_udp_payload = 330; -constexpr int DETECTION_LIST_PDU_LENGTH = 35328; -constexpr int OBJECT_LIST_PDU_LENGTH = 9393; -constexpr int SENSOR_STATUS_PDU_LENGTH = 76; -constexpr int FILTER_STATUS_PDU_LENGTH = 322; +constexpr int detection_list_pdu_length = 35328; +constexpr int object_list_pdu_length = 9393; +constexpr int sensor_status_pdu_length = 76; +constexpr int filter_status_pdu_length = 322; -constexpr int DETECTION_FILTER_PROPERTIES_NUM = 7; -constexpr int OBJECT_FILTER_PROPERTIES_NUM = 24; -constexpr int MAX_DETECTIONS = 800; -constexpr int MAX_OBJECTS = 50; +constexpr int detection_filter_properties_num = 7; +constexpr int object_filter_properties_num = 24; +constexpr int max_detections = 800; +constexpr int max_objects = 50; -constexpr int SYNC_OK = 1; -constexpr int NEVER_SYNC = 2; -constexpr int SYNC_LOST = 3; +constexpr int sync_ok = 1; +constexpr int never_sync = 2; +constexpr int sync_lost = 3; -constexpr int PLUG_RIGHT = 0; -constexpr int PLUG_LEFT = 1; +constexpr int plug_right = 0; +constexpr int plug_left = 1; -constexpr int FREQUENCY_SLOT_LOW = 0; -constexpr int FREQUENCY_SLOT_MID = 1; -constexpr int FREQUENCY_SLOT_HIGH = 2; +constexpr int frequency_slot_low = 0; +constexpr int frequency_slot_mid = 1; +constexpr int frequency_slot_high = 2; -constexpr int HCC_WORLDWIDE = 1; -constexpr int HCC_JAPAN = 2; +constexpr int hcc_worldwide = 1; +constexpr int hcc_japan = 2; -constexpr int POWERSAVE_STANDSTILL_OFF = 0; -constexpr int POWERSAVE_STANDSTILL_ON = 1; +constexpr int powersave_standstill_off = 0; +constexpr int powersave_standstill_on = 1; -constexpr int VDY_OK = 0; -constexpr int VDY_NOTOK = 1; +constexpr int vdy_ok = 0; +constexpr int vdy_notok = 1; -constexpr int STATE_INIT = 0; -constexpr int STATE_OK = 1; -constexpr int STATE_INVALID = 2; +constexpr int state_init = 0; +constexpr int state_ok = 1; +constexpr int state_invalid = 2; -constexpr int BLOCKAGE_STATUS_BLIND = 0; -constexpr int BLOCKAGE_STATUS_HIGH = 1; -constexpr int BLOCKAGE_STATUS_MID = 2; -constexpr int BLOCKAGE_STATUS_LOW = 3; -constexpr int BLOCKAGE_STATUS_NONE = 4; +constexpr int blockage_status_blind = 0; +constexpr int blockage_status_high = 1; +constexpr int blockage_status_mid = 2; +constexpr int blockage_status_low = 3; +constexpr int blockage_status_none = 4; -constexpr int BLOCKAGE_TEST_FAILED = 0; -constexpr int BLOCKAGE_TEST_PASSED = 1; -constexpr int BLOCKAGE_TEST_ONGOING = 2; +constexpr int blockage_test_failed = 0; +constexpr int blockage_test_passed = 1; +constexpr int blockage_test_ongoing = 2; #pragma pack(push, 1) @@ -352,7 +352,7 @@ struct DetectionListPacket big_float32_buf_t origin_yaw{}; big_float32_buf_t origin_yaw_std{}; uint8_t list_invalid_flags{}; - DetectionPacket detections[MAX_DETECTIONS]; + DetectionPacket detections[max_detections]; big_float32_buf_t list_rad_vel_domain_min{}; big_float32_buf_t list_rad_vel_domain_max{}; big_uint32_buf_t number_of_detections{}; @@ -439,7 +439,7 @@ struct ObjectListPacket big_uint32_buf_t event_data_qualifier{}; uint8_t extended_qualifier{}; uint8_t number_of_objects{}; - ObjectPacket objects[MAX_OBJECTS]; + ObjectPacket objects[max_objects]; }; struct StatusConfigurationPacket @@ -573,8 +573,8 @@ struct FilterStatusPacket uint8_t filter_configuration_counter{}; uint8_t detection_sort_index{}; uint8_t object_sort_index{}; - FilterStatusEntryPacket detection_filters[DETECTION_FILTER_PROPERTIES_NUM]; - FilterStatusEntryPacket object_filters[OBJECT_FILTER_PROPERTIES_NUM]; + FilterStatusEntryPacket detection_filters[detection_filter_properties_num]; + FilterStatusEntryPacket object_filters[object_filter_properties_num]; }; #pragma pack(pop) diff --git a/nebula_common/include/nebula_common/continental/continental_srr520.hpp b/nebula_common/include/nebula_common/continental/continental_srr520.hpp index 34f1c3514..54b6955f1 100644 --- a/nebula_common/include/nebula_common/continental/continental_srr520.hpp +++ b/nebula_common/include/nebula_common/continental/continental_srr520.hpp @@ -64,49 +64,49 @@ using boost::endian::big_uint32_buf_t; using boost::endian::big_uint64_buf_t; // CAN MSG IDS -constexpr int RDI_NEAR_HEADER_CAN_MESSAGE_ID = 900; -constexpr int RDI_NEAR_ELEMENT_CAN_MESSAGE_ID = 901; -constexpr int RDI_HRR_HEADER_CAN_MESSAGE_ID = 1100; -constexpr int RDI_HRR_ELEMENT_CAN_MESSAGE_ID = 1101; -constexpr int OBJECT_HEADER_CAN_MESSAGE_ID = 1200; -constexpr int OBJECT_CAN_MESSAGE_ID = 1201; -constexpr int CRC_LIST_CAN_MESSAGE_ID = 800; -constexpr int STATUS_CAN_MESSAGE_ID = 700; -constexpr int SYNC_FOLLOW_UP_CAN_MESSAGE_ID = 53; -constexpr int VEH_DYN_CAN_MESSAGE_ID = 600; -constexpr int SENSOR_CONFIG_CAN_MESSAGE_ID = 601; +constexpr int rdi_near_header_can_message_id = 900; +constexpr int rdi_near_element_can_message_id = 901; +constexpr int rdi_hrr_header_can_message_id = 1100; +constexpr int rdi_hrr_element_can_message_id = 1101; +constexpr int object_header_can_message_id = 1200; +constexpr int object_can_message_id = 1201; +constexpr int crc_list_can_message_id = 800; +constexpr int status_can_message_id = 700; +constexpr int sync_follow_up_can_message_id = 53; +constexpr int veh_dyn_can_message_id = 600; +constexpr int sensor_config_can_message_id = 601; // CRC IDS -constexpr int NEAR_CRC_ID = 0; -constexpr int HRR_CRC_ID = 1; -constexpr int OBJECT_CRC_ID = 2; -constexpr int TIME_DOMAIN_ID = +constexpr int near_crc_id = 0; +constexpr int hrr_crc_id = 1; +constexpr int object_crc_id = 2; +constexpr int time_domain_id = 0; // For details, please refer to AUTOSAR's "Time Synchronization over CAN" document -constexpr int RDI_NEAR_HEADER_PACKET_SIZE = 32; -constexpr int RDI_NEAR_ELEMENT_PACKET_SIZE = 64; -constexpr int RDI_HRR_HEADER_PACKET_SIZE = 32; -constexpr int RDI_HRR_ELEMENT_PACKET_SIZE = 64; -constexpr int OBJECT_HEADER_PACKET_SIZE = 32; -constexpr int OBJECT_PACKET_SIZE = 64; -constexpr int CRC_LIST_PACKET_SIZE = 4; -constexpr int STATUS_PACKET_SIZE = 64; -constexpr int SYNC_FOLLOW_UP_CAN_PACKET_SIZE = 8; -constexpr int VEH_DYN_CAN_PACKET_SIZE = 8; -constexpr int CONFIGURATION_PACKET_SIZE = 16; - -constexpr int RDI_NEAR_PACKET_NUM = 50; -constexpr int RDI_HRR_PACKET_NUM = 20; -constexpr int OBJECT_PACKET_NUM = 20; - -constexpr int FRAGMENTS_PER_DETECTION_PACKET = 10; -constexpr int FRAGMENTS_PER_OBJECT_PACKET = 2; -constexpr int DETECTION_FRAGMENT_SIZE = 6; -constexpr int OBJECT_FRAGMENT_SIZE = 31; - -constexpr int MAX_RDI_NEAR_DETECTIONS = RDI_NEAR_PACKET_NUM * FRAGMENTS_PER_DETECTION_PACKET; -constexpr int MAX_RDI_HRR_DETECTIONS = RDI_HRR_PACKET_NUM * FRAGMENTS_PER_DETECTION_PACKET; -constexpr int MAX_OBJECTS = OBJECT_PACKET_NUM * FRAGMENTS_PER_OBJECT_PACKET; +constexpr int rdi_near_header_packet_size = 32; +constexpr int rdi_near_element_packet_size = 64; +constexpr int rdi_hrr_header_packet_size = 32; +constexpr int rdi_hrr_element_packet_size = 64; +constexpr int object_header_packet_size = 32; +constexpr int object_packet_size = 64; +constexpr int crc_list_packet_size = 4; +constexpr int status_packet_size = 64; +constexpr int sync_follow_up_can_packet_size = 8; +constexpr int veh_dyn_can_packet_size = 8; +constexpr int configuration_packet_size = 16; + +constexpr int rdi_near_packet_num = 50; +constexpr int rdi_hrr_packet_num = 20; +constexpr int object_packet_num = 20; + +constexpr int fragments_per_detection_packet = 10; +constexpr int fragments_per_object_packet = 2; +constexpr int detection_fragment_size = 6; +constexpr int object_fragment_size = 31; + +constexpr int max_rdi_near_detections = rdi_near_packet_num * fragments_per_detection_packet; +constexpr int max_rdi_hrr_detections = rdi_hrr_packet_num * fragments_per_detection_packet; +constexpr int max_objects = object_packet_num * fragments_per_object_packet; #pragma pack(push, 1) @@ -162,12 +162,12 @@ struct ScanHeaderPacket struct DetectionFragmentPacket { - uint8_t data[DETECTION_FRAGMENT_SIZE]; + uint8_t data[detection_fragment_size]; }; struct DetectionPacket { - DetectionFragmentPacket fragments[FRAGMENTS_PER_DETECTION_PACKET]; // 0 - 59 + DetectionFragmentPacket fragments[fragments_per_detection_packet]; // 0 - 59 uint8_t reserved0; // 60 uint8_t reserved1; // 61 uint8_t u_message_counter; // 62 @@ -192,12 +192,12 @@ struct ObjectHeaderPacket struct ObjectFragmentPacket { - uint8_t data[OBJECT_FRAGMENT_SIZE]; + uint8_t data[object_fragment_size]; }; struct ObjectPacket { - ObjectFragmentPacket fragments[FRAGMENTS_PER_OBJECT_PACKET]; // 0 - 61 + ObjectFragmentPacket fragments[fragments_per_object_packet]; // 0 - 61 uint8_t u_message_counter; // 62 uint8_t u_sequence_counter; // 63 }; diff --git a/nebula_common/include/nebula_common/hesai/hesai_common.hpp b/nebula_common/include/nebula_common/hesai/hesai_common.hpp index b38038473..6f70b8ec6 100644 --- a/nebula_common/include/nebula_common/hesai/hesai_common.hpp +++ b/nebula_common/include/nebula_common/hesai/hesai_common.hpp @@ -77,12 +77,12 @@ inline std::ostream & operator<<(std::ostream & os, HesaiSensorConfiguration con struct HesaiCalibrationConfigurationBase : public CalibrationConfigurationBase { - virtual nebula::Status LoadFromBytes(const std::vector & buf) = 0; - virtual nebula::Status LoadFromFile(const std::string & calibration_file) = 0; - virtual nebula::Status SaveToFileFromBytes( + virtual nebula::Status load_from_bytes(const std::vector & buf) = 0; + virtual nebula::Status load_from_file(const std::string & calibration_file) = 0; + virtual nebula::Status save_to_file_from_bytes( const std::string & calibration_file, const std::vector & buf) = 0; - [[nodiscard]] virtual std::tuple getFovPadding() const = 0; + [[nodiscard]] virtual std::tuple get_fov_padding() const = 0; }; /// @brief struct for Hesai calibration configuration @@ -91,7 +91,7 @@ struct HesaiCalibrationConfiguration : public HesaiCalibrationConfigurationBase std::map elev_angle_map; std::map azimuth_offset_map; - inline nebula::Status LoadFromFile(const std::string & calibration_file) override + inline nebula::Status load_from_file(const std::string & calibration_file) override { std::ifstream ifs(calibration_file); if (!ifs) { @@ -100,19 +100,19 @@ struct HesaiCalibrationConfiguration : public HesaiCalibrationConfigurationBase std::ostringstream ss; ss << ifs.rdbuf(); // reading data ifs.close(); - return LoadFromString(ss.str()); + return load_from_string(ss.str()); } - nebula::Status LoadFromBytes(const std::vector & buf) override + nebula::Status load_from_bytes(const std::vector & buf) override { std::string calibration_string = std::string(buf.begin(), buf.end()); - return LoadFromString(calibration_string); + return load_from_string(calibration_string); } /// @brief Loading calibration data /// @param calibration_content /// @return Resulting status - inline nebula::Status LoadFromString(const std::string & calibration_content) + inline nebula::Status load_from_string(const std::string & calibration_content) { std::stringstream ss; ss << calibration_content; @@ -144,7 +144,7 @@ struct HesaiCalibrationConfiguration : public HesaiCalibrationConfigurationBase /// @brief Saving calibration data (not used) /// @param calibration_file /// @return Resulting status - inline nebula::Status SaveToFile(const std::string & calibration_file) + inline nebula::Status save_to_file(const std::string & calibration_file) { std::ofstream ofs(calibration_file); if (!ofs) { @@ -162,18 +162,18 @@ struct HesaiCalibrationConfiguration : public HesaiCalibrationConfigurationBase return Status::OK; } - nebula::Status SaveToFileFromBytes( + nebula::Status save_to_file_from_bytes( const std::string & calibration_file, const std::vector & buf) override { std::string calibration_string = std::string(buf.begin(), buf.end()); - return SaveFileFromString(calibration_file, calibration_string); + return save_file_from_string(calibration_file, calibration_string); } /// @brief Saving calibration data from string /// @param calibration_file path /// @param calibration_string calibration string /// @return Resulting status - inline nebula::Status SaveFileFromString( + inline nebula::Status save_file_from_string( const std::string & calibration_file, const std::string & calibration_string) { std::ofstream ofs(calibration_file); @@ -185,7 +185,7 @@ struct HesaiCalibrationConfiguration : public HesaiCalibrationConfigurationBase return Status::OK; } - [[nodiscard]] std::tuple getFovPadding() const override + [[nodiscard]] std::tuple get_fov_padding() const override { float min = INFINITY; float max = -INFINITY; @@ -222,7 +222,7 @@ struct HesaiCorrection : public HesaiCalibrationConfigurationBase /// @brief Load correction data from file /// @param buf Binary buffer /// @return Resulting status - inline nebula::Status LoadFromBytes(const std::vector & buf) override + inline nebula::Status load_from_bytes(const std::vector & buf) override { size_t index; for (index = 0; index < buf.size() - 1; index++) { @@ -323,7 +323,7 @@ struct HesaiCorrection : public HesaiCalibrationConfigurationBase /// @brief Load correction data from file /// @param correction_file path /// @return Resulting status - inline nebula::Status LoadFromFile(const std::string & correction_file) override + inline nebula::Status load_from_file(const std::string & correction_file) override { std::ifstream ifs(correction_file, std::ios::in | std::ios::binary); if (!ifs) { @@ -336,7 +336,7 @@ struct HesaiCorrection : public HesaiCalibrationConfigurationBase ifs.read(reinterpret_cast(&c), sizeof(unsigned char)); buf.emplace_back(c); } - LoadFromBytes(buf); + load_from_bytes(buf); ifs.close(); return Status::OK; @@ -346,7 +346,7 @@ struct HesaiCorrection : public HesaiCalibrationConfigurationBase /// @param correction_file path /// @param buf correction binary /// @return Resulting status - inline nebula::Status SaveToFileFromBytes( + inline nebula::Status save_to_file_from_bytes( const std::string & correction_file, const std::vector & buf) override { std::cerr << "Saving in: " << correction_file << "\n"; @@ -374,17 +374,17 @@ struct HesaiCorrection : public HesaiCalibrationConfigurationBase return Status::INVALID_CALIBRATION_FILE; } - static const int STEP3 = 200 * 256; + static const int g_step3 = 200 * 256; /// @brief Get azimuth adjustment for channel and precision azimuth /// @param ch The channel id /// @param azi The precision azimuth in (0.01 / 256) degree unit /// @return The azimuth adjustment in 0.01 degree unit - [[nodiscard]] int8_t getAzimuthAdjustV3(uint8_t ch, uint32_t azi) const + [[nodiscard]] int8_t get_azimuth_adjust_v3(uint8_t ch, uint32_t azi) const { - unsigned int i = std::floor(1.f * azi / STEP3); - unsigned int l = azi - i * STEP3; - float k = 1.f * l / STEP3; + unsigned int i = std::floor(1.f * azi / g_step3); + unsigned int l = azi - i * g_step3; + float k = 1.f * l / g_step3; return round((1 - k) * azimuthOffset[ch * 180 + i] + k * azimuthOffset[ch * 180 + i + 1]); } @@ -392,15 +392,15 @@ struct HesaiCorrection : public HesaiCalibrationConfigurationBase /// @param ch The channel id /// @param azi The precision azimuth in (0.01 / 256) degree unit /// @return The elevation adjustment in 0.01 degree unit - [[nodiscard]] int8_t getElevationAdjustV3(uint8_t ch, uint32_t azi) const + [[nodiscard]] int8_t get_elevation_adjust_v3(uint8_t ch, uint32_t azi) const { - unsigned int i = std::floor(1.f * azi / STEP3); - unsigned int l = azi - i * STEP3; - float k = 1.f * l / STEP3; + unsigned int i = std::floor(1.f * azi / g_step3); + unsigned int l = azi - i * g_step3; + float k = 1.f * l / g_step3; return round((1 - k) * elevationOffset[ch * 180 + i] + k * elevationOffset[ch * 180 + i + 1]); } - [[nodiscard]] std::tuple getFovPadding() const override + [[nodiscard]] std::tuple get_fov_padding() const override { // TODO(mojomex): calculate instead of hard-coding // The reason this is tricky is that an upper bound over all azimuth/elevation combinations has @@ -429,11 +429,11 @@ struct HesaiCorrection : public HesaiCalibrationConfigurationBase */ -/// @brief Convert return mode name to ReturnMode enum (Hesai-specific ReturnModeFromString) +/// @brief Convert return mode name to ReturnMode enum (Hesai-specific return_mode_from_string) /// @param return_mode Return mode name (Upper and lower case letters must match) /// @param sensor_model Model for correct conversion /// @return Corresponding ReturnMode -inline ReturnMode ReturnModeFromStringHesai( +inline ReturnMode return_mode_from_string_hesai( const std::string & return_mode, const SensorModel & sensor_model) { switch (sensor_model) { @@ -474,7 +474,8 @@ inline ReturnMode ReturnModeFromStringHesai( /// @param return_mode Return mode number from the hardware response /// @param sensor_model Model for correct conversion /// @return Corresponding ReturnMode -inline ReturnMode ReturnModeFromIntHesai(const int return_mode, const SensorModel & sensor_model) +inline ReturnMode return_mode_from_int_hesai( + const int return_mode, const SensorModel & sensor_model) { switch (sensor_model) { case SensorModel::HESAI_PANDARXT32: @@ -512,7 +513,8 @@ inline ReturnMode ReturnModeFromIntHesai(const int return_mode, const SensorMode /// @param return_mode target ReturnMode /// @param sensor_model Model for correct conversion /// @return Corresponding return mode number for the hardware -inline int IntFromReturnModeHesai(const ReturnMode return_mode, const SensorModel & sensor_model) +inline int int_from_return_mode_hesai( + const ReturnMode return_mode, const SensorModel & sensor_model) { switch (sensor_model) { case SensorModel::HESAI_PANDARXT32: diff --git a/nebula_common/include/nebula_common/nebula_common.hpp b/nebula_common/include/nebula_common/nebula_common.hpp index 36044a9e8..2a9de45f7 100644 --- a/nebula_common/include/nebula_common/nebula_common.hpp +++ b/nebula_common/include/nebula_common/nebula_common.hpp @@ -72,7 +72,7 @@ enum class ReturnMode : uint8_t { /// @brief Convert ReturnMode enum to ReturnType enum for Pandar AT, XTM (temporary, not used) /// @param mode /// @return Corresponding mode -inline ReturnType ReturnModeToReturnType(const ReturnMode & mode) +inline ReturnType return_mode_to_return_type(const ReturnMode & mode) { switch (mode) { case ReturnMode::SINGLE_STRONGEST: @@ -140,7 +140,7 @@ inline ReturnType ReturnModeToReturnType(const ReturnMode & mode) /// @brief Convert ReturnMode enum to integer /// @param mode /// @return Corresponding number -inline uint8_t ReturnModeToInt(const ReturnMode & mode) +inline uint8_t return_mode_to_int(const ReturnMode & mode) { switch (mode) { case ReturnMode::SINGLE_STRONGEST: @@ -554,7 +554,7 @@ struct CalibrationConfigurationBase /// @brief Convert sensor name to SensorModel enum (Upper and lower case letters must match) /// @param sensor_model Sensor name (Upper and lower case letters must match) /// @return Corresponding SensorModel -inline SensorModel SensorModelFromString(const std::string & sensor_model) +inline SensorModel sensor_model_from_string(const std::string & sensor_model) { // Hesai if (sensor_model == "Pandar64") return SensorModel::HESAI_PANDAR64; @@ -584,7 +584,7 @@ inline SensorModel SensorModelFromString(const std::string & sensor_model) return SensorModel::UNKNOWN; } -inline std::string SensorModelToString(const SensorModel & sensor_model) +inline std::string sensor_model_to_string(const SensorModel & sensor_model) { switch (sensor_model) { // Hesai @@ -639,7 +639,7 @@ inline std::string SensorModelToString(const SensorModel & sensor_model) /// @brief Convert return mode name to ReturnMode enum /// @param return_mode Return mode name (Upper and lower case letters must match) /// @return Corresponding ReturnMode -inline ReturnMode ReturnModeFromString(const std::string & return_mode) +inline ReturnMode return_mode_from_string(const std::string & return_mode) { if (return_mode == "SingleFirst") return ReturnMode::SINGLE_FIRST; if (return_mode == "SingleStrongest") return ReturnMode::SINGLE_STRONGEST; @@ -652,7 +652,7 @@ inline ReturnMode ReturnModeFromString(const std::string & return_mode) /// @brief Converts String to PTP Profile /// @param ptp_profile Profile as String /// @return Corresponding PtpProfile -inline PtpProfile PtpProfileFromString(const std::string & ptp_profile) +inline PtpProfile ptp_profile_from_string(const std::string & ptp_profile) { // Hesai auto tmp_str = ptp_profile; @@ -692,7 +692,7 @@ inline std::ostream & operator<<(std::ostream & os, nebula::drivers::PtpProfile /// @brief Converts String to PTP TransportType /// @param transport_type Transport as String /// @return Corresponding PtpTransportType -inline PtpTransportType PtpTransportTypeFromString(const std::string & transport_type) +inline PtpTransportType ptp_transport_type_from_string(const std::string & transport_type) { // Hesai auto tmp_str = transport_type; @@ -728,7 +728,7 @@ inline std::ostream & operator<<(std::ostream & os, nebula::drivers::PtpTranspor /// @brief Converts String to PTP SwitchType /// @param switch_type Switch as String /// @return Corresponding PtpSwitchType -inline PtpSwitchType PtpSwitchTypeFromString(const std::string & switch_type) +inline PtpSwitchType ptp_switch_type_from_string(const std::string & switch_type) { // Hesai auto tmp_str = switch_type; @@ -761,13 +761,13 @@ inline std::ostream & operator<<(std::ostream & os, nebula::drivers::PtpSwitchTy return os; } -[[maybe_unused]] pcl::PointCloud::Ptr convertPointXYZIRADTToPointXYZIR( +[[maybe_unused]] pcl::PointCloud::Ptr convert_point_xyziradt_to_point_xyzir( const pcl::PointCloud::ConstPtr & input_pointcloud); -[[maybe_unused]] pcl::PointCloud::Ptr convertPointXYZIRCAEDTToPointXYZIR( +[[maybe_unused]] pcl::PointCloud::Ptr convert_point_xyzircaedt_to_point_xyzir( const pcl::PointCloud::ConstPtr & input_pointcloud); -pcl::PointCloud::Ptr convertPointXYZIRCAEDTToPointXYZIRADT( +pcl::PointCloud::Ptr convert_point_xyzircaedt_to_point_xyziradt( const pcl::PointCloud::ConstPtr & input_pointcloud, double stamp); /// @brief Converts degrees to radians diff --git a/nebula_common/include/nebula_common/robosense/robosense_common.hpp b/nebula_common/include/nebula_common/robosense/robosense_common.hpp index d7bb496d4..9c58001ac 100644 --- a/nebula_common/include/nebula_common/robosense/robosense_common.hpp +++ b/nebula_common/include/nebula_common/robosense/robosense_common.hpp @@ -30,7 +30,7 @@ namespace drivers { // Flag for detecting Bpearl version -constexpr uint8_t BPEARL_V4_FLAG = 0x04; +constexpr uint8_t bpearl_v4_flag = 0x04; /// @brief struct for Robosense sensor configuration struct RobosenseSensorConfiguration : LidarConfigurationBase @@ -53,10 +53,10 @@ inline std::ostream & operator<<(std::ostream & os, RobosenseSensorConfiguration return os; } -/// @brief Convert return mode name to ReturnMode enum (Robosense-specific ReturnModeFromString) +/// @brief Convert return mode name to ReturnMode enum (Robosense-specific return_mode_from_string) /// @param return_mode Return mode name (Upper and lower case letters must match) /// @return Corresponding ReturnMode -inline ReturnMode ReturnModeFromStringRobosense(const std::string & return_mode) +inline ReturnMode return_mode_from_string_robosense(const std::string & return_mode) { if (return_mode == "Dual") return ReturnMode::DUAL; if (return_mode == "Strongest") return ReturnMode::SINGLE_STRONGEST; @@ -80,10 +80,10 @@ struct RobosenseCalibrationConfiguration : CalibrationConfigurationBase { std::vector calibration; - void SetChannelSize(const size_t channel_num) { calibration.resize(channel_num); } + void set_channel_size(const size_t channel_num) { calibration.resize(channel_num); } template - inline nebula::Status LoadFromStream(stream_t & stream) + inline nebula::Status load_from_stream(stream_t & stream) { std::string header; std::getline(stream, header); @@ -134,14 +134,14 @@ struct RobosenseCalibrationConfiguration : CalibrationConfigurationBase return load_status; } - inline nebula::Status LoadFromFile(const std::string & calibration_file) + inline nebula::Status load_from_file(const std::string & calibration_file) { std::ifstream ifs(calibration_file); if (!ifs) { return Status::INVALID_CALIBRATION_FILE; } - const auto status = LoadFromStream(ifs); + const auto status = load_from_stream(ifs); ifs.close(); return status; } @@ -149,21 +149,21 @@ struct RobosenseCalibrationConfiguration : CalibrationConfigurationBase /// @brief Loading calibration data (not used) /// @param calibration_content /// @return Resulting status - inline nebula::Status LoadFromString(const std::string & calibration_content) + inline nebula::Status load_from_string(const std::string & calibration_content) { std::stringstream ss; ss << calibration_content; - const auto status = LoadFromStream(ss); + const auto status = load_from_stream(ss); return status; } - // inline nebula::Status LoadFromDifop(const std::string & calibration_file) + // inline nebula::Status load_from_difop(const std::string & calibration_file) /// @brief Saving calibration data (not used) /// @param calibration_file /// @return Resulting status - inline nebula::Status SaveFile(const std::string & calibration_file) + inline nebula::Status save_file(const std::string & calibration_file) { std::ofstream ofs(calibration_file); if (!ofs) { @@ -182,12 +182,12 @@ struct RobosenseCalibrationConfiguration : CalibrationConfigurationBase return Status::OK; } - [[nodiscard]] inline ChannelCorrection GetCorrection(const size_t channel_id) const + [[nodiscard]] inline ChannelCorrection get_correction(const size_t channel_id) const { return calibration[channel_id]; } - void CreateCorrectedChannels() + void create_corrected_channels() { for (auto & correction : calibration) { uint16_t channel = 0; diff --git a/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp b/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp index 2c68eedb8..a6977f659 100644 --- a/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp +++ b/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp @@ -53,7 +53,7 @@ inline std::ostream & operator<<(std::ostream & os, VelodyneSensorConfiguration struct VelodyneCalibrationConfiguration : CalibrationConfigurationBase { VelodyneCalibration velodyne_calibration; - inline nebula::Status LoadFromFile(const std::string & calibration_file) + inline nebula::Status load_from_file(const std::string & calibration_file) { velodyne_calibration.read(calibration_file); if (!velodyne_calibration.initialized) { @@ -62,17 +62,17 @@ struct VelodyneCalibrationConfiguration : CalibrationConfigurationBase return Status::OK; } } - inline nebula::Status SaveFile(const std::string & calibration_file) + inline nebula::Status save_file(const std::string & calibration_file) { velodyne_calibration.write(calibration_file); return Status::OK; } }; -/// @brief Convert return mode name to ReturnMode enum (Velodyne-specific ReturnModeFromString) +/// @brief Convert return mode name to ReturnMode enum (Velodyne-specific return_mode_from_string) /// @param return_mode Return mode name (Upper and lower case letters must match) /// @return Corresponding ReturnMode -inline ReturnMode ReturnModeFromStringVelodyne(const std::string & return_mode) +inline ReturnMode return_mode_from_string_velodyne(const std::string & return_mode) { if (return_mode == "Strongest") return ReturnMode::SINGLE_STRONGEST; if (return_mode == "Last") return ReturnMode::SINGLE_LAST; diff --git a/nebula_common/src/nebula_common.cpp b/nebula_common/src/nebula_common.cpp index c87331421..8bd56fed2 100644 --- a/nebula_common/src/nebula_common.cpp +++ b/nebula_common/src/nebula_common.cpp @@ -6,7 +6,7 @@ namespace nebula { namespace drivers { -[[maybe_unused]] pcl::PointCloud::Ptr convertPointXYZIRADTToPointXYZIR( +[[maybe_unused]] pcl::PointCloud::Ptr convert_point_xyziradt_to_point_xyzir( const pcl::PointCloud::ConstPtr & input_pointcloud) { pcl::PointCloud::Ptr output_pointcloud(new pcl::PointCloud); @@ -27,7 +27,7 @@ namespace drivers return output_pointcloud; } -pcl::PointCloud::Ptr convertPointXYZIRCAEDTToPointXYZIR( +pcl::PointCloud::Ptr convert_point_xyzircaedt_to_point_xyzir( const pcl::PointCloud::ConstPtr & input_pointcloud) { pcl::PointCloud::Ptr output_pointcloud(new pcl::PointCloud); @@ -48,7 +48,7 @@ pcl::PointCloud::Ptr convertPointXYZIRCAEDTToPointXYZIR( return output_pointcloud; } -pcl::PointCloud::Ptr convertPointXYZIRCAEDTToPointXYZIRADT( +pcl::PointCloud::Ptr convert_point_xyzircaedt_to_point_xyziradt( const pcl::PointCloud::ConstPtr & input_pointcloud, const double stamp) { pcl::PointCloud::Ptr output_pointcloud(new pcl::PointCloud); diff --git a/nebula_common/src/velodyne/velodyne_calibration_decoder.cpp b/nebula_common/src/velodyne/velodyne_calibration_decoder.cpp index d4d69e018..2de7c71ab 100644 --- a/nebula_common/src/velodyne/velodyne_calibration_decoder.cpp +++ b/nebula_common/src/velodyne/velodyne_calibration_decoder.cpp @@ -28,45 +28,45 @@ namespace nebula { namespace drivers { -const char NUM_LASERS[] = "num_lasers"; -const char DISTANCE_RESOLUTION[] = "distance_resolution"; -const char LASERS[] = "lasers"; -const char LASER_ID[] = "laser_id"; -const char ROT_CORRECTION[] = "rot_correction"; -const char VERT_CORRECTION[] = "vert_correction"; -const char DIST_CORRECTION[] = "dist_correction"; -const char TWO_PT_CORRECTION_AVAILABLE[] = "two_pt_correction_available"; -const char DIST_CORRECTION_X[] = "dist_correction_x"; -const char DIST_CORRECTION_Y[] = "dist_correction_y"; -const char VERT_OFFSET_CORRECTION[] = "vert_offset_correction"; -const char HORIZ_OFFSET_CORRECTION[] = "horiz_offset_correction"; -const char MAX_INTENSITY[] = "max_intensity"; -const char MIN_INTENSITY[] = "min_intensity"; -const char FOCAL_DISTANCE[] = "focal_distance"; -const char FOCAL_SLOPE[] = "focal_slope"; +const char g_num_lasers[] = "num_lasers"; +const char g_distance_resolution[] = "distance_resolution"; +const char g_lasers[] = "lasers"; +const char g_laser_id[] = "laser_id"; +const char g_rot_correction[] = "rot_correction"; +const char g_vert_correction[] = "vert_correction"; +const char g_dist_correction[] = "dist_correction"; +const char g_two_pt_correction_available[] = "two_pt_correction_available"; +const char g_dist_correction_x[] = "dist_correction_x"; +const char g_dist_correction_y[] = "dist_correction_y"; +const char g_vert_offset_correction[] = "vert_offset_correction"; +const char g_horiz_offset_correction[] = "horiz_offset_correction"; +const char g_max_intensity[] = "max_intensity"; +const char g_min_intensity[] = "min_intensity"; +const char g_focal_distance[] = "focal_distance"; +const char g_focal_slope[] = "focal_slope"; /** Read calibration for a single laser. */ void operator>>(const YAML::Node & node, std::pair & correction) { - node[LASER_ID] >> correction.first; - node[ROT_CORRECTION] >> correction.second.rot_correction; - node[VERT_CORRECTION] >> correction.second.vert_correction; - node[DIST_CORRECTION] >> correction.second.dist_correction; + node[g_laser_id] >> correction.first; + node[g_rot_correction] >> correction.second.rot_correction; + node[g_vert_correction] >> correction.second.vert_correction; + node[g_dist_correction] >> correction.second.dist_correction; #ifdef HAVE_NEW_YAMLCPP - if (node[TWO_PT_CORRECTION_AVAILABLE]) - node[TWO_PT_CORRECTION_AVAILABLE] >> correction.second.two_pt_correction_available; + if (node[g_two_pt_correction_available]) + node[g_two_pt_correction_available] >> correction.second.two_pt_correction_available; #else if (const YAML::Node * pName = node.FindValue(TWO_PT_CORRECTION_AVAILABLE)) *pName >> correction.second.two_pt_correction_available; #endif else correction.second.two_pt_correction_available = false; - node[DIST_CORRECTION_X] >> correction.second.dist_correction_x; - node[DIST_CORRECTION_Y] >> correction.second.dist_correction_y; - node[VERT_OFFSET_CORRECTION] >> correction.second.vert_offset_correction; + node[g_dist_correction_x] >> correction.second.dist_correction_x; + node[g_dist_correction_y] >> correction.second.dist_correction_y; + node[g_vert_offset_correction] >> correction.second.vert_offset_correction; #ifdef HAVE_NEW_YAMLCPP - if (node[HORIZ_OFFSET_CORRECTION]) - node[HORIZ_OFFSET_CORRECTION] >> correction.second.horiz_offset_correction; + if (node[g_horiz_offset_correction]) + node[g_horiz_offset_correction] >> correction.second.horiz_offset_correction; #else if (const YAML::Node * pName = node.FindValue(HORIZ_OFFSET_CORRECTION)) *pName >> correction.second.horiz_offset_correction; @@ -76,8 +76,8 @@ void operator>>(const YAML::Node & node, std::pair const YAML::Node * max_intensity_node = NULL; #ifdef HAVE_NEW_YAMLCPP - if (node[MAX_INTENSITY]) { - const YAML::Node max_intensity_node_ref = node[MAX_INTENSITY]; + if (node[g_max_intensity]) { + const YAML::Node max_intensity_node_ref = node[g_max_intensity]; max_intensity_node = &max_intensity_node_ref; } #else @@ -93,8 +93,8 @@ void operator>>(const YAML::Node & node, std::pair const YAML::Node * min_intensity_node = NULL; #ifdef HAVE_NEW_YAMLCPP - if (node[MIN_INTENSITY]) { - const YAML::Node min_intensity_node_ref = node[MIN_INTENSITY]; + if (node[g_min_intensity]) { + const YAML::Node min_intensity_node_ref = node[g_min_intensity]; min_intensity_node = &min_intensity_node_ref; } #else @@ -107,8 +107,8 @@ void operator>>(const YAML::Node & node, std::pair } else { correction.second.min_intensity = 0; } - node[FOCAL_DISTANCE] >> correction.second.focal_distance; - node[FOCAL_SLOPE] >> correction.second.focal_slope; + node[g_focal_distance] >> correction.second.focal_distance; + node[g_focal_slope] >> correction.second.focal_slope; // Calculate cached values correction.second.cos_rot_correction = cosf(correction.second.rot_correction); @@ -123,10 +123,10 @@ void operator>>(const YAML::Node & node, std::pair void operator>>(const YAML::Node & node, VelodyneCalibration & calibration) { int num_lasers; - node[NUM_LASERS] >> num_lasers; + node[g_num_lasers] >> num_lasers; float distance_resolution_m; - node[DISTANCE_RESOLUTION] >> distance_resolution_m; - const YAML::Node & lasers = node[LASERS]; + node[g_distance_resolution] >> distance_resolution_m; + const YAML::Node & lasers = node[g_lasers]; calibration.laser_corrections.clear(); calibration.num_lasers = num_lasers; calibration.distance_resolution_m = distance_resolution_m; @@ -172,22 +172,22 @@ YAML::Emitter & operator<<( YAML::Emitter & out, const std::pair correction) { out << YAML::BeginMap; - out << YAML::Key << LASER_ID << YAML::Value << correction.first; - out << YAML::Key << ROT_CORRECTION << YAML::Value << correction.second.rot_correction; - out << YAML::Key << VERT_CORRECTION << YAML::Value << correction.second.vert_correction; - out << YAML::Key << DIST_CORRECTION << YAML::Value << correction.second.dist_correction; - out << YAML::Key << TWO_PT_CORRECTION_AVAILABLE << YAML::Value + out << YAML::Key << g_laser_id << YAML::Value << correction.first; + out << YAML::Key << g_rot_correction << YAML::Value << correction.second.rot_correction; + out << YAML::Key << g_vert_correction << YAML::Value << correction.second.vert_correction; + out << YAML::Key << g_dist_correction << YAML::Value << correction.second.dist_correction; + out << YAML::Key << g_two_pt_correction_available << YAML::Value << correction.second.two_pt_correction_available; - out << YAML::Key << DIST_CORRECTION_X << YAML::Value << correction.second.dist_correction_x; - out << YAML::Key << DIST_CORRECTION_Y << YAML::Value << correction.second.dist_correction_y; - out << YAML::Key << VERT_OFFSET_CORRECTION << YAML::Value + out << YAML::Key << g_dist_correction_x << YAML::Value << correction.second.dist_correction_x; + out << YAML::Key << g_dist_correction_y << YAML::Value << correction.second.dist_correction_y; + out << YAML::Key << g_vert_offset_correction << YAML::Value << correction.second.vert_offset_correction; - out << YAML::Key << HORIZ_OFFSET_CORRECTION << YAML::Value + out << YAML::Key << g_horiz_offset_correction << YAML::Value << correction.second.horiz_offset_correction; - out << YAML::Key << MAX_INTENSITY << YAML::Value << correction.second.max_intensity; - out << YAML::Key << MIN_INTENSITY << YAML::Value << correction.second.min_intensity; - out << YAML::Key << FOCAL_DISTANCE << YAML::Value << correction.second.focal_distance; - out << YAML::Key << FOCAL_SLOPE << YAML::Value << correction.second.focal_slope; + out << YAML::Key << g_max_intensity << YAML::Value << correction.second.max_intensity; + out << YAML::Key << g_min_intensity << YAML::Value << correction.second.min_intensity; + out << YAML::Key << g_focal_distance << YAML::Value << correction.second.focal_distance; + out << YAML::Key << g_focal_slope << YAML::Value << correction.second.focal_slope; out << YAML::EndMap; return out; } @@ -195,9 +195,9 @@ YAML::Emitter & operator<<( YAML::Emitter & operator<<(YAML::Emitter & out, const VelodyneCalibration & calibration) { out << YAML::BeginMap; - out << YAML::Key << NUM_LASERS << YAML::Value << calibration.laser_corrections.size(); - out << YAML::Key << DISTANCE_RESOLUTION << YAML::Value << calibration.distance_resolution_m; - out << YAML::Key << LASERS << YAML::Value << YAML::BeginSeq; + out << YAML::Key << g_num_lasers << YAML::Value << calibration.laser_corrections.size(); + out << YAML::Key << g_distance_resolution << YAML::Value << calibration.distance_resolution_m; + out << YAML::Key << g_lasers << YAML::Value << YAML::BeginSeq; for (std::map::const_iterator it = calibration.laser_corrections_map.begin(); it != calibration.laser_corrections_map.end(); it++) { diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_common/nebula_driver_base.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/nebula_driver_base.hpp index 71aed2985..fbfe30674 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_common/nebula_driver_base.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_common/nebula_driver_base.hpp @@ -41,7 +41,7 @@ class NebulaDriverBase /// @brief Virtual function for setting calibration configuration /// @param calibration_configuration CalibrationConfiguration including file path /// @return Resulting status - virtual Status SetCalibrationConfiguration( + virtual Status set_calibration_configuration( const CalibrationConfigurationBase & calibration_configuration) = 0; }; diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp index b017e6efd..e80c186ae 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_ars548_decoder.hpp @@ -45,54 +45,54 @@ class ContinentalARS548Decoder : public ContinentalPacketsDecoder /// @brief Get current status of this driver /// @return Current status - Status GetStatus() override; + Status get_status() override; /// @brief Function for parsing NebulaPackets /// @param nebula_packets /// @return Resulting flag - bool ProcessPacket(std::unique_ptr packet_msg) override; + bool process_packet(std::unique_ptr packet_msg) override; /// @brief Register function to call when a new detection list is processed /// @param detection_list_callback /// @return Resulting status - Status RegisterDetectionListCallback( + Status register_detection_list_callback( std::function)> detection_list_callback); /// @brief Register function to call when a new object list is processed /// @param object_list_callback /// @return Resulting status - Status RegisterObjectListCallback( + Status register_object_list_callback( std::function)> object_list_callback); /// @brief Register function to call when a new sensor status message is processed /// @param object_list_callback /// @return Resulting status - Status RegisterSensorStatusCallback( + Status register_sensor_status_callback( std::function sensor_status_callback); /// @brief Register function to call when a new sensor status message is processed /// @param object_list_callback /// @return Resulting status - Status RegisterPacketsCallback( + Status register_packets_callback( std::function)> packets_callback); private: /// @brief Function for parsing detection lists /// @param data /// @return Resulting flag - bool ParseDetectionsListPacket(const nebula_msgs::msg::NebulaPacket & packet_msg); + bool parse_detections_list_packet(const nebula_msgs::msg::NebulaPacket & packet_msg); /// @brief Function for parsing object lists /// @param data /// @return Resulting flag - bool ParseObjectsListPacket(const nebula_msgs::msg::NebulaPacket & packet_msg); + bool parse_objects_list_packet(const nebula_msgs::msg::NebulaPacket & packet_msg); /// @brief Function for parsing sensor status messages /// @param data /// @return Resulting flag - bool ParseSensorStatusPacket(const nebula_msgs::msg::NebulaPacket & packet_msg); + bool parse_sensor_status_packet(const nebula_msgs::msg::NebulaPacket & packet_msg); std::function msg)> detection_list_callback_{}; diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_packets_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_packets_decoder.hpp index 933490542..1914e66a4 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_packets_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_packets_decoder.hpp @@ -41,12 +41,12 @@ class ContinentalPacketsDecoder /// @brief Get current status of this driver /// @return Current status - virtual Status GetStatus() = 0; + virtual Status get_status() = 0; /// @brief Virtual function for parsing a NebulaPacket /// @param packet_msg /// @return Resulting flag - virtual bool ProcessPacket(std::unique_ptr packet_msg) = 0; + virtual bool process_packet(std::unique_ptr packet_msg) = 0; }; } // namespace drivers } // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_srr520_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_srr520_decoder.hpp index c70cb4706..70bfb04b7 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_srr520_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_continental/decoders/continental_srr520_decoder.hpp @@ -47,129 +47,129 @@ class ContinentalSRR520Decoder : public ContinentalPacketsDecoder /// @brief Get current status of this driver /// @return Current status - Status GetStatus() override; + Status get_status() override; /// @brief Function for parsing NebulaPackets /// @param nebula_packets /// @return Resulting flag - bool ProcessPacket(std::unique_ptr packet_msg) override; + bool process_packet(std::unique_ptr packet_msg) override; /// @brief Register function to call whenever a new RDI near detection list is processed /// @param detection_list_callback /// @return Resulting status - Status RegisterNearDetectionListCallback( + Status register_near_detection_list_callback( std::function)> detection_list_callback); /// @brief Register function to call whenever a new RDI HRR detection list is processed /// @param detection_list_callback /// @return Resulting status - Status RegisterHRRDetectionListCallback( + Status register_hrr_detection_list_callback( std::function)> detection_list_callback); /// @brief Register function to call whenever a new object list is processed /// @param object_list_callback /// @return Resulting status - Status RegisterObjectListCallback( + Status register_object_list_callback( std::function)> object_list_callback); /// @brief Register function to call whenever a new object list is processed /// @param object_list_callback /// @return Resulting status - Status RegisterStatusCallback( + Status register_status_callback( std::function)> status_callback); /// @brief Register function to call whenever a sync follow-up packet is processed /// @param sync_follow_up_callback /// @return Resulting status - Status RegisterSyncFollowUpCallback( + Status register_sync_follow_up_callback( std::function sync_follow_up_callback); /// @brief Register function to call whenever enough packets have been processed /// @param object_list_callback /// @return Resulting status - Status RegisterPacketsCallback( + Status register_packets_callback( std::function)> nebula_packets_callback); /// @brief Setting rclcpp::Logger /// @param node Logger - void SetLogger(std::shared_ptr node); + void set_logger(std::shared_ptr node); private: /// @brief Process a new near detection header packet /// @param buffer The buffer containing the packet /// @param stamp The stamp in nanoseconds - void ProcessNearHeaderPacket(std::unique_ptr packet_msg); + void process_near_header_packet(std::unique_ptr packet_msg); /// @brief Process a new near element packet /// @param buffer The buffer containing the packet /// @param stamp The stamp in nanoseconds - void ProcessNearElementPacket(std::unique_ptr packet_msg); + void process_near_element_packet(std::unique_ptr packet_msg); /// @brief Process a new hrr header packet /// @param buffer The buffer containing the packet /// @param stamp The stamp in nanoseconds - void ProcessHRRHeaderPacket(std::unique_ptr packet_msg); + void process_hrr_header_packet(std::unique_ptr packet_msg); /// @brief Process a new hrr element packet /// @param buffer The buffer containing the packet /// @param stamp The stamp in nanoseconds - void ProcessHRRElementPacket(std::unique_ptr packet_msg); + void process_hrr_element_packet(std::unique_ptr packet_msg); /// @brief Process a new object header packet /// @param buffer The buffer containing the packet /// @param stamp The stamp in nanoseconds - void ProcessObjectHeaderPacket(std::unique_ptr packet_msg); + void process_object_header_packet(std::unique_ptr packet_msg); /// @brief Process a new object element packet /// @param buffer The buffer containing the packet /// @param stamp The stamp in nanoseconds - void ProcessObjectElementPacket(std::unique_ptr packet_msg); + void process_object_element_packet(std::unique_ptr packet_msg); /// @brief Process a new crc list packet /// @param buffer The buffer containing the packet /// @param stamp The stamp in nanoseconds - void ProcessCRCListPacket(std::unique_ptr packet_msg); + void process_crc_list_packet(std::unique_ptr packet_msg); /// @brief Process a new Near detections crc list packet /// @param buffer The buffer containing the packet /// @param stamp The stamp in nanoseconds - void ProcessNearCRCListPacket(std::unique_ptr packet_msg); + void process_near_crc_list_packet(std::unique_ptr packet_msg); /// @brief Process a new HRR crc list packet /// @param buffer The buffer containing the packet /// @param stamp The stamp in nanoseconds - void ProcessHRRCRCListPacket( + void process_hrrcrc_list_packet( std::unique_ptr packet_msg); // cspell:ignore HRRCRC /// @brief Process a new objects crc list packet /// @param buffer The buffer containing the packet /// @param stamp The stamp in nanoseconds - void ProcessObjectCRCListPacket(std::unique_ptr packet_msg); + void process_object_crc_list_packet(std::unique_ptr packet_msg); /// @brief Process a new sensor status packet /// @param buffer The buffer containing the status packet /// @param stamp The stamp in nanoseconds - void ProcessSensorStatusPacket(std::unique_ptr packet_msg); + void process_sensor_status_packet(std::unique_ptr packet_msg); /// @brief Process a new sensor status packet /// @param buffer The buffer containing the status packet /// @param stamp The stamp in nanoseconds - void ProcessSyncFollowUpPacket(std::unique_ptr packet_msg); + void process_sync_follow_up_packet(std::unique_ptr packet_msg); /// @brief Printing the string to RCLCPP_INFO_STREAM /// @param info Target string - void PrintInfo(std::string info); + void print_info(std::string info); /// @brief Printing the string to RCLCPP_ERROR_STREAM /// @param error Target string - void PrintError(std::string error); + void print_error(std::string error); /// @brief Printing the string to RCLCPP_DEBUG_STREAM /// @param debug Target string - void PrintDebug(std::string debug); + void print_debug(std::string debug); std::function msg)> near_detection_list_callback_{}; diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp index bc275b347..fb23460d0 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector.hpp @@ -45,12 +45,13 @@ class AngleCorrector /// angle unit /// @param channel_id The laser channel's id /// @return The corrected angles (azimuth, elevation) in radians and their sin/cos values - virtual CorrectedAngleData getCorrectedAngleData(uint32_t block_azimuth, uint32_t channel_id) = 0; + virtual CorrectedAngleData get_corrected_angle_data( + uint32_t block_azimuth, uint32_t channel_id) = 0; - virtual bool passedEmitAngle(uint32_t last_azimuth, uint32_t current_azimuth) = 0; - virtual bool passedTimestampResetAngle(uint32_t last_azimuth, uint32_t current_azimuth) = 0; - virtual bool isInsideFoV(uint32_t last_azimuth, uint32_t current_azimuth) = 0; - virtual bool isInsideOverlap(uint32_t last_azimuth, uint32_t current_azimuth) = 0; + virtual bool passed_emit_angle(uint32_t last_azimuth, uint32_t current_azimuth) = 0; + virtual bool passed_timestamp_reset_angle(uint32_t last_azimuth, uint32_t current_azimuth) = 0; + virtual bool is_inside_fov(uint32_t last_azimuth, uint32_t current_azimuth) = 0; + virtual bool is_inside_overlap(uint32_t last_azimuth, uint32_t current_azimuth) = 0; }; } // namespace nebula::drivers diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_calibration_based.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_calibration_based.hpp index b10c664ef..981257b8e 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_calibration_based.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/angle_corrector_calibration_based.hpp @@ -35,16 +35,16 @@ template class AngleCorrectorCalibrationBased : public AngleCorrector { private: - static constexpr size_t MAX_AZIMUTH = 360 * AngleUnit; + static constexpr size_t max_azimuth = 360 * AngleUnit; std::array elevation_angle_rad_{}; std::array azimuth_offset_rad_{}; - std::array block_azimuth_rad_{}; + std::array block_azimuth_rad_{}; std::array elevation_cos_{}; std::array elevation_sin_{}; - std::array, MAX_AZIMUTH> azimuth_cos_{}; - std::array, MAX_AZIMUTH> azimuth_sin_{}; + std::array, max_azimuth> azimuth_cos_{}; + std::array, max_azimuth> azimuth_sin_{}; public: uint32_t emit_angle_raw_; @@ -95,15 +95,15 @@ class AngleCorrectorCalibrationBased : public AngleCorrector(emit_angle_raw, MAX_AZIMUTH); + emit_angle_raw_ = normalize_angle(emit_angle_raw, max_azimuth); int32_t fov_start_raw = std::floor(fov_start_azimuth_deg * AngleUnit); fov_start_raw -= correction_max; - fov_start_raw_ = normalize_angle(fov_start_raw, MAX_AZIMUTH); + fov_start_raw_ = normalize_angle(fov_start_raw, max_azimuth); int32_t fov_end_raw = std::ceil(fov_end_azimuth_deg * AngleUnit); fov_end_raw -= correction_min; - fov_end_raw_ = normalize_angle(fov_end_raw, MAX_AZIMUTH); + fov_end_raw_ = normalize_angle(fov_end_raw, max_azimuth); // Reset timestamp on FoV start if FoV < 360 deg and scan is cut at FoV end. // Otherwise, reset timestamp on publish @@ -115,7 +115,7 @@ class AngleCorrectorCalibrationBased : public AngleCorrector(timestamp_reset_angle_raw, MAX_AZIMUTH); + timestamp_reset_angle_raw_ = normalize_angle(timestamp_reset_angle_raw, max_azimuth); } else { timestamp_reset_angle_raw_ = fov_start_raw_; } @@ -124,7 +124,7 @@ class AngleCorrectorCalibrationBased : public AngleCorrector(AngleUnit)); for (size_t channel_id = 0; channel_id < ChannelN; ++channel_id) { @@ -137,7 +137,7 @@ class AngleCorrectorCalibrationBased : public AngleCorrector class AngleCorrectorCorrectionBased : public AngleCorrector { private: - static constexpr size_t MAX_AZIMUTH = 360 * AngleUnit; + static constexpr size_t max_azimuth = 360 * AngleUnit; const std::shared_ptr correction_; rclcpp::Logger logger_; - std::array cos_{}; - std::array sin_{}; + std::array cos_{}; + std::array sin_{}; struct FrameAngleInfo { @@ -62,7 +62,7 @@ class AngleCorrectorCorrectionBased : public AngleCorrector /// @brief For a given azimuth value, find its corresponding output field /// @param azimuth The azimuth to get the field for /// @return The correct output field, as specified in @ref HesaiCorrection - int findField(uint32_t azimuth) + int find_field(uint32_t azimuth) { // Assumes that: // * none of the startFrames are defined as > 360 deg (< 0 not possible since they are unsigned) @@ -83,7 +83,7 @@ class AngleCorrectorCorrectionBased : public AngleCorrector bool are_corrected_angles_above_threshold(uint32_t azi, double threshold, bool any, bool eq_ok) { for (size_t channel_id = 0; channel_id < ChannelN; ++channel_id) { - auto azi_corr = getCorrectedAngleData(azi, channel_id).azimuth_rad; + auto azi_corr = get_corrected_angle_data(azi, channel_id).azimuth_rad; if (!any && (azi_corr < threshold || (!eq_ok && azi_corr == threshold))) return false; if (any && (azi_corr > threshold || (eq_ok && azi_corr == threshold))) return true; } @@ -100,7 +100,7 @@ class AngleCorrectorCorrectionBased : public AngleCorrector if (end - start <= 1) { bool result_start = are_corrected_angles_above_threshold( - normalize_angle(start, MAX_AZIMUTH), threshold, any, eq_ok); + normalize_angle(start, max_azimuth), threshold, any, eq_ok); if (result_start) return start; return end; } @@ -108,7 +108,7 @@ class AngleCorrectorCorrectionBased : public AngleCorrector uint32_t next = (start + end) / 2; bool result_next = are_corrected_angles_above_threshold( - normalize_angle(next, MAX_AZIMUTH), threshold, any, eq_ok); + normalize_angle(next, max_azimuth), threshold, any, eq_ok); if (result_next) return bin_search(start, next, threshold, any, eq_ok); return bin_search(next + 1, end, threshold, any, eq_ok); } @@ -130,8 +130,8 @@ class AngleCorrectorCorrectionBased : public AngleCorrector // Trigonometry lookup tables // //////////////////////////////////////// - for (size_t i = 0; i < MAX_AZIMUTH; ++i) { - float rad = 2.f * i * M_PIf / MAX_AZIMUTH; + for (size_t i = 0; i < max_azimuth; ++i) { + float rad = 2.f * i * M_PIf / max_azimuth; cos_[i] = cosf(rad); sin_[i] = sinf(rad); } @@ -148,7 +148,7 @@ class AngleCorrectorCorrectionBased : public AngleCorrector for (size_t field_id = 0; field_id < correction_->frameNumber; ++field_id) { auto frame_start = correction_->startFrame[field_id]; auto frame_end = correction_->endFrame[field_id]; - if (frame_end < frame_start) frame_end += MAX_AZIMUTH; + if (frame_end < frame_start) frame_end += max_azimuth; FrameAngleInfo & angle_info = frame_angle_info_.emplace_back(); @@ -177,33 +177,33 @@ class AngleCorrectorCorrectionBased : public AngleCorrector } } - CorrectedAngleData getCorrectedAngleData(uint32_t block_azimuth, uint32_t channel_id) override + CorrectedAngleData get_corrected_angle_data(uint32_t block_azimuth, uint32_t channel_id) override { - int field = findField(block_azimuth); + int field = find_field(block_azimuth); int32_t elevation = correction_->elevation[channel_id] + - correction_->getElevationAdjustV3(channel_id, block_azimuth) * + correction_->get_elevation_adjust_v3(channel_id, block_azimuth) * static_cast(AngleUnit / 100); // Allow negative angles in the radian value. This makes visualization of this field nicer and // should have no other mathematical implications in downstream modules. - float elevation_rad = 2.f * elevation * M_PI / MAX_AZIMUTH; + float elevation_rad = 2.f * elevation * M_PI / max_azimuth; // Then, normalize the integer value to the positive [0, MAX_AZIMUTH] range for array indexing - elevation = (MAX_AZIMUTH + elevation) % MAX_AZIMUTH; + elevation = (max_azimuth + elevation) % max_azimuth; - int32_t azimuth = (block_azimuth + MAX_AZIMUTH - correction_->startFrame[field]) * 2 - + int32_t azimuth = (block_azimuth + max_azimuth - correction_->startFrame[field]) * 2 - correction_->azimuth[channel_id] + - correction_->getAzimuthAdjustV3(channel_id, block_azimuth) * + correction_->get_azimuth_adjust_v3(channel_id, block_azimuth) * static_cast(AngleUnit / 100); - azimuth = (MAX_AZIMUTH + azimuth) % MAX_AZIMUTH; + azimuth = (max_azimuth + azimuth) % max_azimuth; - float azimuth_rad = 2.f * azimuth * M_PI / MAX_AZIMUTH; + float azimuth_rad = 2.f * azimuth * M_PI / max_azimuth; return {azimuth_rad, elevation_rad, sin_[azimuth], cos_[azimuth], sin_[elevation], cos_[elevation]}; } - bool passedEmitAngle(uint32_t last_azimuth, uint32_t current_azimuth) override + bool passed_emit_angle(uint32_t last_azimuth, uint32_t current_azimuth) override { for (const auto & frame_angles : frame_angle_info_) { if (angle_is_between(last_azimuth, current_azimuth, frame_angles.scan_emit, false)) @@ -213,7 +213,7 @@ class AngleCorrectorCorrectionBased : public AngleCorrector return false; } - bool passedTimestampResetAngle(uint32_t last_azimuth, uint32_t current_azimuth) override + bool passed_timestamp_reset_angle(uint32_t last_azimuth, uint32_t current_azimuth) override { for (const auto & frame_angles : frame_angle_info_) { if (angle_is_between(last_azimuth, current_azimuth, frame_angles.timestamp_reset, false)) @@ -223,7 +223,7 @@ class AngleCorrectorCorrectionBased : public AngleCorrector return false; } - bool isInsideFoV(uint32_t last_azimuth, uint32_t current_azimuth) override + bool is_inside_fov(uint32_t last_azimuth, uint32_t current_azimuth) override { for (const auto & frame_angles : frame_angle_info_) { if ( @@ -235,7 +235,7 @@ class AngleCorrectorCorrectionBased : public AngleCorrector return false; } - bool isInsideOverlap(uint32_t last_azimuth, uint32_t current_azimuth) override + bool is_inside_overlap(uint32_t last_azimuth, uint32_t current_azimuth) override { for (const auto & frame_angles : frame_angle_info_) { if ( diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp index b1b5bc798..f7d3d29bb 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp @@ -77,16 +77,16 @@ class HesaiDecoder : public HesaiScanDecoder rclcpp::Logger logger_; /// @brief For each channel, its firing offset relative to the block in nanoseconds - std::array channel_firing_offset_ns_; + std::array channel_firing_offset_ns_; /// @brief For each return mode, the firing offset of each block relative to its packet in /// nanoseconds - std::array, SensorT::packet_t::MAX_RETURNS> + std::array, SensorT::packet_t::max_returns> block_firing_offset_ns_; /// @brief Validates and parse PandarPacket. Currently only checks size, not checksums etc. /// @param packet The incoming PandarPacket /// @return Whether the packet was parsed successfully - bool parsePacket(const std::vector & packet) + bool parse_packet(const std::vector & packet) { if (packet.size() < sizeof(typename SensorT::packet_t)) { RCLCPP_ERROR_STREAM( @@ -109,14 +109,14 @@ class HesaiDecoder : public HesaiScanDecoder /// @param start_block_id The first block in the group of returns /// @param n_blocks The number of returns in the group (has to align with the `n_returns` field in /// the packet footer) - void convertReturns(size_t start_block_id, size_t n_blocks) + void convert_returns(size_t start_block_id, size_t n_blocks) { uint64_t packet_timestamp_ns = hesai_packet::get_timestamp_ns(packet_); - uint32_t raw_azimuth = packet_.body.blocks[start_block_id].getAzimuth(); + uint32_t raw_azimuth = packet_.body.blocks[start_block_id].get_azimuth(); std::vector return_units; - for (size_t channel_id = 0; channel_id < SensorT::packet_t::N_CHANNELS; ++channel_id) { + for (size_t channel_id = 0; channel_id < SensorT::packet_t::n_channels; ++channel_id) { // Find the units corresponding to the same return group as the current one. // These are used to find duplicates in multi-return mode. return_units.clear(); @@ -132,16 +132,16 @@ class HesaiDecoder : public HesaiScanDecoder continue; } - float distance = getDistance(unit); + float distance = get_distance(unit); if ( - distance < SensorT::MIN_RANGE || SensorT::MAX_RANGE < distance || + distance < SensorT::min_range || SensorT::max_range < distance || distance < sensor_configuration_->min_range || sensor_configuration_->max_range < distance) { continue; } - auto return_type = sensor_.getReturnType( + auto return_type = sensor_.get_return_type( static_cast(packet_.tail.return_mode), block_offset, return_units); @@ -160,7 +160,7 @@ class HesaiDecoder : public HesaiScanDecoder } if ( - fabsf(getDistance(*return_units[return_idx]) - distance) < + fabsf(get_distance(*return_units[return_idx]) - distance) < sensor_configuration_->dual_return_distance_threshold) { is_below_multi_return_threshold = true; break; @@ -173,7 +173,7 @@ class HesaiDecoder : public HesaiScanDecoder } CorrectedAngleData corrected_angle_data = - angle_corrector_.getCorrectedAngleData(raw_azimuth, channel_id); + angle_corrector_.get_corrected_angle_data(raw_azimuth, channel_id); float azimuth = corrected_angle_data.azimuth_rad; bool in_fov = angle_is_between(scan_cut_angles_.fov_min, scan_cut_angles_.fov_max, azimuth); @@ -184,7 +184,7 @@ class HesaiDecoder : public HesaiScanDecoder bool in_current_scan = true; if ( - angle_corrector_.isInsideOverlap(last_azimuth_, raw_azimuth) && + angle_corrector_.is_inside_overlap(last_azimuth_, raw_azimuth) && angle_is_between( scan_cut_angles_.scan_emit_angle, scan_cut_angles_.scan_emit_angle + deg2rad(20), azimuth)) { @@ -198,7 +198,7 @@ class HesaiDecoder : public HesaiScanDecoder NebulaPoint & point = pc->emplace_back(); point.distance = distance; point.intensity = unit.reflectivity; - point.time_stamp = getPointTimeRelative( + point.time_stamp = get_point_time_relative( scan_timestamp_ns, packet_timestamp_ns, block_offset + start_block_id, channel_id); point.return_type = static_cast(return_type); @@ -219,7 +219,7 @@ class HesaiDecoder : public HesaiScanDecoder } /// @brief Get the distance of the given unit in meters - float getDistance(const typename SensorT::packet_t::body_t::block_t::unit_t & unit) + float get_distance(const typename SensorT::packet_t::body_t::block_t::unit_t & unit) { return unit.distance * hesai_packet::get_dis_unit(packet_); } @@ -230,11 +230,11 @@ class HesaiDecoder : public HesaiScanDecoder /// @param packet_timestamp_ns The timestamp of the current PandarPacket in nanoseconds /// @param block_id The block index of the point /// @param channel_id The channel index of the point - uint32_t getPointTimeRelative( + uint32_t get_point_time_relative( uint64_t scan_timestamp_ns, uint64_t packet_timestamp_ns, size_t block_id, size_t channel_id) { auto point_to_packet_offset_ns = - sensor_.getPacketRelativePointTimeOffset(block_id, channel_id, packet_); + sensor_.get_packet_relative_point_time_offset(block_id, channel_id, packet_); auto packet_to_scan_offset_ns = static_cast(packet_timestamp_ns - scan_timestamp_ns); return packet_to_scan_offset_ns + point_to_packet_offset_ns; } @@ -259,8 +259,8 @@ class HesaiDecoder : public HesaiScanDecoder decode_pc_ = std::make_shared(); output_pc_ = std::make_shared(); - decode_pc_->reserve(SensorT::MAX_SCAN_BUFFER_POINTS); - output_pc_->reserve(SensorT::MAX_SCAN_BUFFER_POINTS); + decode_pc_->reserve(SensorT::max_scan_buffer_points); + output_pc_->reserve(SensorT::max_scan_buffer_points); scan_cut_angles_ = { deg2rad(sensor_configuration_->cloud_min_angle), @@ -269,14 +269,14 @@ class HesaiDecoder : public HesaiScanDecoder int unpack(const std::vector & packet) override { - if (!parsePacket(packet)) { + if (!parse_packet(packet)) { return -1; } // This is the first scan, set scan timestamp to whatever packet arrived first if (decode_scan_timestamp_ns_ == 0) { decode_scan_timestamp_ns_ = hesai_packet::get_timestamp_ns(packet_) + - sensor_.getEarliestPointTimeOffsetForBlock(0, packet_); + sensor_.get_earliest_point_time_offset_for_block(0, packet_); } if (has_scanned_) { @@ -285,13 +285,13 @@ class HesaiDecoder : public HesaiScanDecoder } const size_t n_returns = hesai_packet::get_n_returns(packet_.tail.return_mode); - for (size_t block_id = 0; block_id < SensorT::packet_t::N_BLOCKS; block_id += n_returns) { - auto block_azimuth = packet_.body.blocks[block_id].getAzimuth(); + for (size_t block_id = 0; block_id < SensorT::packet_t::n_blocks; block_id += n_returns) { + auto block_azimuth = packet_.body.blocks[block_id].get_azimuth(); - if (angle_corrector_.passedTimestampResetAngle(last_azimuth_, block_azimuth)) { + if (angle_corrector_.passed_timestamp_reset_angle(last_azimuth_, block_azimuth)) { uint64_t new_scan_timestamp_ns = hesai_packet::get_timestamp_ns(packet_) + - sensor_.getEarliestPointTimeOffsetForBlock(block_id, packet_); + sensor_.get_earliest_point_time_offset_for_block(block_id, packet_); if (sensor_configuration_->cut_angle == sensor_configuration_->cloud_max_angle) { // In the non-360 deg case, if the cut angle and FoV end coincide, the old pointcloud has @@ -308,14 +308,14 @@ class HesaiDecoder : public HesaiScanDecoder } } - if (!angle_corrector_.isInsideFoV(last_azimuth_, block_azimuth)) { + if (!angle_corrector_.is_inside_fov(last_azimuth_, block_azimuth)) { last_azimuth_ = block_azimuth; continue; } - convertReturns(block_id, n_returns); + convert_returns(block_id, n_returns); - if (angle_corrector_.passedEmitAngle(last_azimuth_, block_azimuth)) { + if (angle_corrector_.passed_emit_angle(last_azimuth_, block_azimuth)) { // The current `decode` pointcloud is ready for publishing, swap buffers to continue with // the last `output` pointcloud as the `decode pointcloud. std::swap(decode_pc_, output_pc_); @@ -329,9 +329,9 @@ class HesaiDecoder : public HesaiScanDecoder return last_azimuth_; } - bool hasScanned() override { return has_scanned_; } + bool has_scanned() override { return has_scanned_; } - std::tuple getPointcloud() override + std::tuple get_pointcloud() override { double scan_timestamp_s = static_cast(output_scan_timestamp_ns_) * 1e-9; return std::make_pair(output_pc_, scan_timestamp_s); diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp index b05d64406..5b4d12e0d 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp @@ -57,7 +57,7 @@ struct DateTime /// @brief Get seconds since epoch /// @return Whole seconds since epoch - [[nodiscard]] uint64_t getSeconds() const + [[nodiscard]] uint64_t get_seconds() const { std::tm tm{}; tm.tm_year = year - 1900 + YearOffset; @@ -78,7 +78,7 @@ struct SecondsSinceEpoch /// @brief Get seconds since epoch /// @return Whole seconds since epoch - [[nodiscard]] uint64_t getSeconds() const + [[nodiscard]] uint64_t get_seconds() const { uint64_t seconds = 0; for (unsigned char second : this->seconds) { @@ -145,7 +145,7 @@ struct Block UnitT units[UnitN]; using unit_t = UnitT; - [[nodiscard]] uint32_t getAzimuth() const { return azimuth; } + [[nodiscard]] uint32_t get_azimuth() const { return azimuth; } }; template @@ -156,7 +156,7 @@ struct FineAzimuthBlock uint8_t fine_azimuth; UnitT units[UnitN]; - [[nodiscard]] uint32_t getAzimuth() const { return (azimuth << 8) + fine_azimuth; } + [[nodiscard]] uint32_t get_azimuth() const { return (azimuth << 8) + fine_azimuth; } }; template @@ -169,7 +169,7 @@ struct SOBBlock uint16_t azimuth; UnitT units[UnitN]; - [[nodiscard]] uint32_t getAzimuth() const { return azimuth; } + [[nodiscard]] uint32_t get_azimuth() const { return azimuth; } }; template @@ -189,10 +189,10 @@ struct Body template struct PacketBase { - static constexpr size_t N_BLOCKS = nBlocks; - static constexpr size_t N_CHANNELS = nChannels; - static constexpr size_t MAX_RETURNS = maxReturns; - static constexpr size_t DEGREE_SUBDIVISIONS = degreeSubdivisions; + static constexpr size_t n_blocks = nBlocks; + static constexpr size_t n_channels = nChannels; + static constexpr size_t max_returns = maxReturns; + static constexpr size_t degree_subdivisions = degreeSubdivisions; }; #pragma pack(pop) @@ -228,7 +228,7 @@ inline int get_n_returns(uint8_t return_mode) template uint64_t get_timestamp_ns(const PacketT & packet) { - return packet.tail.date_time.getSeconds() * 1000000000 + packet.tail.timestamp * 1000; + return packet.tail.date_time.get_seconds() * 1000000000 + packet.tail.timestamp * 1000; } /// @brief Get the distance unit of the given packet type in meters. Distance values in the packet, diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp index 91e2a7767..c4ac84299 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp @@ -42,11 +42,11 @@ class HesaiScanDecoder /// @brief Indicates whether one full scan is ready /// @return Whether a scan is ready - virtual bool hasScanned() = 0; + virtual bool has_scanned() = 0; /// @brief Returns the point cloud and timestamp of the last scan /// @return A tuple of point cloud and timestamp in nanoseconds - virtual std::tuple getPointcloud() = 0; + virtual std::tuple get_pointcloud() = 0; }; } // namespace nebula::drivers diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_sensor.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_sensor.hpp index 6492d35b0..9f48e9977 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_sensor.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_sensor.hpp @@ -43,7 +43,7 @@ class HesaiSensor /// blocks) /// @return true if the reflectivity of the unit is strictly greater than that of all other units /// in return_units, false otherwise - static bool isStrongestReturn( + static bool is_strongest_return( uint32_t return_idx, const std::vector & return_units) { @@ -66,7 +66,7 @@ class HesaiSensor /// length 2 for dual-return with both units having the same channel but coming from different /// blocks) /// @return true if the unit is identical to any other one in return_units, false otherwise - static bool isDuplicateReturn( + static bool is_duplicate_return( uint32_t return_idx, const std::vector & return_units) { @@ -89,8 +89,8 @@ class HesaiSensor using packet_t = PacketT; using angle_corrector_t = typename std::conditional< (AngleCorrection == AngleCorrectionType::CALIBRATION), - AngleCorrectorCalibrationBased, - AngleCorrectorCorrectionBased>::type; + AngleCorrectorCalibrationBased, + AngleCorrectorCorrectionBased>::type; HesaiSensor() = default; virtual ~HesaiSensor() = default; @@ -101,7 +101,7 @@ class HesaiSensor /// @param channel_id The point's channel id /// @param packet The packet /// @return The relative time offset in nanoseconds - virtual int getPacketRelativePointTimeOffset( + virtual int get_packet_relative_point_time_offset( uint32_t block_id, uint32_t channel_id, const PacketT & packet) = 0; /// @brief For a given start block index, find the earliest (lowest) relative time offset of any @@ -110,15 +110,15 @@ class HesaiSensor /// @param packet The packet /// @return The lowest point time offset (relative to the packet timestamp) of any point in or /// after the start block, in nanoseconds - int getEarliestPointTimeOffsetForBlock(uint32_t start_block_id, const PacketT & packet) + int get_earliest_point_time_offset_for_block(uint32_t start_block_id, const PacketT & packet) { unsigned int n_returns = hesai_packet::get_n_returns(packet.tail.return_mode); int min_offset_ns = 0x7FFFFFFF; // MAXINT (max. positive value) for (uint32_t block_id = start_block_id; block_id < start_block_id + n_returns; ++block_id) { - for (uint32_t channel_id = 0; channel_id < PacketT::N_CHANNELS; ++channel_id) { - min_offset_ns = - std::min(min_offset_ns, getPacketRelativePointTimeOffset(block_id, channel_id, packet)); + for (uint32_t channel_id = 0; channel_id < PacketT::n_channels; ++channel_id) { + min_offset_ns = std::min( + min_offset_ns, get_packet_relative_point_time_offset(block_id, channel_id, packet)); } } @@ -139,11 +139,11 @@ class HesaiSensor /// @param return_units The units corresponding to all the returns in the group. These are usually /// from the same column across adjascent blocks. /// @return The return type of the point - virtual ReturnType getReturnType( + virtual ReturnType get_return_type( hesai_packet::return_mode::ReturnMode return_mode, unsigned int return_idx, const std::vector & return_units) { - if (isDuplicateReturn(return_idx, return_units)) { + if (is_duplicate_return(return_idx, return_units)) { return ReturnType::IDENTICAL; } @@ -157,7 +157,7 @@ class HesaiSensor case hesai_packet::return_mode::SINGLE_LAST: return ReturnType::LAST; case hesai_packet::return_mode::DUAL_LAST_STRONGEST: - if (isStrongestReturn(return_idx, return_units)) { + if (is_strongest_return(return_idx, return_units)) { return return_idx == 0 ? ReturnType::LAST_STRONGEST : ReturnType::STRONGEST; } else { return return_idx == 0 ? ReturnType::LAST : ReturnType::SECONDSTRONGEST; @@ -167,7 +167,7 @@ class HesaiSensor case hesai_packet::return_mode::DUAL_FIRST_LAST: return return_idx == 0 ? ReturnType::FIRST : ReturnType::LAST; case hesai_packet::return_mode::DUAL_FIRST_STRONGEST: - if (isStrongestReturn(return_idx, return_units)) { + if (is_strongest_return(return_idx, return_units)) { return return_idx == 0 ? ReturnType::FIRST_STRONGEST : ReturnType::STRONGEST; } else { return return_idx == 0 ? ReturnType::FIRST : ReturnType::SECONDSTRONGEST; diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e3x.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e3x.hpp index 80c43425d..c6818f9f1 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e3x.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e3x.hpp @@ -58,7 +58,7 @@ struct Tail128E3X /// @brief Get the azimuth state of the given block in the packet /// @param block_id The block ID (i.e. its index in the packet). Valid IDs are 0 and 1. /// @return The azimuth state number of the block - uint8_t geAzimuthState(unsigned int block_id) const + uint8_t get_azimuth_state(unsigned int block_id) const { return (azimuth_state >> (14 - block_id * 2)) & 0b11; } @@ -66,7 +66,7 @@ struct Tail128E3X struct Packet128E3X : public PacketBase<2, 128, 2, 100> { - using body_t = Body, Packet128E3X::N_BLOCKS>; + using body_t = Body, Packet128E3X::n_blocks>; Header12B header; body_t body; uint32_t crc_body; @@ -87,7 +87,7 @@ class Pandar128E3X : public HesaiSensor private: enum OperationalState { HIGH_RESOLUTION = 0, SHUTDOWN = 1, STANDARD = 2, ENERGY_SAVING = 3 }; - static constexpr int hires_as0_far_offset_ns_[128] = { + static constexpr int hires_as0_far_offset_ns[128] = { 4436, -1, 776, 2431, 4436, -1, 6441, -1, -1, 776, 2431, 6441, -1, -1, -1, -1, -1, 6441, -1, 776, 2431, -1, -1, -1, 4436, 10381, 14951, 12666, 14951, 19521, 19521, 8096, 12666, 12666, 10381, 24091, 17236, 24091, 14951, @@ -99,7 +99,7 @@ class Pandar128E3X : public HesaiSensor 4436, -1, -1, -1, 6441, -1, -1, -1, 776, 4436, -1, 2431, -1, -1, 776, -1, 4436, 6441, -1, -1, 2431, 776, 6441, -1}; - static constexpr int hires_as0_near_offset_ns_[128] = { + static constexpr int hires_as0_near_offset_ns[128] = { 5201, -1, 1541, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 7206, -1, -1, 3196, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 27056, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, @@ -108,7 +108,7 @@ class Pandar128E3X : public HesaiSensor -1, -1, -1, -1, -1, -1, -1, -1, -1, 5681, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1}; - static constexpr int hires_as1_far_offset_ns_[128] = { + static constexpr int hires_as1_far_offset_ns[128] = { -1, 776, -1, -1, -1, 2781, -1, 4786, 6441, -1, -1, -1, 776, 6441, 2781, 776, 4786, -1, 4786, -1, -1, 2781, 6441, 4786, -1, 10731, 15301, 13016, 15301, 19871, 19871, 8446, 13016, 13016, 10731, 24441, 17586, 24441, 15301, @@ -120,7 +120,7 @@ class Pandar128E3X : public HesaiSensor -1, 2781, 776, 4786, -1, 6441, 6441, 4786, -1, -1, 4786, -1, 2781, 6441, -1, 776, -1, -1, 6441, 2781, -1, -1, -1, 776}; - static constexpr int hires_as1_near_offset_ns_[128] = { + static constexpr int hires_as1_near_offset_ns[128] = { -1, -1, -1, -1, -1, 4026, -1, -1, 7206, -1, -1, -1, -1, -1, 3546, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 12126, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 27406, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, @@ -129,7 +129,7 @@ class Pandar128E3X : public HesaiSensor -1, -1, -1, -1, -1, -1, 2021, -1, -1, -1, -1, -1, -1, -1, -1, 7686, -1, -1, -1, -1, -1, -1, -1, -1, 1541, -1, -1, -1, -1, -1, -1, -1, -1}; - static constexpr int hires_as2_far_offset_ns_[128] = { + static constexpr int hires_as2_far_offset_ns[128] = { 4436, -1, 776, 2781, 4436, -1, 6 - 191, -1, -1, 776, 2781, 6091, -1, -1, -1, -1, -1, 6091, -1, 776, 2781, -1, -1, -1, 4436, 10381, 14951, 12666, 14951, 19521, 19521, 8096, 12666, 12666, 10381, 24091, 17236, 24091, 14951, @@ -141,7 +141,7 @@ class Pandar128E3X : public HesaiSensor 4436, -1, -1, -1, 6091, -1, -1, -1, 776, 4436, -1, 2781, -1, -1, 776, -1, 4436, 6091, -1, -1, 2781, 776, 6091, -1}; - static constexpr int hires_as2_near_offset_ns_[128] = { + static constexpr int hires_as2_near_offset_ns[128] = { -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 7336, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 14061, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 27056, -1, -1, -1, @@ -150,7 +150,7 @@ class Pandar128E3X : public HesaiSensor 2021, -1, -1, 3546, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 5201, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 1541, -1, -1}; - static constexpr int hires_as3_far_offset_ns_[128] = { + static constexpr int hires_as3_far_offset_ns[128] = { -1, 776, -1, -1, -1, 2431, -1, 4086, 6091, -1, -1, -1, 776, 6091, 2431, 776, 4086, -1, 4086, -1, -1, 2431, 6091, 4086, -1, 10031, 14601, 12316, 14601, 19171, 19171, 7746, 12316, 12316, 10031, 23741, 16886, 23741, 14601, @@ -162,7 +162,7 @@ class Pandar128E3X : public HesaiSensor -1, 2431, 776, 4086, -1, 6091, 6091, 4086, -1, -1, 4086, -1, 2431, 6091, -1, 776, -1, -1, 6091, 2431, -1, -1, -1, 776}; - static constexpr int hires_as3_near_offset_ns_[128] = { + static constexpr int hires_as3_near_offset_ns[128] = { -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 4851, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, @@ -171,7 +171,7 @@ class Pandar128E3X : public HesaiSensor -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 5331, -1, -1, -1, -1, -1, -1, -1, -1, 3196, -1, -1, -1, -1, -1, 6856, -1, -1, -1, -1, 1541}; - static constexpr int standard_as0_far_offset_ns_[128] = { + static constexpr int standard_as0_far_offset_ns[128] = { 4436, 28554, 776, 2431, 4436, 30559, 6441, 32564, 34219, 776, 2431, 6441, 28554, 34219, 30559, 28554, 32564, 6441, 32564, 776, 2431, 30559, 34219, 32564, 4436, 38509, 43079, 12666, 43079, 19521, 19521, 36224, 12666, 12666, 38509, 52219, 17236, 52219, 43079, @@ -183,7 +183,7 @@ class Pandar128E3X : public HesaiSensor 4436, 30559, 28554, 32564, 6441, 34219, 34219, 32564, 776, 4436, 32564, 2431, 30559, 34219, 776, 28554, 4436, 6441, 34219, 30559, 2431, 776, 6441, 28554}; - static constexpr int standard_as0_near_offset_ns_[128] = { + static constexpr int standard_as0_near_offset_ns[128] = { 5201, -1, 1541, -1, -1, 31804, -1, -1, 34984, -1, -1, -1, -1, -1, 31324, -1, -1, 7206, -1, -1, 3196, -1, -1, -1, -1, 39904, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 27056, -1, -1, -1, -1, -1, -1, 55184, -1, @@ -193,7 +193,7 @@ class Pandar128E3X : public HesaiSensor -1, -1, -1, -1, -1, 29799, -1, -1, 5681, -1, -1, -1, -1, -1, 35464, -1, -1, -1, -1, -1, -1, -1, -1, 29319, -1, -1, -1, -1, -1, -1, -1, -1}; - static constexpr int standard_as1_far_offset_ns_[128] = { + static constexpr int standard_as1_far_offset_ns[128] = { 4436, 28554, 776, 2781, 4436, 30209, 6091, 31864, 33869, 776, 2781, 6091, 28554, 33869, 30209, 28554, 31864, 6091, 31864, 776, 2781, 30209, 33869, 31864, 4436, 37809, 42379, 12666, 42379, 19521, 19521, 35524, 12666, 12666, 37809, 51519, 17236, 51519, 42379, @@ -205,7 +205,7 @@ class Pandar128E3X : public HesaiSensor 4436, 30209, 28554, 31864, 6091, 33869, 33869, 31864, 776, 4436, 31864, 2781, 30209, 33869, 776, 28554, 4436, 6091, 33869, 30209, 2781, 776, 6091, 28554}; - static constexpr int standard_as1_near_offset_ns_[128] = { + static constexpr int standard_as1_near_offset_ns[128] = { -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 7336, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 32629, -1, -1, -1, -1, -1, -1, -1, -1, 14061, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, @@ -217,11 +217,11 @@ class Pandar128E3X : public HesaiSensor -1, -1, 34634, -1, -1, 1541, -1, 29319}; public: - static constexpr float MIN_RANGE = 0.1; - static constexpr float MAX_RANGE = 230.0; - static constexpr size_t MAX_SCAN_BUFFER_POINTS = 691200; + static constexpr float min_range = 0.1; + static constexpr float max_range = 230.0; + static constexpr size_t max_scan_buffer_points = 691200; - int getPacketRelativePointTimeOffset( + int get_packet_relative_point_time_offset( uint32_t block_id, uint32_t channel_id, const packet_t & packet) override { auto n_returns = hesai_packet::get_n_returns(packet.tail.return_mode); @@ -231,32 +231,32 @@ class Pandar128E3X : public HesaiSensor bool is_hires_mode = packet.tail.operational_state == OperationalState::HIGH_RESOLUTION; bool is_nearfield = (hesai_packet::get_dis_unit(packet) * packet.body.blocks[block_id].units[channel_id].distance) <= 2.85f; - auto azimuth_state = packet.tail.geAzimuthState(block_id); + auto azimuth_state = packet.tail.get_azimuth_state(block_id); if (is_hires_mode && azimuth_state == 0 && !is_nearfield) - channel_offset_ns = hires_as0_far_offset_ns_[channel_id]; + channel_offset_ns = hires_as0_far_offset_ns[channel_id]; else if (is_hires_mode && azimuth_state == 0 && is_nearfield) - channel_offset_ns = hires_as0_near_offset_ns_[channel_id]; + channel_offset_ns = hires_as0_near_offset_ns[channel_id]; else if (is_hires_mode && azimuth_state == 1 && !is_nearfield) - channel_offset_ns = hires_as1_far_offset_ns_[channel_id]; + channel_offset_ns = hires_as1_far_offset_ns[channel_id]; else if (is_hires_mode && azimuth_state == 1 && is_nearfield) - channel_offset_ns = hires_as1_near_offset_ns_[channel_id]; + channel_offset_ns = hires_as1_near_offset_ns[channel_id]; else if (is_hires_mode && azimuth_state == 2 && !is_nearfield) - channel_offset_ns = hires_as2_far_offset_ns_[channel_id]; + channel_offset_ns = hires_as2_far_offset_ns[channel_id]; else if (is_hires_mode && azimuth_state == 2 && is_nearfield) - channel_offset_ns = hires_as2_near_offset_ns_[channel_id]; + channel_offset_ns = hires_as2_near_offset_ns[channel_id]; else if (is_hires_mode && azimuth_state == 3 && !is_nearfield) - channel_offset_ns = hires_as3_far_offset_ns_[channel_id]; + channel_offset_ns = hires_as3_far_offset_ns[channel_id]; else if (is_hires_mode && azimuth_state == 3 && is_nearfield) - channel_offset_ns = hires_as3_near_offset_ns_[channel_id]; + channel_offset_ns = hires_as3_near_offset_ns[channel_id]; else if (!is_hires_mode && azimuth_state == 0 && !is_nearfield) - channel_offset_ns = standard_as0_far_offset_ns_[channel_id]; + channel_offset_ns = standard_as0_far_offset_ns[channel_id]; else if (!is_hires_mode && azimuth_state == 0 && is_nearfield) - channel_offset_ns = standard_as0_near_offset_ns_[channel_id]; + channel_offset_ns = standard_as0_near_offset_ns[channel_id]; else if (!is_hires_mode && azimuth_state == 1 && !is_nearfield) - channel_offset_ns = standard_as1_far_offset_ns_[channel_id]; + channel_offset_ns = standard_as1_far_offset_ns[channel_id]; else if (!is_hires_mode && azimuth_state == 1 && is_nearfield) - channel_offset_ns = standard_as1_near_offset_ns_[channel_id]; + channel_offset_ns = standard_as1_near_offset_ns[channel_id]; else throw std::runtime_error( "Invalid combination of operational state and azimuth state and nearfield firing"); @@ -264,11 +264,12 @@ class Pandar128E3X : public HesaiSensor return block_offset_ns + channel_offset_ns; } - ReturnType getReturnType( + ReturnType get_return_type( hesai_packet::return_mode::ReturnMode return_mode, unsigned int return_idx, const std::vector & return_units) override { - auto return_type = HesaiSensor::getReturnType(return_mode, return_idx, return_units); + auto return_type = + HesaiSensor::get_return_type(return_mode, return_idx, return_units); if (return_type == ReturnType::IDENTICAL) { return return_type; } diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp index 9e5cc862f..aec5e4be1 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_128e4x.hpp @@ -43,7 +43,7 @@ class Pandar128E4X : public HesaiSensor private: enum OperationalState { HIGH_RESOLUTION = 0, STANDARD = 1 }; - static constexpr int firing_time_offset_static_ns_[128] = { + static constexpr int firing_time_offset_static_ns[128] = { 49758, 43224, 36690, 30156, 21980, 15446, 8912, 2378, 49758, 43224, 36690, 30156, 2378, 15446, 8912, 21980, 43224, 30156, 49758, 15446, 36690, 2378, 21980, 8912, 34312, 45002, 38468, 40846, 40846, 34312, 51536, 47380, 31934, 47380, 31934, 51536, 38468, 27778, 27778, @@ -55,7 +55,7 @@ class Pandar128E4X : public HesaiSensor 43224, 36690, 30156, 21980, 15446, 8912, 2378, 43224, 49758, 30156, 36690, 21980, 15446, 2378, 8912, 49758, 43224, 36690, 30156, 21980, 15446, 8912, 2378, 30156}; - static constexpr int firing_time_offset_as0_ns_[128] = { + static constexpr int firing_time_offset_as0_ns[128] = { -1, -1, -1, -1, 21980, 15446, 8912, 2378, -1, -1, -1, -1, 2378, 15446, 8912, 21980, -1, 2378, 21980, 8912, 6534, 17224, 10690, 13068, 13068, 6534, 23758, 19602, 4156, 19602, 4156, 23758, 13068, 13068, 23758, 10690, 4156, 19602, 19602, @@ -67,7 +67,7 @@ class Pandar128E4X : public HesaiSensor -1, -1, -1, -1, 21980, 15446, 8912, 2378, -1, -1, -1, -1, 21980, 15446, 2378, 8912, -1, -1, -1, -1, 21980, 15446, 8912, 2378}; - static constexpr int firing_time_offset_as1_ns_[128] = { + static constexpr int firing_time_offset_as1_ns[128] = { 21980, 15446, 8912, 2378, -1, -1, -1, -1, 21980, 15446, 8912, 2378, -1, -1, -1, -1, 8912, -1, -1, -1, 6534, 17224, 10690, 13068, 13068, 6534, 23758, 19602, 4156, 19602, 4156, 23758, 13068, 13068, 23758, 10690, 4156, 19602, 19602, @@ -80,11 +80,11 @@ class Pandar128E4X : public HesaiSensor -1, -1, -1, 21980, 15446, 8912, 2378, -1, -1, -1, -1}; public: - static constexpr float MIN_RANGE = 0.1; - static constexpr float MAX_RANGE = 230.0; - static constexpr size_t MAX_SCAN_BUFFER_POINTS = 691200; + static constexpr float min_range = 0.1; + static constexpr float max_range = 230.0; + static constexpr size_t max_scan_buffer_points = 691200; - int getPacketRelativePointTimeOffset( + int get_packet_relative_point_time_offset( uint32_t block_id, uint32_t channel_id, const packet_t & packet) override { auto n_returns = hesai_packet::get_n_returns(packet.tail.return_mode); @@ -97,26 +97,27 @@ class Pandar128E4X : public HesaiSensor int channel_offset_ns = 0; bool is_hires_mode = packet.tail.operational_state == OperationalState::HIGH_RESOLUTION; - auto azimuth_state = packet.tail.geAzimuthState(block_id); + auto azimuth_state = packet.tail.get_azimuth_state(block_id); if (!is_hires_mode) { - channel_offset_ns = firing_time_offset_static_ns_[channel_id]; + channel_offset_ns = firing_time_offset_static_ns[channel_id]; } else { if (azimuth_state == 0) { - channel_offset_ns = firing_time_offset_as0_ns_[channel_id]; + channel_offset_ns = firing_time_offset_as0_ns[channel_id]; } else /* azimuth_state == 1 */ { - channel_offset_ns = firing_time_offset_as1_ns_[channel_id]; + channel_offset_ns = firing_time_offset_as1_ns[channel_id]; } } return block_offset_ns + 43346 + channel_offset_ns; } - ReturnType getReturnType( + ReturnType get_return_type( hesai_packet::return_mode::ReturnMode return_mode, unsigned int return_idx, const std::vector & return_units) override { - auto return_type = HesaiSensor::getReturnType(return_mode, return_idx, return_units); + auto return_type = + HesaiSensor::get_return_type(return_mode, return_idx, return_units); if (return_type == ReturnType::IDENTICAL) { return return_type; } diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_40.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_40.hpp index d53da2ff1..4fc108241 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_40.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_40.hpp @@ -43,7 +43,7 @@ struct Tail40P struct Packet40P : public PacketBase<10, 40, 2, 100> { - using body_t = Body, Packet40P::N_BLOCKS>; + using body_t = Body, Packet40P::n_blocks>; body_t body; Tail40P tail; }; @@ -71,11 +71,11 @@ class Pandar40 : public HesaiSensor -21920, -9500, -43520, -29770, -17350, -4920, -42220, -28470, -16040, -3620}; public: - static constexpr float MIN_RANGE = 0.3f; - static constexpr float MAX_RANGE = 200.f; - static constexpr size_t MAX_SCAN_BUFFER_POINTS = 144000; + static constexpr float min_range = 0.3f; + static constexpr float max_range = 200.f; + static constexpr size_t max_scan_buffer_points = 144000; - int getPacketRelativePointTimeOffset( + int get_packet_relative_point_time_offset( uint32_t block_id, uint32_t channel_id, const packet_t & packet) override { auto n_returns = hesai_packet::get_n_returns(packet.tail.return_mode); diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_64.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_64.hpp index c3920fd98..bd320064f 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_64.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_64.hpp @@ -29,7 +29,7 @@ namespace hesai_packet using Tail64 = Tail40P; struct Packet64 : public PacketBase<6, 64, 2, 100> { - using body_t = Body, Packet64::N_BLOCKS>; + using body_t = Body, Packet64::n_blocks>; Header8B header; body_t body; Tail64 tail; @@ -42,7 +42,7 @@ struct Packet64 : public PacketBase<6, 64, 2, 100> class Pandar64 : public HesaiSensor { private: - static constexpr int firing_time_offset_ns_[64] = { + static constexpr int firing_time_offset_ns[64] = { -23180, -21876, -20572, -19268, -17964, -16660, -11444, -46796, -7532, -36956, -50732, -54668, -40892, -44828, -31052, -34988, -48764, -52700, -38924, -42860, -29084, -33020, -46796, -25148, -36956, -50732, -27116, -40892, -44828, -31052, -34988, -48764, -25148, @@ -51,16 +51,16 @@ class Pandar64 : public HesaiSensor -11444, -6228, -15356, -10140, -4924, -3620, -14052, -8836, -12748}; public: - static constexpr float MIN_RANGE = 0.3f; - static constexpr float MAX_RANGE = 200.f; - static constexpr size_t MAX_SCAN_BUFFER_POINTS = 230400; + static constexpr float min_range = 0.3f; + static constexpr float max_range = 200.f; + static constexpr size_t max_scan_buffer_points = 230400; - int getPacketRelativePointTimeOffset( + int get_packet_relative_point_time_offset( uint32_t block_id, uint32_t channel_id, const packet_t & packet) override { auto n_returns = hesai_packet::get_n_returns(packet.tail.return_mode); int block_offset_ns = -42580 - 55560 * ((6 - block_id - 1) / n_returns); - return block_offset_ns + firing_time_offset_ns_[channel_id]; + return block_offset_ns + firing_time_offset_ns[channel_id]; } }; diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_at128.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_at128.hpp index c009c4437..dc3538b1b 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_at128.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_at128.hpp @@ -45,7 +45,7 @@ struct TailAT128E2X struct PacketAT128E2X : public PacketBase<2, 128, 2, 100 * 256> { using body_t = - Body, PacketAT128E2X::N_BLOCKS>; + Body, PacketAT128E2X::n_blocks>; Header12B header; body_t body; uint32_t crc_body; @@ -64,7 +64,7 @@ class PandarAT128 : public HesaiSensor { private: - static constexpr int firing_time_offset_ns_[128] = { + static constexpr int firing_time_offset_ns[128] = { 0, 0, 8240, 4112, 4144, 8240, 0, 0, 12424, 4144, 4112, 8264, 12376, 12376, 8264, 12424, 0, 0, 4112, 8240, 4144, 0, 0, 4144, 12424, 8264, 4112, 12376, 12376, 12424, 8264, 848, 2504, 4976, 6616, 6616, 9112, 2504, 848, @@ -77,11 +77,11 @@ class PandarAT128 0, 0, 12424, 8264, 8240, 4144, 8264, 8240, 12376, 12376, 8264}; public: - static constexpr float MIN_RANGE = 1.f; - static constexpr float MAX_RANGE = 180.0f; - static constexpr size_t MAX_SCAN_BUFFER_POINTS = 307200; + static constexpr float min_range = 1.f; + static constexpr float max_range = 180.0f; + static constexpr size_t max_scan_buffer_points = 307200; - int getPacketRelativePointTimeOffset( + int get_packet_relative_point_time_offset( uint32_t block_id, uint32_t channel_id, const packet_t & packet) override { auto n_returns = hesai_packet::get_n_returns(packet.tail.return_mode); @@ -92,7 +92,7 @@ class PandarAT128 block_offset_ns = -9249 - 41666; } - return block_offset_ns + firing_time_offset_ns_[channel_id]; + return block_offset_ns + firing_time_offset_ns[channel_id]; } }; diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt128.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt128.hpp index 85d23ddc4..dd1544507 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt128.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt128.hpp @@ -44,7 +44,7 @@ struct TailQT128C2X struct PacketQT128C2X : public PacketBase<2, 128, 2, 100> { - using body_t = Body, PacketQT128C2X::N_BLOCKS>; + using body_t = Body, PacketQT128C2X::n_blocks>; Header12B header; body_t body; uint32_t crc_body; @@ -90,11 +90,11 @@ class PandarQT128 : public HesaiSensor 17296, 67984, 42640, 45808, 71152, 20464, 96496, 99664, 23632, 74320, 48976}; public: - static constexpr float MIN_RANGE = 0.05; - static constexpr float MAX_RANGE = 50.0; - static constexpr size_t MAX_SCAN_BUFFER_POINTS = 172800; + static constexpr float min_range = 0.05; + static constexpr float max_range = 50.0; + static constexpr size_t max_scan_buffer_points = 172800; - int getPacketRelativePointTimeOffset( + int get_packet_relative_point_time_offset( uint32_t block_id, uint32_t channel_id, const packet_t & packet) override { auto n_returns = hesai_packet::get_n_returns(packet.tail.return_mode); diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt64.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt64.hpp index 14cc494d1..e828c05e8 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt64.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_qt64.hpp @@ -37,7 +37,7 @@ struct TailQT64 struct PacketQT64 : public PacketBase<4, 64, 2, 100> { - using body_t = Body, PacketQT64::N_BLOCKS>; + using body_t = Body, PacketQT64::n_blocks>; Header12B header; body_t body; TailQT64 tail; @@ -51,7 +51,7 @@ struct PacketQT64 : public PacketBase<4, 64, 2, 100> class PandarQT64 : public HesaiSensor { private: - static constexpr int firing_time_offset_ns_[64] = { + static constexpr int firing_time_offset_ns[64] = { 12310, 14370, 16430, 18490, 20540, 22600, 24660, 26710, 29160, 31220, 33280, 35340, 37390, 39450, 41500, 43560, 46610, 48670, 50730, 52780, 54840, 56900, 58950, 61010, 63450, 65520, 67580, 69630, 71690, 73740, 75800, 77860, 80900, @@ -60,16 +60,16 @@ class PandarQT64 : public HesaiSensor 12960, 132050, 134110, 136170, 138220, 140280, 142340, 144390, 146450}; public: - static constexpr float MIN_RANGE = 0.1f; - static constexpr float MAX_RANGE = 60.f; - static constexpr size_t MAX_SCAN_BUFFER_POINTS = 76800; + static constexpr float min_range = 0.1f; + static constexpr float max_range = 60.f; + static constexpr size_t max_scan_buffer_points = 76800; - int getPacketRelativePointTimeOffset( + int get_packet_relative_point_time_offset( uint32_t block_id, uint32_t channel_id, const packet_t & packet) override { auto n_returns = hesai_packet::get_n_returns(packet.tail.return_mode); int block_offset_ns = 25710 + (500000 * (block_id / n_returns)) / 3; - return block_offset_ns + firing_time_offset_ns_[channel_id]; + return block_offset_ns + firing_time_offset_ns[channel_id]; } }; diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32.hpp index 1bbf58b24..690dc68e3 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32.hpp @@ -39,7 +39,7 @@ struct TailXT32 struct PacketXT32 : public PacketBase<8, 32, 2, 100> { - using body_t = Body, PacketXT32::N_BLOCKS>; + using body_t = Body, PacketXT32::n_blocks>; Header12B header; body_t body; TailXT32 tail; @@ -53,11 +53,11 @@ struct PacketXT32 : public PacketBase<8, 32, 2, 100> class PandarXT32 : public HesaiSensor { public: - static constexpr float MIN_RANGE = 0.05f; - static constexpr float MAX_RANGE = 120.0f; - static constexpr size_t MAX_SCAN_BUFFER_POINTS = 256000; + static constexpr float min_range = 0.05f; + static constexpr float max_range = 120.0f; + static constexpr size_t max_scan_buffer_points = 256000; - int getPacketRelativePointTimeOffset( + int get_packet_relative_point_time_offset( uint32_t block_id, uint32_t channel_id, const packet_t & packet) override { auto n_returns = hesai_packet::get_n_returns(packet.tail.return_mode); @@ -66,11 +66,12 @@ class PandarXT32 : public HesaiSensor return block_offset_ns + channel_offset_ns; } - ReturnType getReturnType( + ReturnType get_return_type( hesai_packet::return_mode::ReturnMode return_mode, unsigned int return_idx, const std::vector & return_units) override { - auto return_type = HesaiSensor::getReturnType(return_mode, return_idx, return_units); + auto return_type = + HesaiSensor::get_return_type(return_mode, return_idx, return_units); if (return_type == ReturnType::IDENTICAL) { return return_type; } diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32m.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32m.hpp index 120a2622a..24a31ca11 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32m.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/pandar_xt32m.hpp @@ -29,7 +29,7 @@ namespace hesai_packet using TailXT32M2X = TailXT32; struct PacketXT32M2X : public PacketBase<6, 32, 3, 100> { - using body_t = Body, PacketXT32M2X::N_BLOCKS>; + using body_t = Body, PacketXT32M2X::n_blocks>; Header12B header; body_t body; TailXT32M2X tail; @@ -43,11 +43,11 @@ struct PacketXT32M2X : public PacketBase<6, 32, 3, 100> class PandarXT32M : public HesaiSensor { public: - static constexpr float MIN_RANGE = 0.5f; - static constexpr float MAX_RANGE = 300.0f; - static constexpr size_t MAX_SCAN_BUFFER_POINTS = 384000; + static constexpr float min_range = 0.5f; + static constexpr float max_range = 300.0f; + static constexpr size_t max_scan_buffer_points = 384000; - int getPacketRelativePointTimeOffset( + int get_packet_relative_point_time_offset( uint32_t block_id, uint32_t channel_id, const packet_t & packet) override { auto n_returns = hesai_packet::get_n_returns(packet.tail.return_mode); diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp index db07a8545..ffd629217 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp @@ -40,7 +40,7 @@ class HesaiDriver std::shared_ptr scan_decoder_; template - std::shared_ptr InitializeDecoder( + std::shared_ptr initialize_decoder( const std::shared_ptr & sensor_configuration, const std::shared_ptr & calibration_configuration); @@ -58,18 +58,18 @@ class HesaiDriver /// @brief Get current status of this driver /// @return Current status - Status GetStatus(); + Status get_status(); /// @brief Setting CalibrationConfiguration (not used) /// @param calibration_configuration /// @return Resulting status - Status SetCalibrationConfiguration( + Status set_calibration_configuration( const HesaiCalibrationConfigurationBase & calibration_configuration); /// @brief Convert raw packet to pointcloud /// @param packet Packet to convert /// @return Tuple of pointcloud and timestamp - std::tuple ParseCloudPacket( + std::tuple parse_cloud_packet( const std::vector & packet); }; diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector.hpp index afd22650d..77d4f0a22 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector.hpp @@ -57,14 +57,15 @@ class AngleCorrector /// angle unit /// @param channel_id The laser channel's id /// @return The corrected angles (azimuth, elevation) in radians and their sin/cos values - virtual CorrectedAngleData getCorrectedAngleData(uint32_t block_azimuth, uint32_t channel_id) = 0; + virtual CorrectedAngleData get_corrected_angle_data( + uint32_t block_azimuth, uint32_t channel_id) = 0; /// @brief Returns true if the current azimuth lies in a different (new) scan compared to the last /// azimuth /// @param current_azimuth The current azimuth value in the sensor's angle resolution /// @param last_azimuth The last azimuth in the sensor's angle resolution /// @return true if the current azimuth is in a different scan than the last one, false otherwise - virtual bool hasScanned(int current_azimuth, int last_azimuth) = 0; + virtual bool has_scanned(int current_azimuth, int last_azimuth) = 0; }; } // namespace drivers diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector_calibration_based.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector_calibration_based.hpp index 1cb9557cb..4bde59b38 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector_calibration_based.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/angle_corrector_calibration_based.hpp @@ -29,16 +29,16 @@ template class AngleCorrectorCalibrationBased : public AngleCorrector { private: - static constexpr size_t MAX_AZIMUTH = 360 * AngleUnit; + static constexpr size_t max_azimuth = 360 * AngleUnit; std::array elevation_angle_rad_{}; std::array azimuth_offset_rad_{}; - std::array block_azimuth_rad_{}; + std::array block_azimuth_rad_{}; std::array elevation_cos_{}; std::array elevation_sin_{}; - std::array, MAX_AZIMUTH> azimuth_cos_{}; - std::array, MAX_AZIMUTH> azimuth_sin_{}; + std::array, max_azimuth> azimuth_cos_{}; + std::array, max_azimuth> azimuth_sin_{}; public: explicit AngleCorrectorCalibrationBased( @@ -51,7 +51,7 @@ class AngleCorrectorCalibrationBased : public AngleCorrector } for (size_t channel_id = 0; channel_id < ChannelN; ++channel_id) { - const auto correction = sensor_calibration->GetCorrection(channel_id); + const auto correction = sensor_calibration->get_correction(channel_id); float elevation_angle_deg = correction.elevation; float azimuth_offset_deg = correction.azimuth; @@ -62,7 +62,7 @@ class AngleCorrectorCalibrationBased : public AngleCorrector elevation_sin_[channel_id] = sinf(elevation_angle_rad_[channel_id]); } - for (size_t block_azimuth = 0; block_azimuth < MAX_AZIMUTH; block_azimuth++) { + for (size_t block_azimuth = 0; block_azimuth < max_azimuth; block_azimuth++) { block_azimuth_rad_[block_azimuth] = deg2rad(block_azimuth / static_cast(AngleUnit)); for (size_t channel_id = 0; channel_id < ChannelN; ++channel_id) { @@ -75,7 +75,7 @@ class AngleCorrectorCalibrationBased : public AngleCorrector } } - CorrectedAngleData getCorrectedAngleData(uint32_t block_azimuth, uint32_t channel_id) override + CorrectedAngleData get_corrected_angle_data(uint32_t block_azimuth, uint32_t channel_id) override { float azimuth_rad = block_azimuth_rad_[block_azimuth] + azimuth_offset_rad_[channel_id]; float elevation_rad = elevation_angle_rad_[channel_id]; @@ -90,7 +90,7 @@ class AngleCorrectorCalibrationBased : public AngleCorrector sensor_calibration_->calibration[channel_id].channel}; } - bool hasScanned(int current_azimuth, int last_azimuth) override + bool has_scanned(int current_azimuth, int last_azimuth) override { return current_azimuth < last_azimuth; } diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v3.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v3.hpp index 948f16271..79ad67f74 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v3.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v3.hpp @@ -79,7 +79,7 @@ struct Header struct Packet : public PacketBase<12, 32, 2, 100> { - typedef Body, Packet::N_BLOCKS> body_t; + typedef Body, Packet::n_blocks> body_t; Header header; body_t body; big_uint48_buf_t tail; @@ -163,7 +163,7 @@ class BpearlV3 : public RobosenseSensor< robosense_packet::bpearl_v3::Packet, robosense_packet::bpearl_v3::InfoPacket> { private: - static constexpr int firing_time_offset_ns_single_[12][32]{ + static constexpr int firing_time_offset_ns_single[12][32]{ {0, 256, 512, 768, 1024, 1280, 1536, 1792, 2568, 2824, 3080, 3336, 3592, 3848, 4104, 4360, 128, 384, 640, 896, 1152, 1408, 1664, 1920, 2696, 2952, 3208, 3464, 3720, 3976, 4232, 4488}, {5552, 5808, 6064, 6320, 6576, 6832, 7088, 7344, 8120, 8376, 8632, @@ -200,7 +200,7 @@ class BpearlV3 : public RobosenseSensor< 64408, 64664, 64920, 65176, 65432, 61200, 61456, 61712, 61968, 62224, 62480, 62736, 62992, 63768, 64024, 64280, 64536, 64792, 65048, 65304, 65560}}; - static constexpr int firing_time_offset_ns_dual_[12][32]{ + static constexpr int firing_time_offset_ns_dual[12][32]{ {0, 256, 512, 768, 1024, 1280, 1536, 1792, 2568, 2824, 3080, 3336, 3592, 3848, 4104, 4360, 128, 384, 640, 896, 1152, 1408, 1664, 1920, 2696, 2952, 3208, 3464, 3720, 3976, 4232, 4488}, {0, 256, 512, 768, 1024, 1280, 1536, 1792, 2568, 2824, 3080, 3336, 3592, 3848, 4104, 4360, @@ -236,69 +236,69 @@ class BpearlV3 : public RobosenseSensor< 31096, 31352, 31608, 31864, 32120, 27888, 28144, 28400, 28656, 28912, 29168, 29424, 29680, 30456, 30712, 30968, 31224, 31480, 31736, 31992, 32248}}; - static constexpr uint8_t DUAL_RETURN_FLAG = 0x00; - static constexpr uint8_t STRONGEST_RETURN_FLAG = 0x01; - static constexpr uint8_t LAST_RETURN_FLAG = 0x02; + static constexpr uint8_t dual_return_flag = 0x00; + static constexpr uint8_t strongest_return_flag = 0x01; + static constexpr uint8_t last_return_flag = 0x02; - static constexpr uint8_t SYNC_MODE_GPS_FLAG = 0x00; - static constexpr uint8_t SYNC_MODE_E2E_FLAG = 0x01; - static constexpr uint8_t SYNC_MODE_P2P_FLAG = 0x02; - static constexpr uint8_t SYNC_MODE_GPTP_FLAG = 0x03; + static constexpr uint8_t sync_mode_gps_flag = 0x00; + static constexpr uint8_t sync_mode_e2_e_flag = 0x01; + static constexpr uint8_t sync_mode_p2_p_flag = 0x02; + static constexpr uint8_t sync_mode_gptp_flag = 0x03; - static constexpr uint8_t SYNC_STATUS_INVALID_FLAG = 0x00; - static constexpr uint8_t SYNC_STATUS_GPS_SUCCESS_FLAG = 0x01; - static constexpr uint8_t SYNC_STATUS_PTP_SUCCESS_FLAG = 0x02; + static constexpr uint8_t sync_status_invalid_flag = 0x00; + static constexpr uint8_t sync_status_gps_success_flag = 0x01; + static constexpr uint8_t sync_status_ptp_success_flag = 0x02; public: - static constexpr float MIN_RANGE = 0.1f; - static constexpr float MAX_RANGE = 30.f; - static constexpr size_t MAX_SCAN_BUFFER_POINTS = 1152000; + static constexpr float min_range = 0.1f; + static constexpr float max_range = 30.f; + static constexpr size_t max_scan_buffer_points = 1152000; - int getPacketRelativePointTimeOffset( + int get_packet_relative_point_time_offset( const uint32_t block_id, const uint32_t channel_id, const std::shared_ptr & sensor_configuration) override { if (sensor_configuration->return_mode == ReturnMode::DUAL) - return firing_time_offset_ns_dual_[block_id][channel_id]; + return firing_time_offset_ns_dual[block_id][channel_id]; else - return firing_time_offset_ns_single_[block_id][channel_id]; + return firing_time_offset_ns_single[block_id][channel_id]; } - ReturnMode getReturnMode(const robosense_packet::bpearl_v3::InfoPacket & info_packet) override + ReturnMode get_return_mode(const robosense_packet::bpearl_v3::InfoPacket & info_packet) override { switch (info_packet.return_mode.value()) { - case DUAL_RETURN_FLAG: + case dual_return_flag: return ReturnMode::DUAL; - case STRONGEST_RETURN_FLAG: + case strongest_return_flag: return ReturnMode::SINGLE_STRONGEST; - case LAST_RETURN_FLAG: + case last_return_flag: return ReturnMode::SINGLE_LAST; default: return ReturnMode::UNKNOWN; } } - RobosenseCalibrationConfiguration getSensorCalibration( + RobosenseCalibrationConfiguration get_sensor_calibration( const robosense_packet::bpearl_v3::InfoPacket & info_packet) override { - return info_packet.sensor_calibration.getCalibration(); + return info_packet.sensor_calibration.get_calibration(); } - bool getSyncStatus(const robosense_packet::bpearl_v3::InfoPacket & info_packet) override + bool get_sync_status(const robosense_packet::bpearl_v3::InfoPacket & info_packet) override { switch (info_packet.sync_status.value()) { - case SYNC_STATUS_INVALID_FLAG: + case sync_status_invalid_flag: return false; - case SYNC_STATUS_GPS_SUCCESS_FLAG: + case sync_status_gps_success_flag: return true; - case SYNC_STATUS_PTP_SUCCESS_FLAG: + case sync_status_ptp_success_flag: return true; default: return false; } } - std::map getSensorInfo( + std::map get_sensor_info( const robosense_packet::bpearl_v3::InfoPacket & info_packet) override { std::map sensor_info; @@ -326,13 +326,13 @@ class BpearlV3 : public RobosenseSensor< sensor_info["zero_angle_offset"] = std::to_string(info_packet.zero_angle_offset.value()); switch (info_packet.return_mode.value()) { - case DUAL_RETURN_FLAG: + case dual_return_flag: sensor_info["return_mode"] = "dual"; break; - case STRONGEST_RETURN_FLAG: + case strongest_return_flag: sensor_info["return_mode"] = "strongest"; break; - case LAST_RETURN_FLAG: + case last_return_flag: sensor_info["return_mode"] = "last"; break; default: @@ -341,16 +341,16 @@ class BpearlV3 : public RobosenseSensor< } switch (info_packet.time_sync_mode.value()) { - case SYNC_MODE_GPS_FLAG: + case sync_mode_gps_flag: sensor_info["time_sync_mode"] = "gps"; break; - case SYNC_MODE_E2E_FLAG: + case sync_mode_e2_e_flag: sensor_info["time_sync_mode"] = "e2e"; break; - case SYNC_MODE_P2P_FLAG: + case sync_mode_p2_p_flag: sensor_info["time_sync_mode"] = "p2p"; break; - case SYNC_MODE_GPTP_FLAG: + case sync_mode_gptp_flag: sensor_info["time_sync_mode"] = "gptp"; break; default: @@ -359,13 +359,13 @@ class BpearlV3 : public RobosenseSensor< } switch (info_packet.sync_status.value()) { - case SYNC_STATUS_INVALID_FLAG: + case sync_status_invalid_flag: sensor_info["sync_status"] = "time_sync_invalid"; break; - case SYNC_STATUS_GPS_SUCCESS_FLAG: + case sync_status_gps_success_flag: sensor_info["sync_status"] = "gps_time_sync_successful"; break; - case SYNC_STATUS_PTP_SUCCESS_FLAG: + case sync_status_ptp_success_flag: sensor_info["sync_status"] = "ptp_time_sync_successful"; break; default: diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v4.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v4.hpp index 231fe9e64..fa99806ef 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v4.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/bpearl_v4.hpp @@ -53,7 +53,7 @@ struct Header struct Packet : public PacketBase<12, 32, 2, 100> { - typedef Body, Packet::N_BLOCKS> body_t; + typedef Body, Packet::n_blocks> body_t; Header header; body_t body; big_uint48_buf_t tail; @@ -129,7 +129,7 @@ class BpearlV4 : public RobosenseSensor< robosense_packet::bpearl_v4::Packet, robosense_packet::bpearl_v4::InfoPacket> { private: - static constexpr int firing_time_offset_ns_single_[12][32] = { + static constexpr int firing_time_offset_ns_single[12][32] = { {0, 167, 334, 500, 667, 834, 1001, 1168, 1334, 1501, 1668, 1835, 2002, 2168, 2335, 2502, 2669, 2836, 3002, 3169, 3336, 3503, 3670, 3836, 4003, 4170, 4337, 4504, 4670, 4837, 5004, 5171}, @@ -167,7 +167,7 @@ class BpearlV4 : public RobosenseSensor< 62940, 63107, 63273, 63440, 63607, 63774, 63941, 64107, 64274, 64441, 64608, 64775, 64941, 65108, 65275, 65442, 65609, 65775, 65942, 66109, 66276}}; - static constexpr int firing_time_offset_ns_dual_[12][32]{ + static constexpr int firing_time_offset_ns_dual[12][32]{ {0, 167, 334, 500, 667, 834, 1001, 1168, 1334, 1501, 1668, 1835, 2002, 2168, 2335, 2502, 2669, 2836, 3002, 3169, 3336, 3503, 3670, 3836, 4003, 4170, 4337, 4504, 4670, 4837, 5004, 5171}, @@ -205,72 +205,72 @@ class BpearlV4 : public RobosenseSensor< 29610, 29777, 29943, 30110, 30277, 30444, 30611, 30777, 30944, 31111, 31278, 31445, 31611, 31778, 31945, 32112, 32279, 32445, 32612, 32779, 32946}}; - static constexpr uint8_t DUAL_RETURN_FLAG = 0x00; - static constexpr uint8_t STRONGEST_RETURN_FLAG = 0x04; - static constexpr uint8_t LAST_RETURN_FLAG = 0x05; - static constexpr uint8_t FIRST_RETURN_FLAG = 0x06; + static constexpr uint8_t dual_return_flag = 0x00; + static constexpr uint8_t strongest_return_flag = 0x04; + static constexpr uint8_t last_return_flag = 0x05; + static constexpr uint8_t first_return_flag = 0x06; - static constexpr uint8_t SYNC_MODE_GPS_FLAG = 0x00; - static constexpr uint8_t SYNC_MODE_E2E_FLAG = 0x01; - static constexpr uint8_t SYNC_MODE_P2P_FLAG = 0x02; - static constexpr uint8_t SYNC_MODE_GPTP_FLAG = 0x03; + static constexpr uint8_t sync_mode_gps_flag = 0x00; + static constexpr uint8_t sync_mode_e2e_flag = 0x01; + static constexpr uint8_t sync_mode_p2p_flag = 0x02; + static constexpr uint8_t sync_mode_gptp_flag = 0x03; - static constexpr uint8_t SYNC_STATUS_INVALID_FLAG = 0x00; - static constexpr uint8_t SYNC_STATUS_GPS_SUCCESS_FLAG = 0x01; - static constexpr uint8_t SYNC_STATUS_PTP_SUCCESS_FLAG = 0x02; + static constexpr uint8_t sync_status_invalid_flag = 0x00; + static constexpr uint8_t sync_status_gps_success_flag = 0x01; + static constexpr uint8_t sync_status_ptp_success_flag = 0x02; public: - static constexpr float MIN_RANGE = 0.1f; - static constexpr float MAX_RANGE = 30.f; - static constexpr size_t MAX_SCAN_BUFFER_POINTS = 1152000; + static constexpr float min_range = 0.1f; + static constexpr float max_range = 30.f; + static constexpr size_t max_scan_buffer_points = 1152000; - int getPacketRelativePointTimeOffset( + int get_packet_relative_point_time_offset( const uint32_t block_id, const uint32_t channel_id, const std::shared_ptr & sensor_configuration) override { if (sensor_configuration->return_mode == ReturnMode::DUAL) - return firing_time_offset_ns_dual_[block_id][channel_id]; + return firing_time_offset_ns_dual[block_id][channel_id]; else - return firing_time_offset_ns_single_[block_id][channel_id]; + return firing_time_offset_ns_single[block_id][channel_id]; } - ReturnMode getReturnMode(const robosense_packet::bpearl_v4::InfoPacket & info_packet) override + ReturnMode get_return_mode(const robosense_packet::bpearl_v4::InfoPacket & info_packet) override { switch (info_packet.return_mode.value()) { - case DUAL_RETURN_FLAG: + case dual_return_flag: return ReturnMode::DUAL; - case STRONGEST_RETURN_FLAG: + case strongest_return_flag: return ReturnMode::SINGLE_STRONGEST; - case LAST_RETURN_FLAG: + case last_return_flag: return ReturnMode::SINGLE_LAST; - case FIRST_RETURN_FLAG: + case first_return_flag: return ReturnMode::SINGLE_FIRST; default: return ReturnMode::UNKNOWN; } } - RobosenseCalibrationConfiguration getSensorCalibration( + RobosenseCalibrationConfiguration get_sensor_calibration( const robosense_packet::bpearl_v4::InfoPacket & info_packet) override { - return info_packet.sensor_calibration.getCalibration(); + return info_packet.sensor_calibration.get_calibration(); } - bool getSyncStatus(const robosense_packet::bpearl_v4::InfoPacket & info_packet) override + bool get_sync_status(const robosense_packet::bpearl_v4::InfoPacket & info_packet) override { switch (info_packet.time_sync_state.value()) { - case SYNC_STATUS_INVALID_FLAG: + case sync_status_invalid_flag: return false; - case SYNC_STATUS_GPS_SUCCESS_FLAG: + case sync_status_gps_success_flag: return true; - case SYNC_STATUS_PTP_SUCCESS_FLAG: + case sync_status_ptp_success_flag: return true; default: return false; } } - std::map getSensorInfo( + std::map get_sensor_info( const robosense_packet::bpearl_v4::InfoPacket & info_packet) override { std::map sensor_info; @@ -296,16 +296,16 @@ class BpearlV4 : public RobosenseSensor< sensor_info["serial_number"] = info_packet.serial_number.to_string(); switch (info_packet.return_mode.value()) { - case DUAL_RETURN_FLAG: + case dual_return_flag: sensor_info["return_mode"] = "dual"; break; - case STRONGEST_RETURN_FLAG: + case strongest_return_flag: sensor_info["return_mode"] = "strongest"; break; - case LAST_RETURN_FLAG: + case last_return_flag: sensor_info["return_mode"] = "last"; break; - case FIRST_RETURN_FLAG: + case first_return_flag: sensor_info["return_mode"] = "first"; break; default: @@ -314,16 +314,16 @@ class BpearlV4 : public RobosenseSensor< } switch (info_packet.time_sync_mode.value()) { - case SYNC_MODE_GPS_FLAG: + case sync_mode_gps_flag: sensor_info["time_sync_mode"] = "gps"; break; - case SYNC_MODE_E2E_FLAG: + case sync_mode_e2e_flag: sensor_info["time_sync_mode"] = "e2e"; break; - case SYNC_MODE_P2P_FLAG: + case sync_mode_p2p_flag: sensor_info["time_sync_mode"] = "p2p"; break; - case SYNC_MODE_GPTP_FLAG: + case sync_mode_gptp_flag: sensor_info["time_sync_mode"] = "gptp"; break; default: @@ -332,13 +332,13 @@ class BpearlV4 : public RobosenseSensor< } switch (info_packet.time_sync_state.value()) { - case SYNC_STATUS_INVALID_FLAG: + case sync_status_invalid_flag: sensor_info["time_sync_state"] = "time_sync_invalid"; break; - case SYNC_STATUS_GPS_SUCCESS_FLAG: + case sync_status_gps_success_flag: sensor_info["time_sync_state"] = "gps_time_sync_successful"; break; - case SYNC_STATUS_PTP_SUCCESS_FLAG: + case sync_status_ptp_success_flag: sensor_info["time_sync_state"] = "ptp_time_sync_successful"; break; default: diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/helios.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/helios.hpp index 5da0320e5..e1b2ecb2c 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/helios.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/helios.hpp @@ -57,7 +57,7 @@ struct Header struct Packet : public PacketBase<12, 32, 2, 100> { - typedef Body, Packet::N_BLOCKS> body_t; + typedef Body, Packet::n_blocks> body_t; Header header; body_t body; big_uint48_buf_t tail; @@ -168,7 +168,7 @@ class Helios : public RobosenseSensor { private: - static constexpr int firing_time_offset_ns_single_[12][32] = { + static constexpr int firing_time_offset_ns_single[12][32] = { {0, 1570, 3150, 4720, 6300, 7870, 9450, 11360, 13260, 15170, 17080, 18990, 20560, 22140, 23710, 25290, 26530, 29010, 27770, 30250, 31490, 32730, 33980, 35220, 36460, 37700, 38940, 40180, 41420, 42670, 43910, 45150}, @@ -206,7 +206,7 @@ class Helios 630100, 631670, 633250, 634820, 636400, 637640, 640120, 638880, 641360, 642600, 645090, 643850, 646330, 647570, 648810, 650050, 651290, 652540, 653780, 655020, 656260}}; - static constexpr int firing_time_offset_ns_dual_[12][32]{ + static constexpr int firing_time_offset_ns_dual[12][32]{ {0, 1570, 3150, 4720, 6300, 7870, 9450, 11360, 13260, 15170, 17080, 18990, 20560, 22140, 23710, 25290, 26530, 27770, 29010, 30250, 31490, 32730, 33980, 35220, 36460, 37700, 38940, 40180, 41420, 42670, 43910, 45150}, @@ -244,72 +244,72 @@ class Helios 296770, 298340, 299920, 301490, 303060, 304310, 305550, 306790, 308030, 309270, 310510, 311750, 313000, 314240, 315480, 316720, 317960, 319200, 320440, 321680, 322930}}; - static constexpr uint8_t DUAL_RETURN_FLAG = 0x00; - static constexpr uint8_t STRONGEST_RETURN_FLAG = 0x04; - static constexpr uint8_t LAST_RETURN_FLAG = 0x05; - static constexpr uint8_t FIRST_RETURN_FLAG = 0x06; + static constexpr uint8_t dual_return_flag = 0x00; + static constexpr uint8_t strongest_return_flag = 0x04; + static constexpr uint8_t last_return_flag = 0x05; + static constexpr uint8_t first_return_flag = 0x06; - static constexpr uint8_t SYNC_MODE_GPS_FLAG = 0x00; - static constexpr uint8_t SYNC_MODE_E2E_FLAG = 0x01; - static constexpr uint8_t SYNC_MODE_P2P_FLAG = 0x02; - static constexpr uint8_t SYNC_MODE_GPTP_FLAG = 0x03; + static constexpr uint8_t sync_mode_gps_flag = 0x00; + static constexpr uint8_t sync_mode_e2e_flag = 0x01; + static constexpr uint8_t sync_mode_p2p_flag = 0x02; + static constexpr uint8_t sync_mode_gptp_flag = 0x03; - static constexpr uint8_t SYNC_STATUS_INVALID_FLAG = 0x00; - static constexpr uint8_t SYNC_STATUS_GPS_SUCCESS_FLAG = 0x01; - static constexpr uint8_t SYNC_STATUS_PTP_SUCCESS_FLAG = 0x02; + static constexpr uint8_t sync_status_invalid_flag = 0x00; + static constexpr uint8_t sync_status_gps_success_flag = 0x01; + static constexpr uint8_t sync_status_ptp_success_flag = 0x02; public: - static constexpr float MIN_RANGE = 0.2f; - static constexpr float MAX_RANGE = 150.f; - static constexpr size_t MAX_SCAN_BUFFER_POINTS = 1152000; + static constexpr float min_range = 0.2f; + static constexpr float max_range = 150.f; + static constexpr size_t max_scan_buffer_points = 1152000; - int getPacketRelativePointTimeOffset( + int get_packet_relative_point_time_offset( const uint32_t block_id, const uint32_t channel_id, const std::shared_ptr & sensor_configuration) override { if (sensor_configuration->return_mode == ReturnMode::DUAL) - return firing_time_offset_ns_dual_[block_id][channel_id]; + return firing_time_offset_ns_dual[block_id][channel_id]; else - return firing_time_offset_ns_single_[block_id][channel_id]; + return firing_time_offset_ns_single[block_id][channel_id]; } - ReturnMode getReturnMode(const robosense_packet::helios::InfoPacket & info_packet) override + ReturnMode get_return_mode(const robosense_packet::helios::InfoPacket & info_packet) override { switch (info_packet.return_mode.value()) { - case DUAL_RETURN_FLAG: + case dual_return_flag: return ReturnMode::DUAL; - case STRONGEST_RETURN_FLAG: + case strongest_return_flag: return ReturnMode::SINGLE_STRONGEST; - case LAST_RETURN_FLAG: + case last_return_flag: return ReturnMode::SINGLE_LAST; - case FIRST_RETURN_FLAG: + case first_return_flag: return ReturnMode::SINGLE_FIRST; default: return ReturnMode::UNKNOWN; } } - RobosenseCalibrationConfiguration getSensorCalibration( + RobosenseCalibrationConfiguration get_sensor_calibration( const robosense_packet::helios::InfoPacket & info_packet) override { - return info_packet.sensor_calibration.getCalibration(); + return info_packet.sensor_calibration.get_calibration(); } - bool getSyncStatus(const robosense_packet::helios::InfoPacket & info_packet) override + bool get_sync_status(const robosense_packet::helios::InfoPacket & info_packet) override { switch (info_packet.sync_status.value()) { - case SYNC_STATUS_INVALID_FLAG: + case sync_status_invalid_flag: return false; - case SYNC_STATUS_GPS_SUCCESS_FLAG: + case sync_status_gps_success_flag: return true; - case SYNC_STATUS_PTP_SUCCESS_FLAG: + case sync_status_ptp_success_flag: return true; default: return false; } } - std::map getSensorInfo( + std::map get_sensor_info( const robosense_packet::helios::InfoPacket & info_packet) override { std::map sensor_info; @@ -346,16 +346,16 @@ class Helios sensor_info["zero_angle_offset"] = std::to_string(info_packet.zero_angle_offset.value()); switch (info_packet.return_mode.value()) { - case DUAL_RETURN_FLAG: + case dual_return_flag: sensor_info["return_mode"] = "dual"; break; - case STRONGEST_RETURN_FLAG: + case strongest_return_flag: sensor_info["return_mode"] = "strongest"; break; - case LAST_RETURN_FLAG: + case last_return_flag: sensor_info["return_mode"] = "last"; break; - case FIRST_RETURN_FLAG: + case first_return_flag: sensor_info["return_mode"] = "first"; break; default: @@ -364,16 +364,16 @@ class Helios } switch (info_packet.time_sync_mode.value()) { - case SYNC_MODE_GPS_FLAG: + case sync_mode_gps_flag: sensor_info["time_sync_mode"] = "gps"; break; - case SYNC_MODE_E2E_FLAG: + case sync_mode_e2e_flag: sensor_info["time_sync_mode"] = "e2e"; break; - case SYNC_MODE_P2P_FLAG: + case sync_mode_p2p_flag: sensor_info["time_sync_mode"] = "p2p"; break; - case SYNC_MODE_GPTP_FLAG: + case sync_mode_gptp_flag: sensor_info["time_sync_mode"] = "gptp"; break; default: @@ -382,13 +382,13 @@ class Helios } switch (info_packet.sync_status.value()) { - case SYNC_STATUS_INVALID_FLAG: + case sync_status_invalid_flag: sensor_info["sync_status"] = "time_sync_invalid"; break; - case SYNC_STATUS_GPS_SUCCESS_FLAG: + case sync_status_gps_success_flag: sensor_info["sync_status"] = "gps_time_sync_successful"; break; - case SYNC_STATUS_PTP_SUCCESS_FLAG: + case sync_status_ptp_success_flag: sensor_info["sync_status"] = "ptp_time_sync_successful"; break; default: diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_decoder.hpp index 48bf77eb2..869d24797 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_decoder.hpp @@ -63,7 +63,7 @@ class RobosenseDecoder : public RobosenseScanDecoder /// @brief Validates and parses MsopPacket. Currently only checks size, not checksums etc. /// @param msop_packet The incoming MsopPacket /// @return Whether the packet was parsed successfully - bool parsePacket(const std::vector & msop_packet) + bool parse_packet(const std::vector & msop_packet) { if (msop_packet.size() < sizeof(typename SensorT::packet_t)) { RCLCPP_ERROR_STREAM( @@ -83,14 +83,14 @@ class RobosenseDecoder : public RobosenseScanDecoder /// points and appends them to the point cloud /// @param start_block_id The first block in the group of returns /// @param n_blocks The number of returns in the group - void convertReturns(size_t start_block_id, size_t n_blocks) + void convert_returns(size_t start_block_id, size_t n_blocks) { uint64_t packet_timestamp_ns = robosense_packet::get_timestamp_ns(packet_); uint32_t raw_azimuth = packet_.body.blocks[start_block_id].get_azimuth(); std::vector return_units; - for (size_t channel_id = 0; channel_id < SensorT::packet_t::N_CHANNELS; ++channel_id) { + for (size_t channel_id = 0; channel_id < SensorT::packet_t::n_channels; ++channel_id) { // Find the units corresponding to the same return group as the current one. // These are used to find duplicates in multi-return mode. return_units.clear(); @@ -106,14 +106,14 @@ class RobosenseDecoder : public RobosenseScanDecoder continue; } - auto distance = getDistance(unit); + auto distance = get_distance(unit); - if (distance < SensorT::MIN_RANGE || distance > SensorT::MAX_RANGE) { + if (distance < SensorT::min_range || distance > SensorT::max_range) { continue; } auto return_type = - sensor_.getReturnType(sensor_configuration_->return_mode, block_offset, return_units); + sensor_.get_return_type(sensor_configuration_->return_mode, block_offset, return_units); // Keep only last of multiple identical points if (return_type == ReturnType::IDENTICAL && block_offset != n_blocks - 1) { @@ -130,7 +130,7 @@ class RobosenseDecoder : public RobosenseScanDecoder } if ( - fabsf(getDistance(*return_units[return_idx]) - distance) < + fabsf(get_distance(*return_units[return_idx]) - distance) < sensor_configuration_->dual_return_distance_threshold) { is_below_multi_return_threshold = true; break; @@ -146,11 +146,12 @@ class RobosenseDecoder : public RobosenseScanDecoder point.distance = distance; point.intensity = unit.reflectivity.value(); point.time_stamp = - getPointTimeRelative(packet_timestamp_ns, block_offset + start_block_id, channel_id); + get_point_time_relative(packet_timestamp_ns, block_offset + start_block_id, channel_id); point.return_type = static_cast(return_type); - auto corrected_angle_data = angle_corrector_.getCorrectedAngleData(raw_azimuth, channel_id); + auto corrected_angle_data = + angle_corrector_.get_corrected_angle_data(raw_azimuth, channel_id); point.channel = corrected_angle_data.corrected_channel_id; // The raw_azimuth and channel are only used as indices, sin/cos functions use the precise @@ -172,15 +173,15 @@ class RobosenseDecoder : public RobosenseScanDecoder /// @brief Checks whether the last processed block was the last block of a scan /// @param current_phase The azimuth of the last processed block /// @return Whether the scan has completed - bool checkScanCompleted(int current_phase) + bool check_scan_completed(int current_phase) { - return angle_corrector_.hasScanned(current_phase, last_phase_); + return angle_corrector_.has_scanned(current_phase, last_phase_); } /// @brief Get the distance of the given unit in meters /// @param unit The unit to get the distance from /// @return The distance in meters - float getDistance(const typename SensorT::packet_t::body_t::block_t::unit_t & unit) + float get_distance(const typename SensorT::packet_t::body_t::block_t::unit_t & unit) { return unit.distance.value() * robosense_packet::get_dis_unit(packet_); } @@ -190,10 +191,10 @@ class RobosenseDecoder : public RobosenseScanDecoder /// @param packet_timestamp_ns The timestamp of the current MsopPacket in nanoseconds /// @param block_id The block index of the point /// @param channel_id The channel index of the point - uint32_t getPointTimeRelative(uint64_t packet_timestamp_ns, size_t block_id, size_t channel_id) + uint32_t get_point_time_relative(uint64_t packet_timestamp_ns, size_t block_id, size_t channel_id) { auto point_to_packet_offset_ns = - sensor_.getPacketRelativePointTimeOffset(block_id, channel_id, sensor_configuration_); + sensor_.get_packet_relative_point_time_offset(block_id, channel_id, sensor_configuration_); auto packet_to_scan_offset_ns = static_cast(packet_timestamp_ns - decode_scan_timestamp_ns_); return packet_to_scan_offset_ns + point_to_packet_offset_ns; @@ -217,13 +218,13 @@ class RobosenseDecoder : public RobosenseScanDecoder decode_pc_.reset(new NebulaPointCloud); output_pc_.reset(new NebulaPointCloud); - decode_pc_->reserve(SensorT::MAX_SCAN_BUFFER_POINTS); - output_pc_->reserve(SensorT::MAX_SCAN_BUFFER_POINTS); + decode_pc_->reserve(SensorT::max_scan_buffer_points); + output_pc_->reserve(SensorT::max_scan_buffer_points); } int unpack(const std::vector & msop_packet) override { - if (!parsePacket(msop_packet)) { + if (!parse_packet(msop_packet)) { return -1; } @@ -241,15 +242,15 @@ class RobosenseDecoder : public RobosenseScanDecoder const size_t n_returns = robosense_packet::get_n_returns(sensor_configuration_->return_mode); int current_azimuth; - for (size_t block_id = 0; block_id < SensorT::packet_t::N_BLOCKS; block_id += n_returns) { + for (size_t block_id = 0; block_id < SensorT::packet_t::n_blocks; block_id += n_returns) { current_azimuth = - (360 * SensorT::packet_t::DEGREE_SUBDIVISIONS + + (360 * SensorT::packet_t::degree_subdivisions + packet_.body.blocks[block_id].get_azimuth() - static_cast( - sensor_configuration_->scan_phase * SensorT::packet_t::DEGREE_SUBDIVISIONS)) % - (360 * SensorT::packet_t::DEGREE_SUBDIVISIONS); + sensor_configuration_->scan_phase * SensorT::packet_t::degree_subdivisions)) % + (360 * SensorT::packet_t::degree_subdivisions); - bool scan_completed = checkScanCompleted(current_azimuth); + bool scan_completed = check_scan_completed(current_azimuth); if (scan_completed) { std::swap(decode_pc_, output_pc_); decode_pc_->clear(); @@ -261,19 +262,19 @@ class RobosenseDecoder : public RobosenseScanDecoder // remainder of the packet decode_scan_timestamp_ns_ = robosense_packet::get_timestamp_ns(packet_) + - sensor_.getEarliestPointTimeOffsetForBlock(block_id, sensor_configuration_); + sensor_.get_earliest_point_time_offset_for_block(block_id, sensor_configuration_); } - convertReturns(block_id, n_returns); + convert_returns(block_id, n_returns); last_phase_ = current_azimuth; } return last_phase_; } - bool hasScanned() override { return has_scanned_; } + bool has_scanned() override { return has_scanned_; } - std::tuple getPointcloud() override + std::tuple get_pointcloud() override { double scan_timestamp_s = static_cast(output_scan_timestamp_ns_) * 1e-9; return std::make_pair(output_pc_, scan_timestamp_s); diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder.hpp index 02a00792c..db3b4db79 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder.hpp @@ -45,7 +45,7 @@ class RobosenseInfoDecoder : public RobosenseInfoDecoderBase /// @brief Validates and parses DIFOP packet. Currently only checks size, not checksums etc. /// @param raw_packet The incoming DIFOP packet /// @return Whether the packet was parsed successfully - bool parsePacket(const std::vector & raw_packet) override + bool parse_packet(const std::vector & raw_packet) override { const auto packet_size = raw_packet.size(); if (packet_size < sizeof(typename SensorT::info_t)) { @@ -73,25 +73,25 @@ class RobosenseInfoDecoder : public RobosenseInfoDecoderBase /// @brief Get the sensor telemetry /// @return The sensor telemetry - std::map getSensorInfo() override + std::map get_sensor_info() override { - return sensor_.getSensorInfo(packet_); + return sensor_.get_sensor_info(packet_); } /// @brief Get the laser return mode /// @return The laser return mode - ReturnMode getReturnMode() override { return sensor_.getReturnMode(packet_); } + ReturnMode get_return_mode() override { return sensor_.get_return_mode(packet_); } /// @brief Get sensor calibration /// @return The sensor calibration - RobosenseCalibrationConfiguration getSensorCalibration() override + RobosenseCalibrationConfiguration get_sensor_calibration() override { - return sensor_.getSensorCalibration(packet_); + return sensor_.get_sensor_calibration(packet_); } /// @brief Get the status of time synchronization /// @return True if the sensor's clock is synchronized - bool getSyncStatus() override { return sensor_.getSyncStatus(packet_); } + bool get_sync_status() override { return sensor_.get_sync_status(packet_); } }; } // namespace drivers diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder_base.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder_base.hpp index 87d8279f0..7ffc48b0e 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder_base.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_info_decoder_base.hpp @@ -39,23 +39,23 @@ class RobosenseInfoDecoderBase /// @brief Parses DIFOP and add its telemetry /// @param raw_packet The incoming DIFOP packet /// @return Whether the packet was parsed successfully - virtual bool parsePacket(const std::vector & raw_packet) = 0; + virtual bool parse_packet(const std::vector & raw_packet) = 0; /// @brief Get the sensor telemetry /// @return The sensor telemetry - virtual std::map getSensorInfo() = 0; + virtual std::map get_sensor_info() = 0; /// @brief Get the laser return mode /// @return The laser return mode - virtual ReturnMode getReturnMode() = 0; + virtual ReturnMode get_return_mode() = 0; /// @brief Get sensor calibration /// @return The sensor calibration - virtual RobosenseCalibrationConfiguration getSensorCalibration() = 0; + virtual RobosenseCalibrationConfiguration get_sensor_calibration() = 0; /// @brief Get the status of time synchronization /// @return True if the sensor's clock is synchronized - virtual bool getSyncStatus() = 0; + virtual bool get_sync_status() = 0; }; } // namespace drivers diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_packet.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_packet.hpp index b4a7d3061..b15b34eb0 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_packet.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_packet.hpp @@ -42,11 +42,11 @@ struct Timestamp uint64_t get_time_in_ns() const { - constexpr uint64_t NS_IN_SECOND = 1000000000ULL; - constexpr uint64_t NS_IN_MICROSECOND = 1000ULL; + constexpr uint64_t ns_in_second = 1000000000ULL; + constexpr uint64_t ns_in_microsecond = 1000ULL; - uint64_t total_nanoseconds = seconds.value() * NS_IN_SECOND; - total_nanoseconds += microseconds.value() * NS_IN_MICROSECOND; + uint64_t total_nanoseconds = seconds.value() * ns_in_second; + total_nanoseconds += microseconds.value() * ns_in_microsecond; return total_nanoseconds; } @@ -86,10 +86,10 @@ struct Body template struct PacketBase { - static constexpr size_t N_BLOCKS = nBlocks; - static constexpr size_t N_CHANNELS = nChannels; - static constexpr size_t MAX_RETURNS = maxReturns; - static constexpr size_t DEGREE_SUBDIVISIONS = degreeSubdivisions; + static constexpr size_t n_blocks = nBlocks; + static constexpr size_t n_channels = nChannels; + static constexpr size_t max_returns = maxReturns; + static constexpr size_t degree_subdivisions = degreeSubdivisions; }; struct IpAddress @@ -145,16 +145,16 @@ struct FovSetting big_uint16_buf_t fov_end; }; -constexpr uint8_t ANGLE_SIGN_FLAG = 0x00; +constexpr uint8_t angle_sign_flag = 0x00; struct ChannelAngleCorrection { big_uint8_buf_t sign; big_uint16_buf_t angle; - [[nodiscard]] float getAngle() const + [[nodiscard]] float get_angle() const { - return sign.value() == ANGLE_SIGN_FLAG ? static_cast(angle.value()) / 100.0f + return sign.value() == angle_sign_flag ? static_cast(angle.value()) / 100.0f : static_cast(angle.value()) / -100.0f; } }; @@ -174,13 +174,13 @@ struct SensorCalibration CorrectedVerticalAngle corrected_vertical_angle; CorrectedHorizontalAngle corrected_horizontal_angle; - RobosenseCalibrationConfiguration getCalibration() const + RobosenseCalibrationConfiguration get_calibration() const { RobosenseCalibrationConfiguration calibration; for (size_t i = 0; i < 32; ++i) { ChannelCorrection channel_correction; - channel_correction.azimuth = corrected_horizontal_angle.angles[i].getAngle(); - channel_correction.elevation = corrected_vertical_angle.angles[i].getAngle(); + channel_correction.azimuth = corrected_horizontal_angle.angles[i].get_angle(); + channel_correction.elevation = corrected_vertical_angle.angles[i].get_angle(); calibration.calibration.push_back(channel_correction); } return calibration; diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_scan_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_scan_decoder.hpp index b87f4f078..1af28156d 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_scan_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_scan_decoder.hpp @@ -42,11 +42,11 @@ class RobosenseScanDecoder /// @brief Indicates whether one full scan is ready /// @return Whether a scan is ready - virtual bool hasScanned() = 0; + virtual bool has_scanned() = 0; /// @brief Returns the point cloud and timestamp of the last scan /// @return A tuple of point cloud and timestamp in nanoseconds - virtual std::tuple getPointcloud() = 0; + virtual std::tuple get_pointcloud() = 0; }; } // namespace drivers diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_sensor.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_sensor.hpp index b88e49181..438bd2a9f 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_sensor.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/robosense_sensor.hpp @@ -38,7 +38,7 @@ class RobosenseSensor public: typedef PacketT packet_t; typedef InfoPacketT info_t; - typedef class AngleCorrectorCalibrationBased + typedef class AngleCorrectorCalibrationBased angle_corrector_t; RobosenseSensor() = default; @@ -50,7 +50,7 @@ class RobosenseSensor /// @param channel_id The point's channel id /// @param sensor_configuration The sensor configuration /// @return The relative time offset in nanoseconds - virtual int getPacketRelativePointTimeOffset( + virtual int get_packet_relative_point_time_offset( uint32_t block_id, uint32_t channel_id, const std::shared_ptr & sensor_configuration) = 0; @@ -60,7 +60,7 @@ class RobosenseSensor /// @param sensor_configuration The sensor configuration /// @return The lowest point time offset (relative to the packet timestamp) of any point in or /// after the start block, in nanoseconds - int getEarliestPointTimeOffsetForBlock( + int get_earliest_point_time_offset_for_block( uint32_t start_block_id, const std::shared_ptr & sensor_configuration) { @@ -68,10 +68,10 @@ class RobosenseSensor int min_offset_ns = std::numeric_limits::max(); for (uint32_t block_id = start_block_id; block_id < start_block_id + n_returns; ++block_id) { - for (uint32_t channel_id = 0; channel_id < PacketT::N_CHANNELS; ++channel_id) { + for (uint32_t channel_id = 0; channel_id < PacketT::n_channels; ++channel_id) { min_offset_ns = std::min( min_offset_ns, - getPacketRelativePointTimeOffset(block_id, channel_id, sensor_configuration)); + get_packet_relative_point_time_offset(block_id, channel_id, sensor_configuration)); } } @@ -111,7 +111,7 @@ class RobosenseSensor /// @param return_units The units corresponding to all the returns in the group. These are usually /// from the same column across adjascent blocks. /// @return The return type of the point - virtual ReturnType getReturnType( + virtual ReturnType get_return_type( ReturnMode return_mode, unsigned int return_idx, const std::vector & return_units) { @@ -137,13 +137,13 @@ class RobosenseSensor } } - virtual ReturnMode getReturnMode(const info_t & info_packet) = 0; + virtual ReturnMode get_return_mode(const info_t & info_packet) = 0; - virtual RobosenseCalibrationConfiguration getSensorCalibration(const info_t & info_packet) = 0; + virtual RobosenseCalibrationConfiguration get_sensor_calibration(const info_t & info_packet) = 0; - virtual bool getSyncStatus(const info_t & info_packet) = 0; + virtual bool get_sync_status(const info_t & info_packet) = 0; - virtual std::map getSensorInfo(const info_t & info_packet) = 0; + virtual std::map get_sensor_info(const info_t & info_packet) = 0; }; } // namespace drivers } // namespace nebula diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/robosense_driver.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/robosense_driver.hpp index 55ea8d31d..623e956e5 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/robosense_driver.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/robosense_driver.hpp @@ -54,18 +54,18 @@ class RobosenseDriver : NebulaDriverBase /// @brief Get current status of this driver /// @return Current status - Status GetStatus(); + Status get_status(); /// @brief Setting CalibrationConfiguration (not used) /// @param calibration_configuration /// @return Resulting status - Status SetCalibrationConfiguration( + Status set_calibration_configuration( const CalibrationConfigurationBase & calibration_configuration) override; /// @brief Convert RobosenseScan message to point cloud /// @param robosense_scan Message /// @return tuple of Point cloud and timestamp - std::tuple ParseCloudPacket( + std::tuple parse_cloud_packet( const std::vector & packet); }; diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/robosense_info_driver.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/robosense_info_driver.hpp index 859e97c4e..6fa649cc6 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/robosense_info_driver.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/robosense_info_driver.hpp @@ -52,19 +52,19 @@ class RobosenseInfoDriver /// @brief Get current status of this driver /// @return Current status - Status GetStatus(); + Status get_status(); - Status DecodeInfoPacket(const std::vector & packet); + Status decode_info_packet(const std::vector & packet); - std::map GetSensorInfo(); + std::map get_sensor_info(); - ReturnMode GetReturnMode(); + ReturnMode get_return_mode(); - RobosenseCalibrationConfiguration GetSensorCalibration(); + RobosenseCalibrationConfiguration get_sensor_calibration(); /// @brief Get the status of time synchronization /// @return True if the sensor's clock is synchronized - bool GetSyncStatus(); + bool get_sync_status(); }; } // namespace drivers diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp index 82cff5ac6..355309474 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp @@ -43,54 +43,54 @@ namespace drivers /** * Raw Velodyne packet constants and structures. */ -static const int SIZE_BLOCK = 100; -static const int RAW_SCAN_SIZE = 3; -static const int SCANS_PER_BLOCK = 32; -static const int BLOCK_DATA_SIZE = (SCANS_PER_BLOCK * RAW_SCAN_SIZE); +static const int g_size_block = 100; +static const int g_raw_scan_size = 3; +static const int g_scans_per_block = 32; +static const int g_block_data_size = (g_scans_per_block * g_raw_scan_size); -static const double ROTATION_RESOLUTION = 0.01; // [deg] -static const uint16_t ROTATION_MAX_UNITS = 36000u; // [deg/100] +static const double g_rotation_resolution = 0.01; // [deg] +static const uint16_t g_rotation_max_units = 36000u; // [deg/100] -static const size_t RETURN_MODE_INDEX = 1204; +static const size_t g_return_mode_index = 1204; /** @todo make this work for both big and little-endian machines */ -static const uint16_t UPPER_BANK = 0xeeff; -static const uint16_t LOWER_BANK = 0xddff; +static const uint16_t g_upper_bank = 0xeeff; +static const uint16_t g_lower_bank = 0xddff; /** Return Modes **/ -static const uint16_t RETURN_MODE_STRONGEST = 55; -static const uint16_t RETURN_MODE_LAST = 56; -static const uint16_t RETURN_MODE_DUAL = 57; +static const uint16_t g_return_mode_strongest = 55; +static const uint16_t g_return_mode_last = 56; +static const uint16_t g_return_mode_dual = 57; /** Special Defines for VLP16 support **/ -static const int VLP16_FIRINGS_PER_BLOCK = 2; -static const int VLP16_SCANS_PER_FIRING = 16; -static const float VLP16_BLOCK_DURATION = 110.592f; // [µs] -static const float VLP16_DSR_TOFFSET = 2.304f; // [µs] -static const float VLP16_FIRING_TOFFSET = 55.296f; // [µs] +static const int g_vlp16_firings_per_block = 2; +static const int g_vlp16_scans_per_firing = 16; +static const float g_vlp16_block_duration = 110.592f; // [µs] +static const float g_vlp16_dsr_toffset = 2.304f; // [µs] +static const float g_vlp16_firing_toffset = 55.296f; // [µs] /** Special Defines for VLP32 support **/ -static const float VLP32_CHANNEL_DURATION = 2.304f; // [µs] -static const float VLP32_SEQ_DURATION = 55.296f; // [µs] +static const float g_vlp32_channel_duration = 2.304f; // [µs] +static const float g_vlp32_seq_duration = 55.296f; // [µs] /** Special Definitions for VLS128 support **/ -static const float VLP128_DISTANCE_RESOLUTION = 0.004f; // [m] +static const float g_vlp128_distance_resolution = 0.004f; // [m] /** Special Definitions for VLS128 support **/ // These are used to detect which bank of 32 lasers is in this block -static const uint16_t VLS128_BANK_1 = 0xeeff; -static const uint16_t VLS128_BANK_2 = 0xddff; -static const uint16_t VLS128_BANK_3 = 0xccff; -static const uint16_t VLS128_BANK_4 = 0xbbff; +static const uint16_t g_vls128_bank_1 = 0xeeff; +static const uint16_t g_vls128_bank_2 = 0xddff; +static const uint16_t g_vls128_bank_3 = 0xccff; +static const uint16_t g_vls128_bank_4 = 0xbbff; -static const float VLS128_CHANNEL_DURATION = +static const float g_vls128_channel_duration = 2.665f; // [µs] Channels corresponds to one laser firing -static const float VLS128_SEQ_DURATION = +static const float g_vls128_seq_duration = 53.3f; // [µs] Sequence is a set of laser firings including recharging -static const size_t OFFSET_FIRST_AZIMUTH = 2; -static const size_t OFFSET_LAST_AZIMUTH = 1102; -static const uint32_t DEGREE_SUBDIVISIONS = 100; +static const size_t g_offset_first_azimuth = 2; +static const size_t g_offset_last_azimuth = 1102; +static const uint32_t g_degree_subdivisions = 100; /** \brief Raw Velodyne data block. * @@ -103,7 +103,7 @@ typedef struct raw_block { uint16_t header; ///< UPPER_BANK or LOWER_BANK uint16_t rotation; ///< 0-35999, divide by 100 to get degrees - uint8_t data[BLOCK_DATA_SIZE]; + uint8_t data[g_block_data_size]; } raw_block_t; /** used for unpacking the first two data bytes in a block @@ -116,10 +116,10 @@ union two_bytes { uint8_t bytes[2]; }; -static const int PACKET_SIZE = 1206; -static const int BLOCKS_PER_PACKET = 12; -static const int PACKET_STATUS_SIZE = 4; -static const int SCANS_PER_PACKET = (SCANS_PER_BLOCK * BLOCKS_PER_PACKET); +static const int g_packet_size = 1206; +static const int g_blocks_per_packet = 12; +static const int g_packet_status_size = 4; +static const int g_scans_per_packet = (g_scans_per_block * g_blocks_per_packet); /** \brief Raw Velodyne packet. * @@ -135,9 +135,9 @@ static const int SCANS_PER_PACKET = (SCANS_PER_BLOCK * BLOCKS_PER_PACKET); */ typedef struct raw_packet { - raw_block_t blocks[BLOCKS_PER_PACKET]; + raw_block_t blocks[g_blocks_per_packet]; uint16_t revolution; - uint8_t status[PACKET_STATUS_SIZE]; + uint8_t status[g_packet_status_size]; } raw_packet_t; /** \brief Velodyne echo types */ @@ -167,7 +167,7 @@ class VelodyneScanDecoder /// @param packet The packet buffer to extract azimuths from /// @param packet_seconds The packet's timestamp in seconds, including the sub-second part /// @param phase The sensor's scan phase used for scan cutting - void checkAndHandleScanComplete( + void check_and_handle_scan_complete( const std::vector & packet, double packet_seconds, const uint32_t phase) { if (has_scanned_) { @@ -178,13 +178,13 @@ class VelodyneScanDecoder has_scanned_ = false; processed_packets_++; - uint32_t packet_first_azm = packet[OFFSET_FIRST_AZIMUTH]; // lower word of azimuth block 0 - packet_first_azm |= packet[OFFSET_FIRST_AZIMUTH + 1] << 8; // higher word of azimuth block 0 + uint32_t packet_first_azm = packet[g_offset_first_azimuth]; // lower word of azimuth block 0 + packet_first_azm |= packet[g_offset_first_azimuth + 1] << 8; // higher word of azimuth block 0 - uint32_t packet_last_azm = packet[OFFSET_LAST_AZIMUTH]; - packet_last_azm |= packet[OFFSET_LAST_AZIMUTH + 1] << 8; + uint32_t packet_last_azm = packet[g_offset_last_azimuth]; + packet_last_azm |= packet[g_offset_last_azimuth + 1] << 8; - const uint32_t max_azi = 360 * DEGREE_SUBDIVISIONS; + const uint32_t max_azi = 360 * g_degree_subdivisions; uint32_t packet_first_azm_phased = (max_azi + packet_first_azm - phase) % max_azi; uint32_t packet_last_azm_phased = (max_azi + packet_last_azm - phase) % max_azi; @@ -225,15 +225,15 @@ class VelodyneScanDecoder /// @brief Virtual function for parsing VelodynePacket based on packet structure /// @param pandar_packet /// @return Resulting flag - virtual bool parsePacket(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) = 0; + virtual bool parse_packet(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) = 0; /// @brief Virtual function for getting the flag indicating whether one cycle is ready /// @return Readied - bool hasScanned() { return has_scanned_; } + bool has_scanned() { return has_scanned_; } /// @brief Calculation of points in each packet /// @return # of points - virtual int pointsPerPacket() = 0; + virtual int points_per_packet() = 0; /// @brief Virtual function for getting the constructed point cloud /// @return tuple of Point cloud and timestamp diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp16_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp16_decoder.hpp index 8d95758ca..ac54c8c49 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp16_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp16_decoder.hpp @@ -30,7 +30,7 @@ namespace drivers { namespace vlp16 { -constexpr uint32_t MAX_POINTS = 300000; +constexpr uint32_t max_points = 300000; /// @brief Velodyne LiDAR decoder (VLP16) class Vlp16Decoder : public VelodyneScanDecoder { @@ -47,7 +47,7 @@ class Vlp16Decoder : public VelodyneScanDecoder void unpack(const std::vector & packet, double packet_seconds) override; /// @brief Calculation of points in each packet /// @return # of points - int pointsPerPacket() override; + int points_per_packet() override; /// @brief Get the constructed point cloud /// @return tuple of Point cloud and timestamp std::tuple get_pointcloud() override; @@ -60,10 +60,10 @@ class Vlp16Decoder : public VelodyneScanDecoder /// @brief Parsing VelodynePacket based on packet structure /// @param velodyne_packet /// @return Resulting flag - bool parsePacket(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) override; - float sin_rot_table_[ROTATION_MAX_UNITS]; - float cos_rot_table_[ROTATION_MAX_UNITS]; - float rotation_radians_[ROTATION_MAX_UNITS]; + bool parse_packet(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) override; + float sin_rot_table_[g_rotation_max_units]; + float cos_rot_table_[g_rotation_max_units]; + float rotation_radians_[g_rotation_max_units]; int phase_; int max_pts_; double last_block_timestamp_; diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp32_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp32_decoder.hpp index e17e967c0..21394b06c 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp32_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp32_decoder.hpp @@ -46,7 +46,7 @@ class Vlp32Decoder : public VelodyneScanDecoder void unpack(const std::vector & packet, double packet_seconds) override; /// @brief Calculation of points in each packet /// @return # of points - int pointsPerPacket() override; + int points_per_packet() override; /// @brief Get the constructed point cloud /// @return tuple of Point cloud and timestamp std::tuple get_pointcloud() override; @@ -59,10 +59,10 @@ class Vlp32Decoder : public VelodyneScanDecoder /// @brief Parsing VelodynePacket based on packet structure /// @param velodyne_packet /// @return Resulting flag - bool parsePacket(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) override; - float sin_rot_table_[ROTATION_MAX_UNITS]; - float cos_rot_table_[ROTATION_MAX_UNITS]; - float rotation_radians_[ROTATION_MAX_UNITS]; + bool parse_packet(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) override; + float sin_rot_table_[g_rotation_max_units]; + float cos_rot_table_[g_rotation_max_units]; + float rotation_radians_[g_rotation_max_units]; std::vector> timing_offsets_; int phase_; int max_pts_; diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vls128_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vls128_decoder.hpp index 59335e126..70aeb49c4 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vls128_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vls128_decoder.hpp @@ -46,7 +46,7 @@ class Vls128Decoder : public VelodyneScanDecoder void unpack(const std::vector & packet, double packet_seconds) override; /// @brief Calculation of points in each packet /// @return # of points - int pointsPerPacket() override; + int points_per_packet() override; /// @brief Get the constructed point cloud /// @return tuple of Point cloud and timestamp std::tuple get_pointcloud() override; @@ -59,10 +59,10 @@ class Vls128Decoder : public VelodyneScanDecoder /// @brief Parsing VelodynePacket based on packet structure /// @param velodyne_packet /// @return Resulting flag - bool parsePacket(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) override; - float sin_rot_table_[ROTATION_MAX_UNITS]; - float cos_rot_table_[ROTATION_MAX_UNITS]; - float rotation_radians_[ROTATION_MAX_UNITS]; + bool parse_packet(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) override; + float sin_rot_table_[g_rotation_max_units]; + float cos_rot_table_[g_rotation_max_units]; + float rotation_radians_[g_rotation_max_units]; float vls_128_laser_azimuth_cache_[16]; int phase_; int max_pts_; diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/velodyne_driver.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/velodyne_driver.hpp index fc52e68dc..07334922c 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/velodyne_driver.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/velodyne_driver.hpp @@ -60,17 +60,17 @@ class VelodyneDriver : NebulaDriverBase /// @brief Setting CalibrationConfiguration (not used) /// @param calibration_configuration /// @return Resulting status - Status SetCalibrationConfiguration( + Status set_calibration_configuration( const CalibrationConfigurationBase & calibration_configuration) override; /// @brief Get current status of this driver /// @return Current status - Status GetStatus(); + Status get_status(); /// @brief Convert VelodyneScan message to point cloud /// @param velodyne_scan Message /// @return tuple of Point cloud and timestamp - std::tuple ParseCloudPacket( + std::tuple parse_cloud_packet( const std::vector & packet, double packet_seconds); }; diff --git a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp index 548867b7f..0382793e6 100644 --- a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_ars548_decoder.cpp @@ -32,12 +32,12 @@ ContinentalARS548Decoder::ContinentalARS548Decoder( config_ptr_ = sensor_configuration; } -Status ContinentalARS548Decoder::GetStatus() +Status ContinentalARS548Decoder::get_status() { return Status::OK; } -Status ContinentalARS548Decoder::RegisterDetectionListCallback( +Status ContinentalARS548Decoder::register_detection_list_callback( std::function)> detection_list_callback) { @@ -45,7 +45,7 @@ Status ContinentalARS548Decoder::RegisterDetectionListCallback( return Status::OK; } -Status ContinentalARS548Decoder::RegisterObjectListCallback( +Status ContinentalARS548Decoder::register_object_list_callback( std::function)> object_list_callback) { @@ -53,21 +53,21 @@ Status ContinentalARS548Decoder::RegisterObjectListCallback( return Status::OK; } -Status ContinentalARS548Decoder::RegisterSensorStatusCallback( +Status ContinentalARS548Decoder::register_sensor_status_callback( std::function sensor_status_callback) { sensor_status_callback_ = std::move(sensor_status_callback); return Status::OK; } -Status ContinentalARS548Decoder::RegisterPacketsCallback( +Status ContinentalARS548Decoder::register_packets_callback( std::function)> nebula_packets_callback) { nebula_packets_callback_ = std::move(nebula_packets_callback); return Status::OK; } -bool ContinentalARS548Decoder::ProcessPacket( +bool ContinentalARS548Decoder::process_packet( std::unique_ptr packet_msg) { const auto & data = packet_msg->data; @@ -83,28 +83,28 @@ bool ContinentalARS548Decoder::ProcessPacket( return false; } - if (header.method_id.value() == DETECTION_LIST_METHOD_ID) { + if (header.method_id.value() == detection_list_method_id) { if ( - data.size() != DETECTION_LIST_UDP_PAYLOAD || - header.length.value() != DETECTION_LIST_PDU_LENGTH) { + data.size() != detection_list_udp_payload || + header.length.value() != detection_list_pdu_length) { return false; } - ParseDetectionsListPacket(*packet_msg); - } else if (header.method_id.value() == OBJECT_LIST_METHOD_ID) { - if (data.size() != OBJECT_LIST_UDP_PAYLOAD || header.length.value() != OBJECT_LIST_PDU_LENGTH) { + parse_detections_list_packet(*packet_msg); + } else if (header.method_id.value() == object_list_method_id) { + if (data.size() != object_list_udp_payload || header.length.value() != object_list_pdu_length) { return false; } - ParseObjectsListPacket(*packet_msg); - } else if (header.method_id.value() == SENSOR_STATUS_METHOD_ID) { + parse_objects_list_packet(*packet_msg); + } else if (header.method_id.value() == sensor_status_method_id) { if ( - data.size() != SENSOR_STATUS_UDP_PAYLOAD || - header.length.value() != SENSOR_STATUS_PDU_LENGTH) { + data.size() != sensor_status_udp_payload || + header.length.value() != sensor_status_pdu_length) { return false; } - ParseSensorStatusPacket(*packet_msg); + parse_sensor_status_packet(*packet_msg); } // Some messages are not parsed but are still sent to the user (e.g., filters) @@ -119,7 +119,7 @@ bool ContinentalARS548Decoder::ProcessPacket( return true; } -bool ContinentalARS548Decoder::ParseDetectionsListPacket( +bool ContinentalARS548Decoder::parse_detections_list_packet( const nebula_msgs::msg::NebulaPacket & packet_msg) { auto msg_ptr = std::make_unique(); @@ -246,7 +246,7 @@ bool ContinentalARS548Decoder::ParseDetectionsListPacket( return true; } -bool ContinentalARS548Decoder::ParseObjectsListPacket( +bool ContinentalARS548Decoder::parse_objects_list_packet( const nebula_msgs::msg::NebulaPacket & packet_msg) { auto msg_ptr = std::make_unique(); @@ -400,7 +400,7 @@ bool ContinentalARS548Decoder::ParseObjectsListPacket( return true; } -bool ContinentalARS548Decoder::ParseSensorStatusPacket( +bool ContinentalARS548Decoder::parse_sensor_status_packet( const nebula_msgs::msg::NebulaPacket & packet_msg) { SensorStatusPacket sensor_status_packet; @@ -410,13 +410,13 @@ bool ContinentalARS548Decoder::ParseSensorStatusPacket( radar_status_.timestamp_seconds = sensor_status_packet.stamp.timestamp_seconds.value(); switch (sensor_status_packet.stamp.timestamp_sync_status) { - case SYNC_OK: + case sync_ok: radar_status_.timestamp_sync_status = "1:SYNC_OK"; break; - case NEVER_SYNC: + case never_sync: radar_status_.timestamp_sync_status = "2:NEVER_SYNC"; break; - case SYNC_LOST: + case sync_lost: radar_status_.timestamp_sync_status = "3:SYNC_LOST"; break; default: @@ -430,10 +430,10 @@ bool ContinentalARS548Decoder::ParseSensorStatusPacket( radar_status_.sw_version_patch = sensor_status_packet.sw_version_patch; switch (sensor_status_packet.status.plug_orientation) { - case PLUG_RIGHT: + case plug_right: radar_status_.plug_orientation = "0:PLUG_RIGHT"; break; - case PLUG_LEFT: + case plug_left: radar_status_.plug_orientation = "1:PLUG_LEFT"; break; default: @@ -456,13 +456,13 @@ bool ContinentalARS548Decoder::ParseSensorStatusPacket( radar_status_.wheel_base = sensor_status_packet.status.wheelbase.value(); switch (sensor_status_packet.status.frequency_slot) { - case FREQUENCY_SLOT_LOW: + case frequency_slot_low: radar_status_.frequency_slot = "0:Low (76.23 GHz)"; break; - case FREQUENCY_SLOT_MID: + case frequency_slot_mid: radar_status_.frequency_slot = "1:Mid (76.48 GHz)"; break; - case FREQUENCY_SLOT_HIGH: + case frequency_slot_high: radar_status_.frequency_slot = "2:High (76.73 GHz)"; break; default: @@ -475,10 +475,10 @@ bool ContinentalARS548Decoder::ParseSensorStatusPacket( radar_status_.time_slot = sensor_status_packet.status.time_slot; switch (sensor_status_packet.status.hcc) { - case HCC_WORLDWIDE: + case hcc_worldwide: radar_status_.hcc = "1:Worldwide"; break; - case HCC_JAPAN: + case hcc_japan: radar_status_.hcc = "1:Japan"; break; default: @@ -487,10 +487,10 @@ bool ContinentalARS548Decoder::ParseSensorStatusPacket( } switch (sensor_status_packet.status.powersave_standstill) { - case POWERSAVE_STANDSTILL_OFF: + case powersave_standstill_off: radar_status_.power_save_standstill = "0:Off"; break; - case POWERSAVE_STANDSTILL_ON: + case powersave_standstill_on: radar_status_.power_save_standstill = "1:On"; break; default: @@ -516,9 +516,9 @@ bool ContinentalARS548Decoder::ParseSensorStatusPacket( auto vdy_value_to_string = [](uint8_t value) -> std::string { switch (value) { - case VDY_OK: + case vdy_ok: return "0:VDY_OK"; - case VDY_NOTOK: + case vdy_notok: return "1:VDY_NOTOK"; default: return std::to_string(value) + ":Invalid"; @@ -541,13 +541,13 @@ bool ContinentalARS548Decoder::ParseSensorStatusPacket( vdy_value_to_string(sensor_status_packet.characteristic_speed_status); switch (sensor_status_packet.radar_status) { - case STATE_INIT: + case state_init: radar_status_.radar_status = "0:STATE_INIT"; break; - case STATE_OK: + case state_ok: radar_status_.radar_status = "1:STATE_OK"; break; - case STATE_INVALID: + case state_invalid: radar_status_.radar_status = "2:STATE_INVALID"; break; default: @@ -596,19 +596,19 @@ bool ContinentalARS548Decoder::ParseSensorStatusPacket( const uint8_t & blockage_status1 = (sensor_status_packet.blockage_status & 0xf0) >> 4; switch (blockage_status0) { - case BLOCKAGE_STATUS_BLIND: + case blockage_status_blind: radar_status_.blockage_status = "0:Blind"; break; - case BLOCKAGE_STATUS_HIGH: + case blockage_status_high: radar_status_.blockage_status = "1:High"; break; - case BLOCKAGE_STATUS_MID: + case blockage_status_mid: radar_status_.blockage_status = "2:Mid"; break; - case BLOCKAGE_STATUS_LOW: + case blockage_status_low: radar_status_.blockage_status = "3:Low"; break; - case BLOCKAGE_STATUS_NONE: + case blockage_status_none: radar_status_.blockage_status = "4:None"; break; default: @@ -617,13 +617,13 @@ bool ContinentalARS548Decoder::ParseSensorStatusPacket( } switch (blockage_status1) { - case BLOCKAGE_TEST_FAILED: + case blockage_test_failed: radar_status_.blockage_status += ". 0:Self test failed"; break; - case BLOCKAGE_TEST_PASSED: + case blockage_test_passed: radar_status_.blockage_status += ". 1:Self test passed"; break; - case BLOCKAGE_TEST_ONGOING: + case blockage_test_ongoing: radar_status_.blockage_status += ". 2:Self test ongoing"; break; default: diff --git a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp index d559b5714..086a15841 100644 --- a/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_continental/decoders/continental_srr520_decoder.cpp @@ -46,12 +46,12 @@ ContinentalSRR520Decoder::ContinentalSRR520Decoder( object_list_ptr_ = std::make_unique(); } -Status ContinentalSRR520Decoder::GetStatus() +Status ContinentalSRR520Decoder::get_status() { return Status::OK; } -Status ContinentalSRR520Decoder::RegisterNearDetectionListCallback( +Status ContinentalSRR520Decoder::register_near_detection_list_callback( std::function)> detection_list_callback) { @@ -59,7 +59,7 @@ Status ContinentalSRR520Decoder::RegisterNearDetectionListCallback( return Status::OK; } -Status ContinentalSRR520Decoder::RegisterHRRDetectionListCallback( +Status ContinentalSRR520Decoder::register_hrr_detection_list_callback( std::function)> detection_list_callback) { @@ -67,7 +67,7 @@ Status ContinentalSRR520Decoder::RegisterHRRDetectionListCallback( return Status::OK; } -Status ContinentalSRR520Decoder::RegisterObjectListCallback( +Status ContinentalSRR520Decoder::register_object_list_callback( std::function)> object_list_callback) { @@ -75,28 +75,28 @@ Status ContinentalSRR520Decoder::RegisterObjectListCallback( return Status::OK; } -Status ContinentalSRR520Decoder::RegisterStatusCallback( +Status ContinentalSRR520Decoder::register_status_callback( std::function)> status_callback) { status_callback_ = std::move(status_callback); return Status::OK; } -Status ContinentalSRR520Decoder::RegisterSyncFollowUpCallback( +Status ContinentalSRR520Decoder::register_sync_follow_up_callback( std::function sync_follow_up_callback) { sync_follow_up_callback_ = std::move(sync_follow_up_callback); return Status::OK; } -Status ContinentalSRR520Decoder::RegisterPacketsCallback( +Status ContinentalSRR520Decoder::register_packets_callback( std::function)> nebula_packets_callback) { nebula_packets_callback_ = std::move(nebula_packets_callback); return Status::OK; } -bool ContinentalSRR520Decoder::ProcessPacket( +bool ContinentalSRR520Decoder::process_packet( std::unique_ptr packet_msg) { const uint32_t can_message_id = (static_cast(packet_msg->data[0]) << 24) | @@ -106,87 +106,87 @@ bool ContinentalSRR520Decoder::ProcessPacket( std::size_t payload_size = packet_msg->data.size() - 4; - if (can_message_id == RDI_NEAR_HEADER_CAN_MESSAGE_ID) { - if (payload_size != RDI_NEAR_HEADER_PACKET_SIZE) { - PrintError("RDI_NEAR_HEADER_CAN_MESSAGE_ID message with invalid size"); + if (can_message_id == rdi_near_header_can_message_id) { + if (payload_size != rdi_near_header_packet_size) { + print_error("rdi_near_header_can_message_id message with invalid size"); return false; } - ProcessNearHeaderPacket(std::move(packet_msg)); - } else if (can_message_id == RDI_NEAR_ELEMENT_CAN_MESSAGE_ID) { - if (payload_size != RDI_NEAR_ELEMENT_PACKET_SIZE) { - PrintError("RDI_NEAR_ELEMENT_CAN_MESSAGE_ID message with invalid size"); + process_near_header_packet(std::move(packet_msg)); + } else if (can_message_id == rdi_near_element_can_message_id) { + if (payload_size != rdi_near_element_packet_size) { + print_error("rdi_near_element_can_message_id message with invalid size"); return false; } - ProcessNearElementPacket(std::move(packet_msg)); - } else if (can_message_id == RDI_HRR_HEADER_CAN_MESSAGE_ID) { - if (payload_size != RDI_HRR_HEADER_PACKET_SIZE) { - PrintError("RDI_HRR_HEADER_CAN_MESSAGE_ID message with invalid size"); + process_near_element_packet(std::move(packet_msg)); + } else if (can_message_id == rdi_hrr_header_can_message_id) { + if (payload_size != rdi_hrr_header_packet_size) { + print_error("rdi_hrr_header_can_message_id message with invalid size"); return false; } - ProcessHRRHeaderPacket(std::move(packet_msg)); - } else if (can_message_id == RDI_HRR_ELEMENT_CAN_MESSAGE_ID) { - if (payload_size != RDI_HRR_ELEMENT_PACKET_SIZE) { - PrintError("RDI_HRR_ELEMENT_CAN_MESSAGE_ID message with invalid size"); + process_hrr_header_packet(std::move(packet_msg)); + } else if (can_message_id == rdi_hrr_element_can_message_id) { + if (payload_size != rdi_hrr_element_packet_size) { + print_error("rdi_hrr_element_can_message_id message with invalid size"); return false; } - ProcessHRRElementPacket(std::move(packet_msg)); - } else if (can_message_id == OBJECT_HEADER_CAN_MESSAGE_ID) { - if (payload_size != OBJECT_HEADER_PACKET_SIZE) { - PrintError("OBJECT_HEADER_CAN_MESSAGE_ID message with invalid size"); + process_hrr_element_packet(std::move(packet_msg)); + } else if (can_message_id == object_header_can_message_id) { + if (payload_size != object_header_packet_size) { + print_error("object_header_can_message_id message with invalid size"); return false; } - ProcessObjectHeaderPacket(std::move(packet_msg)); - } else if (can_message_id == OBJECT_CAN_MESSAGE_ID) { - if (payload_size != OBJECT_PACKET_SIZE) { - PrintError("OBJECT_ELEMENT_CAN_MESSAGE_ID message with invalid size"); + process_object_header_packet(std::move(packet_msg)); + } else if (can_message_id == object_can_message_id) { + if (payload_size != object_packet_size) { + print_error("object_element_can_message_id message with invalid size"); return false; } - ProcessObjectElementPacket(std::move(packet_msg)); - } else if (can_message_id == CRC_LIST_CAN_MESSAGE_ID) { - if (payload_size != CRC_LIST_PACKET_SIZE) { - PrintError("CRC_LIST_CAN_MESSAGE_ID message with invalid size"); + process_object_element_packet(std::move(packet_msg)); + } else if (can_message_id == crc_list_can_message_id) { + if (payload_size != crc_list_packet_size) { + print_error("crc_list_can_message_id message with invalid size"); return false; } - ProcessCRCListPacket(std::move(packet_msg)); - } else if (can_message_id == STATUS_CAN_MESSAGE_ID) { - if (payload_size != STATUS_PACKET_SIZE) { - PrintError("CRC_LIST_CAN_MESSAGE_ID message with invalid size"); + process_crc_list_packet(std::move(packet_msg)); + } else if (can_message_id == status_can_message_id) { + if (payload_size != status_packet_size) { + print_error("crc_list_can_message_id message with invalid size"); return false; } - ProcessSensorStatusPacket(std::move(packet_msg)); - } else if (can_message_id == SYNC_FOLLOW_UP_CAN_MESSAGE_ID) { - if (payload_size != SYNC_FOLLOW_UP_CAN_PACKET_SIZE) { - PrintError("SYNC_FOLLOW_UP_CAN_MESSAGE_ID message with invalid size"); + process_sensor_status_packet(std::move(packet_msg)); + } else if (can_message_id == sync_follow_up_can_message_id) { + if (payload_size != sync_follow_up_can_packet_size) { + print_error("sync_follow_up_can_message_id message with invalid size"); return false; } - ProcessSyncFollowUpPacket(std::move(packet_msg)); + process_sync_follow_up_packet(std::move(packet_msg)); } else if ( - can_message_id != VEH_DYN_CAN_MESSAGE_ID && can_message_id != SENSOR_CONFIG_CAN_MESSAGE_ID) { - PrintError("Unrecognized message ID=" + std::to_string(can_message_id)); + can_message_id != veh_dyn_can_message_id && can_message_id != sensor_config_can_message_id) { + print_error("Unrecognized message ID=" + std::to_string(can_message_id)); return false; } return true; } -void ContinentalSRR520Decoder::ProcessNearHeaderPacket( +void ContinentalSRR520Decoder::process_near_header_packet( std::unique_ptr packet_msg) { - constexpr float V_AMBIGUOUS_RESOLUTION = 0.003051851f; - constexpr float V_AMBIGUOUS_MIN_VALUE = -100.f; - constexpr float MAX_RANGE_RESOLUTION = 0.1f; + constexpr float v_ambiguous_resolution = 0.003051851f; + constexpr float v_ambiguous_min_value = -100.f; + constexpr float max_range_resolution = 0.1f; first_rdi_near_packet_ = false; - static_assert(sizeof(ScanHeaderPacket) == RDI_NEAR_HEADER_PACKET_SIZE); - static_assert(sizeof(DetectionPacket) == RDI_NEAR_ELEMENT_PACKET_SIZE); - assert(packet_msg->data.size() == RDI_NEAR_HEADER_PACKET_SIZE + 4); + static_assert(sizeof(ScanHeaderPacket) == rdi_near_header_packet_size); + static_assert(sizeof(DetectionPacket) == rdi_near_element_packet_size); + assert(packet_msg->data.size() == rdi_near_header_packet_size + 4); std::memcpy( &rdi_near_header_packet_, packet_msg->data.data() + 4 * sizeof(uint8_t), @@ -217,9 +217,9 @@ void ContinentalSRR520Decoder::ProcessNearHeaderPacket( near_detection_list_ptr_->sequence_counter = rdi_near_header_packet_.u_sequence_counter; near_detection_list_ptr_->cycle_counter = rdi_near_header_packet_.u_cycle_counter.value(); near_detection_list_ptr_->v_ambiguous = - V_AMBIGUOUS_RESOLUTION * rdi_near_header_packet_.u_v_ambiguous.value() + V_AMBIGUOUS_MIN_VALUE; + v_ambiguous_resolution * rdi_near_header_packet_.u_v_ambiguous.value() + v_ambiguous_min_value; near_detection_list_ptr_->max_range = - MAX_RANGE_RESOLUTION * rdi_near_header_packet_.u_max_range.value(); + max_range_resolution * rdi_near_header_packet_.u_max_range.value(); near_detection_list_ptr_->detections.reserve( rdi_near_header_packet_.u_number_of_detections.value()); @@ -227,23 +227,23 @@ void ContinentalSRR520Decoder::ProcessNearHeaderPacket( rdi_near_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); } -void ContinentalSRR520Decoder::ProcessNearElementPacket( +void ContinentalSRR520Decoder::process_near_element_packet( std::unique_ptr packet_msg) { - constexpr auto RANGE_RESOLUTION = 0.024420024; - constexpr auto AZIMUTH_RESOLUTION = 0.006159986; - constexpr auto RANGE_RATE_RESOLUTION = 0.014662757; - constexpr auto RCS_RESOLUTION = 0.476190476; - constexpr auto SNR_RESOLUTION = 1.7; + constexpr auto range_resolution = 0.024420024; + constexpr auto azimuth_resolution = 0.006159986; + constexpr auto range_rate_resolution = 0.014662757; + constexpr auto rcs_resolution = 0.476190476; + constexpr auto snr_resolution = 1.7; - constexpr auto AZIMUTH_MIN_VALUE = -1.570796327; - constexpr auto RANGE_RATE_MIN_VALUE = -15.f; - constexpr auto RCS_MIN_VALUE = -40.f; - constexpr auto SNR_MIN_VALUE = 11.f; + constexpr auto azimuth_min_value = -1.570796327; + constexpr auto range_rate_min_value = -15.f; + constexpr auto rcs_min_value = -40.f; + constexpr auto snr_min_value = 11.f; if (rdi_near_packets_ptr_->packets.size() == 0) { if (!first_rdi_near_packet_) { - PrintError("Near element before header. This can happen during the first iteration"); + print_error("Near element before header. This can happen during the first iteration"); } return; } @@ -261,8 +261,8 @@ void ContinentalSRR520Decoder::ProcessNearElementPacket( std::memcpy( &detection_packet, packet_msg->data.data() + 4 * sizeof(uint8_t), sizeof(DetectionPacket)); - static_assert(sizeof(DetectionPacket) == RDI_NEAR_ELEMENT_PACKET_SIZE); - assert(packet_msg->data.size() == RDI_NEAR_ELEMENT_PACKET_SIZE + 4); + static_assert(sizeof(DetectionPacket) == rdi_near_element_packet_size); + assert(packet_msg->data.size() == rdi_near_element_packet_size + 4); assert(rdi_near_header_packet_.u_sequence_counter == detection_packet.u_sequence_counter); assert( rdi_near_packets_ptr_->packets.size() == @@ -279,21 +279,21 @@ void ContinentalSRR520Decoder::ProcessNearElementPacket( uint16_t u_range = (static_cast(data[0]) << 4) | (static_cast(data[1] & 0xF0) >> 4); assert(u_range <= 4095); - detection_msg.range = RANGE_RESOLUTION * u_range; + detection_msg.range = range_resolution * u_range; uint16_t u_azimuth = (static_cast(data[1] & 0x0f) << 5) | (static_cast(data[2] & 0xF8) >> 3); assert(u_azimuth <= 510); - detection_msg.azimuth_angle = AZIMUTH_RESOLUTION * u_azimuth + AZIMUTH_MIN_VALUE; + detection_msg.azimuth_angle = azimuth_resolution * u_azimuth + azimuth_min_value; uint16_t u_range_rate = (static_cast(data[2] & 0x07) << 8) | static_cast(data[3]); assert(u_range_rate <= 2046); - detection_msg.range_rate = RANGE_RATE_RESOLUTION * u_range_rate + RANGE_RATE_MIN_VALUE; + detection_msg.range_rate = range_rate_resolution * u_range_rate + range_rate_min_value; uint16_t u_rcs = (data[4] & 0xFE) >> 1; assert(u_rcs <= 126); - detection_msg.rcs = RCS_RESOLUTION * u_rcs + RCS_MIN_VALUE; + detection_msg.rcs = rcs_resolution * u_rcs + rcs_min_value; detection_msg.pdh00 = 100 * (data[4] & 0x01); detection_msg.pdh01 = 100 * ((data[5] & 0x80) >> 7); @@ -302,7 +302,7 @@ void ContinentalSRR520Decoder::ProcessNearElementPacket( detection_msg.pdh04 = 100 * ((data[5] & 0x10) >> 4); uint8_t u_snr = data[5] & 0x0f; - detection_msg.snr = SNR_RESOLUTION * u_snr + SNR_MIN_VALUE; + detection_msg.snr = snr_resolution * u_snr + snr_min_value; near_detection_list_ptr_->detections.push_back(detection_msg); parsed_detections++; @@ -311,7 +311,7 @@ void ContinentalSRR520Decoder::ProcessNearElementPacket( rdi_near_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); } -void ContinentalSRR520Decoder::ProcessHRRHeaderPacket( +void ContinentalSRR520Decoder::process_hrr_header_packet( std::unique_ptr packet_msg) { constexpr float V_AMBIGUOUS_RESOLUTION = 0.003051851f; @@ -320,8 +320,8 @@ void ContinentalSRR520Decoder::ProcessHRRHeaderPacket( first_rdi_hrr_packet_ = false; - static_assert(sizeof(ScanHeaderPacket) == RDI_HRR_HEADER_PACKET_SIZE); - assert(packet_msg->data.size() == RDI_HRR_HEADER_PACKET_SIZE + 4); + static_assert(sizeof(ScanHeaderPacket) == rdi_hrr_header_packet_size); + assert(packet_msg->data.size() == rdi_hrr_header_packet_size + 4); std::memcpy( &rdi_hrr_header_packet_, packet_msg->data.data() + 4 * sizeof(uint8_t), @@ -362,7 +362,7 @@ void ContinentalSRR520Decoder::ProcessHRRHeaderPacket( rdi_hrr_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); } -void ContinentalSRR520Decoder::ProcessHRRElementPacket( +void ContinentalSRR520Decoder::process_hrr_element_packet( std::unique_ptr packet_msg) { constexpr auto RANGE_RESOLUTION = 0.024420024; @@ -378,7 +378,7 @@ void ContinentalSRR520Decoder::ProcessHRRElementPacket( if (rdi_hrr_packets_ptr_->packets.size() == 0) { if (!first_rdi_hrr_packet_) { - PrintError("HRR element before header. This can happen during the first iteration"); + print_error("HRR element before header. This can happen during the first iteration"); } return; } @@ -396,8 +396,8 @@ void ContinentalSRR520Decoder::ProcessHRRElementPacket( std::memcpy( &detection_packet, packet_msg->data.data() + 4 * sizeof(uint8_t), sizeof(DetectionPacket)); - static_assert(sizeof(DetectionPacket) == RDI_HRR_ELEMENT_PACKET_SIZE); - assert(packet_msg->data.size() == RDI_HRR_ELEMENT_PACKET_SIZE + 4); + static_assert(sizeof(DetectionPacket) == rdi_hrr_element_packet_size); + assert(packet_msg->data.size() == rdi_hrr_element_packet_size + 4); assert(rdi_hrr_header_packet_.u_sequence_counter == detection_packet.u_sequence_counter); assert( rdi_hrr_packets_ptr_->packets.size() == @@ -446,7 +446,7 @@ void ContinentalSRR520Decoder::ProcessHRRElementPacket( rdi_hrr_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); } -void ContinentalSRR520Decoder::ProcessObjectHeaderPacket( +void ContinentalSRR520Decoder::process_object_header_packet( std::unique_ptr packet_msg) { constexpr auto VX_RESOLUTION = 0.003051851; @@ -456,8 +456,8 @@ void ContinentalSRR520Decoder::ProcessObjectHeaderPacket( first_object_packet_ = false; - static_assert(sizeof(ObjectHeaderPacket) == OBJECT_HEADER_PACKET_SIZE); - assert(packet_msg->data.size() == OBJECT_HEADER_PACKET_SIZE + 4); + static_assert(sizeof(ObjectHeaderPacket) == object_header_packet_size); + assert(packet_msg->data.size() == object_header_packet_size + 4); std::memcpy( &object_header_packet_, packet_msg->data.data() + 4 * sizeof(uint8_t), @@ -495,7 +495,7 @@ void ContinentalSRR520Decoder::ProcessObjectHeaderPacket( object_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); } -void ContinentalSRR520Decoder::ProcessObjectElementPacket( +void ContinentalSRR520Decoder::process_object_element_packet( std::unique_ptr packet_msg) { constexpr auto DIST_RESOLUTION = 0.009155553; @@ -517,7 +517,7 @@ void ContinentalSRR520Decoder::ProcessObjectElementPacket( if (object_packets_ptr_->packets.size() == 0) { if (!first_object_packet_) { - PrintError("Object element before header. This can happen during the first iteration"); + print_error("Object element before header. This can happen during the first iteration"); } return; } @@ -530,8 +530,8 @@ void ContinentalSRR520Decoder::ProcessObjectElementPacket( ObjectPacket object_packet; std::memcpy(&object_packet, packet_msg->data.data() + 4 * sizeof(uint8_t), sizeof(ObjectPacket)); - static_assert(sizeof(ObjectPacket) == OBJECT_PACKET_SIZE); - assert(packet_msg->data.size() == OBJECT_PACKET_SIZE + 4); + static_assert(sizeof(ObjectPacket) == object_packet_size); + assert(packet_msg->data.size() == object_packet_size + 4); assert(object_header_packet_.u_sequence_counter == object_packet.u_sequence_counter); assert( object_packets_ptr_->packets.size() == @@ -645,28 +645,28 @@ void ContinentalSRR520Decoder::ProcessObjectElementPacket( object_packets_ptr_->packets.emplace_back(std::move(*packet_msg)); } -void ContinentalSRR520Decoder::ProcessCRCListPacket( +void ContinentalSRR520Decoder::process_crc_list_packet( std::unique_ptr packet_msg) { const auto crc_id = packet_msg->data[4]; // first 4 bits are the can id - if (crc_id == NEAR_CRC_ID) { - ProcessNearCRCListPacket(std::move(packet_msg)); - } else if (crc_id == HRR_CRC_ID) { - ProcessHRRCRCListPacket(std::move(packet_msg)); // cspell: ignore HRRCRC - } else if (crc_id == OBJECT_CRC_ID) { - ProcessObjectCRCListPacket(std::move(packet_msg)); + if (crc_id == near_crc_id) { + process_near_crc_list_packet(std::move(packet_msg)); + } else if (crc_id == hrr_crc_id) { + process_hrrcrc_list_packet(std::move(packet_msg)); // cspell: ignore HRRCRC + } else if (crc_id == object_crc_id) { + process_object_crc_list_packet(std::move(packet_msg)); } else { - PrintError(std::string("Unrecognized CRC id=") + std::to_string(crc_id)); + print_error(std::string("Unrecognized CRC id=") + std::to_string(crc_id)); } } -void ContinentalSRR520Decoder::ProcessNearCRCListPacket( +void ContinentalSRR520Decoder::process_near_crc_list_packet( std::unique_ptr packet_msg) { - if (rdi_near_packets_ptr_->packets.size() != RDI_NEAR_PACKET_NUM + 1) { + if (rdi_near_packets_ptr_->packets.size() != rdi_near_packet_num + 1) { if (!first_rdi_near_packet_) { - PrintError("Incorrect number of RDI Near elements before CRC list"); + print_error("Incorrect number of RDI Near elements before CRC list"); } rdi_near_packets_ptr_ = std::make_unique(); @@ -681,7 +681,7 @@ void ContinentalSRR520Decoder::ProcessNearCRCListPacket( crc16_packets(rdi_near_packets_ptr_->packets.begin(), rdi_near_packets_ptr_->packets.end(), 4); if (transmitted_crc != computed_crc) { - PrintError( + print_error( "RDI Near: Transmitted CRC list does not coincide with the computed one. Ignoring packet"); rdi_near_packets_ptr_ = std::make_unique(); @@ -705,12 +705,12 @@ void ContinentalSRR520Decoder::ProcessNearCRCListPacket( std::make_unique(); } -void ContinentalSRR520Decoder::ProcessHRRCRCListPacket( +void ContinentalSRR520Decoder::process_hrrcrc_list_packet( std::unique_ptr packet_msg) { - if (rdi_hrr_packets_ptr_->packets.size() != RDI_HRR_PACKET_NUM + 1) { + if (rdi_hrr_packets_ptr_->packets.size() != rdi_hrr_packet_num + 1) { if (!first_rdi_hrr_packet_) { - PrintError("Incorrect number of RDI HRR elements before CRC list"); + print_error("Incorrect number of RDI HRR elements before CRC list"); } rdi_hrr_packets_ptr_ = std::make_unique(); @@ -725,7 +725,7 @@ void ContinentalSRR520Decoder::ProcessHRRCRCListPacket( crc16_packets(rdi_hrr_packets_ptr_->packets.begin(), rdi_hrr_packets_ptr_->packets.end(), 4); if (transmitted_crc != computed_crc) { - PrintError( + print_error( "RDI HRR: Transmitted CRC list does not coincide with the computed one. Ignoring packet"); rdi_hrr_packets_ptr_ = std::make_unique(); hrr_detection_list_ptr_ = @@ -748,12 +748,12 @@ void ContinentalSRR520Decoder::ProcessHRRCRCListPacket( std::make_unique(); } -void ContinentalSRR520Decoder::ProcessObjectCRCListPacket( +void ContinentalSRR520Decoder::process_object_crc_list_packet( std::unique_ptr packet_msg) { - if (object_packets_ptr_->packets.size() != OBJECT_PACKET_NUM + 1) { + if (object_packets_ptr_->packets.size() != object_packet_num + 1) { if (!first_object_packet_) { - PrintError("Incorrect number of object packages before CRC list"); + print_error("Incorrect number of object packages before CRC list"); } object_packets_ptr_ = std::make_unique(); @@ -767,7 +767,7 @@ void ContinentalSRR520Decoder::ProcessObjectCRCListPacket( crc16_packets(object_packets_ptr_->packets.begin(), object_packets_ptr_->packets.end(), 4); if (transmitted_crc != computed_crc) { - PrintError( + print_error( "Object: Transmitted CRC list does not coincide with the computed one. Ignoring packet"); object_packets_ptr_ = std::make_unique(); @@ -789,16 +789,16 @@ void ContinentalSRR520Decoder::ProcessObjectCRCListPacket( object_list_ptr_ = std::make_unique(); } -void ContinentalSRR520Decoder::ProcessSensorStatusPacket( +void ContinentalSRR520Decoder::process_sensor_status_packet( std::unique_ptr packet_msg) { - static_assert(sizeof(StatusPacket) == STATUS_PACKET_SIZE); + static_assert(sizeof(StatusPacket) == status_packet_size); - constexpr float STATUS_DISTANCE_RESOLUTION = 1e-3f; - constexpr float STATUS_DISTANCE_MIN_VALUE = -32.767; - constexpr float STATUS_ANGLE_RESOLUTION = 9.58766f; - constexpr float STATUS_ANGLE_MIN_VALUE = -3.14159f; - constexpr auto STATUS_ANGLE_STD_RESOLUTION = 1.52593e-05; + constexpr float status_distance_resolution = 1e-3f; + constexpr float status_distance_min_value = -32.767; + constexpr float status_angle_resolution = 9.58766f; + constexpr float status_angle_min_value = -3.14159f; + constexpr auto status_angle_std_resolution = 1.52593e-05; StatusPacket status_packet; std::memcpy(&status_packet, packet_msg->data.data() + 4 * sizeof(uint8_t), sizeof(status_packet)); @@ -852,36 +852,36 @@ void ContinentalSRR520Decoder::ProcessSensorStatusPacket( key_value.key = "long_pos"; key_value.value = std::to_string( - STATUS_DISTANCE_RESOLUTION * status_packet.u_long_pos.value() + STATUS_DISTANCE_MIN_VALUE); + status_distance_resolution * status_packet.u_long_pos.value() + status_distance_min_value); diagnostic_values.push_back(key_value); key_value.key = "lat_pos"; key_value.value = std::to_string( - STATUS_DISTANCE_RESOLUTION * status_packet.u_lat_pos.value() + STATUS_DISTANCE_MIN_VALUE); + status_distance_resolution * status_packet.u_lat_pos.value() + status_distance_min_value); diagnostic_values.push_back(key_value); key_value.key = "vert_pos"; key_value.value = std::to_string( - STATUS_DISTANCE_RESOLUTION * status_packet.u_vert_pos.value() + STATUS_DISTANCE_MIN_VALUE); + status_distance_resolution * status_packet.u_vert_pos.value() + status_distance_min_value); diagnostic_values.push_back(key_value); key_value.key = "long_pos_cog"; key_value.value = std::to_string( - STATUS_DISTANCE_RESOLUTION * status_packet.u_long_pos_cog.value() + STATUS_DISTANCE_MIN_VALUE); + status_distance_resolution * status_packet.u_long_pos_cog.value() + status_distance_min_value); diagnostic_values.push_back(key_value); key_value.key = "wheelbase"; - key_value.value = std::to_string(STATUS_DISTANCE_RESOLUTION * status_packet.u_wheelbase.value()); + key_value.value = std::to_string(status_distance_resolution * status_packet.u_wheelbase.value()); diagnostic_values.push_back(key_value); key_value.key = "yaw_angle"; key_value.value = std::to_string( - STATUS_ANGLE_RESOLUTION * status_packet.u_yaw_angle.value() + STATUS_ANGLE_MIN_VALUE); + status_angle_resolution * status_packet.u_yaw_angle.value() + status_angle_min_value); diagnostic_values.push_back(key_value); key_value.key = "cover_damping"; key_value.value = std::to_string( - STATUS_DISTANCE_RESOLUTION * status_packet.u_cover_damping.value() + STATUS_DISTANCE_MIN_VALUE); + status_distance_resolution * status_packet.u_cover_damping.value() + status_distance_min_value); diagnostic_values.push_back(key_value); uint8_t plug_orientation = status_packet.u_plug_orientation & 0b1; @@ -1119,17 +1119,17 @@ void ContinentalSRR520Decoder::ProcessSensorStatusPacket( key_value.key = "aln_current_azimuth_std"; key_value.value = - std::to_string(STATUS_ANGLE_STD_RESOLUTION * status_packet.u_aln_current_azimuth_std.value()); + std::to_string(status_angle_std_resolution * status_packet.u_aln_current_azimuth_std.value()); diagnostic_values.push_back(key_value); key_value.key = "aln_current_azimuth"; key_value.value = std::to_string( - STATUS_ANGLE_RESOLUTION * status_packet.u_aln_current_azimuth.value() + STATUS_ANGLE_MIN_VALUE); + status_angle_resolution * status_packet.u_aln_current_azimuth.value() + status_angle_min_value); diagnostic_values.push_back(key_value); key_value.key = "aln_current_delta"; key_value.value = std::to_string( - STATUS_ANGLE_RESOLUTION * status_packet.u_aln_current_delta.value() + STATUS_ANGLE_MIN_VALUE); + status_angle_resolution * status_packet.u_aln_current_delta.value() + status_angle_min_value); diagnostic_values.push_back(key_value); uint16_t computed_crc = crc16_packet(packet_msg->data.begin() + 4, packet_msg->data.end() - 3); @@ -1156,7 +1156,7 @@ void ContinentalSRR520Decoder::ProcessSensorStatusPacket( } } -void ContinentalSRR520Decoder::ProcessSyncFollowUpPacket( +void ContinentalSRR520Decoder::process_sync_follow_up_packet( std::unique_ptr packet_msg) { if (sync_follow_up_callback_) { @@ -1173,12 +1173,12 @@ void ContinentalSRR520Decoder::ProcessSyncFollowUpPacket( } } -void ContinentalSRR520Decoder::SetLogger(std::shared_ptr logger) +void ContinentalSRR520Decoder::set_logger(std::shared_ptr logger) { parent_node_logger_ptr_ = logger; } -void ContinentalSRR520Decoder::PrintInfo(std::string info) +void ContinentalSRR520Decoder::print_info(std::string info) { if (parent_node_logger_ptr_) { RCLCPP_INFO_STREAM((*parent_node_logger_ptr_), info); @@ -1187,7 +1187,7 @@ void ContinentalSRR520Decoder::PrintInfo(std::string info) } } -void ContinentalSRR520Decoder::PrintError(std::string error) +void ContinentalSRR520Decoder::print_error(std::string error) { if (parent_node_logger_ptr_) { RCLCPP_ERROR_STREAM((*parent_node_logger_ptr_), error); @@ -1196,7 +1196,7 @@ void ContinentalSRR520Decoder::PrintError(std::string error) } } -void ContinentalSRR520Decoder::PrintDebug(std::string debug) +void ContinentalSRR520Decoder::print_debug(std::string debug) { if (parent_node_logger_ptr_) { RCLCPP_DEBUG_STREAM((*parent_node_logger_ptr_), debug); diff --git a/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp b/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp index 22269bba7..78b824131 100644 --- a/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp +++ b/nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp @@ -28,32 +28,32 @@ HesaiDriver::HesaiDriver( switch (sensor_configuration->sensor_model) { case SensorModel::HESAI_PANDAR64: - scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); + scan_decoder_ = initialize_decoder(sensor_configuration, calibration_data); break; case SensorModel::HESAI_PANDAR40P: case SensorModel::HESAI_PANDAR40M: - scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); + scan_decoder_ = initialize_decoder(sensor_configuration, calibration_data); break; case SensorModel::HESAI_PANDARQT64: - scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); + scan_decoder_ = initialize_decoder(sensor_configuration, calibration_data); break; case SensorModel::HESAI_PANDARQT128: - scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); + scan_decoder_ = initialize_decoder(sensor_configuration, calibration_data); break; case SensorModel::HESAI_PANDARXT32: - scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); + scan_decoder_ = initialize_decoder(sensor_configuration, calibration_data); break; case SensorModel::HESAI_PANDARXT32M: - scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); + scan_decoder_ = initialize_decoder(sensor_configuration, calibration_data); break; case SensorModel::HESAI_PANDARAT128: - scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); + scan_decoder_ = initialize_decoder(sensor_configuration, calibration_data); break; case SensorModel::HESAI_PANDAR128_E3X: - scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); + scan_decoder_ = initialize_decoder(sensor_configuration, calibration_data); break; case SensorModel::HESAI_PANDAR128_E4X: - scan_decoder_ = InitializeDecoder(sensor_configuration, calibration_data); + scan_decoder_ = initialize_decoder(sensor_configuration, calibration_data); break; case SensorModel::UNKNOWN: driver_status_ = nebula::Status::INVALID_SENSOR_MODEL; @@ -65,7 +65,7 @@ HesaiDriver::HesaiDriver( } template -std::shared_ptr HesaiDriver::InitializeDecoder( +std::shared_ptr HesaiDriver::initialize_decoder( const std::shared_ptr & sensor_configuration, const std::shared_ptr & calibration_configuration) @@ -75,7 +75,7 @@ std::shared_ptr HesaiDriver::InitializeDecoder( sensor_configuration, std::dynamic_pointer_cast(calibration_configuration)); } -std::tuple HesaiDriver::ParseCloudPacket( +std::tuple HesaiDriver::parse_cloud_packet( const std::vector & packet) { std::tuple pointcloud; @@ -87,8 +87,8 @@ std::tuple HesaiDriver::ParseCloudPacket( } scan_decoder_->unpack(packet); - if (scan_decoder_->hasScanned()) { - pointcloud = scan_decoder_->getPointcloud(); + if (scan_decoder_->has_scanned()) { + pointcloud = scan_decoder_->get_pointcloud(); } // todo @@ -101,15 +101,15 @@ std::tuple HesaiDriver::ParseCloudPacket( return pointcloud; } -Status HesaiDriver::SetCalibrationConfiguration( +Status HesaiDriver::set_calibration_configuration( const HesaiCalibrationConfigurationBase & calibration_configuration) { throw std::runtime_error( - "SetCalibrationConfiguration. Not yet implemented (" + + "set_calibration_configuration. Not yet implemented (" + calibration_configuration.calibration_file + ")"); } -Status HesaiDriver::GetStatus() +Status HesaiDriver::get_status() { return driver_status_; } diff --git a/nebula_decoders/src/nebula_decoders_robosense/robosense_driver.cpp b/nebula_decoders/src/nebula_decoders_robosense/robosense_driver.cpp index 96984e120..7c943b043 100644 --- a/nebula_decoders/src/nebula_decoders_robosense/robosense_driver.cpp +++ b/nebula_decoders/src/nebula_decoders_robosense/robosense_driver.cpp @@ -40,20 +40,20 @@ RobosenseDriver::RobosenseDriver( } } -Status RobosenseDriver::GetStatus() +Status RobosenseDriver::get_status() { return driver_status_; } -Status RobosenseDriver::SetCalibrationConfiguration( +Status RobosenseDriver::set_calibration_configuration( const CalibrationConfigurationBase & calibration_configuration) { throw std::runtime_error( - "SetCalibrationConfiguration. Not yet implemented (" + + "set_calibration_configuration. Not yet implemented (" + calibration_configuration.calibration_file + ")"); } -std::tuple RobosenseDriver::ParseCloudPacket( +std::tuple RobosenseDriver::parse_cloud_packet( const std::vector & packet) { std::tuple pointcloud; @@ -65,8 +65,8 @@ std::tuple RobosenseDriver::ParseCloudPack } scan_decoder_->unpack(packet); - if (scan_decoder_->hasScanned()) { - pointcloud = scan_decoder_->getPointcloud(); + if (scan_decoder_->has_scanned()) { + pointcloud = scan_decoder_->get_pointcloud(); } return pointcloud; diff --git a/nebula_decoders/src/nebula_decoders_robosense/robosense_info_driver.cpp b/nebula_decoders/src/nebula_decoders_robosense/robosense_info_driver.cpp index cfdc566eb..e44d078e0 100644 --- a/nebula_decoders/src/nebula_decoders_robosense/robosense_info_driver.cpp +++ b/nebula_decoders/src/nebula_decoders_robosense/robosense_info_driver.cpp @@ -38,36 +38,36 @@ RobosenseInfoDriver::RobosenseInfoDriver( } } -Status RobosenseInfoDriver::GetStatus() +Status RobosenseInfoDriver::get_status() { return driver_status_; } -Status RobosenseInfoDriver::DecodeInfoPacket(const std::vector & packet) +Status RobosenseInfoDriver::decode_info_packet(const std::vector & packet) { - const auto parsed = info_decoder_->parsePacket(packet); + const auto parsed = info_decoder_->parse_packet(packet); if (parsed) return nebula::Status::OK; return nebula::Status::ERROR_1; } -std::map RobosenseInfoDriver::GetSensorInfo() +std::map RobosenseInfoDriver::get_sensor_info() { - return info_decoder_->getSensorInfo(); + return info_decoder_->get_sensor_info(); } -ReturnMode RobosenseInfoDriver::GetReturnMode() +ReturnMode RobosenseInfoDriver::get_return_mode() { - return info_decoder_->getReturnMode(); + return info_decoder_->get_return_mode(); } -RobosenseCalibrationConfiguration RobosenseInfoDriver::GetSensorCalibration() +RobosenseCalibrationConfiguration RobosenseInfoDriver::get_sensor_calibration() { - return info_decoder_->getSensorCalibration(); + return info_decoder_->get_sensor_calibration(); } -bool RobosenseInfoDriver::GetSyncStatus() +bool RobosenseInfoDriver::get_sync_status() { - return info_decoder_->getSyncStatus(); + return info_decoder_->get_sync_status(); } } // namespace drivers diff --git a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp index 1fa656612..666a7c399 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp @@ -27,14 +27,14 @@ Vlp16Decoder::Vlp16Decoder( overflow_pc_.reset(new NebulaPointCloud); // Set up cached values for sin and cos of all the possible headings - for (uint16_t rot_index = 0; rot_index < ROTATION_MAX_UNITS; ++rot_index) { - float rotation = angles::from_degrees(ROTATION_RESOLUTION * rot_index); + for (uint16_t rot_index = 0; rot_index < g_rotation_max_units; ++rot_index) { + float rotation = angles::from_degrees(g_rotation_resolution * rot_index); rotation_radians_[rot_index] = rotation; cos_rot_table_[rot_index] = cosf(rotation); sin_rot_table_[rot_index] = sinf(rotation); } // timing table calculation, from velodyne user manual p.64 - timing_offsets_.resize(BLOCKS_PER_PACKET); + timing_offsets_.resize(g_blocks_per_packet); for (size_t i = 0; i < timing_offsets_.size(); ++i) { timing_offsets_[i].resize(32); } @@ -80,9 +80,9 @@ std::tuple Vlp16Decoder::get_pointcloud() return std::make_tuple(scan_pc_, scan_timestamp_); } -int Vlp16Decoder::pointsPerPacket() +int Vlp16Decoder::points_per_packet() { - return BLOCKS_PER_PACKET * VLP16_FIRINGS_PER_BLOCK * VLP16_SCANS_PER_FIRING; + return g_blocks_per_packet * g_vlp16_firings_per_block * g_vlp16_scans_per_firing; } void Vlp16Decoder::reset_pointcloud(double time_stamp) @@ -136,18 +136,18 @@ void Vlp16Decoder::reset_overflow(double time_stamp) void Vlp16Decoder::unpack(const std::vector & packet, double packet_seconds) { - checkAndHandleScanComplete(packet, packet_seconds, phase_); + check_and_handle_scan_complete(packet, packet_seconds, phase_); const raw_packet_t * raw = (const raw_packet_t *)packet.data(); float last_azimuth_diff = 0; uint16_t azimuth_next; - const uint8_t return_mode = packet[RETURN_MODE_INDEX]; - const bool dual_return = (return_mode == RETURN_MODE_DUAL); + const uint8_t return_mode = packet[g_return_mode_index]; + const bool dual_return = (return_mode == g_return_mode_dual); - for (uint block = 0; block < BLOCKS_PER_PACKET; block++) { + for (uint block = 0; block < g_blocks_per_packet; block++) { // Cache block for use. const raw_block_t & current_block = raw->blocks[block]; - if (UPPER_BANK != raw->blocks[block].header) { + if (g_upper_bank != raw->blocks[block].header) { // Do not flood the log with messages, only issue at most one // of these warnings per minute. return; // bad packet: skip the rest @@ -162,7 +162,7 @@ void Vlp16Decoder::unpack(const std::vector & packet, double packet_sec } else { azimuth = azimuth_next; } - if (block < static_cast(BLOCKS_PER_PACKET - (1 + dual_return))) { + if (block < static_cast(g_blocks_per_packet - (1 + dual_return))) { // Get the next block rotation to calculate how far we rotate between blocks. azimuth_next = raw->blocks[block + (1 + dual_return)].rotation; @@ -176,7 +176,7 @@ void Vlp16Decoder::unpack(const std::vector & packet, double packet_sec // same as the last to the second to last. // Assumes RPM doesn't change much between blocks. azimuth_diff = - (block == static_cast(BLOCKS_PER_PACKET - dual_return - 1) ? 0 : last_azimuth_diff); + (block == static_cast(g_blocks_per_packet - dual_return - 1) ? 0 : last_azimuth_diff); } // Condition added to avoid calculating points which are not in the interesting defined area @@ -186,8 +186,8 @@ void Vlp16Decoder::unpack(const std::vector & packet, double packet_sec azimuth >= sensor_configuration_->cloud_min_angle * 100 && azimuth <= sensor_configuration_->cloud_max_angle * 100) || (sensor_configuration_->cloud_min_angle > sensor_configuration_->cloud_max_angle)) { - for (int firing = 0, k = 0; firing < VLP16_FIRINGS_PER_BLOCK; ++firing) { - for (int dsr = 0; dsr < VLP16_SCANS_PER_FIRING; dsr++, k += RAW_SCAN_SIZE) { + for (int firing = 0, k = 0; firing < g_vlp16_firings_per_block; ++firing) { + for (int dsr = 0; dsr < g_vlp16_scans_per_firing; dsr++, k += g_raw_scan_size) { union two_bytes current_return; union two_bytes other_return; // Distance extraction. @@ -228,8 +228,8 @@ void Vlp16Decoder::unpack(const std::vector & packet, double packet_sec // Correct for the laser rotation as a function of timing during the firings. float azimuth_corrected_f = azimuth + - (azimuth_diff * ((dsr * VLP16_DSR_TOFFSET) + (firing * VLP16_FIRING_TOFFSET)) / - VLP16_BLOCK_DURATION) - + (azimuth_diff * ((dsr * g_vlp16_dsr_toffset) + (firing * g_vlp16_firing_toffset)) / + g_vlp16_block_duration) - corrections.rot_correction * 180.0 / M_PI * 100; if (azimuth_corrected_f < 0.0) { @@ -269,7 +269,7 @@ void Vlp16Decoder::unpack(const std::vector & packet, double packet_sec // Determine return type. uint8_t return_type; switch (return_mode) { - case RETURN_MODE_DUAL: + case g_return_mode_dual: if ( (other_return.bytes[0] == 0 && other_return.bytes[1] == 0) || (other_return.bytes[0] == current_return.bytes[0] && @@ -297,10 +297,10 @@ void Vlp16Decoder::unpack(const std::vector & packet, double packet_sec } } break; - case RETURN_MODE_STRONGEST: + case g_return_mode_strongest: return_type = static_cast(drivers::ReturnType::STRONGEST); break; - case RETURN_MODE_LAST: + case g_return_mode_last: return_type = static_cast(drivers::ReturnType::LAST); break; default: @@ -329,7 +329,7 @@ void Vlp16Decoder::unpack(const std::vector & packet, double packet_sec } } -bool Vlp16Decoder::parsePacket( +bool Vlp16Decoder::parse_packet( [[maybe_unused]] const velodyne_msgs::msg::VelodynePacket & velodyne_packet) { return 0; diff --git a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp index 15224c0a5..f55378e67 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp @@ -27,8 +27,8 @@ Vlp32Decoder::Vlp32Decoder( overflow_pc_.reset(new NebulaPointCloud); // Set up cached values for sin and cos of all the possible headings - for (uint16_t rot_index = 0; rot_index < ROTATION_MAX_UNITS; ++rot_index) { - float rotation = angles::from_degrees(ROTATION_RESOLUTION * rot_index); + for (uint16_t rot_index = 0; rot_index < g_rotation_max_units; ++rot_index) { + float rotation = angles::from_degrees(g_rotation_resolution * rot_index); rotation_radians_[rot_index] = rotation; cos_rot_table_[rot_index] = cosf(rotation); sin_rot_table_[rot_index] = sinf(rotation); @@ -78,9 +78,9 @@ std::tuple Vlp32Decoder::get_pointcloud() return std::make_tuple(scan_pc_, scan_timestamp_); } -int Vlp32Decoder::pointsPerPacket() +int Vlp32Decoder::points_per_packet() { - return BLOCKS_PER_PACKET * SCANS_PER_BLOCK; + return g_blocks_per_packet * g_scans_per_block; } void Vlp32Decoder::reset_pointcloud(double time_stamp) @@ -135,17 +135,17 @@ void Vlp32Decoder::reset_overflow(double time_stamp) void Vlp32Decoder::unpack(const std::vector & packet, double packet_seconds) { - checkAndHandleScanComplete(packet, packet_seconds, phase_); + check_and_handle_scan_complete(packet, packet_seconds, phase_); const raw_packet_t * raw = (const raw_packet_t *)packet.data(); float last_azimuth_diff = 0; uint16_t azimuth_next; - uint8_t return_mode = packet[RETURN_MODE_INDEX]; - const bool dual_return = (return_mode == RETURN_MODE_DUAL); + uint8_t return_mode = packet[g_return_mode_index]; + const bool dual_return = (return_mode == g_return_mode_dual); - for (uint i = 0; i < BLOCKS_PER_PACKET; i++) { + for (uint i = 0; i < g_blocks_per_packet; i++) { int bank_origin = 0; - if (raw->blocks[i].header == LOWER_BANK) { + if (raw->blocks[i].header == g_lower_bank) { // lower bank lasers are [32..63] bank_origin = 32; } @@ -158,7 +158,7 @@ void Vlp32Decoder::unpack(const std::vector & packet, double packet_sec } else { azimuth = azimuth_next; } - if (i < static_cast(BLOCKS_PER_PACKET - (1 + dual_return))) { + if (i < static_cast(g_blocks_per_packet - (1 + dual_return))) { // Get the next block rotation to calculate how far we rotate between blocks azimuth_next = raw->blocks[i + (1 + dual_return)].rotation; @@ -171,11 +171,12 @@ void Vlp32Decoder::unpack(const std::vector & packet, double packet_sec // This makes the assumption the difference between the last block and the next packet is the // same as the last to the second to last. // Assumes RPM doesn't change much between blocks. - azimuth_diff = - (i == static_cast(BLOCKS_PER_PACKET - (4 * dual_return) - 1)) ? 0 : last_azimuth_diff; + azimuth_diff = (i == static_cast(g_blocks_per_packet - (4 * dual_return) - 1)) + ? 0 + : last_azimuth_diff; } - for (uint j = 0, k = 0; j < SCANS_PER_BLOCK; j++, k += RAW_SCAN_SIZE) { + for (uint j = 0, k = 0; j < g_scans_per_block; j++, k += g_raw_scan_size) { float x, y, z; uint8_t intensity; const uint8_t laser_number = j + bank_origin; @@ -230,7 +231,7 @@ void Vlp32Decoder::unpack(const std::vector & packet, double packet_sec const float cos_vert_angle = corrections.cos_vert_correction; const float sin_vert_angle = corrections.sin_vert_correction; float azimuth_corrected_f = - azimuth + (azimuth_diff * VLP32_CHANNEL_DURATION / VLP32_SEQ_DURATION * j) - + azimuth + (azimuth_diff * g_vlp32_channel_duration / g_vlp32_seq_duration * j) - corrections.rot_correction * 180.0 / M_PI * 100; if (azimuth_corrected_f < 0) { azimuth_corrected_f += 36000; @@ -329,7 +330,7 @@ void Vlp32Decoder::unpack(const std::vector & packet, double packet_sec nebula::drivers::ReturnType return_type; switch (return_mode) { - case RETURN_MODE_DUAL: + case g_return_mode_dual: if ( (other_return.bytes[0] == 0 && other_return.bytes[1] == 0) || (other_return.bytes[0] == current_return.bytes[0] && @@ -356,10 +357,10 @@ void Vlp32Decoder::unpack(const std::vector & packet, double packet_sec } } break; - case RETURN_MODE_STRONGEST: + case g_return_mode_strongest: return_type = drivers::ReturnType::STRONGEST; break; - case RETURN_MODE_LAST: + case g_return_mode_last: return_type = drivers::ReturnType::LAST; break; default: @@ -385,7 +386,7 @@ void Vlp32Decoder::unpack(const std::vector & packet, double packet_sec } } -bool Vlp32Decoder::parsePacket( +bool Vlp32Decoder::parse_packet( [[maybe_unused]] const velodyne_msgs::msg::VelodynePacket & velodyne_packet) { return 0; diff --git a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp index 4d55c54cc..056384619 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp @@ -27,8 +27,8 @@ Vls128Decoder::Vls128Decoder( overflow_pc_.reset(new NebulaPointCloud); // Set up cached values for sin and cos of all the possible headings - for (uint16_t rot_index = 0; rot_index < ROTATION_MAX_UNITS; ++rot_index) { - float rotation = angles::from_degrees(ROTATION_RESOLUTION * rot_index); + for (uint16_t rot_index = 0; rot_index < g_rotation_max_units; ++rot_index) { + float rotation = angles::from_degrees(g_rotation_resolution * rot_index); rotation_radians_[rot_index] = rotation; cos_rot_table_[rot_index] = cosf(rotation); sin_rot_table_[rot_index] = sinf(rotation); @@ -37,7 +37,8 @@ Vls128Decoder::Vls128Decoder( phase_ = (uint16_t)round(sensor_configuration_->scan_phase * 100); for (uint8_t i = 0; i < 16; i++) { - vls_128_laser_azimuth_cache_[i] = (VLS128_CHANNEL_DURATION / VLS128_SEQ_DURATION) * (i + i / 8); + vls_128_laser_azimuth_cache_[i] = + (g_vls128_channel_duration / g_vls128_seq_duration) * (i + i / 8); } // timing table calculation, from velodyne user manual p.64 timing_offsets_.resize(3); @@ -80,9 +81,9 @@ std::tuple Vls128Decoder::get_pointcloud() return std::make_tuple(scan_pc_, scan_timestamp_); } -int Vls128Decoder::pointsPerPacket() +int Vls128Decoder::points_per_packet() { - return BLOCKS_PER_PACKET * SCANS_PER_BLOCK; + return g_blocks_per_packet * g_scans_per_block; } void Vls128Decoder::reset_pointcloud(double time_stamp) @@ -137,31 +138,32 @@ void Vls128Decoder::reset_overflow(double time_stamp) void Vls128Decoder::unpack(const std::vector & packet, double packet_seconds) { - checkAndHandleScanComplete(packet, packet_seconds, phase_); + check_and_handle_scan_complete(packet, packet_seconds, phase_); const raw_packet_t * raw = (const raw_packet_t *)packet.data(); float last_azimuth_diff = 0; uint16_t azimuth_next; - const uint8_t return_mode = packet[RETURN_MODE_INDEX]; - const bool dual_return = (return_mode == RETURN_MODE_DUAL); + const uint8_t return_mode = packet[g_return_mode_index]; + const bool dual_return = (return_mode == g_return_mode_dual); - for (uint block = 0; block < static_cast(BLOCKS_PER_PACKET - (4 * dual_return)); block++) { + for (uint block = 0; block < static_cast(g_blocks_per_packet - (4 * dual_return)); + block++) { // Cache block for use. const raw_block_t & current_block = raw->blocks[block]; uint bank_origin = 0; // Used to detect which bank of 32 lasers is in this block. switch (current_block.header) { - case VLS128_BANK_1: + case g_vls128_bank_1: bank_origin = 0; break; - case VLS128_BANK_2: + case g_vls128_bank_2: bank_origin = 32; break; - case VLS128_BANK_3: + case g_vls128_bank_3: bank_origin = 64; break; - case VLS128_BANK_4: + case g_vls128_bank_4: bank_origin = 96; break; default: @@ -179,7 +181,7 @@ void Vls128Decoder::unpack(const std::vector & packet, double packet_se } else { azimuth = azimuth_next; } - if (block < static_cast(BLOCKS_PER_PACKET - (1 + dual_return))) { + if (block < static_cast(g_blocks_per_packet - (1 + dual_return))) { // Get the next block rotation to calculate how far we rotate between blocks azimuth_next = raw->blocks[block + (1 + dual_return)].rotation; @@ -192,7 +194,7 @@ void Vls128Decoder::unpack(const std::vector & packet, double packet_se // This makes the assumption the difference between the last block and the next packet is the // same as the last to the second to last. // Assumes RPM doesn't change much between blocks. - azimuth_diff = (block == static_cast(BLOCKS_PER_PACKET - (4 * dual_return) - 1)) + azimuth_diff = (block == static_cast(g_blocks_per_packet - (4 * dual_return) - 1)) ? 0 : last_azimuth_diff; } @@ -206,7 +208,7 @@ void Vls128Decoder::unpack(const std::vector & packet, double packet_se azimuth <= sensor_configuration_->cloud_max_angle * 100) || // azimuth <= sensor_configuration_->cloud_max_angle) || (sensor_configuration_->cloud_min_angle > sensor_configuration_->cloud_max_angle)) { - for (size_t j = 0, k = 0; j < SCANS_PER_BLOCK; j++, k += RAW_SCAN_SIZE) { + for (size_t j = 0, k = 0; j < g_scans_per_block; j++, k += g_raw_scan_size) { union two_bytes current_return { }; union two_bytes other_return { @@ -241,7 +243,7 @@ void Vls128Decoder::unpack(const std::vector & packet, double packet_se const VelodyneLaserCorrection & corrections = calibration_configuration_->velodyne_calibration.laser_corrections[laser_number]; - float distance = current_return.uint * VLP128_DISTANCE_RESOLUTION; + float distance = current_return.uint * g_vlp128_distance_resolution; if (distance > 1e-6) { distance += corrections.dist_correction; } @@ -289,7 +291,7 @@ void Vls128Decoder::unpack(const std::vector & packet, double packet_se // Determine return type. uint8_t return_type; switch (return_mode) { - case RETURN_MODE_DUAL: + case g_return_mode_dual: if ( (other_return.bytes[0] == 0 && other_return.bytes[1] == 0) || (other_return.bytes[0] == current_return.bytes[0] && @@ -316,10 +318,10 @@ void Vls128Decoder::unpack(const std::vector & packet, double packet_se } } break; - case RETURN_MODE_STRONGEST: + case g_return_mode_strongest: return_type = static_cast(drivers::ReturnType::STRONGEST); break; - case RETURN_MODE_LAST: + case g_return_mode_last: return_type = static_cast(drivers::ReturnType::LAST); break; default: @@ -348,7 +350,7 @@ void Vls128Decoder::unpack(const std::vector & packet, double packet_se // block++) } -bool Vls128Decoder::parsePacket( +bool Vls128Decoder::parse_packet( [[maybe_unused]] const velodyne_msgs::msg::VelodynePacket & velodyne_packet) { return 0; diff --git a/nebula_decoders/src/nebula_decoders_velodyne/velodyne_driver.cpp b/nebula_decoders/src/nebula_decoders_velodyne/velodyne_driver.cpp index 5892c782e..72a2ba00a 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/velodyne_driver.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/velodyne_driver.cpp @@ -41,15 +41,15 @@ VelodyneDriver::VelodyneDriver( } } -Status VelodyneDriver::SetCalibrationConfiguration( +Status VelodyneDriver::set_calibration_configuration( const CalibrationConfigurationBase & calibration_configuration) { throw std::runtime_error( - "SetCalibrationConfiguration. Not yet implemented (" + + "set_calibration_configuration. Not yet implemented (" + calibration_configuration.calibration_file + ")"); } -std::tuple VelodyneDriver::ParseCloudPacket( +std::tuple VelodyneDriver::parse_cloud_packet( const std::vector & packet, double packet_seconds) { std::tuple pointcloud; @@ -61,13 +61,13 @@ std::tuple VelodyneDriver::ParseCloudPacke } scan_decoder_->unpack(packet, packet_seconds); - if (scan_decoder_->hasScanned()) { + if (scan_decoder_->has_scanned()) { pointcloud = scan_decoder_->get_pointcloud(); } return pointcloud; } -Status VelodyneDriver::GetStatus() +Status VelodyneDriver::get_status() { return driver_status_; } diff --git a/nebula_examples/include/hesai/hesai_ros_offline_extract_bag_pcd.hpp b/nebula_examples/include/hesai/hesai_ros_offline_extract_bag_pcd.hpp index 247fd1ba0..876459bd0 100644 --- a/nebula_examples/include/hesai/hesai_ros_offline_extract_bag_pcd.hpp +++ b/nebula_examples/include/hesai/hesai_ros_offline_extract_bag_pcd.hpp @@ -44,16 +44,16 @@ class HesaiRosOfflineExtractBag final : public rclcpp::Node std::shared_ptr sensor_cfg_ptr_; std::shared_ptr correction_cfg_ptr_; - Status InitializeDriver( + Status initialize_driver( std::shared_ptr sensor_configuration, std::shared_ptr calibration_configuration); - Status GetParameters( + Status get_parameters( drivers::HesaiSensorConfiguration & sensor_configuration, drivers::HesaiCalibrationConfiguration & calibration_configuration, drivers::HesaiCorrection & correction_configuration); - static inline std::chrono::nanoseconds SecondsToChronoNanoSeconds(const double seconds) + static inline std::chrono::nanoseconds seconds_to_chrono_nano_seconds(const double seconds) { return std::chrono::duration_cast( std::chrono::duration(seconds)); @@ -63,19 +63,19 @@ class HesaiRosOfflineExtractBag final : public rclcpp::Node explicit HesaiRosOfflineExtractBag( const rclcpp::NodeOptions & options, const std::string & node_name); - Status GetStatus(); - Status ReadBag(); + Status get_status(); + Status read_bag(); private: - std::string bag_path; - std::string storage_id; - std::string out_path; - std::string format; - std::string target_topic; - std::string correction_file_path; - int out_num; - int skip_num; - bool only_xyz; + std::string bag_path_; + std::string storage_id_; + std::string out_path_; + std::string format_; + std::string target_topic_; + std::string correction_file_path_; + int out_num_; + int skip_num_; + bool only_xyz_; }; } // namespace ros diff --git a/nebula_examples/include/hesai/hesai_ros_offline_extract_pcd.hpp b/nebula_examples/include/hesai/hesai_ros_offline_extract_pcd.hpp index d7afb4dcc..559ab7066 100644 --- a/nebula_examples/include/hesai/hesai_ros_offline_extract_pcd.hpp +++ b/nebula_examples/include/hesai/hesai_ros_offline_extract_pcd.hpp @@ -47,7 +47,7 @@ class HesaiRosOfflineExtractSample final : public rclcpp::Node /// @param sensor_configuration SensorConfiguration for this driver /// @param calibration_configuration CalibrationConfiguration for this driver /// @return Resulting status - Status InitializeDriver( + Status initialize_driver( std::shared_ptr sensor_configuration, std::shared_ptr calibration_configuration); @@ -56,7 +56,7 @@ class HesaiRosOfflineExtractSample final : public rclcpp::Node /// @param calibration_configuration Output of CalibrationConfiguration /// @param correction_configuration Output of CorrectionConfiguration (for AT) /// @return Resulting status - Status GetParameters( + Status get_parameters( drivers::HesaiSensorConfiguration & sensor_configuration, drivers::HesaiCalibrationConfiguration & calibration_configuration, drivers::HesaiCorrection & correction_configuration); @@ -64,7 +64,7 @@ class HesaiRosOfflineExtractSample final : public rclcpp::Node /// @brief Convert seconds to chrono::nanoseconds /// @param seconds /// @return chrono::nanoseconds - static inline std::chrono::nanoseconds SecondsToChronoNanoSeconds(const double seconds) + static inline std::chrono::nanoseconds seconds_to_chrono_nano_seconds(const double seconds) { return std::chrono::duration_cast( std::chrono::duration(seconds)); @@ -76,18 +76,18 @@ class HesaiRosOfflineExtractSample final : public rclcpp::Node /// @brief Get current status of this driver /// @return Current status - Status GetStatus(); + Status get_status(); /// @brief Read the specified bag file and output point clouds to PCD files - Status ReadBag(); + Status read_bag(); private: - std::string bag_path; - std::string storage_id; - std::string out_path; - std::string format; - std::string target_topic; - std::string correction_file_path; + std::string bag_path_; + std::string storage_id_; + std::string out_path_; + std::string format_; + std::string target_topic_; + std::string correction_file_path_; }; } // namespace ros diff --git a/nebula_examples/include/velodyne/velodyne_ros_offline_extract_bag_pcd.hpp b/nebula_examples/include/velodyne/velodyne_ros_offline_extract_bag_pcd.hpp index 64e3d7435..60ed115c1 100644 --- a/nebula_examples/include/velodyne/velodyne_ros_offline_extract_bag_pcd.hpp +++ b/nebula_examples/include/velodyne/velodyne_ros_offline_extract_bag_pcd.hpp @@ -45,7 +45,7 @@ class VelodyneRosOfflineExtractBag final : public rclcpp::Node /// @param sensor_configuration SensorConfiguration for this driver /// @param calibration_configuration CalibrationConfiguration for this driver /// @return Resulting status - Status InitializeDriver( + Status initialize_driver( std::shared_ptr sensor_configuration, std::shared_ptr calibration_configuration); @@ -53,14 +53,14 @@ class VelodyneRosOfflineExtractBag final : public rclcpp::Node /// @param sensor_configuration Output of SensorConfiguration /// @param calibration_configuration Output of CalibrationConfiguration /// @return Resulting status - Status GetParameters( + Status get_parameters( drivers::VelodyneSensorConfiguration & sensor_configuration, drivers::VelodyneCalibrationConfiguration & calibration_configuration); /// @brief Convert seconds to chrono::nanoseconds /// @param seconds /// @return chrono::nanoseconds - static inline std::chrono::nanoseconds SecondsToChronoNanoSeconds(const double seconds) + static inline std::chrono::nanoseconds seconds_to_chrono_nano_seconds(const double seconds) { return std::chrono::duration_cast( std::chrono::duration(seconds)); @@ -72,20 +72,20 @@ class VelodyneRosOfflineExtractBag final : public rclcpp::Node /// @brief Get current status of this driver /// @return Current status - Status GetStatus(); + Status get_status(); /// @brief Read the specified bag file and output point clouds to PCD files - Status ReadBag(); + Status read_bag(); private: - std::string bag_path; - std::string storage_id; - std::string out_path; - std::string format; - std::string target_topic; - int out_num; - int skip_num; - bool only_xyz; + std::string bag_path_; + std::string storage_id_; + std::string out_path_; + std::string format_; + std::string target_topic_; + int out_num_; + int skip_num_; + bool only_xyz_; }; } // namespace ros diff --git a/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp b/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp index 62f988243..5c12857e3 100644 --- a/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp +++ b/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd.cpp @@ -22,6 +22,8 @@ #include "rosbag2_cpp/writers/sequential_writer.hpp" #include "rosbag2_storage/storage_options.hpp" +#include + #include namespace nebula @@ -37,7 +39,7 @@ HesaiRosOfflineExtractBag::HesaiRosOfflineExtractBag( drivers::HesaiCorrection correction_configuration; wrapper_status_ = - GetParameters(sensor_configuration, calibration_configuration, correction_configuration); + get_parameters(sensor_configuration, calibration_configuration, correction_configuration); if (Status::OK != wrapper_status_) { RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error: " << wrapper_status_); return; @@ -52,11 +54,11 @@ HesaiRosOfflineExtractBag::HesaiRosOfflineExtractBag( RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Driver "); if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { correction_cfg_ptr_ = std::make_shared(correction_configuration); - wrapper_status_ = InitializeDriver( + wrapper_status_ = initialize_driver( std::const_pointer_cast(sensor_cfg_ptr_), correction_cfg_ptr_); } else { - wrapper_status_ = InitializeDriver( + wrapper_status_ = initialize_driver( std::const_pointer_cast(sensor_cfg_ptr_), calibration_cfg_ptr_); } @@ -64,31 +66,31 @@ HesaiRosOfflineExtractBag::HesaiRosOfflineExtractBag( RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); } -Status HesaiRosOfflineExtractBag::InitializeDriver( +Status HesaiRosOfflineExtractBag::initialize_driver( std::shared_ptr sensor_configuration, std::shared_ptr calibration_configuration) { driver_ptr_ = std::make_shared( std::static_pointer_cast(sensor_configuration), calibration_configuration); - return driver_ptr_->GetStatus(); + return driver_ptr_->get_status(); } -Status HesaiRosOfflineExtractBag::GetStatus() +Status HesaiRosOfflineExtractBag::get_status() { return wrapper_status_; } -Status HesaiRosOfflineExtractBag::GetParameters( +Status HesaiRosOfflineExtractBag::get_parameters( drivers::HesaiSensorConfiguration & sensor_configuration, drivers::HesaiCalibrationConfiguration & calibration_configuration, drivers::HesaiCorrection & correction_configuration) { auto sensor_model_ = this->declare_parameter("sensor_model", "", param_read_only()); - sensor_configuration.sensor_model = nebula::drivers::SensorModelFromString(sensor_model_); + sensor_configuration.sensor_model = nebula::drivers::sensor_model_from_string(sensor_model_); auto return_mode_ = this->declare_parameter("return_mode", "", param_read_only()); sensor_configuration.return_mode = - nebula::drivers::ReturnModeFromStringHesai(return_mode_, sensor_configuration.sensor_model); + nebula::drivers::return_mode_from_string_hesai(return_mode_, sensor_configuration.sensor_model); this->declare_parameter("frame_id", "pandar", param_read_only()); sensor_configuration.frame_id = this->get_parameter("frame_id").as_string(); @@ -110,18 +112,18 @@ Status HesaiRosOfflineExtractBag::GetParameters( calibration_configuration.calibration_file = this->declare_parameter("calibration_file", "", param_read_only()); if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - correction_file_path = + correction_file_path_ = this->declare_parameter("correction_file", "", param_read_only()); } - bag_path = this->declare_parameter("bag_path", "", param_read_only()); - storage_id = this->declare_parameter("storage_id", "sqlite3", param_read_only()); - out_path = this->declare_parameter("out_path", "", param_read_only()); - format = this->declare_parameter("format", "cdr", param_read_only()); - out_num = this->declare_parameter("out_num", 3, param_read_only()); - skip_num = this->declare_parameter("skip_num", 3, param_read_only()); - only_xyz = this->declare_parameter("only_xyz", false, param_read_only()); - target_topic = this->declare_parameter("target_topic", "", param_read_only()); + bag_path_ = this->declare_parameter("bag_path", "", param_read_only()); + storage_id_ = this->declare_parameter("storage_id", "sqlite3", param_read_only()); + out_path_ = this->declare_parameter("out_path", "", param_read_only()); + format_ = this->declare_parameter("format", "cdr", param_read_only()); + out_num_ = this->declare_parameter("out_num", 3, param_read_only()); + skip_num_ = this->declare_parameter("skip_num", 3, param_read_only()); + only_xyz_ = this->declare_parameter("only_xyz", false, param_read_only()); + target_topic_ = this->declare_parameter("target_topic", "", param_read_only()); if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { return Status::INVALID_SENSOR_MODEL; @@ -136,7 +138,7 @@ Status HesaiRosOfflineExtractBag::GetParameters( return Status::INVALID_CALIBRATION_FILE; } else { auto cal_status = - calibration_configuration.LoadFromFile(calibration_configuration.calibration_file); + calibration_configuration.load_from_file(calibration_configuration.calibration_file); if (cal_status != Status::OK) { RCLCPP_ERROR_STREAM( this->get_logger(), @@ -145,13 +147,13 @@ Status HesaiRosOfflineExtractBag::GetParameters( } } if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - if (correction_file_path.empty()) { + if (correction_file_path_.empty()) { return Status::INVALID_CALIBRATION_FILE; } else { - auto cal_status = correction_configuration.LoadFromFile(correction_file_path); + auto cal_status = correction_configuration.load_from_file(correction_file_path_); if (cal_status != Status::OK) { RCLCPP_ERROR_STREAM( - this->get_logger(), "Given Correction File: '" << correction_file_path << "'"); + this->get_logger(), "Given Correction File: '" << correction_file_path_ << "'"); return cal_status; } } @@ -161,22 +163,22 @@ Status HesaiRosOfflineExtractBag::GetParameters( return Status::OK; } -Status HesaiRosOfflineExtractBag::ReadBag() +Status HesaiRosOfflineExtractBag::read_bag() { rosbag2_storage::StorageOptions storage_options; rosbag2_cpp::ConverterOptions converter_options; - std::cout << bag_path << std::endl; - std::cout << storage_id << std::endl; - std::cout << out_path << std::endl; - std::cout << format << std::endl; - std::cout << target_topic << std::endl; - std::cout << out_num << std::endl; - std::cout << skip_num << std::endl; - std::cout << only_xyz << std::endl; - - rcpputils::fs::path o_dir(out_path); - auto target_topic_name = target_topic; + std::cout << bag_path_ << std::endl; + std::cout << storage_id_ << std::endl; + std::cout << out_path_ << std::endl; + std::cout << format_ << std::endl; + std::cout << target_topic_ << std::endl; + std::cout << out_num_ << std::endl; + std::cout << skip_num_ << std::endl; + std::cout << only_xyz_ << std::endl; + + rcpputils::fs::path o_dir(out_path_); + auto target_topic_name = target_topic_; if (target_topic_name.substr(0, 1) == "/") { target_topic_name = target_topic_name.substr(1); } @@ -189,9 +191,9 @@ Status HesaiRosOfflineExtractBag::ReadBag() pcl::PCDWriter pcd_writer; std::unique_ptr bag_writer{}; - storage_options.uri = bag_path; - storage_options.storage_id = storage_id; - converter_options.output_serialization_format = format; + storage_options.uri = bag_path_; + storage_options.storage_id = storage_id_; + converter_options.output_serialization_format = format_; rosbag2_cpp::Reader reader(std::make_unique()); reader.open(storage_options, converter_options); @@ -202,11 +204,11 @@ Status HesaiRosOfflineExtractBag::ReadBag() std::cout << "Found topic name " << bag_message->topic_name << std::endl; - if (bag_message->topic_name != target_topic) { + if (bag_message->topic_name != target_topic_) { continue; } - std::cout << (cnt + 1) << ", " << (out_cnt + 1) << "/" << out_num << std::endl; + std::cout << (cnt + 1) << ", " << (out_cnt + 1) << "/" << out_num_ << std::endl; pandar_msgs::msg::PandarScan extracted_msg; rclcpp::Serialization serialization; rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data); @@ -219,7 +221,7 @@ Status HesaiRosOfflineExtractBag::ReadBag() nebula_msg.header = extracted_msg.header; for (auto & pkt : extracted_msg.packets) { std::vector pkt_data(pkt.data.begin(), std::next(pkt.data.begin(), pkt.size)); - auto pointcloud_ts = driver_ptr_->ParseCloudPacket(pkt_data); + auto pointcloud_ts = driver_ptr_->parse_cloud_packet(pkt_data); auto pointcloud = std::get<0>(pointcloud_ts); nebula_msgs::msg::NebulaPacket nebula_pkt; @@ -247,9 +249,9 @@ Status HesaiRosOfflineExtractBag::ReadBag() bag_writer->write(bag_message); cnt++; - if (skip_num < cnt) { + if (skip_num_ < cnt) { out_cnt++; - if (only_xyz) { + if (only_xyz_) { pcl::PointCloud cloud_xyz; pcl::copyPointCloud(*pointcloud, cloud_xyz); pcd_writer.writeBinary((o_dir / fn).string(), cloud_xyz); @@ -258,7 +260,7 @@ Status HesaiRosOfflineExtractBag::ReadBag() } } - if (out_num <= out_cnt) { + if (out_num_ <= out_cnt) { break; } } diff --git a/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd_main.cpp b/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd_main.cpp index db6a66285..04f3a1658 100644 --- a/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd_main.cpp +++ b/nebula_examples/src/hesai/hesai_ros_offline_extract_bag_pcd_main.cpp @@ -32,10 +32,10 @@ int main(int argc, char * argv[]) exec.add_node(hesai_driver->get_node_base_interface()); RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Get Status"); - nebula::Status driver_status = hesai_driver->GetStatus(); + nebula::Status driver_status = hesai_driver->get_status(); if (driver_status == nebula::Status::OK) { RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Reading Started"); - driver_status = hesai_driver->ReadBag(); + driver_status = hesai_driver->read_bag(); } else { RCLCPP_ERROR_STREAM(rclcpp::get_logger(node_name), driver_status); } diff --git a/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp b/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp index c1dd6d78c..dcef69d83 100644 --- a/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp +++ b/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd.cpp @@ -20,6 +20,8 @@ #include "rosbag2_cpp/reader.hpp" #include "rosbag2_cpp/readers/sequential_reader.hpp" #include "rosbag2_storage/storage_options.hpp" + +#include // #include // #include @@ -38,7 +40,7 @@ HesaiRosOfflineExtractSample::HesaiRosOfflineExtractSample( drivers::HesaiCorrection correction_configuration; wrapper_status_ = - GetParameters(sensor_configuration, calibration_configuration, correction_configuration); + get_parameters(sensor_configuration, calibration_configuration, correction_configuration); if (Status::OK != wrapper_status_) { RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error: " << wrapper_status_); return; @@ -53,11 +55,11 @@ HesaiRosOfflineExtractSample::HesaiRosOfflineExtractSample( RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Driver "); if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { correction_cfg_ptr_ = std::make_shared(correction_configuration); - wrapper_status_ = InitializeDriver( + wrapper_status_ = initialize_driver( std::const_pointer_cast(sensor_cfg_ptr_), correction_cfg_ptr_); } else { - wrapper_status_ = InitializeDriver( + wrapper_status_ = initialize_driver( std::const_pointer_cast(sensor_cfg_ptr_), calibration_cfg_ptr_); } @@ -65,7 +67,7 @@ HesaiRosOfflineExtractSample::HesaiRosOfflineExtractSample( RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); } -Status HesaiRosOfflineExtractSample::InitializeDriver( +Status HesaiRosOfflineExtractSample::initialize_driver( std::shared_ptr sensor_configuration, std::shared_ptr calibration_configuration) { @@ -73,24 +75,24 @@ Status HesaiRosOfflineExtractSample::InitializeDriver( driver_ptr_ = std::make_shared( std::static_pointer_cast(sensor_configuration), calibration_configuration); - return driver_ptr_->GetStatus(); + return driver_ptr_->get_status(); } -Status HesaiRosOfflineExtractSample::GetStatus() +Status HesaiRosOfflineExtractSample::get_status() { return wrapper_status_; } -Status HesaiRosOfflineExtractSample::GetParameters( +Status HesaiRosOfflineExtractSample::get_parameters( drivers::HesaiSensorConfiguration & sensor_configuration, drivers::HesaiCalibrationConfiguration & calibration_configuration, drivers::HesaiCorrection & correction_configuration) { auto sensor_model_ = this->declare_parameter("sensor_model", ""); - sensor_configuration.sensor_model = nebula::drivers::SensorModelFromString(sensor_model_); + sensor_configuration.sensor_model = nebula::drivers::sensor_model_from_string(sensor_model_); auto return_mode_ = this->declare_parameter("return_mode", "", param_read_only()); sensor_configuration.return_mode = - nebula::drivers::ReturnModeFromStringHesai(return_mode_, sensor_configuration.sensor_model); + nebula::drivers::return_mode_from_string_hesai(return_mode_, sensor_configuration.sensor_model); sensor_configuration.frame_id = declare_parameter("frame_id", "pandar", param_read_only()); @@ -112,14 +114,15 @@ Status HesaiRosOfflineExtractSample::GetParameters( calibration_configuration.calibration_file = declare_parameter("calibration_file", "", param_read_only()); if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - correction_file_path = declare_parameter("correction_file", "", param_read_only()); + correction_file_path_ = + declare_parameter("correction_file", "", param_read_only()); } - bag_path = declare_parameter("bag_path", "", param_read_only()); - storage_id = declare_parameter("storage_id", "sqlite3", param_read_only()); - out_path = declare_parameter("out_path", "", param_read_only()); - format = declare_parameter("format", "cdr", param_read_only()); - target_topic = declare_parameter("target_topic", "", param_read_only()); + bag_path_ = declare_parameter("bag_path", "", param_read_only()); + storage_id_ = declare_parameter("storage_id", "sqlite3", param_read_only()); + out_path_ = declare_parameter("out_path", "", param_read_only()); + format_ = declare_parameter("format", "cdr", param_read_only()); + target_topic_ = declare_parameter("target_topic", "", param_read_only()); if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { return Status::INVALID_SENSOR_MODEL; @@ -134,7 +137,7 @@ Status HesaiRosOfflineExtractSample::GetParameters( return Status::INVALID_CALIBRATION_FILE; } else { auto cal_status = - calibration_configuration.LoadFromFile(calibration_configuration.calibration_file); + calibration_configuration.load_from_file(calibration_configuration.calibration_file); if (cal_status != Status::OK) { RCLCPP_ERROR_STREAM( this->get_logger(), @@ -143,13 +146,13 @@ Status HesaiRosOfflineExtractSample::GetParameters( } } if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - if (correction_file_path.empty()) { + if (correction_file_path_.empty()) { return Status::INVALID_CALIBRATION_FILE; } else { - auto cal_status = correction_configuration.LoadFromFile(correction_file_path); + auto cal_status = correction_configuration.load_from_file(correction_file_path_); if (cal_status != Status::OK) { RCLCPP_ERROR_STREAM( - this->get_logger(), "Given Correction File: '" << correction_file_path << "'"); + this->get_logger(), "Given Correction File: '" << correction_file_path_ << "'"); return cal_status; } } @@ -159,19 +162,19 @@ Status HesaiRosOfflineExtractSample::GetParameters( return Status::OK; } -Status HesaiRosOfflineExtractSample::ReadBag() +Status HesaiRosOfflineExtractSample::read_bag() { rosbag2_storage::StorageOptions storage_options; rosbag2_cpp::ConverterOptions converter_options; - std::cout << bag_path << std::endl; - std::cout << storage_id << std::endl; - std::cout << out_path << std::endl; - std::cout << format << std::endl; - std::cout << target_topic << std::endl; + std::cout << bag_path_ << std::endl; + std::cout << storage_id_ << std::endl; + std::cout << out_path_ << std::endl; + std::cout << format_ << std::endl; + std::cout << target_topic_ << std::endl; - rcpputils::fs::path o_dir(out_path); - auto target_topic_name = target_topic; + rcpputils::fs::path o_dir(out_path_); + auto target_topic_name = target_topic_; if (target_topic_name.substr(0, 1) == "/") { target_topic_name = target_topic_name.substr(1); } @@ -183,9 +186,9 @@ Status HesaiRosOfflineExtractSample::ReadBag() pcl::PCDWriter writer; - storage_options.uri = bag_path; - storage_options.storage_id = storage_id; - converter_options.output_serialization_format = format; + storage_options.uri = bag_path_; + storage_options.storage_id = storage_id_; + converter_options.output_serialization_format = format_; rosbag2_cpp::Reader reader(std::make_unique()); reader.open(storage_options, converter_options); @@ -194,7 +197,7 @@ Status HesaiRosOfflineExtractSample::ReadBag() std::cout << "Found topic name " << bag_message->topic_name << std::endl; - if (bag_message->topic_name != target_topic) { + if (bag_message->topic_name != target_topic_) { continue; } @@ -207,7 +210,7 @@ Status HesaiRosOfflineExtractSample::ReadBag() << bag_message->time_stamp << std::endl; for (auto & pkt : extracted_msg.packets) { - auto pointcloud_ts = driver_ptr_->ParseCloudPacket( + auto pointcloud_ts = driver_ptr_->parse_cloud_packet( std::vector(pkt.data.begin(), std::next(pkt.data.begin(), pkt.size))); auto pointcloud = std::get<0>(pointcloud_ts); diff --git a/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd_main.cpp b/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd_main.cpp index a4839dac0..d7a0be90b 100644 --- a/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd_main.cpp +++ b/nebula_examples/src/hesai/hesai_ros_offline_extract_pcd_main.cpp @@ -33,10 +33,10 @@ int main(int argc, char * argv[]) exec.add_node(hesai_driver->get_node_base_interface()); RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Get Status"); - nebula::Status driver_status = hesai_driver->GetStatus(); + nebula::Status driver_status = hesai_driver->get_status(); if (driver_status == nebula::Status::OK) { RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Reading Started"); - driver_status = hesai_driver->ReadBag(); + driver_status = hesai_driver->read_bag(); } else { RCLCPP_ERROR_STREAM(rclcpp::get_logger(node_name), driver_status); } diff --git a/nebula_examples/src/velodyne/velodyne_ros_offline_extract_bag_pcd.cpp b/nebula_examples/src/velodyne/velodyne_ros_offline_extract_bag_pcd.cpp index 5c9c67e39..37e78a07b 100644 --- a/nebula_examples/src/velodyne/velodyne_ros_offline_extract_bag_pcd.cpp +++ b/nebula_examples/src/velodyne/velodyne_ros_offline_extract_bag_pcd.cpp @@ -30,7 +30,7 @@ VelodyneRosOfflineExtractBag::VelodyneRosOfflineExtractBag( drivers::VelodyneCalibrationConfiguration calibration_configuration; drivers::VelodyneSensorConfiguration sensor_configuration; - wrapper_status_ = GetParameters(sensor_configuration, calibration_configuration); + wrapper_status_ = get_parameters(sensor_configuration, calibration_configuration); if (Status::OK != wrapper_status_) { RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error: " << wrapper_status_); return; @@ -44,27 +44,27 @@ VelodyneRosOfflineExtractBag::VelodyneRosOfflineExtractBag( std::make_shared(sensor_configuration); RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Driver "); - wrapper_status_ = InitializeDriver(sensor_cfg_ptr_, calibration_cfg_ptr_); + wrapper_status_ = initialize_driver(sensor_cfg_ptr_, calibration_cfg_ptr_); RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); } -Status VelodyneRosOfflineExtractBag::InitializeDriver( +Status VelodyneRosOfflineExtractBag::initialize_driver( std::shared_ptr sensor_configuration, std::shared_ptr calibration_configuration) { // driver should be initialized here with proper decoder driver_ptr_ = std::make_shared(sensor_configuration, calibration_configuration); - return driver_ptr_->GetStatus(); + return driver_ptr_->get_status(); } -Status VelodyneRosOfflineExtractBag::GetStatus() +Status VelodyneRosOfflineExtractBag::get_status() { return wrapper_status_; } -Status VelodyneRosOfflineExtractBag::GetParameters( +Status VelodyneRosOfflineExtractBag::get_parameters( drivers::VelodyneSensorConfiguration & sensor_configuration, drivers::VelodyneCalibrationConfiguration & calibration_configuration) { @@ -76,7 +76,7 @@ Status VelodyneRosOfflineExtractBag::GetParameters( descriptor.additional_constraints = ""; this->declare_parameter("sensor_model", ""); sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); + nebula::drivers::sensor_model_from_string(this->get_parameter("sensor_model").as_string()); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -86,7 +86,7 @@ Status VelodyneRosOfflineExtractBag::GetParameters( descriptor.additional_constraints = ""; this->declare_parameter("return_mode", "", descriptor); sensor_configuration.return_mode = - nebula::drivers::ReturnModeFromString(this->get_parameter("return_mode").as_string()); + nebula::drivers::return_mode_from_string(this->get_parameter("return_mode").as_string()); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -192,7 +192,7 @@ Status VelodyneRosOfflineExtractBag::GetParameters( descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; this->declare_parameter("bag_path", "", descriptor); - bag_path = this->get_parameter("bag_path").as_string(); + bag_path_ = this->get_parameter("bag_path").as_string(); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -201,7 +201,7 @@ Status VelodyneRosOfflineExtractBag::GetParameters( descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; this->declare_parameter("storage_id", "sqlite3", descriptor); - storage_id = this->get_parameter("storage_id").as_string(); + storage_id_ = this->get_parameter("storage_id").as_string(); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -210,7 +210,7 @@ Status VelodyneRosOfflineExtractBag::GetParameters( descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; this->declare_parameter("out_path", "", descriptor); - out_path = this->get_parameter("out_path").as_string(); + out_path_ = this->get_parameter("out_path").as_string(); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -219,7 +219,7 @@ Status VelodyneRosOfflineExtractBag::GetParameters( descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; this->declare_parameter("format", "cdr", descriptor); - format = this->get_parameter("format").as_string(); + format_ = this->get_parameter("format").as_string(); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -228,7 +228,7 @@ Status VelodyneRosOfflineExtractBag::GetParameters( descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; this->declare_parameter("out_num", 3, descriptor); - out_num = this->get_parameter("out_num").as_int(); + out_num_ = this->get_parameter("out_num").as_int(); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -237,7 +237,7 @@ Status VelodyneRosOfflineExtractBag::GetParameters( descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; this->declare_parameter("skip_num", 3, descriptor); - skip_num = this->get_parameter("skip_num").as_int(); + skip_num_ = this->get_parameter("skip_num").as_int(); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -246,7 +246,7 @@ Status VelodyneRosOfflineExtractBag::GetParameters( descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; this->declare_parameter("only_xyz", false, descriptor); - only_xyz = this->get_parameter("only_xyz").as_bool(); + only_xyz_ = this->get_parameter("only_xyz").as_bool(); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -255,7 +255,7 @@ Status VelodyneRosOfflineExtractBag::GetParameters( descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; this->declare_parameter("target_topic", "", descriptor); - target_topic = this->get_parameter("target_topic").as_string(); + target_topic_ = this->get_parameter("target_topic").as_string(); } if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { @@ -272,7 +272,7 @@ Status VelodyneRosOfflineExtractBag::GetParameters( return Status::INVALID_CALIBRATION_FILE; } else { auto cal_status = - calibration_configuration.LoadFromFile(calibration_configuration.calibration_file); + calibration_configuration.load_from_file(calibration_configuration.calibration_file); if (cal_status != Status::OK) { RCLCPP_ERROR_STREAM( this->get_logger(), @@ -288,22 +288,22 @@ Status VelodyneRosOfflineExtractBag::GetParameters( return Status::OK; } -Status VelodyneRosOfflineExtractBag::ReadBag() +Status VelodyneRosOfflineExtractBag::read_bag() { rosbag2_storage::StorageOptions storage_options; rosbag2_cpp::ConverterOptions converter_options; - std::cout << bag_path << std::endl; - std::cout << storage_id << std::endl; - std::cout << out_path << std::endl; - std::cout << format << std::endl; - std::cout << target_topic << std::endl; - std::cout << out_num << std::endl; - std::cout << skip_num << std::endl; - std::cout << only_xyz << std::endl; + std::cout << bag_path_ << std::endl; + std::cout << storage_id_ << std::endl; + std::cout << out_path_ << std::endl; + std::cout << format_ << std::endl; + std::cout << target_topic_ << std::endl; + std::cout << out_num_ << std::endl; + std::cout << skip_num_ << std::endl; + std::cout << only_xyz_ << std::endl; - rcpputils::fs::path o_dir(out_path); - auto target_topic_name = target_topic; + rcpputils::fs::path o_dir(out_path_); + auto target_topic_name = target_topic_; if (target_topic_name.substr(0, 1) == "/") { target_topic_name = target_topic_name.substr(1); } @@ -314,13 +314,13 @@ Status VelodyneRosOfflineExtractBag::ReadBag() } // return Status::OK; - pcl::PCDWriter writer; + pcl::PCDWriter pcd_writer; - std::unique_ptr writer_; + std::unique_ptr bag_writer; bool needs_open = true; - storage_options.uri = bag_path; - storage_options.storage_id = storage_id; - converter_options.output_serialization_format = format; // "cdr"; + storage_options.uri = bag_path_; + storage_options.storage_id = storage_id_; + converter_options.output_serialization_format = format_; // "cdr"; { rosbag2_cpp::Reader reader(std::make_unique()); reader.open(storage_options, converter_options); @@ -331,8 +331,8 @@ Status VelodyneRosOfflineExtractBag::ReadBag() std::cout << "Found topic name " << bag_message->topic_name << std::endl; - if (bag_message->topic_name == target_topic) { - std::cout << (cnt + 1) << ", " << (out_cnt + 1) << "/" << out_num << std::endl; + if (bag_message->topic_name == target_topic_) { + std::cout << (cnt + 1) << ", " << (out_cnt + 1) << "/" << out_num_ << std::endl; velodyne_msgs::msg::VelodyneScan extracted_msg; rclcpp::Serialization serialization; rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data); @@ -346,7 +346,7 @@ Status VelodyneRosOfflineExtractBag::ReadBag() // std::make_shared(extracted_msg)); for (auto & pkt : extracted_msg.packets) { - auto pointcloud_ts = driver_ptr_->ParseCloudPacket( + auto pointcloud_ts = driver_ptr_->parse_cloud_packet( std::vector(pkt.data.begin(), std::next(pkt.data.begin(), pkt.data.size())), pkt.stamp.sec); auto pointcloud = std::get<0>(pointcloud_ts); @@ -362,26 +362,26 @@ Status VelodyneRosOfflineExtractBag::ReadBag() {(o_dir / std::to_string(bag_message->time_stamp)).string(), "sqlite3"}); const rosbag2_cpp::ConverterOptions converter_options_w( {rmw_get_serialization_format(), rmw_get_serialization_format()}); - writer_ = std::make_unique(); - writer_->open(storage_options_w, converter_options_w); - writer_->create_topic( + bag_writer = std::make_unique(); + bag_writer->open(storage_options_w, converter_options_w); + bag_writer->create_topic( {bag_message->topic_name, "velodyne_msgs/msg/VelodyneScan", rmw_get_serialization_format(), ""}); needs_open = false; } - writer_->write(bag_message); + bag_writer->write(bag_message); cnt++; - if (skip_num < cnt) { + if (skip_num_ < cnt) { out_cnt++; - if (only_xyz) { + if (only_xyz_) { pcl::PointCloud cloud_xyz; pcl::copyPointCloud(*pointcloud, cloud_xyz); - writer.writeBinary((o_dir / fn).string(), cloud_xyz); + pcd_writer.writeBinary((o_dir / fn).string(), cloud_xyz); } else { - writer.writeBinary((o_dir / fn).string(), *pointcloud); + pcd_writer.writeBinary((o_dir / fn).string(), *pointcloud); } } - if (out_num <= out_cnt) { + if (out_num_ <= out_cnt) { break; } } diff --git a/nebula_examples/src/velodyne/velodyne_ros_offline_extract_bag_pcd_main.cpp b/nebula_examples/src/velodyne/velodyne_ros_offline_extract_bag_pcd_main.cpp index bea1fc950..b383626e1 100644 --- a/nebula_examples/src/velodyne/velodyne_ros_offline_extract_bag_pcd_main.cpp +++ b/nebula_examples/src/velodyne/velodyne_ros_offline_extract_bag_pcd_main.cpp @@ -33,10 +33,10 @@ int main(int argc, char * argv[]) exec.add_node(velodyne_driver->get_node_base_interface()); RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Get Status"); - nebula::Status driver_status = velodyne_driver->GetStatus(); + nebula::Status driver_status = velodyne_driver->get_status(); if (driver_status == nebula::Status::OK) { RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Reading Started"); - driver_status = velodyne_driver->ReadBag(); + driver_status = velodyne_driver->read_bag(); // exec.spin(); } else { RCLCPP_ERROR_STREAM(rclcpp::get_logger(node_name), driver_status); diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp index 9044bf5c6..bf2fc26ba 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_common/nebula_hw_interface_base.hpp @@ -34,7 +34,7 @@ class NebulaHwInterfaceBase * @param buffer Buffer containing the data received from the UDP socket * @return Status::OK if no error occurred. */ - virtual void ReceiveSensorPacketCallback([[maybe_unused]] std::vector & buffer) {} + virtual void receive_sensor_packet_callback([[maybe_unused]] std::vector & buffer) {} // virtual Status RegisterScanCallback( // std::function>>)> scan_callback) = 0; @@ -48,29 +48,29 @@ class NebulaHwInterfaceBase /// @brief Virtual function for starting the interface that handles UDP streams /// @return Resulting status - virtual Status SensorInterfaceStart() = 0; + virtual Status sensor_interface_start() = 0; /// @brief Virtual function for stopping the interface that handles UDP streams /// @return Resulting status - virtual Status SensorInterfaceStop() = 0; + virtual Status sensor_interface_stop() = 0; // You may want to also implement GpsInterfaceStart() and ReceiveGpsCallback, but that is sensor // specific. /// @brief Virtual function for setting sensor configuration /// @param sensor_configuration SensorConfiguration for this interface /// @return Resulting status - virtual Status SetSensorConfiguration( + virtual Status set_sensor_configuration( std::shared_ptr sensor_configuration) = 0; /// @brief Virtual function for printing sensor configuration /// @param sensor_configuration SensorConfiguration for the checking /// @return Resulting status - virtual Status GetSensorConfiguration(SensorConfigurationBase & sensor_configuration) = 0; + virtual Status get_sensor_configuration(SensorConfigurationBase & sensor_configuration) = 0; /// @brief Virtual function for printing calibration configuration /// @param calibration_configuration CalibrationConfiguration for the checking /// @return Resulting status - virtual Status GetCalibrationConfiguration( + virtual Status get_calibration_configuration( [[maybe_unused]] CalibrationConfigurationBase & calibration_configuration) { return Status::NOT_IMPLEMENTED; diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp index 0563dc605..b840442e6 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_ars548_hw_interface.hpp @@ -42,22 +42,22 @@ class ContinentalARS548HwInterface /// @brief Starting the interface that handles UDP streams /// @return Resulting status - Status SensorInterfaceStart(); + Status sensor_interface_start(); /// @brief Function for stopping the interface that handles UDP streams /// @return Resulting status - Status SensorInterfaceStop(); + Status sensor_interface_stop(); /// @brief Setting sensor configuration /// @param sensor_configuration SensorConfiguration for this interface /// @return Resulting status - Status SetSensorConfiguration( + Status set_sensor_configuration( std::shared_ptr sensor_configuration); /// @brief Registering callback /// @param callback Callback function /// @return Resulting status - Status RegisterPacketCallback( + Status register_packet_callback( std::function)> packet_callback); /// @brief Set the sensor mounting parameters @@ -68,7 +68,7 @@ class ContinentalARS548HwInterface /// @param pitch_autosar Desired pitch value in autosar coordinates /// @param plug_orientation Desired plug orientation (0 = PLUG_RIGHT, 1 = PLUG_LEFT) /// @return Resulting status - Status SetSensorMounting( + Status set_sensor_mounting( float longitudinal_autosar, float lateral_autosar, float vertical_autosar, float yaw_autosar, float pitch_autosar, uint8_t plug_orientation); @@ -78,7 +78,7 @@ class ContinentalARS548HwInterface /// @param height_autosar Desired height value /// @param wheel_base_autosar Desired wheel base value /// @return Resulting status - Status SetVehicleParameters( + Status set_vehicle_parameters( float length_autosar, float width_autosar, float height_autosar, float wheel_base_autosar); /// @brief Set the radar parameters @@ -90,75 +90,75 @@ class ContinentalARS548HwInterface /// @param hcc Desired hcc value (1 = Worldwide, 2 = Japan) /// @param power_save_standstill Desired power_save_standstill value (0 = Off, 1 = On) /// @return Resulting status - Status SetRadarParameters( + Status set_radar_parameters( uint16_t maximum_distance, uint8_t frequency_slot, uint8_t cycle_time, uint8_t time_slot, uint8_t hcc, uint8_t power_save_standstill); /// @brief Set the sensor ip address /// @param sensor_ip_address Desired sensor ip address /// @return Resulting status - Status SetSensorIPAddress(const std::string & sensor_ip_address); + Status set_sensor_ip_address(const std::string & sensor_ip_address); /// @brief Set the current lateral acceleration /// @param lateral_acceleration Current lateral acceleration /// @return Resulting status - Status SetAccelerationLateralCog(float lateral_acceleration); + Status set_acceleration_lateral_cog(float lateral_acceleration); /// @brief Set the current longitudinal acceleration /// @param longitudinal_acceleration Current longitudinal acceleration /// @return Resulting status - Status SetAccelerationLongitudinalCog(float longitudinal_acceleration); + Status set_acceleration_longitudinal_cog(float longitudinal_acceleration); /// @brief Set the characteristic speed /// @param characteristic_speed Characteristic speed /// @return Resulting status - Status SetCharacteristicSpeed(float characteristic_speed); + Status set_characteristic_speed(float characteristic_speed); /// @brief Set the current direction /// @param direction Current driving direction /// @return Resulting status - Status SetDrivingDirection(int direction); + Status set_driving_direction(int direction); /// @brief Set the current steering angle /// @param angle_rad Current steering angle in radians /// @return Resulting status - Status SetSteeringAngleFrontAxle(float angle_rad); + Status set_steering_angle_front_axle(float angle_rad); /// @brief Set the current vehicle velocity /// @param velocity Current vehicle velocity /// @return Resulting status - Status SetVelocityVehicle(float velocity); + Status set_velocity_vehicle(float velocity); /// @brief Set the current yaw rate /// @param yaw_rate Current yaw rate /// @return Resulting status - Status SetYawRate(float yaw_rate); + Status set_yaw_rate(float yaw_rate); /// @brief Setting rclcpp::Logger /// @param node Logger - void SetLogger(std::shared_ptr node); + void set_logger(std::shared_ptr node); private: /// @brief Printing the string to RCLCPP_INFO_STREAM /// @param info Target string - void PrintInfo(std::string info); + void print_info(std::string info); /// @brief Printing the string to RCLCPP_ERROR_STREAM /// @param error Target string - void PrintError(std::string error); + void print_error(std::string error); /// @brief Printing the string to RCLCPP_DEBUG_STREAM /// @param debug Target string - void PrintDebug(std::string debug); + void print_debug(std::string debug); /// @brief Callback function to receive the Cloud Packet data from the UDP Driver /// @param buffer Buffer containing the data received from the UDP socket - void ReceiveSensorPacketCallbackWithSender( + void receive_sensor_packet_callback_with_sender( std::vector & buffer, const std::string & sender_ip); /// @brief Callback function to receive the Cloud Packet data from the UDP Driver /// @param buffer Buffer containing the data received from the UDP socket - void ReceiveSensorPacketCallback(std::vector & buffer); + void receive_sensor_packet_callback(std::vector & buffer); std::unique_ptr<::drivers::common::IoContext> sensor_io_context_ptr_; std::unique_ptr<::drivers::udp_driver::UdpDriver> sensor_udp_driver_ptr_; diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp index ddbbe1619..7fba579ca 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_continental/continental_srr520_hw_interface.hpp @@ -45,16 +45,16 @@ class ContinentalSRR520HwInterface /// @brief Starting the interface that handles UDP streams /// @return Resulting status - Status SensorInterfaceStart(); + Status sensor_interface_start(); /// @brief Function for stopping the interface that handles UDP streams /// @return Resulting status - Status SensorInterfaceStop(); + Status sensor_interface_stop(); /// @brief Setting sensor configuration /// @param sensor_configuration SensorConfiguration for this interface /// @return Resulting status - Status SetSensorConfiguration( + Status set_sensor_configuration( const std::shared_ptr< const nebula::drivers::continental_srr520::ContinentalSRR520SensorConfiguration> new_config_ptr); @@ -62,14 +62,14 @@ class ContinentalSRR520HwInterface /// @brief Registering callback /// @param callback Callback function /// @return Resulting status - Status RegisterPacketCallback( + Status register_packet_callback( std::function)> packet_callback); /// @brief Sensor synchronization routine - void SensorSync(); + void sensor_sync(); /// @brief Process a new Sync Follow-up request - void SensorSyncFollowUp(builtin_interfaces::msg::Time stamp); + void sensor_sync_follow_up(builtin_interfaces::msg::Time stamp); /// @brief Configure the sensor /// @param sensor_id Desired sensor id @@ -83,7 +83,7 @@ class ContinentalSRR520HwInterface /// @param plug_bottom Desired plug bottom /// @param reset Rest the sensor to its default values /// @return Resulting status - Status ConfigureSensor( + Status configure_sensor( uint8_t sensor_id, float longitudinal_autosar, float lateral_autosar, float vertical_autosar, float yaw_autosar, float longitudinal_cog, float wheelbase, float cover_damping, bool plug_bottom, bool reset); @@ -95,34 +95,34 @@ class ContinentalSRR520HwInterface /// @param longitudinal_velocity Longitudinal velocity /// @param driving_direction Driving direction /// @return Resulting status - Status SetVehicleDynamics( + Status set_vehicle_dynamics( float longitudinal_acceleration, float lateral_acceleration, float yaw_rate, float longitudinal_velocity, bool standstill); /// @brief Setting rclcpp::Logger /// @param node Logger - void SetLogger(std::shared_ptr node); + void set_logger(std::shared_ptr node); private: /// @brief Send a Fd frame /// @param data a buffer containing the data to send template - bool SendFrame(const std::array & data, int can_frame_id); + bool send_frame(const std::array & data, int can_frame_id); /// @brief Printing the string to RCLCPP_INFO_STREAM /// @param info Target string - void PrintInfo(std::string info); + void print_info(std::string info); /// @brief Printing the string to RCLCPP_ERROR_STREAM /// @param error Target string - void PrintError(std::string error); + void print_error(std::string error); /// @brief Printing the string to RCLCPP_DEBUG_STREAM /// @param debug Target string - void PrintDebug(std::string debug); + void print_debug(std::string debug); /// @brief Main loop of the CAN receiver thread - void ReceiveLoop(); + void receive_loop(); std::unique_ptr<::drivers::socketcan::SocketCanReceiver> can_receiver_ptr_; std::unique_ptr<::drivers::socketcan::SocketCanSender> can_sender_ptr_; diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_robosense/robosense_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_robosense/robosense_hw_interface.hpp index 7ae25bf21..5642398b2 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_robosense/robosense_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_robosense/robosense_hw_interface.hpp @@ -37,11 +37,11 @@ namespace nebula { namespace drivers { -constexpr uint16_t MTU_SIZE = 1248; -constexpr uint16_t HELIOS_PACKET_SIZE = 1248; -constexpr uint16_t HELIOS_INFO_PACKET_SIZE = 1248; -constexpr uint16_t BPEARL_PACKET_SIZE = 1248; -constexpr uint16_t BPEARL_INFO_PACKET_SIZE = 1248; +constexpr uint16_t mtu_size = 1248; +constexpr uint16_t helios_packet_size = 1248; +constexpr uint16_t helios_info_packet_size = 1248; +constexpr uint16_t bpearl_packet_size = 1248; +constexpr uint16_t bpearl_info_packet_size = 1248; /// @brief Hardware interface of Robosense driver class RobosenseHwInterface @@ -60,11 +60,11 @@ class RobosenseHwInterface /// @brief Printing the string to RCLCPP_INFO_STREAM /// @param info Target string - void PrintInfo(std::string info); + void print_info(std::string info); /// @brief Printing the string to RCLCPP_DEBUG_STREAM /// @param debug Target string - void PrintDebug(std::string debug); + void print_debug(std::string debug); public: /// @brief Constructor @@ -72,39 +72,39 @@ class RobosenseHwInterface /// @brief Callback function to receive the Cloud Packet data from the UDP Driver /// @param buffer Buffer containing the data received from the UDP socket - void ReceiveSensorPacketCallback(std::vector & buffer); + void receive_sensor_packet_callback(std::vector & buffer); /// @brief Callback function to receive the Info Packet data from the UDP Driver /// @param buffer Buffer containing the data received from the UDP socket - void ReceiveInfoPacketCallback(std::vector & buffer); + void receive_info_packet_callback(std::vector & buffer); /// @brief Starting the interface that handles UDP streams for MSOP packets /// @return Resulting status - Status SensorInterfaceStart(); + Status sensor_interface_start(); /// @brief Starting the interface that handles UDP streams for DIFOP packets /// @return Resulting status - Status InfoInterfaceStart(); + Status info_interface_start(); /// @brief Setting sensor configuration /// @param sensor_configuration SensorConfiguration for this interface /// @return Resulting status - Status SetSensorConfiguration( + Status set_sensor_configuration( std::shared_ptr sensor_configuration); /// @brief Registering callback for RobosenseScan /// @param scan_callback Callback function /// @return Resulting status - Status RegisterScanCallback(std::function &)> scan_callback); + Status register_scan_callback(std::function &)> scan_callback); /// @brief Registering callback for RobosensePacket /// @param scan_callback Callback function /// @return Resulting status - Status RegisterInfoCallback(std::function &)> info_callback); + Status register_info_callback(std::function &)> info_callback); /// @brief Setting rclcpp::Logger /// @param node Logger - void SetLogger(std::shared_ptr logger); + void set_logger(std::shared_ptr logger); }; } // namespace drivers diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_velodyne/velodyne_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_velodyne/velodyne_hw_interface.hpp index 3911b37d3..0d7c3650b 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_velodyne/velodyne_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_velodyne/velodyne_hw_interface.hpp @@ -60,52 +60,52 @@ class VelodyneHwInterface std::mutex mtx_inflight_request_; - std::string TARGET_STATUS{"/cgi/status.json"}; - std::string TARGET_DIAG{"/cgi/diag.json"}; - std::string TARGET_SNAPSHOT{"/cgi/snapshot.hdl"}; - std::string TARGET_SETTING{"/cgi/setting"}; - std::string TARGET_FOV{"/cgi/setting/fov"}; - std::string TARGET_HOST{"/cgi/setting/host"}; - std::string TARGET_NET{"/cgi/setting/net"}; - std::string TARGET_SAVE{"/cgi/save"}; - std::string TARGET_RESET{"/cgi/reset"}; - void StringCallback(const std::string & str); - - std::string HttpGetRequest(const std::string & endpoint); - std::string HttpPostRequest(const std::string & endpoint, const std::string & body); + std::string target_status_{"/cgi/status.json"}; + std::string target_diag_{"/cgi/diag.json"}; + std::string target_snapshot_{"/cgi/snapshot.hdl"}; + std::string target_setting_{"/cgi/setting"}; + std::string target_fov_{"/cgi/setting/fov"}; + std::string target_host_{"/cgi/setting/host"}; + std::string target_net_{"/cgi/setting/net"}; + std::string target_save_{"/cgi/save"}; + std::string target_reset_{"/cgi/reset"}; + void string_callback(const std::string & str); + + std::string http_get_request(const std::string & endpoint); + std::string http_post_request(const std::string & endpoint, const std::string & body); /// @brief Get a one-off HTTP client to communicate with the hardware /// @param ctx IO Context /// @param hcd Got http client driver /// @return Resulting status - VelodyneStatus GetHttpClientDriverOnce( + VelodyneStatus get_http_client_driver_once( std::shared_ptr ctx, std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> & hcd); /// @brief Get a one-off HTTP client to communicate with the hardware (without specifying /// io_context) /// @param hcd Got http client driver /// @return Resulting status - VelodyneStatus GetHttpClientDriverOnce( + VelodyneStatus get_http_client_driver_once( std::unique_ptr<::drivers::tcp_driver::HttpClientDriver> & hcd); /// @brief Checking the current settings and changing the difference point /// @param sensor_configuration Current SensorConfiguration /// @param tree Current settings (property_tree) /// @return Resulting status - VelodyneStatus CheckAndSetConfig( + VelodyneStatus check_and_set_config( std::shared_ptr sensor_configuration, boost::property_tree::ptree tree); - std::shared_ptr parent_node_logger; + std::shared_ptr parent_node_logger_; /// @brief Printing the string to RCLCPP_INFO_STREAM /// @param info Target string - void PrintInfo(std::string info); + void print_info(std::string info); /// @brief Printing the string to RCLCPP_ERROR_STREAM /// @param error Target string - void PrintError(std::string error); + void print_error(std::string error); /// @brief Printing the string to RCLCPP_DEBUG_STREAM /// @param debug Target string - void PrintDebug(std::string debug); + void print_debug(std::string debug); public: /// @brief Constructor @@ -113,118 +113,118 @@ class VelodyneHwInterface /// @brief Callback function to receive the Cloud Packet data from the UDP Driver /// @param buffer Buffer containing the data received from the UDP socket - void ReceiveSensorPacketCallback(std::vector & buffer); + void receive_sensor_packet_callback(std::vector & buffer); /// @brief Starting the interface that handles UDP streams /// @return Resulting status - Status SensorInterfaceStart(); + Status sensor_interface_start(); /// @brief Function for stopping the interface that handles UDP streams /// @return Resulting status - Status SensorInterfaceStop(); + Status sensor_interface_stop(); /// @brief Printing sensor configuration /// @param sensor_configuration SensorConfiguration for this interface /// @return Resulting status - Status GetSensorConfiguration(SensorConfigurationBase & sensor_configuration); + Status get_sensor_configuration(SensorConfigurationBase & sensor_configuration); /// @brief Printing calibration configuration /// @param calibration_configuration CalibrationConfiguration for the checking /// @return Resulting status - Status GetCalibrationConfiguration(CalibrationConfigurationBase & calibration_configuration); + Status get_calibration_configuration(CalibrationConfigurationBase & calibration_configuration); /// @brief Initializing sensor configuration /// @param sensor_configuration SensorConfiguration for this interface /// @return Resulting status - Status InitializeSensorConfiguration( + Status initialize_sensor_configuration( std::shared_ptr sensor_configuration); /// @brief Setting sensor configuration with InitializeSensorConfiguration & /// CheckAndSetConfigBySnapshotAsync /// @param sensor_configuration SensorConfiguration for this interface /// @return Resulting status - Status SetSensorConfiguration( + Status set_sensor_configuration( std::shared_ptr sensor_configuration); /// @brief Registering callback for PandarScan /// @param scan_callback Callback function /// @return Resulting status - Status RegisterScanCallback(std::function & packet)> scan_callback); + Status register_scan_callback(std::function & packet)> scan_callback); /// @brief Parsing JSON string to property_tree /// @param str JSON string /// @return property_tree - boost::property_tree::ptree ParseJson(const std::string & str); + boost::property_tree::ptree parse_json(const std::string & str); /// @brief Initializing HTTP client (sync) /// @return Resulting status - VelodyneStatus InitHttpClient(); + VelodyneStatus init_http_client(); /// @brief Getting the current operational state and parameters of the sensor (sync) /// @return Resulting JSON string - std::string GetStatus(); + std::string get_status(); /// @brief Getting diagnostic information from the sensor (sync) /// @return Resulting JSON string - std::string GetDiag(); + std::string get_diag(); /// @brief Getting current sensor configuration and status data (sync) /// @return Resulting JSON string - std::string GetSnapshot(); + std::string get_snapshot(); /// @brief Setting Motor RPM (sync) /// @param rpm the RPM of the motor /// @return Resulting status - VelodyneStatus SetRpm(uint16_t rpm); + VelodyneStatus set_rpm(uint16_t rpm); /// @brief Setting Field of View Start (sync) /// @param fov_start FOV start /// @return Resulting status - VelodyneStatus SetFovStart(uint16_t fov_start); + VelodyneStatus set_fov_start(uint16_t fov_start); /// @brief Setting Field of View End (sync) /// @param fov_end FOV end /// @return Resulting status - VelodyneStatus SetFovEnd(uint16_t fov_end); + VelodyneStatus set_fov_end(uint16_t fov_end); /// @brief Setting Return Type (sync) /// @param return_mode ReturnMode /// @return Resulting status - VelodyneStatus SetReturnType(ReturnMode return_mode); + VelodyneStatus set_return_type(ReturnMode return_mode); /// @brief Save Configuration to the LiDAR memory (sync) /// @return Resulting status - VelodyneStatus SaveConfig(); + VelodyneStatus save_config(); /// @brief Resets the sensor (sync) /// @return Resulting status - VelodyneStatus ResetSystem(); + VelodyneStatus reset_system(); /// @brief Turn laser state on (sync) /// @return Resulting status - VelodyneStatus LaserOn(); + VelodyneStatus laser_on(); /// @brief Turn laser state off (sync) /// @return Resulting status - VelodyneStatus LaserOff(); + VelodyneStatus laser_off(); /// @brief Turn laser state on/off (sync) /// @param on is ON /// @return Resulting status - VelodyneStatus LaserOnOff(bool on); + VelodyneStatus laser_on_off(bool on); /// @brief Setting host (destination) IP address (sync) /// @param addr destination IP address /// @return Resulting status - VelodyneStatus SetHostAddr(std::string addr); + VelodyneStatus set_host_addr(std::string addr); /// @brief Setting host (destination) data port (sync) /// @param dport destination data port /// @return Resulting status - VelodyneStatus SetHostDport(uint16_t dport); + VelodyneStatus set_host_dport(uint16_t dport); /// @brief Setting host (destination) telemetry port (sync) /// @param tport destination telemetry port /// @return Resulting status - VelodyneStatus SetHostTport(uint16_t tport); + VelodyneStatus set_host_tport(uint16_t tport); /// @brief Setting network (sensor) IP address (sync) /// @param addr sensor IP address /// @return Resulting status - VelodyneStatus SetNetAddr(std::string addr); + VelodyneStatus set_net_addr(std::string addr); /// @brief Setting the network mask of the sensor (sync) /// @param mask Network mask /// @return Resulting status - VelodyneStatus SetNetMask(std::string mask); + VelodyneStatus set_net_mask(std::string mask); /// @brief Setting the gateway address of the sensor (sync) /// @param gateway Gateway address /// @return Resulting status - VelodyneStatus SetNetGateway(std::string gateway); + VelodyneStatus set_net_gateway(std::string gateway); /// @brief This determines if the sensor is to rely on a DHCP server for its IP address (sync) /// @param use_dhcp DHCP on /// @return Resulting status - VelodyneStatus SetNetDhcp(bool use_dhcp); + VelodyneStatus set_net_dhcp(bool use_dhcp); /// @brief Setting rclcpp::Logger /// @param node Logger - void SetLogger(std::shared_ptr node); + void set_logger(std::shared_ptr node); }; } // namespace drivers diff --git a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp index 679f4d05e..213ed86e4 100644 --- a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_ars548_hw_interface.cpp @@ -29,14 +29,14 @@ ContinentalARS548HwInterface::ContinentalARS548HwInterface() { } -Status ContinentalARS548HwInterface::SetSensorConfiguration( +Status ContinentalARS548HwInterface::set_sensor_configuration( std::shared_ptr new_config_ptr) { config_ptr_ = new_config_ptr; return Status::OK; } -Status ContinentalARS548HwInterface::SensorInterfaceStart() +Status ContinentalARS548HwInterface::sensor_interface_start() { try { sensor_udp_driver_ptr_->init_receiver( @@ -46,7 +46,7 @@ Status ContinentalARS548HwInterface::SensorInterfaceStart() sensor_udp_driver_ptr_->receiver()->open(); sensor_udp_driver_ptr_->receiver()->bind(); sensor_udp_driver_ptr_->receiver()->asyncReceiveWithSender(std::bind( - &ContinentalARS548HwInterface::ReceiveSensorPacketCallbackWithSender, this, + &ContinentalARS548HwInterface::receive_sensor_packet_callback_with_sender, this, std::placeholders::_1, std::placeholders::_2)); sensor_udp_driver_ptr_->init_sender( @@ -67,24 +67,24 @@ Status ContinentalARS548HwInterface::SensorInterfaceStart() return Status::OK; } -Status ContinentalARS548HwInterface::RegisterPacketCallback( +Status ContinentalARS548HwInterface::register_packet_callback( std::function)> packet_callback) { packet_callback_ = std::move(packet_callback); return Status::OK; } -void ContinentalARS548HwInterface::ReceiveSensorPacketCallbackWithSender( +void ContinentalARS548HwInterface::receive_sensor_packet_callback_with_sender( std::vector & buffer, const std::string & sender_ip) { if (sender_ip == config_ptr_->sensor_ip) { - ReceiveSensorPacketCallback(buffer); + receive_sensor_packet_callback(buffer); } } -void ContinentalARS548HwInterface::ReceiveSensorPacketCallback(std::vector & buffer) +void ContinentalARS548HwInterface::receive_sensor_packet_callback(std::vector & buffer) { if (buffer.size() < sizeof(HeaderPacket)) { - PrintError("Unrecognized packet. Too short"); + print_error("Unrecognized packet. Too short"); return; } @@ -100,12 +100,12 @@ void ContinentalARS548HwInterface::ReceiveSensorPacketCallback(std::vector 100.f || vertical_autosar < 0.01f || vertical_autosar > 10.f || yaw_autosar < -M_PI || yaw_autosar > M_PI || pitch_autosar < -M_PI_2 || pitch_autosar > M_PI_2) { - PrintError("Invalid SetSensorMounting values"); + print_error("Invalid SetSensorMounting values"); return Status::SENSOR_CONFIG_ERROR; } ConfigurationPacket configuration_packet{}; - configuration_packet.header.service_id = CONFIGURATION_SERVICE_ID; - configuration_packet.header.method_id = CONFIGURATION_METHOD_ID; - configuration_packet.header.length = CONFIGURATION_PAYLOAD_LENGTH; + configuration_packet.header.service_id = configuration_service_id; + configuration_packet.header.method_id = configuration_method_id; + configuration_packet.header.length = configuration_payload_length; configuration_packet.configuration.longitudinal = longitudinal_autosar; configuration_packet.configuration.lateral = lateral_autosar; configuration_packet.configuration.vertical = vertical_autosar; @@ -133,12 +133,12 @@ Status ContinentalARS548HwInterface::SetSensorMounting( std::vector send_vector(sizeof(ConfigurationPacket)); std::memcpy(send_vector.data(), &configuration_packet, sizeof(ConfigurationPacket)); - PrintInfo("longitudinal_autosar = " + std::to_string(longitudinal_autosar)); - PrintInfo("lateral_autosar = " + std::to_string(lateral_autosar)); - PrintInfo("vertical_autosar = " + std::to_string(vertical_autosar)); - PrintInfo("yaw_autosar = " + std::to_string(yaw_autosar)); - PrintInfo("pitch_autosar = " + std::to_string(pitch_autosar)); - PrintInfo("plug_orientation = " + std::to_string(plug_orientation)); + print_info("longitudinal_autosar = " + std::to_string(longitudinal_autosar)); + print_info("lateral_autosar = " + std::to_string(lateral_autosar)); + print_info("vertical_autosar = " + std::to_string(vertical_autosar)); + print_info("yaw_autosar = " + std::to_string(yaw_autosar)); + print_info("pitch_autosar = " + std::to_string(pitch_autosar)); + print_info("plug_orientation = " + std::to_string(plug_orientation)); if (!sensor_udp_driver_ptr_->sender()->isOpen()) { return Status::ERROR_1; @@ -149,22 +149,22 @@ Status ContinentalARS548HwInterface::SetSensorMounting( return Status::OK; } -Status ContinentalARS548HwInterface::SetVehicleParameters( +Status ContinentalARS548HwInterface::set_vehicle_parameters( float length_autosar, float width_autosar, float height_autosar, float wheel_base_autosar) { if ( length_autosar < 0.01f || length_autosar > 100.f || width_autosar < 0.01f || width_autosar > 100.f || height_autosar < 0.01f || height_autosar > 100.f || wheel_base_autosar < 0.01f || wheel_base_autosar > 100.f) { - PrintError("Invalid SetVehicleParameters values"); + print_error("Invalid SetVehicleParameters values"); return Status::SENSOR_CONFIG_ERROR; } ConfigurationPacket configuration_packet{}; - static_assert(sizeof(ConfigurationPacket) == CONFIGURATION_UDP_LENGTH); - configuration_packet.header.service_id = CONFIGURATION_SERVICE_ID; - configuration_packet.header.method_id = CONFIGURATION_METHOD_ID; - configuration_packet.header.length = CONFIGURATION_PAYLOAD_LENGTH; + static_assert(sizeof(ConfigurationPacket) == configuration_udp_length); + configuration_packet.header.service_id = configuration_service_id; + configuration_packet.header.method_id = configuration_method_id; + configuration_packet.header.length = configuration_payload_length; configuration_packet.configuration.length = length_autosar; configuration_packet.configuration.width = width_autosar; configuration_packet.configuration.height = height_autosar; @@ -174,10 +174,10 @@ Status ContinentalARS548HwInterface::SetVehicleParameters( std::vector send_vector(sizeof(ConfigurationPacket)); std::memcpy(send_vector.data(), &configuration_packet, sizeof(ConfigurationPacket)); - PrintInfo("length_autosar = " + std::to_string(length_autosar)); - PrintInfo("width_autosar = " + std::to_string(width_autosar)); - PrintInfo("height_autosar = " + std::to_string(height_autosar)); - PrintInfo("wheel_base_autosar = " + std::to_string(wheel_base_autosar)); + print_info("length_autosar = " + std::to_string(length_autosar)); + print_info("width_autosar = " + std::to_string(width_autosar)); + print_info("height_autosar = " + std::to_string(height_autosar)); + print_info("wheel_base_autosar = " + std::to_string(wheel_base_autosar)); if (!sensor_udp_driver_ptr_->sender()->isOpen()) { return Status::ERROR_1; @@ -188,7 +188,7 @@ Status ContinentalARS548HwInterface::SetVehicleParameters( return Status::OK; } -Status ContinentalARS548HwInterface::SetRadarParameters( +Status ContinentalARS548HwInterface::set_radar_parameters( uint16_t maximum_distance, uint8_t frequency_slot, uint8_t cycle_time, uint8_t time_slot, uint8_t hcc, uint8_t power_save_standstill) { @@ -196,15 +196,15 @@ Status ContinentalARS548HwInterface::SetRadarParameters( maximum_distance < 93 || maximum_distance > 1514 || frequency_slot > 2 || cycle_time < 50 || cycle_time > 100 || time_slot < 10 || time_slot > 90 || hcc < 1 || hcc > 2 || power_save_standstill > 1) { - PrintError("Invalid SetRadarParameters values"); + print_error("Invalid SetRadarParameters values"); return Status::SENSOR_CONFIG_ERROR; } ConfigurationPacket configuration_packet{}; - static_assert(sizeof(ConfigurationPacket) == CONFIGURATION_UDP_LENGTH); - configuration_packet.header.service_id = CONFIGURATION_SERVICE_ID; - configuration_packet.header.method_id = CONFIGURATION_METHOD_ID; - configuration_packet.header.length = CONFIGURATION_PAYLOAD_LENGTH; + static_assert(sizeof(ConfigurationPacket) == configuration_udp_length); + configuration_packet.header.service_id = configuration_service_id; + configuration_packet.header.method_id = configuration_method_id; + configuration_packet.header.length = configuration_payload_length; configuration_packet.configuration.maximum_distance = maximum_distance; configuration_packet.configuration.frequency_slot = frequency_slot; configuration_packet.configuration.cycle_time = cycle_time; @@ -216,12 +216,12 @@ Status ContinentalARS548HwInterface::SetRadarParameters( std::vector send_vector(sizeof(ConfigurationPacket)); std::memcpy(send_vector.data(), &configuration_packet, sizeof(ConfigurationPacket)); - PrintInfo("maximum_distance = " + std::to_string(maximum_distance)); - PrintInfo("frequency_slot = " + std::to_string(frequency_slot)); - PrintInfo("cycle_time = " + std::to_string(cycle_time)); - PrintInfo("time_slot = " + std::to_string(time_slot)); - PrintInfo("hcc = " + std::to_string(hcc)); - PrintInfo("power_save_standstill = " + std::to_string(power_save_standstill)); + print_info("maximum_distance = " + std::to_string(maximum_distance)); + print_info("frequency_slot = " + std::to_string(frequency_slot)); + print_info("cycle_time = " + std::to_string(cycle_time)); + print_info("time_slot = " + std::to_string(time_slot)); + print_info("hcc = " + std::to_string(hcc)); + print_info("power_save_standstill = " + std::to_string(power_save_standstill)); if (!sensor_udp_driver_ptr_->sender()->isOpen()) { return Status::ERROR_1; @@ -232,7 +232,7 @@ Status ContinentalARS548HwInterface::SetRadarParameters( return Status::OK; } -Status ContinentalARS548HwInterface::SetSensorIPAddress(const std::string & sensor_ip_address) +Status ContinentalARS548HwInterface::set_sensor_ip_address(const std::string & sensor_ip_address) { std::array ip_bytes; @@ -240,17 +240,17 @@ Status ContinentalARS548HwInterface::SetSensorIPAddress(const std::string & sens auto sensor_ip = boost::asio::ip::address::from_string(sensor_ip_address); ip_bytes = sensor_ip.to_v4().to_bytes(); } catch (const std::exception & ex) { - PrintError("Setting invalid IP=" + sensor_ip_address); + print_error("Setting invalid IP=" + sensor_ip_address); return Status::SENSOR_CONFIG_ERROR; } - PrintInfo("New sensor IP = " + sensor_ip_address); + print_info("New sensor IP = " + sensor_ip_address); ConfigurationPacket configuration{}; - static_assert(sizeof(ConfigurationPacket) == CONFIGURATION_UDP_LENGTH); - configuration.header.service_id = CONFIGURATION_SERVICE_ID; - configuration.header.method_id = CONFIGURATION_METHOD_ID; - configuration.header.length = CONFIGURATION_PAYLOAD_LENGTH; + static_assert(sizeof(ConfigurationPacket) == configuration_udp_length); + configuration.header.service_id = configuration_service_id; + configuration.header.method_id = configuration_method_id; + configuration.header.length = configuration_payload_length; configuration.configuration.sensor_ip_address00 = ip_bytes[0]; configuration.configuration.sensor_ip_address01 = ip_bytes[1]; configuration.configuration.sensor_ip_address02 = ip_bytes[2]; @@ -273,23 +273,23 @@ Status ContinentalARS548HwInterface::SetSensorIPAddress(const std::string & sens return Status::OK; } -Status ContinentalARS548HwInterface::SetAccelerationLateralCog(float lateral_acceleration) +Status ContinentalARS548HwInterface::set_acceleration_lateral_cog(float lateral_acceleration) { - constexpr uint16_t ACCELERATION_LATERAL_COG_SERVICE_ID = 0; - constexpr uint16_t ACCELERATION_LATERAL_COG_METHOD_ID = 321; - constexpr uint8_t ACCELERATION_LATERAL_COG_LENGTH = 32; - const int ACCELERATION_LATERAL_COG_UDP_LENGTH = 40; + constexpr uint16_t acceleration_lateral_cog_service_id = 0; + constexpr uint16_t acceleration_lateral_cog_method_id = 321; + constexpr uint8_t acceleration_lateral_cog_length = 32; + const int acceleration_lateral_cog_udp_length = 40; if (lateral_acceleration < -65.f || lateral_acceleration > 65.f) { - PrintError("Invalid lateral_acceleration value"); + print_error("Invalid lateral_acceleration value"); return Status::ERROR_1; } AccelerationLateralCoGPacket acceleration_lateral_cog{}; - static_assert(sizeof(AccelerationLateralCoGPacket) == ACCELERATION_LATERAL_COG_UDP_LENGTH); - acceleration_lateral_cog.header.service_id = ACCELERATION_LATERAL_COG_SERVICE_ID; - acceleration_lateral_cog.header.method_id = ACCELERATION_LATERAL_COG_METHOD_ID; - acceleration_lateral_cog.header.length = ACCELERATION_LATERAL_COG_LENGTH; + static_assert(sizeof(AccelerationLateralCoGPacket) == acceleration_lateral_cog_udp_length); + acceleration_lateral_cog.header.service_id = acceleration_lateral_cog_service_id; + acceleration_lateral_cog.header.method_id = acceleration_lateral_cog_method_id; + acceleration_lateral_cog.header.length = acceleration_lateral_cog_length; acceleration_lateral_cog.acceleration_lateral = lateral_acceleration; std::vector send_vector(sizeof(AccelerationLateralCoGPacket)); @@ -304,24 +304,25 @@ Status ContinentalARS548HwInterface::SetAccelerationLateralCog(float lateral_acc return Status::OK; } -Status ContinentalARS548HwInterface::SetAccelerationLongitudinalCog(float longitudinal_acceleration) +Status ContinentalARS548HwInterface::set_acceleration_longitudinal_cog( + float longitudinal_acceleration) { - constexpr uint16_t ACCELERATION_LONGITUDINAL_COG_SERVICE_ID = 0; - constexpr uint16_t ACCELERATION_LONGITUDINAL_COG_METHOD_ID = 322; - constexpr uint8_t ACCELERATION_LONGITUDINAL_COG_LENGTH = 32; - const int ACCELERATION_LONGITUDINAL_COG_UDP_LENGTH = 40; + constexpr uint16_t acceleration_longitudinal_cog_service_id = 0; + constexpr uint16_t acceleration_longitudinal_cog_method_id = 322; + constexpr uint8_t acceleration_longitudinal_cog_length = 32; + const int acceleration_longitudinal_cog_udp_length = 40; if (longitudinal_acceleration < -65.f || longitudinal_acceleration > 65.f) { - PrintError("Invalid longitudinal_acceleration value"); + print_error("Invalid longitudinal_acceleration value"); return Status::ERROR_1; } AccelerationLongitudinalCoGPacket acceleration_longitudinal_cog{}; static_assert( - sizeof(AccelerationLongitudinalCoGPacket) == ACCELERATION_LONGITUDINAL_COG_UDP_LENGTH); - acceleration_longitudinal_cog.header.service_id = ACCELERATION_LONGITUDINAL_COG_SERVICE_ID; - acceleration_longitudinal_cog.header.method_id = ACCELERATION_LONGITUDINAL_COG_METHOD_ID; - acceleration_longitudinal_cog.header.length = ACCELERATION_LONGITUDINAL_COG_LENGTH; + sizeof(AccelerationLongitudinalCoGPacket) == acceleration_longitudinal_cog_udp_length); + acceleration_longitudinal_cog.header.service_id = acceleration_longitudinal_cog_service_id; + acceleration_longitudinal_cog.header.method_id = acceleration_longitudinal_cog_method_id; + acceleration_longitudinal_cog.header.length = acceleration_longitudinal_cog_length; acceleration_longitudinal_cog.acceleration_lateral = longitudinal_acceleration; std::vector send_vector(sizeof(AccelerationLongitudinalCoGPacket)); @@ -337,23 +338,23 @@ Status ContinentalARS548HwInterface::SetAccelerationLongitudinalCog(float longit return Status::OK; } -Status ContinentalARS548HwInterface::SetCharacteristicSpeed(float characteristic_speed) +Status ContinentalARS548HwInterface::set_characteristic_speed(float characteristic_speed) { - constexpr uint16_t CHARACTERISTIC_SPEED_SERVICE_ID = 0; - constexpr uint16_t CHARACTERISTIC_SPEED_METHOD_ID = 328; - constexpr uint8_t CHARACTERISTIC_SPEED_LENGTH = 11; - const int CHARACTERISTIC_SPEED_UDP_LENGTH = 19; + constexpr uint16_t characteristic_speed_service_id = 0; + constexpr uint16_t characteristic_speed_method_id = 328; + constexpr uint8_t characteristic_speed_length = 11; + const int characteristic_speed_udp_length = 19; if (characteristic_speed < 0.f || characteristic_speed > 255.f) { - PrintError("Invalid characteristic_speed value"); + print_error("Invalid characteristic_speed value"); return Status::ERROR_1; } CharacteristicSpeedPacket characteristic_speed_packet{}; - static_assert(sizeof(CharacteristicSpeedPacket) == CHARACTERISTIC_SPEED_UDP_LENGTH); - characteristic_speed_packet.header.service_id = CHARACTERISTIC_SPEED_SERVICE_ID; - characteristic_speed_packet.header.method_id = CHARACTERISTIC_SPEED_METHOD_ID; - characteristic_speed_packet.header.length = CHARACTERISTIC_SPEED_LENGTH; + static_assert(sizeof(CharacteristicSpeedPacket) == characteristic_speed_udp_length); + characteristic_speed_packet.header.service_id = characteristic_speed_service_id; + characteristic_speed_packet.header.method_id = characteristic_speed_method_id; + characteristic_speed_packet.header.length = characteristic_speed_length; characteristic_speed_packet.characteristic_speed = characteristic_speed; std::vector send_vector(sizeof(CharacteristicSpeedPacket)); @@ -368,28 +369,28 @@ Status ContinentalARS548HwInterface::SetCharacteristicSpeed(float characteristic return Status::OK; } -Status ContinentalARS548HwInterface::SetDrivingDirection(int direction) +Status ContinentalARS548HwInterface::set_driving_direction(int direction) { - constexpr uint16_t DRIVING_DIRECTION_SERVICE_ID = 0; - constexpr uint16_t DRIVING_DIRECTION_METHOD_ID = 325; - constexpr uint8_t DRIVING_DIRECTION_LENGTH = 22; - constexpr uint8_t DRIVING_DIRECTION_STANDSTILL = 0; - constexpr uint8_t DRIVING_DIRECTION_FORWARD = 1; - constexpr uint8_t DRIVING_DIRECTION_BACKWARDS = 2; - const int DRIVING_DIRECTION_UDP_LENGTH = 30; + constexpr uint16_t driving_direction_service_id = 0; + constexpr uint16_t driving_direction_method_id = 325; + constexpr uint8_t driving_direction_length = 22; + constexpr uint8_t driving_direction_standstill = 0; + constexpr uint8_t driving_direction_forward = 1; + constexpr uint8_t driving_direction_backwards = 2; + const int driving_direction_udp_length = 30; DrivingDirectionPacket driving_direction_packet{}; - static_assert(sizeof(DrivingDirectionPacket) == DRIVING_DIRECTION_UDP_LENGTH); - driving_direction_packet.header.service_id = DRIVING_DIRECTION_SERVICE_ID; - driving_direction_packet.header.method_id = DRIVING_DIRECTION_METHOD_ID; - driving_direction_packet.header.length = DRIVING_DIRECTION_LENGTH; + static_assert(sizeof(DrivingDirectionPacket) == driving_direction_udp_length); + driving_direction_packet.header.service_id = driving_direction_service_id; + driving_direction_packet.header.method_id = driving_direction_method_id; + driving_direction_packet.header.length = driving_direction_length; if (direction == 0) { - driving_direction_packet.driving_direction = DRIVING_DIRECTION_STANDSTILL; + driving_direction_packet.driving_direction = driving_direction_standstill; } else if (direction > 0) { - driving_direction_packet.driving_direction = DRIVING_DIRECTION_FORWARD; + driving_direction_packet.driving_direction = driving_direction_forward; } else { - driving_direction_packet.driving_direction = DRIVING_DIRECTION_BACKWARDS; + driving_direction_packet.driving_direction = driving_direction_backwards; } std::vector send_vector(sizeof(DrivingDirectionPacket)); @@ -404,23 +405,23 @@ Status ContinentalARS548HwInterface::SetDrivingDirection(int direction) return Status::OK; } -Status ContinentalARS548HwInterface::SetSteeringAngleFrontAxle(float angle_rad) +Status ContinentalARS548HwInterface::set_steering_angle_front_axle(float angle_rad) { - constexpr uint16_t STEERING_ANGLE_SERVICE_ID = 0; - constexpr uint16_t STEERING_ANGLE_METHOD_ID = 327; - constexpr uint8_t STEERING_ANGLE_LENGTH = 32; - const int STEERING_ANGLE_UDP_LENGTH = 40; + constexpr uint16_t steering_angle_service_id = 0; + constexpr uint16_t steering_angle_method_id = 327; + constexpr uint8_t steering_angle_length = 32; + const int steering_angle_udp_length = 40; if (angle_rad < -90.f || angle_rad > 90.f) { - PrintError("Invalid angle_rad value"); + print_error("Invalid angle_rad value"); return Status::ERROR_1; } SteeringAngleFrontAxlePacket steering_angle_front_axle_packet{}; - static_assert(sizeof(SteeringAngleFrontAxlePacket) == STEERING_ANGLE_UDP_LENGTH); - steering_angle_front_axle_packet.header.service_id = STEERING_ANGLE_SERVICE_ID; - steering_angle_front_axle_packet.header.method_id = STEERING_ANGLE_METHOD_ID; - steering_angle_front_axle_packet.header.length = STEERING_ANGLE_LENGTH; + static_assert(sizeof(SteeringAngleFrontAxlePacket) == steering_angle_udp_length); + steering_angle_front_axle_packet.header.service_id = steering_angle_service_id; + steering_angle_front_axle_packet.header.method_id = steering_angle_method_id; + steering_angle_front_axle_packet.header.length = steering_angle_length; steering_angle_front_axle_packet.steering_angle_front_axle = angle_rad; std::vector send_vector(sizeof(SteeringAngleFrontAxlePacket)); @@ -436,23 +437,23 @@ Status ContinentalARS548HwInterface::SetSteeringAngleFrontAxle(float angle_rad) return Status::OK; } -Status ContinentalARS548HwInterface::SetVelocityVehicle(float velocity_kmh) +Status ContinentalARS548HwInterface::set_velocity_vehicle(float velocity_kmh) { if (velocity_kmh < 0.f || velocity_kmh > 350.f) { - PrintError("Invalid velocity value"); + print_error("Invalid velocity value"); return Status::ERROR_1; } - constexpr uint16_t VELOCITY_VEHICLE_SERVICE_ID = 0; - constexpr uint16_t VELOCITY_VEHICLE_METHOD_ID = 323; - constexpr uint8_t VELOCITY_VEHICLE_LENGTH = 28; - const int VELOCITY_VEHICLE_UDP_SIZE = 36; + constexpr uint16_t velocity_vehicle_service_id = 0; + constexpr uint16_t velocity_vehicle_method_id = 323; + constexpr uint8_t velocity_vehicle_length = 28; + const int velocity_vehicle_udp_size = 36; VelocityVehiclePacket velocity_vehicle_packet{}; - static_assert(sizeof(VelocityVehiclePacket) == VELOCITY_VEHICLE_UDP_SIZE); - velocity_vehicle_packet.header.service_id = VELOCITY_VEHICLE_SERVICE_ID; - velocity_vehicle_packet.header.method_id = VELOCITY_VEHICLE_METHOD_ID; - velocity_vehicle_packet.header.length = VELOCITY_VEHICLE_LENGTH; + static_assert(sizeof(VelocityVehiclePacket) == velocity_vehicle_udp_size); + velocity_vehicle_packet.header.service_id = velocity_vehicle_service_id; + velocity_vehicle_packet.header.method_id = velocity_vehicle_method_id; + velocity_vehicle_packet.header.length = velocity_vehicle_length; velocity_vehicle_packet.velocity_vehicle = velocity_kmh; std::vector send_vector(sizeof(VelocityVehiclePacket)); @@ -467,23 +468,23 @@ Status ContinentalARS548HwInterface::SetVelocityVehicle(float velocity_kmh) return Status::OK; } -Status ContinentalARS548HwInterface::SetYawRate(float yaw_rate) +Status ContinentalARS548HwInterface::set_yaw_rate(float yaw_rate) { if (yaw_rate < -163.83 || yaw_rate > 163.83) { - PrintError("Invalid yaw rate value"); + print_error("Invalid yaw rate value"); return Status::ERROR_1; } - constexpr uint16_t YAW_RATE_SERVICE_ID = 0; - constexpr uint16_t YAW_RATE_METHOD_ID = 326; - constexpr uint8_t YAW_RATE_LENGTH = 32; - const int YAW_RATE_UDP_SIZE = 40; + constexpr uint16_t yaw_rate_service_id = 0; + constexpr uint16_t yaw_rate_method_id = 326; + constexpr uint8_t yaw_rate_length = 32; + const int yaw_rate_udp_size = 40; YawRatePacket yaw_rate_packet{}; - static_assert(sizeof(YawRatePacket) == YAW_RATE_UDP_SIZE); - yaw_rate_packet.header.service_id = YAW_RATE_SERVICE_ID; - yaw_rate_packet.header.method_id = YAW_RATE_METHOD_ID; - yaw_rate_packet.header.length = YAW_RATE_LENGTH; + static_assert(sizeof(YawRatePacket) == yaw_rate_udp_size); + yaw_rate_packet.header.service_id = yaw_rate_service_id; + yaw_rate_packet.header.method_id = yaw_rate_method_id; + yaw_rate_packet.header.length = yaw_rate_length; yaw_rate_packet.yaw_rate = yaw_rate; std::vector send_vector(sizeof(YawRatePacket)); @@ -498,12 +499,12 @@ Status ContinentalARS548HwInterface::SetYawRate(float yaw_rate) return Status::OK; } -void ContinentalARS548HwInterface::SetLogger(std::shared_ptr logger) +void ContinentalARS548HwInterface::set_logger(std::shared_ptr logger) { parent_node_logger_ptr_ = logger; } -void ContinentalARS548HwInterface::PrintInfo(std::string info) +void ContinentalARS548HwInterface::print_info(std::string info) { if (parent_node_logger_ptr_) { RCLCPP_INFO_STREAM((*parent_node_logger_ptr_), info); @@ -512,7 +513,7 @@ void ContinentalARS548HwInterface::PrintInfo(std::string info) } } -void ContinentalARS548HwInterface::PrintError(std::string error) +void ContinentalARS548HwInterface::print_error(std::string error) { if (parent_node_logger_ptr_) { RCLCPP_ERROR_STREAM((*parent_node_logger_ptr_), error); @@ -521,7 +522,7 @@ void ContinentalARS548HwInterface::PrintError(std::string error) } } -void ContinentalARS548HwInterface::PrintDebug(std::string debug) +void ContinentalARS548HwInterface::print_debug(std::string debug) { if (parent_node_logger_ptr_) { RCLCPP_DEBUG_STREAM((*parent_node_logger_ptr_), debug); diff --git a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_srr520_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_srr520_hw_interface.cpp index bf7985bf0..a115c9260 100644 --- a/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_srr520_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_continental_hw_interfaces/continental_srr520_hw_interface.cpp @@ -31,7 +31,7 @@ ContinentalSRR520HwInterface::ContinentalSRR520HwInterface() { } -Status ContinentalSRR520HwInterface::SetSensorConfiguration( +Status ContinentalSRR520HwInterface::set_sensor_configuration( const std::shared_ptr< const nebula::drivers::continental_srr520::ContinentalSRR520SensorConfiguration> new_config_ptr) @@ -41,7 +41,7 @@ Status ContinentalSRR520HwInterface::SetSensorConfiguration( return Status::OK; } -Status ContinentalSRR520HwInterface::SensorInterfaceStart() +Status ContinentalSRR520HwInterface::sensor_interface_start() { std::lock_guard lock(receiver_mutex_); @@ -53,11 +53,11 @@ Status ContinentalSRR520HwInterface::SensorInterfaceStart() can_receiver_ptr_->SetCanFilters( ::drivers::socketcan::SocketCanReceiver::CanFilterList(config_ptr_->filters)); - PrintInfo(std::string("applied filters: ") + config_ptr_->filters); + print_info(std::string("applied filters: ") + config_ptr_->filters); sensor_interface_active_ = true; receiver_thread_ptr_ = - std::make_unique(&ContinentalSRR520HwInterface::ReceiveLoop, this); + std::make_unique(&ContinentalSRR520HwInterface::receive_loop, this); } catch (const std::exception & ex) { Status status = Status::CAN_CONNECTION_ERROR; std::cerr << status << config_ptr_->interface << std::endl; @@ -67,7 +67,7 @@ Status ContinentalSRR520HwInterface::SensorInterfaceStart() } template -bool ContinentalSRR520HwInterface::SendFrame(const std::array & data, int can_frame_id) +bool ContinentalSRR520HwInterface::send_frame(const std::array & data, int can_frame_id) { ::drivers::socketcan::CanId send_id( can_frame_id, 0, ::drivers::socketcan::FrameType::DATA, ::drivers::socketcan::StandardFrame); @@ -79,12 +79,12 @@ bool ContinentalSRR520HwInterface::SendFrame(const std::array & data std::chrono::duration(config_ptr_->sender_timeout_sec))); return true; } catch (const std::exception & ex) { - PrintError(std::string("Error sending CAN message: ") + ex.what()); + print_error(std::string("Error sending CAN message: ") + ex.what()); return false; } } -void ContinentalSRR520HwInterface::ReceiveLoop() +void ContinentalSRR520HwInterface::receive_loop() { ::drivers::socketcan::CanId receive_id{}; std::chrono::nanoseconds receiver_timeout_nsec; @@ -109,7 +109,7 @@ void ContinentalSRR520HwInterface::ReceiveLoop() receive_id = can_receiver_ptr_->receive_fd( packet_msg_ptr->data.data() + 4 * sizeof(uint8_t), receiver_timeout_nsec); } catch (const std::exception & ex) { - PrintError(std::string("Error receiving CAN FD message: ") + ex.what()); + print_error(std::string("Error receiving CAN FD message: ") + ex.what()); continue; } @@ -131,7 +131,7 @@ void ContinentalSRR520HwInterface::ReceiveLoop() packet_msg_ptr->stamp.nanosec = stamp % 1'000'000'000; if (receive_id.frame_type() == ::drivers::socketcan::FrameType::ERROR) { - PrintError("CAN FD message is an error frame"); + print_error("CAN FD message is an error frame"); continue; } @@ -139,17 +139,17 @@ void ContinentalSRR520HwInterface::ReceiveLoop() } } -Status ContinentalSRR520HwInterface::RegisterPacketCallback( +Status ContinentalSRR520HwInterface::register_packet_callback( std::function)> callback) { nebula_packet_callback_ = std::move(callback); return Status::OK; } -void ContinentalSRR520HwInterface::SensorSyncFollowUp(builtin_interfaces::msg::Time stamp) +void ContinentalSRR520HwInterface::sensor_sync_follow_up(builtin_interfaces::msg::Time stamp) { if (!can_sender_ptr_) { - PrintError("Can sender is invalid so can not do follow up"); + print_error("Can sender is invalid so can not do follow up"); } if (!config_ptr_->sync_use_bus_time || sync_follow_up_sent_) { @@ -166,7 +166,7 @@ void ContinentalSRR520HwInterface::SensorSyncFollowUp(builtin_interfaces::msg::T uint32_t t4r_nanoseconds = t4r.nanosec; std::array data; data[0] = 0x28; // mode 0x18 is without CRC - data[2] = (((static_cast(TIME_DOMAIN_ID) << 4) & 0xF0)) | + data[2] = (((static_cast(time_domain_id) << 4) & 0xF0)) | (sync_counter_ & 0x0F); // Domain and counter data[3] = t4r_seconds & 0x3; // SGW and OVS data[4] = (t4r_nanoseconds & 0xFF000000) >> 24; @@ -179,21 +179,21 @@ void ContinentalSRR520HwInterface::SensorSyncFollowUp(builtin_interfaces::msg::T uint8_t follow_up_crc = crc8h2f(follow_up_crc_array.begin(), follow_up_crc_array.end()); data[1] = follow_up_crc; - SendFrame(data, SYNC_FOLLOW_UP_CAN_MESSAGE_ID); + send_frame(data, sync_follow_up_can_message_id); sync_follow_up_sent_ = true; sync_counter_ = sync_counter_ == 15 ? 0 : sync_counter_ + 1; } -void ContinentalSRR520HwInterface::SensorSync() +void ContinentalSRR520HwInterface::sensor_sync() { if (!can_sender_ptr_) { - PrintError("Can sender is invalid so can not do sync up"); + print_error("Can sender is invalid so can not do sync up"); return; } if (!sync_follow_up_sent_) { - PrintError("We will send a SYNC message without having sent a FollowUp message first!"); + print_error("We will send a SYNC message without having sent a FollowUp message first!"); } auto now = std::chrono::system_clock::now(); @@ -208,7 +208,7 @@ void ContinentalSRR520HwInterface::SensorSync() std::array data; data[0] = 0x20; // mode 0x10 is without CRC - data[2] = (((static_cast(TIME_DOMAIN_ID) << 4) & 0xF0)) | + data[2] = (((static_cast(time_domain_id) << 4) & 0xF0)) | (sync_counter_ & 0x0F); // Domain and counter data[3] = 0; // use data data[4] = (stamp.sec & 0xFF000000) >> 24; @@ -220,7 +220,7 @@ void ContinentalSRR520HwInterface::SensorSync() uint8_t sync_crc = crc8h2f(sync_crc_array.begin(), sync_crc_array.end()); data[1] = sync_crc; - SendFrame(data, SYNC_FOLLOW_UP_CAN_MESSAGE_ID); + send_frame(data, sync_follow_up_can_message_id); if (config_ptr_->sync_use_bus_time) { sync_follow_up_sent_ = false; @@ -228,7 +228,7 @@ void ContinentalSRR520HwInterface::SensorSync() } data[0] = 0x28; // mode 0x18 is without CRC - data[2] = (((static_cast(TIME_DOMAIN_ID) << 4) & 0xF0)) | + data[2] = (((static_cast(time_domain_id) << 4) & 0xF0)) | (sync_counter_ & 0x0F); // Domain and counter data[3] = 0; // SGW and OVS data[4] = (stamp.nanosec & 0xFF000000) >> 24; @@ -241,13 +241,13 @@ void ContinentalSRR520HwInterface::SensorSync() uint8_t follow_up_crc = crc8h2f(follow_up_crc_array.begin(), follow_up_crc_array.end()); data[1] = follow_up_crc; - SendFrame(data, SYNC_FOLLOW_UP_CAN_MESSAGE_ID); + send_frame(data, sync_follow_up_can_message_id); sync_counter_ = sync_counter_ == 15 ? 0 : sync_counter_ + 1; sync_follow_up_sent_ = true; } -Status ContinentalSRR520HwInterface::SensorInterfaceStop() +Status ContinentalSRR520HwInterface::sensor_interface_stop() { { std::lock_guard l(receiver_mutex_); @@ -258,19 +258,19 @@ Status ContinentalSRR520HwInterface::SensorInterfaceStop() return Status::ERROR_1; } -Status ContinentalSRR520HwInterface::ConfigureSensor( +Status ContinentalSRR520HwInterface::configure_sensor( uint8_t sensor_id, float longitudinal_autosar, float lateral_autosar, float vertical_autosar, float yaw_autosar, float longitudinal_cog, float wheelbase, float cover_damping, bool plug_bottom, bool reset) { - PrintInfo("longitudinal_autosar=" + std::to_string(longitudinal_autosar)); - PrintInfo("lateral_autosar=" + std::to_string(lateral_autosar)); - PrintInfo("vertical_autosar=" + std::to_string(vertical_autosar)); - PrintInfo("longitudinal_cog=" + std::to_string(longitudinal_cog)); - PrintInfo("wheelbase=" + std::to_string(wheelbase)); - PrintInfo("yaw_autosar=" + std::to_string(yaw_autosar)); - PrintInfo("sensor_id=" + std::to_string(static_cast(sensor_id))); - PrintInfo("plug_bottom=" + std::to_string(plug_bottom)); + print_info("longitudinal_autosar=" + std::to_string(longitudinal_autosar)); + print_info("lateral_autosar=" + std::to_string(lateral_autosar)); + print_info("vertical_autosar=" + std::to_string(vertical_autosar)); + print_info("longitudinal_cog=" + std::to_string(longitudinal_cog)); + print_info("wheelbase=" + std::to_string(wheelbase)); + print_info("yaw_autosar=" + std::to_string(yaw_autosar)); + print_info("sensor_id=" + std::to_string(static_cast(sensor_id))); + print_info("plug_bottom=" + std::to_string(plug_bottom)); if ( longitudinal_autosar < -32.767f || longitudinal_autosar > 32.767f || @@ -278,7 +278,7 @@ Status ContinentalSRR520HwInterface::ConfigureSensor( vertical_autosar > 32.767f || longitudinal_cog < -32.767f || longitudinal_cog > 32.767f || wheelbase < 0.f || wheelbase > 65.534f || yaw_autosar < -3.14159f || yaw_autosar > 3.14159f || cover_damping < -32.767f || cover_damping > 32.767f) { - PrintError("Sensor configuration values out of range!"); + print_error("Sensor configuration values out of range!"); return Status::SENSOR_CONFIG_ERROR; } @@ -317,14 +317,14 @@ Status ContinentalSRR520HwInterface::ConfigureSensor( uint8_t reset_value = reset ? 0x80 : 0x00; data[15] = plug_value | reset_value; - if (SendFrame(data, SENSOR_CONFIG_CAN_MESSAGE_ID)) { + if (send_frame(data, sensor_config_can_message_id)) { return Status::OK; } else { return Status::CAN_CONNECTION_ERROR; } } -Status ContinentalSRR520HwInterface::SetVehicleDynamics( +Status ContinentalSRR520HwInterface::set_vehicle_dynamics( float longitudinal_acceleration, float lateral_acceleration, float yaw_rate, float longitudinal_velocity, bool standstill) { @@ -332,7 +332,7 @@ Status ContinentalSRR520HwInterface::SetVehicleDynamics( longitudinal_acceleration < -12.7 || longitudinal_acceleration > 12.7 || lateral_acceleration < -12.7 || lateral_acceleration > 12.7 || yaw_rate < -3.14159 || yaw_rate > 3.14159 || abs(longitudinal_velocity) > 100.0) { - PrintError("Vehicle dynamics out of range!"); + print_error("Vehicle dynamics out of range!"); return Status::SENSOR_CONFIG_ERROR; } @@ -361,19 +361,19 @@ Status ContinentalSRR520HwInterface::SetVehicleDynamics( data[6] = 0x00; data[7] = 0x00; - if (SendFrame(data, VEH_DYN_CAN_MESSAGE_ID)) { + if (send_frame(data, veh_dyn_can_message_id)) { return Status::OK; } else { return Status::CAN_CONNECTION_ERROR; } } -void ContinentalSRR520HwInterface::SetLogger(std::shared_ptr logger) +void ContinentalSRR520HwInterface::set_logger(std::shared_ptr logger) { parent_node_logger_ptr_ = logger; } -void ContinentalSRR520HwInterface::PrintInfo(std::string info) +void ContinentalSRR520HwInterface::print_info(std::string info) { if (parent_node_logger_ptr_) { RCLCPP_INFO_STREAM((*parent_node_logger_ptr_), info); @@ -382,7 +382,7 @@ void ContinentalSRR520HwInterface::PrintInfo(std::string info) } } -void ContinentalSRR520HwInterface::PrintError(std::string error) +void ContinentalSRR520HwInterface::print_error(std::string error) { if (parent_node_logger_ptr_) { RCLCPP_ERROR_STREAM((*parent_node_logger_ptr_), error); @@ -391,7 +391,7 @@ void ContinentalSRR520HwInterface::PrintError(std::string error) } } -void ContinentalSRR520HwInterface::PrintDebug(std::string debug) +void ContinentalSRR520HwInterface::print_debug(std::string debug) { if (parent_node_logger_ptr_) { RCLCPP_DEBUG_STREAM((*parent_node_logger_ptr_), debug); diff --git a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp index e349e3f3b..432cb3de4 100644 --- a/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp @@ -2,6 +2,7 @@ #include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" +#include "nebula_common/hesai/hesai_common.hpp" #include "nebula_common/nebula_status.hpp" #include @@ -554,7 +555,7 @@ Status HesaiHwInterface::checkAndSetLidarRange( // Only oversize the FoV if it is not already the full 360deg if (cloud_min_ddeg != 0 || cloud_max_ddeg != 3600) { - auto padding_deg = calibration.getFovPadding(); + auto padding_deg = calibration.get_fov_padding(); cloud_min_ddeg += floor(std::get<0>(padding_deg) * 10); cloud_max_ddeg += ceil(std::get<1>(padding_deg) * 10); } @@ -847,7 +848,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( #ifdef WITH_DEBUG_STDOUT_HESAI_HW_INTERFACE std::cout << "Start CheckAndSetConfig(HesaiConfig)!!" << std::endl; #endif - auto current_return_mode = nebula::drivers::ReturnModeFromIntHesai( + auto current_return_mode = nebula::drivers::return_mode_from_int_hesai( hesai_config.return_mode, sensor_configuration->sensor_model); // Avoids spamming the sensor, which leads to failure when configuring it. auto wait_time = 100ms; @@ -859,7 +860,7 @@ HesaiStatus HesaiHwInterface::CheckAndSetConfig( ss2 << sensor_configuration->return_mode; PrintInfo("Current Configuration return_mode: " + ss2.str()); std::thread t([this, sensor_configuration] { - auto return_mode_int = nebula::drivers::IntFromReturnModeHesai( + auto return_mode_int = nebula::drivers::int_from_return_mode_hesai( sensor_configuration->return_mode, sensor_configuration->sensor_model); if (return_mode_int < 0) { PrintError( diff --git a/nebula_hw_interfaces/src/nebula_robosense_hw_interfaces/robosense_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_robosense_hw_interfaces/robosense_hw_interface.cpp index d8d972488..c91670024 100644 --- a/nebula_hw_interfaces/src/nebula_robosense_hw_interfaces/robosense_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_robosense_hw_interfaces/robosense_hw_interface.cpp @@ -13,7 +13,7 @@ RobosenseHwInterface::RobosenseHwInterface() { } -void RobosenseHwInterface::ReceiveSensorPacketCallback(std::vector & buffer) +void RobosenseHwInterface::receive_sensor_packet_callback(std::vector & buffer) { if (!scan_reception_callback_) { return; @@ -22,7 +22,7 @@ void RobosenseHwInterface::ReceiveSensorPacketCallback(std::vector & bu scan_reception_callback_(buffer); } -void RobosenseHwInterface::ReceiveInfoPacketCallback(std::vector & buffer) +void RobosenseHwInterface::receive_info_packet_callback(std::vector & buffer) { if (!info_reception_callback_) { return; @@ -31,7 +31,7 @@ void RobosenseHwInterface::ReceiveInfoPacketCallback(std::vector & buff info_reception_callback_(buffer); } -Status RobosenseHwInterface::SensorInterfaceStart() +Status RobosenseHwInterface::sensor_interface_start() { try { std::cout << "Starting UDP server for data packets on: " << *sensor_configuration_ << std::endl; @@ -40,8 +40,8 @@ Status RobosenseHwInterface::SensorInterfaceStart() cloud_udp_driver_->receiver()->open(); cloud_udp_driver_->receiver()->bind(); - cloud_udp_driver_->receiver()->asyncReceive( - std::bind(&RobosenseHwInterface::ReceiveSensorPacketCallback, this, std::placeholders::_1)); + cloud_udp_driver_->receiver()->asyncReceive(std::bind( + &RobosenseHwInterface::receive_sensor_packet_callback, this, std::placeholders::_1)); } catch (const std::exception & ex) { Status status = Status::UDP_CONNECTION_ERROR; std::cerr << status << sensor_configuration_->sensor_ip << "," @@ -51,11 +51,11 @@ Status RobosenseHwInterface::SensorInterfaceStart() return Status::OK; } -Status RobosenseHwInterface::InfoInterfaceStart() +Status RobosenseHwInterface::info_interface_start() { try { std::cout << "Starting UDP server for info packets on: " << *sensor_configuration_ << std::endl; - PrintInfo( + print_info( "Starting UDP server for info packets on: " + sensor_configuration_->sensor_ip + ": " + std::to_string(sensor_configuration_->gnss_port)); info_udp_driver_->init_receiver( @@ -64,7 +64,7 @@ Status RobosenseHwInterface::InfoInterfaceStart() info_udp_driver_->receiver()->bind(); info_udp_driver_->receiver()->asyncReceive( - std::bind(&RobosenseHwInterface::ReceiveInfoPacketCallback, this, std::placeholders::_1)); + std::bind(&RobosenseHwInterface::receive_info_packet_callback, this, std::placeholders::_1)); } catch (const std::exception & ex) { Status status = Status::UDP_CONNECTION_ERROR; std::cerr << status << sensor_configuration_->sensor_ip << "," @@ -75,7 +75,7 @@ Status RobosenseHwInterface::InfoInterfaceStart() return Status::OK; } -Status RobosenseHwInterface::SetSensorConfiguration( +Status RobosenseHwInterface::set_sensor_configuration( std::shared_ptr sensor_configuration) { if (!(sensor_configuration->sensor_model == SensorModel::ROBOSENSE_BPEARL_V3 || @@ -89,21 +89,21 @@ Status RobosenseHwInterface::SetSensorConfiguration( return Status::OK; } -Status RobosenseHwInterface::RegisterScanCallback( +Status RobosenseHwInterface::register_scan_callback( std::function &)> scan_callback) { scan_reception_callback_ = std::move(scan_callback); return Status::OK; } -Status RobosenseHwInterface::RegisterInfoCallback( +Status RobosenseHwInterface::register_info_callback( std::function &)> info_callback) { info_reception_callback_ = std::move(info_callback); return Status::OK; } -void RobosenseHwInterface::PrintDebug(std::string debug) +void RobosenseHwInterface::print_debug(std::string debug) { if (parent_node_logger_) { RCLCPP_DEBUG_STREAM((*parent_node_logger_), debug); @@ -112,7 +112,7 @@ void RobosenseHwInterface::PrintDebug(std::string debug) } } -void RobosenseHwInterface::PrintInfo(std::string info) +void RobosenseHwInterface::print_info(std::string info) { if (parent_node_logger_) { RCLCPP_INFO_STREAM((*parent_node_logger_), info); @@ -121,7 +121,7 @@ void RobosenseHwInterface::PrintInfo(std::string info) } } -void RobosenseHwInterface::SetLogger(std::shared_ptr logger) +void RobosenseHwInterface::set_logger(std::shared_ptr logger) { parent_node_logger_ = logger; } diff --git a/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp index 40ba0f754..cca57e0db 100644 --- a/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp @@ -14,7 +14,7 @@ VelodyneHwInterface::VelodyneHwInterface() { } -std::string VelodyneHwInterface::HttpGetRequest(const std::string & endpoint) +std::string VelodyneHwInterface::http_get_request(const std::string & endpoint) { std::lock_guard lock(mtx_inflight_request_); if (!http_client_driver_->client()->isOpen()) { @@ -26,7 +26,7 @@ std::string VelodyneHwInterface::HttpGetRequest(const std::string & endpoint) return response; } -std::string VelodyneHwInterface::HttpPostRequest( +std::string VelodyneHwInterface::http_post_request( const std::string & endpoint, const std::string & body) { std::lock_guard lock(mtx_inflight_request_); @@ -39,24 +39,24 @@ std::string VelodyneHwInterface::HttpPostRequest( return response; } -Status VelodyneHwInterface::InitializeSensorConfiguration( +Status VelodyneHwInterface::initialize_sensor_configuration( std::shared_ptr sensor_configuration) { sensor_configuration_ = sensor_configuration; return Status::OK; } -Status VelodyneHwInterface::SetSensorConfiguration( +Status VelodyneHwInterface::set_sensor_configuration( std::shared_ptr sensor_configuration) { - auto snapshot = GetSnapshot(); - auto tree = ParseJson(snapshot); - VelodyneStatus status = CheckAndSetConfig(sensor_configuration, tree); + auto snapshot = get_snapshot(); + auto tree = parse_json(snapshot); + VelodyneStatus status = check_and_set_config(sensor_configuration, tree); return status; } -Status VelodyneHwInterface::SensorInterfaceStart() +Status VelodyneHwInterface::sensor_interface_start() { try { cloud_udp_driver_->init_receiver( @@ -64,7 +64,7 @@ Status VelodyneHwInterface::SensorInterfaceStart() cloud_udp_driver_->receiver()->open(); cloud_udp_driver_->receiver()->bind(); cloud_udp_driver_->receiver()->asyncReceive( - std::bind(&VelodyneHwInterface::ReceiveSensorPacketCallback, this, std::placeholders::_1)); + std::bind(&VelodyneHwInterface::receive_sensor_packet_callback, this, std::placeholders::_1)); } catch (const std::exception & ex) { Status status = Status::UDP_CONNECTION_ERROR; std::cerr << status << sensor_configuration_->sensor_ip << "," @@ -74,14 +74,14 @@ Status VelodyneHwInterface::SensorInterfaceStart() return Status::OK; } -Status VelodyneHwInterface::RegisterScanCallback( +Status VelodyneHwInterface::register_scan_callback( std::function & packet)> scan_callback) { cloud_packet_callback_ = std::move(scan_callback); return Status::OK; } -void VelodyneHwInterface::ReceiveSensorPacketCallback(std::vector & buffer) +void VelodyneHwInterface::receive_sensor_packet_callback(std::vector & buffer) { if (!cloud_packet_callback_) { return; @@ -89,20 +89,20 @@ void VelodyneHwInterface::ReceiveSensorPacketCallback(std::vector & buf cloud_packet_callback_(buffer); } -Status VelodyneHwInterface::SensorInterfaceStop() +Status VelodyneHwInterface::sensor_interface_stop() { return Status::ERROR_1; } -Status VelodyneHwInterface::GetSensorConfiguration(SensorConfigurationBase & sensor_configuration) +Status VelodyneHwInterface::get_sensor_configuration(SensorConfigurationBase & sensor_configuration) { std::stringstream ss; ss << sensor_configuration; - PrintDebug(ss.str()); + print_debug(ss.str()); return Status::ERROR_1; } -VelodyneStatus VelodyneHwInterface::InitHttpClient() +VelodyneStatus VelodyneHwInterface::init_http_client() { try { http_client_driver_->init_client(sensor_configuration_->sensor_ip, 80); @@ -116,12 +116,12 @@ VelodyneStatus VelodyneHwInterface::InitHttpClient() return Status::OK; } -void VelodyneHwInterface::StringCallback(const std::string & str) +void VelodyneHwInterface::string_callback(const std::string & str) { - std::cout << "VelodyneHwInterface::StringCallback: " << str << std::endl; + std::cout << "VelodyneHwInterface::string_callback: " << str << std::endl; } -boost::property_tree::ptree VelodyneHwInterface::ParseJson(const std::string & str) +boost::property_tree::ptree VelodyneHwInterface::parse_json(const std::string & str) { boost::property_tree::ptree tree; try { @@ -134,20 +134,20 @@ boost::property_tree::ptree VelodyneHwInterface::ParseJson(const std::string & s return tree; } -VelodyneStatus VelodyneHwInterface::CheckAndSetConfig( +VelodyneStatus VelodyneHwInterface::check_and_set_config( std::shared_ptr sensor_configuration, boost::property_tree::ptree tree) { VelodyneStatus status; - const auto & OK = VelodyneStatus::OK; + const auto & ok = VelodyneStatus::OK; std::string target_key = "config.returns"; auto current_return_mode_str = tree.get(target_key); auto current_return_mode = - nebula::drivers::ReturnModeFromStringVelodyne(tree.get(target_key)); + nebula::drivers::return_mode_from_string_velodyne(tree.get(target_key)); if (sensor_configuration->return_mode != current_return_mode) { - status = SetReturnType(sensor_configuration->return_mode); - if (status != OK) return status; + status = set_return_type(sensor_configuration->return_mode); + if (status != ok) return status; std::cout << "VelodyneHwInterface::parse_json(" << target_key << "): " << current_return_mode_str << std::endl; @@ -159,8 +159,8 @@ VelodyneStatus VelodyneHwInterface::CheckAndSetConfig( target_key = "config.rpm"; auto current_rotation_speed = tree.get(target_key); if (sensor_configuration->rotation_speed != current_rotation_speed) { - status = SetRpm(sensor_configuration->rotation_speed); - if (status != OK) return status; + status = set_rpm(sensor_configuration->rotation_speed); + if (status != ok) return status; std::cout << "VelodyneHwInterface::parse_json(" << target_key << "): " << current_rotation_speed << std::endl; @@ -176,8 +176,8 @@ VelodyneStatus VelodyneHwInterface::CheckAndSetConfig( setting_cloud_min_angle = 359; } if (setting_cloud_min_angle != current_cloud_min_angle) { - status = SetFovStart(setting_cloud_min_angle); - if (status != OK) return status; + status = set_fov_start(setting_cloud_min_angle); + if (status != ok) return status; std::cout << "VelodyneHwInterface::parse_json(" << target_key << "): " << current_cloud_min_angle << std::endl; @@ -192,8 +192,8 @@ VelodyneStatus VelodyneHwInterface::CheckAndSetConfig( setting_cloud_max_angle = 359; } if (setting_cloud_max_angle != current_cloud_max_angle) { - status = SetFovEnd(setting_cloud_max_angle); - if (status != OK) return status; + status = set_fov_end(setting_cloud_max_angle); + if (status != ok) return status; std::cout << "VelodyneHwInterface::parse_json(" << target_key << "): " << current_cloud_max_angle << std::endl; @@ -203,8 +203,8 @@ VelodyneStatus VelodyneHwInterface::CheckAndSetConfig( target_key = "config.host.addr"; auto current_host_addr = tree.get(target_key); if (sensor_configuration->host_ip != current_host_addr) { - status = SetHostAddr(sensor_configuration->host_ip); - if (status != OK) return status; + status = set_host_addr(sensor_configuration->host_ip); + if (status != ok) return status; std::cout << "VelodyneHwInterface::parse_json(" << target_key << "): " << current_host_addr << std::endl; @@ -214,8 +214,8 @@ VelodyneStatus VelodyneHwInterface::CheckAndSetConfig( target_key = "config.host.dport"; auto current_host_dport = tree.get(target_key); if (sensor_configuration->data_port != current_host_dport) { - status = SetHostDport(sensor_configuration->data_port); - if (status != OK) return status; + status = set_host_dport(sensor_configuration->data_port); + if (status != ok) return status; std::cout << "VelodyneHwInterface::parse_json(" << target_key << "): " << current_host_dport << std::endl; @@ -226,8 +226,8 @@ VelodyneStatus VelodyneHwInterface::CheckAndSetConfig( target_key = "config.host.tport"; auto current_host_tport = tree.get(target_key); if (sensor_configuration->gnss_port != current_host_tport) { - status = SetHostTport(sensor_configuration->gnss_port); - if (status != OK) return status; + status = set_host_tport(sensor_configuration->gnss_port); + if (status != ok) return status; std::cout << "VelodyneHwInterface::parse_json(" << target_key << "): " << current_host_tport << std::endl; @@ -235,59 +235,59 @@ VelodyneStatus VelodyneHwInterface::CheckAndSetConfig( << std::endl; } - return OK; + return ok; } // sync -std::string VelodyneHwInterface::GetStatus() +std::string VelodyneHwInterface::get_status() { - return HttpGetRequest(TARGET_STATUS); + return http_get_request(target_status_); } -std::string VelodyneHwInterface::GetDiag() +std::string VelodyneHwInterface::get_diag() { - auto rt = HttpGetRequest(TARGET_DIAG); + auto rt = http_get_request(target_diag_); std::cout << "read_response: " << rt << std::endl; return rt; } -std::string VelodyneHwInterface::GetSnapshot() +std::string VelodyneHwInterface::get_snapshot() { - return HttpGetRequest(TARGET_SNAPSHOT); + return http_get_request(target_snapshot_); } -VelodyneStatus VelodyneHwInterface::SetRpm(uint16_t rpm) +VelodyneStatus VelodyneHwInterface::set_rpm(uint16_t rpm) { if (rpm < 300 || 1200 < rpm || rpm % 60 != 0) { return VelodyneStatus::INVALID_RPM_ERROR; } - auto rt = HttpPostRequest(TARGET_SETTING, (boost::format("rpm=%d") % rpm).str()); - StringCallback(rt); + auto rt = http_post_request(target_setting_, (boost::format("rpm=%d") % rpm).str()); + string_callback(rt); return Status::OK; } -VelodyneStatus VelodyneHwInterface::SetFovStart(uint16_t fov_start) +VelodyneStatus VelodyneHwInterface::set_fov_start(uint16_t fov_start) { if (359 < fov_start) { return VelodyneStatus::INVALID_FOV_ERROR; } - auto rt = HttpPostRequest(TARGET_FOV, (boost::format("start=%d") % fov_start).str()); - StringCallback(rt); + auto rt = http_post_request(target_fov_, (boost::format("start=%d") % fov_start).str()); + string_callback(rt); return Status::OK; } -VelodyneStatus VelodyneHwInterface::SetFovEnd(uint16_t fov_end) +VelodyneStatus VelodyneHwInterface::set_fov_end(uint16_t fov_end) { if (359 < fov_end) { return VelodyneStatus::INVALID_FOV_ERROR; } - auto rt = HttpPostRequest(TARGET_FOV, (boost::format("end=%d") % fov_end).str()); - StringCallback(rt); + auto rt = http_post_request(target_fov_, (boost::format("end=%d") % fov_end).str()); + string_callback(rt); return Status::OK; } -VelodyneStatus VelodyneHwInterface::SetReturnType(nebula::drivers::ReturnMode return_mode) +VelodyneStatus VelodyneHwInterface::set_return_type(nebula::drivers::ReturnMode return_mode) { std::string body_str = ""; switch (return_mode) { @@ -303,128 +303,128 @@ VelodyneStatus VelodyneHwInterface::SetReturnType(nebula::drivers::ReturnMode re default: return VelodyneStatus::INVALID_RETURN_MODE_ERROR; } - auto rt = HttpPostRequest(TARGET_SETTING, body_str); - StringCallback(rt); + auto rt = http_post_request(target_setting_, body_str); + string_callback(rt); return Status::OK; } -VelodyneStatus VelodyneHwInterface::SaveConfig() +VelodyneStatus VelodyneHwInterface::save_config() { std::string body_str = "submit"; - auto rt = HttpPostRequest(TARGET_SAVE, body_str); - StringCallback(rt); + auto rt = http_post_request(target_save_, body_str); + string_callback(rt); return Status::OK; } -VelodyneStatus VelodyneHwInterface::ResetSystem() +VelodyneStatus VelodyneHwInterface::reset_system() { std::string body_str = "reset_system"; - auto rt = HttpPostRequest(TARGET_RESET, body_str); - StringCallback(rt); + auto rt = http_post_request(target_reset_, body_str); + string_callback(rt); return Status::OK; } -VelodyneStatus VelodyneHwInterface::LaserOn() +VelodyneStatus VelodyneHwInterface::laser_on() { std::string body_str = "laser=on"; - auto rt = HttpPostRequest(TARGET_SETTING, body_str); - StringCallback(rt); + auto rt = http_post_request(target_setting_, body_str); + string_callback(rt); return Status::OK; } -VelodyneStatus VelodyneHwInterface::LaserOff() +VelodyneStatus VelodyneHwInterface::laser_off() { std::string body_str = "laser=off"; - auto rt = HttpPostRequest(TARGET_SETTING, body_str); - StringCallback(rt); + auto rt = http_post_request(target_setting_, body_str); + string_callback(rt); return Status::OK; } -VelodyneStatus VelodyneHwInterface::LaserOnOff(bool on) +VelodyneStatus VelodyneHwInterface::laser_on_off(bool on) { std::string body_str = (boost::format("laser=%s") % (on ? "on" : "off")).str(); - auto rt = HttpPostRequest(TARGET_SETTING, body_str); - StringCallback(rt); + auto rt = http_post_request(target_setting_, body_str); + string_callback(rt); return Status::OK; } -VelodyneStatus VelodyneHwInterface::SetHostAddr(std::string addr) +VelodyneStatus VelodyneHwInterface::set_host_addr(std::string addr) { - auto rt = HttpPostRequest(TARGET_HOST, (boost::format("addr=%s") % addr).str()); - StringCallback(rt); + auto rt = http_post_request(target_host_, (boost::format("addr=%s") % addr).str()); + string_callback(rt); return Status::OK; } -VelodyneStatus VelodyneHwInterface::SetHostDport(uint16_t dport) +VelodyneStatus VelodyneHwInterface::set_host_dport(uint16_t dport) { - auto rt = HttpPostRequest(TARGET_HOST, (boost::format("dport=%d") % dport).str()); - StringCallback(rt); + auto rt = http_post_request(target_host_, (boost::format("dport=%d") % dport).str()); + string_callback(rt); return Status::OK; } -VelodyneStatus VelodyneHwInterface::SetHostTport(uint16_t tport) +VelodyneStatus VelodyneHwInterface::set_host_tport(uint16_t tport) { - auto rt = HttpPostRequest(TARGET_HOST, (boost::format("tport=%d") % tport).str()); - StringCallback(rt); + auto rt = http_post_request(target_host_, (boost::format("tport=%d") % tport).str()); + string_callback(rt); return Status::OK; } -VelodyneStatus VelodyneHwInterface::SetNetAddr(std::string addr) +VelodyneStatus VelodyneHwInterface::set_net_addr(std::string addr) { - auto rt = HttpPostRequest(TARGET_NET, (boost::format("addr=%s") % addr).str()); - StringCallback(rt); + auto rt = http_post_request(target_net_, (boost::format("addr=%s") % addr).str()); + string_callback(rt); return Status::OK; } -VelodyneStatus VelodyneHwInterface::SetNetMask(std::string mask) +VelodyneStatus VelodyneHwInterface::set_net_mask(std::string mask) { - auto rt = HttpPostRequest(TARGET_NET, (boost::format("mask=%s") % mask).str()); - StringCallback(rt); + auto rt = http_post_request(target_net_, (boost::format("mask=%s") % mask).str()); + string_callback(rt); return Status::OK; } -VelodyneStatus VelodyneHwInterface::SetNetGateway(std::string gateway) +VelodyneStatus VelodyneHwInterface::set_net_gateway(std::string gateway) { - auto rt = HttpPostRequest(TARGET_NET, (boost::format("gateway=%s") % gateway).str()); - StringCallback(rt); + auto rt = http_post_request(target_net_, (boost::format("gateway=%s") % gateway).str()); + string_callback(rt); return Status::OK; } -VelodyneStatus VelodyneHwInterface::SetNetDhcp(bool use_dhcp) +VelodyneStatus VelodyneHwInterface::set_net_dhcp(bool use_dhcp) { auto rt = - HttpPostRequest(TARGET_NET, (boost::format("dhcp=%s") % (use_dhcp ? "on" : "off")).str()); - StringCallback(rt); + http_post_request(target_net_, (boost::format("dhcp=%s") % (use_dhcp ? "on" : "off")).str()); + string_callback(rt); return Status::OK; } -void VelodyneHwInterface::SetLogger(std::shared_ptr logger) +void VelodyneHwInterface::set_logger(std::shared_ptr logger) { - parent_node_logger = logger; + parent_node_logger_ = logger; } -void VelodyneHwInterface::PrintInfo(std::string info) +void VelodyneHwInterface::print_info(std::string info) { - if (parent_node_logger) { - RCLCPP_INFO_STREAM((*parent_node_logger), info); + if (parent_node_logger_) { + RCLCPP_INFO_STREAM((*parent_node_logger_), info); } else { std::cout << info << std::endl; } } -void VelodyneHwInterface::PrintError(std::string error) +void VelodyneHwInterface::print_error(std::string error) { - if (parent_node_logger) { - RCLCPP_ERROR_STREAM((*parent_node_logger), error); + if (parent_node_logger_) { + RCLCPP_ERROR_STREAM((*parent_node_logger_), error); } else { std::cerr << error << std::endl; } } -void VelodyneHwInterface::PrintDebug(std::string debug) +void VelodyneHwInterface::print_debug(std::string debug) { - if (parent_node_logger) { - RCLCPP_DEBUG_STREAM((*parent_node_logger), debug); + if (parent_node_logger_) { + RCLCPP_DEBUG_STREAM((*parent_node_logger_), debug); } else { std::cout << debug << std::endl; } diff --git a/nebula_ros/include/nebula_ros/common/mt_queue.hpp b/nebula_ros/include/nebula_ros/common/mt_queue.hpp index c7fe30c0f..40f26253d 100644 --- a/nebula_ros/include/nebula_ros/common/mt_queue.hpp +++ b/nebula_ros/include/nebula_ros/common/mt_queue.hpp @@ -21,7 +21,7 @@ #include template -class mt_queue +class MtQueue { private: std::mutex mutex_; @@ -30,7 +30,7 @@ class mt_queue size_t capacity_; public: - explicit mt_queue(size_t capacity) : capacity_(capacity) {} + explicit MtQueue(size_t capacity) : capacity_(capacity) {} bool try_push(T && value) { diff --git a/nebula_ros/include/nebula_ros/common/watchdog_timer.hpp b/nebula_ros/include/nebula_ros/common/watchdog_timer.hpp index fc4dd2967..2e571339d 100644 --- a/nebula_ros/include/nebula_ros/common/watchdog_timer.hpp +++ b/nebula_ros/include/nebula_ros/common/watchdog_timer.hpp @@ -40,13 +40,13 @@ class WatchdogTimer std::chrono::duration_cast(expected_update_interval).count()) { timer_ = - node_.create_wall_timer(expected_update_interval, std::bind(&WatchdogTimer::onTimer, this)); + node_.create_wall_timer(expected_update_interval, std::bind(&WatchdogTimer::on_timer, this)); } void update() { last_update_ns_ = node_.get_clock()->now().nanoseconds(); } private: - void onTimer() + void on_timer() { uint64_t now_ns = node_.get_clock()->now().nanoseconds(); diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp index 3c1eaa905..08cc1a93e 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_decoder_wrapper.hpp @@ -53,38 +53,39 @@ class ContinentalARS548DecoderWrapper const nebula::drivers::continental_ars548::ContinentalARS548SensorConfiguration> & config, bool launch_hw); - void ProcessPacket(std::unique_ptr packet_msg); + void process_packet(std::unique_ptr packet_msg); - void OnConfigChange( + void on_config_change( const std::shared_ptr< const nebula::drivers::continental_ars548::ContinentalARS548SensorConfiguration> & new_config); - rcl_interfaces::msg::SetParametersResult OnParameterChange( + rcl_interfaces::msg::SetParametersResult on_parameter_change( const std::vector & p); - nebula::Status Status(); + nebula::Status status(); /// @brief Callback to process new ContinentalArs548DetectionList from the driver /// @param msg The new ContinentalArs548DetectionList from the driver - void DetectionListCallback( + void detection_list_callback( std::unique_ptr msg); /// @brief Callback to process new ContinentalArs548ObjectList from the driver /// @param msg The new ContinentalArs548ObjectList from the driver - void ObjectListCallback(std::unique_ptr msg); + void object_list_callback( + std::unique_ptr msg); /// @brief Callback to process new ContinentalARS548Status from the driver /// @param msg The new ContinentalArs548ObjectList from the driver - void SensorStatusCallback( + void sensor_status_callback( const drivers::continental_ars548::ContinentalARS548Status & sensor_status); /// @brief Callback to process new ContinentalARS548Status from the driver /// @param msg The new ContinentalArs548ObjectList from the driver - void PacketsCallback(std::unique_ptr msg); + void packets_callback(std::unique_ptr msg); private: - nebula::Status InitializeDriver( + nebula::Status initialize_driver( const std::shared_ptr< const nebula::drivers::continental_ars548::ContinentalARS548SensorConfiguration> & config); @@ -92,36 +93,36 @@ class ContinentalARS548DecoderWrapper /// @param msg The ARS548 detection list msg /// @return Resulting detection pointcloud pcl::PointCloud::Ptr - ConvertToPointcloud(const continental_msgs::msg::ContinentalArs548DetectionList & msg); + convert_to_pointcloud(const continental_msgs::msg::ContinentalArs548DetectionList & msg); /// @brief Convert ARS548 objects to a pointcloud /// @param msg The ARS548 object list msg /// @return Resulting object pointcloud - pcl::PointCloud::Ptr ConvertToPointcloud( - const continental_msgs::msg::ContinentalArs548ObjectList & msg); + pcl::PointCloud::Ptr + convert_to_pointcloud(const continental_msgs::msg::ContinentalArs548ObjectList & msg); /// @brief Convert ARS548 detections to a standard RadarScan msg /// @param msg The ARS548 detection list msg /// @return Resulting RadarScan msg - radar_msgs::msg::RadarScan ConvertToRadarScan( + radar_msgs::msg::RadarScan convert_to_radar_scan( const continental_msgs::msg::ContinentalArs548DetectionList & msg); /// @brief Convert ARS548 objects to a standard RadarTracks msg /// @param msg The ARS548 object list msg /// @return Resulting RadarTracks msg - radar_msgs::msg::RadarTracks ConvertToRadarTracks( + radar_msgs::msg::RadarTracks convert_to_radar_tracks( const continental_msgs::msg::ContinentalArs548ObjectList & msg); /// @brief Convert ARS548 objects to a standard MarkerArray msg /// @param msg The ARS548 object list msg /// @return Resulting MarkerArray msg - visualization_msgs::msg::MarkerArray ConvertToMarkers( + visualization_msgs::msg::MarkerArray convert_to_markers( const continental_msgs::msg::ContinentalArs548ObjectList & msg); /// @brief Convert seconds to chrono::nanoseconds /// @param seconds /// @return chrono::nanoseconds - static inline std::chrono::nanoseconds SecondsToChronoNanoSeconds(const double seconds) + static inline std::chrono::nanoseconds seconds_to_chrono_nano_seconds(const double seconds) { return std::chrono::duration_cast( std::chrono::duration(seconds)); @@ -151,8 +152,8 @@ class ContinentalARS548DecoderWrapper std::unordered_set previous_ids_{}; - constexpr static int REFERENCE_POINTS_NUM = 9; - constexpr static std::array, REFERENCE_POINTS_NUM> reference_to_center_ = { + constexpr static int reference_points_num = 9; + constexpr static std::array, reference_points_num> reference_to_center = { {{{-1.0, -1.0}}, {{-1.0, 0.0}}, {{-1.0, 1.0}}, diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp index 2931ca5c1..5279a2688 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_wrapper.hpp @@ -43,35 +43,35 @@ class ContinentalARS548HwInterfaceWrapper config); /// @brief Starts the hw interface and subscribes to the input topics - void SensorInterfaceStart(); + void sensor_interface_start(); - void OnConfigChange( + void on_config_change( const std::shared_ptr & new_config_ptr); /// @brief Get current status of the hw interface /// @return Current status - nebula::Status Status(); + nebula::Status status(); - std::shared_ptr HwInterface() const; + std::shared_ptr hw_interface() const; private: /// @brief Callback to send the odometry information to the radar device /// @param msg The odometry message - void OdometryCallback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); + void odometry_callback(const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg); /// @brief Callback to send the acceleration information to the radar device /// @param msg The acceleration message - void AccelerationCallback(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg); + void acceleration_callback(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg); /// @brief Callback to send the steering angle information to the radar device /// @param msg The steering angle message - void SteeringAngleCallback(const std_msgs::msg::Float32::SharedPtr msg); + void steering_angle_callback(const std_msgs::msg::Float32::SharedPtr msg); /// @brief Service callback to set the new sensor ip /// @param request service request /// @param response service response - void SetNetworkConfigurationRequestCallback( + void set_network_configuration_request_callback( const std::shared_ptr request, const std::shared_ptr @@ -80,7 +80,7 @@ class ContinentalARS548HwInterfaceWrapper /// @brief Service callback to set the new sensor mounting position /// @param request service request /// @param response service response - void SetSensorMountingRequestCallback( + void set_sensor_mounting_request_callback( const std::shared_ptr request, const std::shared_ptr @@ -89,7 +89,7 @@ class ContinentalARS548HwInterfaceWrapper /// @brief Service callback to set the new vehicle parameters /// @param request service request /// @param response service response - void SetVehicleParametersRequestCallback( + void set_vehicle_parameters_request_callback( const std::shared_ptr request, const std::shared_ptr @@ -98,7 +98,7 @@ class ContinentalARS548HwInterfaceWrapper /// @brief Service callback to set the new radar parameters /// @param request service request /// @param response service response - void SetRadarParametersRequestCallback( + void set_radar_parameters_request_callback( const std::shared_ptr request, const std::shared_ptr diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp index 9de45b042..86524980f 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_ros_wrapper.hpp @@ -50,30 +50,30 @@ class ContinentalARS548RosWrapper final : public rclcpp::Node /// @brief Get current status of this driver /// @return Current status - Status GetStatus(); + Status get_status(); /// @brief Start data streaming (Call SensorInterfaceStart of HwInterface) /// @return Resulting status - Status StreamStart(); + Status stream_start(); private: /// @brief Callback from the hw interface's raw data - void ReceivePacketCallback(std::unique_ptr packet_msg_ptr); + void receive_packet_callback(std::unique_ptr packet_msg_ptr); /// @brief Callback from replayed NebulaPackets - void ReceivePacketsCallback(std::unique_ptr packets_msg_ptr); + void receive_packets_callback(std::unique_ptr packets_msg_ptr); /// @brief Retrieve the parameters from ROS and set the driver and hw interface /// @return Resulting status - Status DeclareAndGetSensorConfigParams(); + Status declare_and_get_sensor_config_params(); /// @brief rclcpp parameter callback /// @param parameters Received parameters /// @return SetParametersResult - rcl_interfaces::msg::SetParametersResult OnParameterChange( + rcl_interfaces::msg::SetParametersResult on_parameter_change( const std::vector & p); - Status ValidateAndSetConfig( + Status validate_and_set_config( std::shared_ptr & new_config); @@ -83,7 +83,7 @@ class ContinentalARS548RosWrapper final : public rclcpp::Node config_ptr_{}; /// @brief Stores received packets that have not been processed yet by the decoder thread - mt_queue> packet_queue_; + MtQueue> packet_queue_; /// @brief Thread to isolate decoding from receiving std::thread decoder_thread_; diff --git a/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_wrapper.hpp index 2dbfa3447..9a32d4110 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_srr520_decoder_wrapper.hpp @@ -54,45 +54,46 @@ class ContinentalSRR520DecoderWrapper config, std::shared_ptr hw_interface_ptr); - void ProcessPacket(std::unique_ptr packet_msg); + void process_packet(std::unique_ptr packet_msg); - void OnConfigChange( + void on_config_change( const std::shared_ptr< const nebula::drivers::continental_srr520::ContinentalSRR520SensorConfiguration> & new_config_ptr); - rcl_interfaces::msg::SetParametersResult OnParameterChange( + rcl_interfaces::msg::SetParametersResult on_parameter_change( const std::vector & p); - nebula::Status Status(); + nebula::Status status(); /// @brief Callback to process a new Near ContinentalSrr520DetectionList from the driver /// @param msg The new ContinentalSrr520DetectionList from the driver - void NearDetectionListCallback( + void near_detection_list_callback( std::unique_ptr msg); /// @brief Callback to process a new HRR ContinentalSrr520DetectionList from the driver /// @param msg The new ContinentalSrr520DetectionList from the driver - void HRRDetectionListCallback( + void hrr_detection_list_callback( std::unique_ptr msg); /// @brief Callback to process a new ContinentalSrr520ObjectList from the driver /// @param msg The new ContinentalSrr520ObjectList from the driver - void ObjectListCallback(std::unique_ptr msg); + void object_list_callback( + std::unique_ptr msg); /// @brief Callback to process a new DiagnosticArray from the driver /// @param msg The new DiagnosticArray from the driver - void StatusCallback(std::unique_ptr msg); + void status_callback(std::unique_ptr msg); /// @brief Callback to process a new SyncFollowUp message from the driver - void SyncFollowUpCallback(builtin_interfaces::msg::Time stamp); + void sync_follow_up_callback(builtin_interfaces::msg::Time stamp); /// @brief Callback to process a new NebulaPackets message from the driver /// @param msg The new NebulaPackets from the driver - void PacketsCallback(std::unique_ptr msg); + void packets_callback(std::unique_ptr msg); private: - nebula::Status InitializeDriver( + nebula::Status initialize_driver( const std::shared_ptr< const nebula::drivers::continental_srr520::ContinentalSRR520SensorConfiguration> & config); @@ -100,30 +101,30 @@ class ContinentalSRR520DecoderWrapper /// @param msg The SRR520 detection list msg /// @return Resulting detection pointcloud pcl::PointCloud::Ptr - ConvertToPointcloud(const continental_msgs::msg::ContinentalSrr520DetectionList & msg); + convert_to_pointcloud(const continental_msgs::msg::ContinentalSrr520DetectionList & msg); /// @brief Convert SRR520 objects to a pointcloud /// @param msg The SRR520 object list msg /// @return Resulting object pointcloud - pcl::PointCloud::Ptr ConvertToPointcloud( - const continental_msgs::msg::ContinentalSrr520ObjectList & msg); + pcl::PointCloud::Ptr + convert_to_pointcloud(const continental_msgs::msg::ContinentalSrr520ObjectList & msg); /// @brief Convert SRR520 detections to a standard RadarScan msg /// @param msg The SRR520 detection list msg /// @return Resulting RadarScan msg - radar_msgs::msg::RadarScan ConvertToRadarScan( + radar_msgs::msg::RadarScan convert_to_radar_scan( const continental_msgs::msg::ContinentalSrr520DetectionList & msg); /// @brief Convert SRR520 objects to a standard RadarTracks msg /// @param msg The SRR520 object list msg /// @return Resulting RadarTracks msg - radar_msgs::msg::RadarTracks ConvertToRadarTracks( + radar_msgs::msg::RadarTracks convert_to_radar_tracks( const continental_msgs::msg::ContinentalSrr520ObjectList & msg); /// @brief Convert SRR520 objects to a standard MarkerArray msg /// @param msg The SRR520 object list msg /// @return Resulting MarkerArray msg - visualization_msgs::msg::MarkerArray ConvertToMarkers( + visualization_msgs::msg::MarkerArray convert_to_markers( const continental_msgs::msg::ContinentalSrr520ObjectList & msg); const rclcpp::Node * const parent_node_; diff --git a/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_wrapper.hpp index 3da76cab6..afcc2a4ca 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_srr520_hw_interface_wrapper.hpp @@ -43,37 +43,37 @@ class ContinentalSRR520HwInterfaceWrapper config); /// @brief Starts the hw interface and subscribes to the input topics - void SensorInterfaceStart(); + void sensor_interface_start(); - void OnConfigChange( + void on_config_change( const std::shared_ptr & new_config); /// @brief Get current status of the hw interface /// @return Current status - nebula::Status Status(); + nebula::Status status(); - std::shared_ptr HwInterface() const; + std::shared_ptr hw_interface() const; private: /// @brief Callback to send the odometry information to the radar device /// @param odometry_msg The odometry message /// @param acceleration_msg The acceleration message - void dynamicsCallback( + void dynamics_callback( const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr & odometry_msg, const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr & acceleration_msg); /// @brief Service callback to set the new sensor mounting position /// @param request Empty service request /// @param response Empty service response - void ConfigureSensorRequestCallback( + void configure_sensor_request_callback( const std::shared_ptr request, const std::shared_ptr response); /// @brief Method periodically called to initiate the sensor synchronization mechanism - void syncTimerCallback(); + void sync_timer_callback(); rclcpp::Node * const parent_node_; std::shared_ptr hw_interface_{}; diff --git a/nebula_ros/include/nebula_ros/continental/continental_srr520_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_srr520_ros_wrapper.hpp index 6df11e9b6..3858df8fc 100644 --- a/nebula_ros/include/nebula_ros/continental/continental_srr520_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/continental/continental_srr520_ros_wrapper.hpp @@ -48,30 +48,31 @@ class ContinentalSRR520RosWrapper final : public rclcpp::Node /// @brief Get current status of this driver /// @return Current status - Status GetStatus(); + Status get_status(); /// @brief Start data streaming (Call SensorInterfaceStart of HwInterface) /// @return Resulting status - Status StreamStart(); + Status stream_start(); private: /// @brief Callback from the hw interface's raw data - void ReceivePacketCallback(std::unique_ptr packet_msg_ptr); + void receive_packet_callback(std::unique_ptr packet_msg_ptr); /// @brief Callback from replayed NebulaPackets - void ReceivePacketsCallback(std::unique_ptr packet_packets_msg); + void receive_packets_callback( + std::unique_ptr packet_packets_msg); /// @brief Retrieve the parameters from ROS and set the driver and hw interface /// @return Resulting status - Status DeclareAndGetSensorConfigParams(); + Status declare_and_get_sensor_config_params(); /// @brief rclcpp parameter callback /// @param parameters Received parameters /// @return SetParametersResult - rcl_interfaces::msg::SetParametersResult OnParameterChange( + rcl_interfaces::msg::SetParametersResult on_parameter_change( const std::vector & p); - Status ValidateAndSetConfig( + Status validate_and_set_config( std::shared_ptr & new_config); @@ -81,7 +82,7 @@ class ContinentalSRR520RosWrapper final : public rclcpp::Node config_ptr_{}; /// @brief Stores received packets that have not been processed yet by the decoder thread - mt_queue> packet_queue_; + MtQueue> packet_queue_; /// @brief Thread to isolate decoding from receiving std::thread decoder_thread_; diff --git a/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp index cd77ed937..45db35a7e 100644 --- a/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp @@ -41,26 +41,26 @@ class HesaiDecoderWrapper const std::shared_ptr & calibration, bool publish_packets); - void ProcessCloudPacket(std::unique_ptr packet_msg); + void process_cloud_packet(std::unique_ptr packet_msg); - void OnConfigChange( + void on_config_change( const std::shared_ptr & new_config); - void OnCalibrationChange( + void on_calibration_change( const std::shared_ptr & new_calibration); - nebula::Status Status(); + nebula::Status status(); private: - void PublishCloud( + void publish_cloud( std::unique_ptr pointcloud, const rclcpp::Publisher::SharedPtr & publisher); /// @brief Convert seconds to chrono::nanoseconds /// @param seconds /// @return chrono::nanoseconds - static inline std::chrono::nanoseconds SecondsToChronoNanoSeconds(const double seconds) + static inline std::chrono::nanoseconds seconds_to_chrono_nano_seconds(const double seconds) { return std::chrono::duration_cast( std::chrono::duration(seconds)); diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp index 0292c0f8e..6c93af9d6 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp @@ -57,33 +57,33 @@ class HesaiRosWrapper final : public rclcpp::Node /// @brief Get current status of this driver /// @return Current status - Status GetStatus(); + Status get_status(); /// @brief Start point cloud streaming (Call SensorInterfaceStart of HwInterface) /// @return Resulting status - Status StreamStart(); + Status stream_start(); private: - void ReceiveCloudPacketCallback(std::vector & packet); + void receive_cloud_packet_callback(std::vector & packet); - void ReceiveScanMessageCallback(std::unique_ptr scan_msg); + void receive_scan_message_callback(std::unique_ptr scan_msg); - Status DeclareAndGetSensorConfigParams(); + Status declare_and_get_sensor_config_params(); /// @brief rclcpp parameter callback /// @param parameters Received parameters /// @return SetParametersResult - rcl_interfaces::msg::SetParametersResult OnParameterChange( + rcl_interfaces::msg::SetParametersResult on_parameter_change( const std::vector & p); - Status ValidateAndSetConfig( + Status validate_and_set_config( std::shared_ptr & new_config); /// @brief The ROS 2 parameter holding the calibration file path is called differently depending /// on the sensor model. This function returns the correct parameter name given a model. /// @param model The sensor model /// @return std::string The parameter name - std::string getCalibrationParameterName(drivers::SensorModel model) const; + std::string get_calibration_parameter_name(drivers::SensorModel model) const; /// @brief Load calibration data from the best available source: /// 1. If sensor connected, download and save from sensor @@ -93,7 +93,7 @@ class HesaiRosWrapper final : public rclcpp::Node /// @param ignore_others If true, skip straight so step 3 above, ignoring better calibration /// options /// @return The calibration data if successful, or an error code if not - get_calibration_result_t GetCalibrationData( + get_calibration_result_t get_calibration_data( const std::string & calibration_file_path, bool ignore_others = false); Status wrapper_status_; @@ -101,7 +101,7 @@ class HesaiRosWrapper final : public rclcpp::Node std::shared_ptr sensor_cfg_ptr_{}; /// @brief Stores received packets that have not been processed yet by the decoder thread - mt_queue> packet_queue_; + MtQueue> packet_queue_; /// @brief Thread to isolate decoding from receiving std::thread decoder_thread_; diff --git a/nebula_ros/include/nebula_ros/hesai/hw_interface_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hw_interface_wrapper.hpp index addc4f478..df1f726c3 100644 --- a/nebula_ros/include/nebula_ros/hesai/hw_interface_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hw_interface_wrapper.hpp @@ -31,12 +31,12 @@ class HesaiHwInterfaceWrapper rclcpp::Node * const parent_node, std::shared_ptr & config); - void OnConfigChange( + void on_config_change( const std::shared_ptr & new_config); - nebula::Status Status(); + nebula::Status status(); - std::shared_ptr HwInterface() const; + std::shared_ptr hw_interface() const; private: std::shared_ptr hw_interface_; diff --git a/nebula_ros/include/nebula_ros/hesai/hw_monitor_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hw_monitor_wrapper.hpp index 87b384d1a..340a004c6 100644 --- a/nebula_ros/include/nebula_ros/hesai/hw_monitor_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hw_monitor_wrapper.hpp @@ -40,37 +40,37 @@ class HesaiHwMonitorWrapper const std::shared_ptr & hw_interface, std::shared_ptr & config); - void OnConfigChange( + void on_config_change( const std::shared_ptr & /* new_config */) { } - nebula::Status Status(); + nebula::Status status(); private: - void InitializeHesaiDiagnostics(); + void initialize_hesai_diagnostics(); - std::string GetPtreeValue(boost::property_tree::ptree * pt, const std::string & key); + std::string get_ptree_value(boost::property_tree::ptree * pt, const std::string & key); - std::string GetFixedPrecisionString(double val, int pre); + std::string get_fixed_precision_string(double val, int pre); - void OnHesaiStatusTimer(); + void on_hesai_status_timer(); - void OnHesaiLidarMonitorTimerHttp(); + void on_hesai_lidar_monitor_timer_http(); - void OnHesaiLidarMonitorTimer(); + void on_hesai_lidar_monitor_timer(); - void HesaiCheckStatus(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void hesai_check_status(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - void HesaiCheckPtp(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void hesai_check_ptp(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - void HesaiCheckTemperature(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void hesai_check_temperature(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - void HesaiCheckRpm(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void hesai_check_rpm(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - void HesaiCheckVoltageHttp(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void hesai_check_voltage_http(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); - void HesaiCheckVoltage(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void hesai_check_voltage(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); rclcpp::Logger logger_; diagnostic_updater::Updater diagnostics_updater_; @@ -107,9 +107,9 @@ class HesaiHwMonitorWrapper bool supports_monitor_; - const std::string MSG_NOT_SUPPORTED = "Not supported"; - const std::string MSG_ERROR = "Error"; - const std::string MSG_SEP = ": "; + const std::string MSG_NOT_SUPPORTED_ = "Not supported"; + const std::string MSG_ERROR_ = "Error"; + const std::string MSG_SEP_ = ": "; }; } // namespace ros } // namespace nebula diff --git a/nebula_ros/include/nebula_ros/robosense/decoder_wrapper.hpp b/nebula_ros/include/nebula_ros/robosense/decoder_wrapper.hpp index f0d858e59..868f1c6d0 100644 --- a/nebula_ros/include/nebula_ros/robosense/decoder_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/robosense/decoder_wrapper.hpp @@ -43,22 +43,22 @@ class RobosenseDecoderWrapper const std::shared_ptr & config, const std::shared_ptr & calibration); - void ProcessCloudPacket(std::unique_ptr packet_msg); + void process_cloud_packet(std::unique_ptr packet_msg); - void OnConfigChange( + void on_config_change( const std::shared_ptr & new_config); - nebula::Status Status(); + nebula::Status status(); private: - void PublishCloud( + void publish_cloud( std::unique_ptr pointcloud, const rclcpp::Publisher::SharedPtr & publisher); /// @brief Convert seconds to chrono::nanoseconds /// @param seconds /// @return chrono::nanoseconds - static inline std::chrono::nanoseconds SecondsToChronoNanoSeconds(const double seconds) + static inline std::chrono::nanoseconds seconds_to_chrono_nano_seconds(const double seconds) { return std::chrono::duration_cast( std::chrono::duration(seconds)); diff --git a/nebula_ros/include/nebula_ros/robosense/hw_interface_wrapper.hpp b/nebula_ros/include/nebula_ros/robosense/hw_interface_wrapper.hpp index 29c6efef4..74eabec5c 100644 --- a/nebula_ros/include/nebula_ros/robosense/hw_interface_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/robosense/hw_interface_wrapper.hpp @@ -32,12 +32,12 @@ class RobosenseHwInterfaceWrapper rclcpp::Node * const parent_node, std::shared_ptr & config); - void OnConfigChange( + void on_config_change( const std::shared_ptr & new_config); - nebula::Status Status(); + nebula::Status status(); - std::shared_ptr HwInterface() const; + std::shared_ptr hw_interface() const; private: std::shared_ptr hw_interface_; diff --git a/nebula_ros/include/nebula_ros/robosense/hw_monitor_wrapper.hpp b/nebula_ros/include/nebula_ros/robosense/hw_monitor_wrapper.hpp index a5fe251e6..6f1561d4b 100644 --- a/nebula_ros/include/nebula_ros/robosense/hw_monitor_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/robosense/hw_monitor_wrapper.hpp @@ -43,22 +43,22 @@ class RobosenseHwMonitorWrapper rclcpp::Node * const parent_node, std::shared_ptr & config); - void OnConfigChange( + void on_config_change( const std::shared_ptr & new_config); /// @brief Callback for receiving DIFOP packet /// @param info_msg Received DIFOP packet - void DiagnosticsCallback(const std::map & diag_info); + void diagnostics_callback(const std::map & diag_info); private: /// @brief Initializing diagnostics - void InitializeRobosenseDiagnostics(); + void initialize_robosense_diagnostics(); /// @brief Callback of the timer for getting the current lidar status - void OnRobosenseStatusTimer(); + void on_robosense_status_timer(); /// @brief Check status information from RobosenseLidarStatus for diagnostic_updater /// @param diagnostics DiagnosticStatusWrapper - void RobosenseCheckStatus(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void robosense_check_status(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); rclcpp::Node * parent_; rclcpp::Logger logger_; diff --git a/nebula_ros/include/nebula_ros/robosense/robosense_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/robosense/robosense_ros_wrapper.hpp index 0c66e0f9d..64b1d9adc 100644 --- a/nebula_ros/include/nebula_ros/robosense/robosense_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/robosense/robosense_ros_wrapper.hpp @@ -57,28 +57,28 @@ class RobosenseRosWrapper final : public rclcpp::Node /// @brief Get current status of this driver /// @return Current status - Status GetStatus(); + Status get_status(); /// @brief Start point cloud streaming (Call CloudInterfaceStart of HwInterface) /// @return Resulting status - Status StreamStart(); + Status stream_start(); private: - void ReceiveCloudPacketCallback(std::vector & packet); + void receive_cloud_packet_callback(std::vector & packet); - void ReceiveInfoPacketCallback(std::vector & packet); + void receive_info_packet_callback(std::vector & packet); - void ReceiveScanMessageCallback(std::unique_ptr scan_msg); + void receive_scan_message_callback(std::unique_ptr scan_msg); - nebula::Status DeclareAndGetSensorConfigParams(); + nebula::Status declare_and_get_sensor_config_params(); /// @brief rclcpp parameter callback /// @param parameters Received parameters /// @return SetParametersResult - rcl_interfaces::msg::SetParametersResult OnParameterChange( + rcl_interfaces::msg::SetParametersResult on_parameter_change( const std::vector & p); - nebula::Status ValidateAndSetConfig( + nebula::Status validate_and_set_config( std::shared_ptr & new_config); nebula::Status wrapper_status_; @@ -86,7 +86,7 @@ class RobosenseRosWrapper final : public rclcpp::Node std::shared_ptr sensor_cfg_ptr_{}; /// @brief Stores received packets that have not been processed yet by the decoder thread - mt_queue> packet_queue_; + MtQueue> packet_queue_; /// @brief Thread to isolate decoding from receiving std::thread decoder_thread_; diff --git a/nebula_ros/include/nebula_ros/velodyne/decoder_wrapper.hpp b/nebula_ros/include/nebula_ros/velodyne/decoder_wrapper.hpp index e78e51e0c..dd4349f81 100644 --- a/nebula_ros/include/nebula_ros/velodyne/decoder_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/velodyne/decoder_wrapper.hpp @@ -49,34 +49,34 @@ class VelodyneDecoderWrapper const std::shared_ptr & hw_interface, std::shared_ptr & config); - void ProcessCloudPacket(std::unique_ptr packet_msg); + void process_cloud_packet(std::unique_ptr packet_msg); - void OnConfigChange( + void on_config_change( const std::shared_ptr & new_config); - void OnCalibrationChange( + void on_calibration_change( const std::shared_ptr & new_calibration); - rcl_interfaces::msg::SetParametersResult OnParameterChange( + rcl_interfaces::msg::SetParametersResult on_parameter_change( const std::vector & p); - nebula::Status Status(); + nebula::Status status(); private: /// @brief Load calibration data from file /// @param calibration_file_path The file to read from /// @return The calibration data if successful, or an error code if not - get_calibration_result_t GetCalibrationData(const std::string & calibration_file_path); + get_calibration_result_t get_calibration_data(const std::string & calibration_file_path); - void PublishCloud( + void publish_cloud( std::unique_ptr pointcloud, const rclcpp::Publisher::SharedPtr & publisher); /// @brief Convert seconds to chrono::nanoseconds /// @param seconds /// @return chrono::nanoseconds - static inline std::chrono::nanoseconds SecondsToChronoNanoSeconds(const double seconds) + static inline std::chrono::nanoseconds seconds_to_chrono_nano_seconds(const double seconds) { return std::chrono::duration_cast( std::chrono::duration(seconds)); diff --git a/nebula_ros/include/nebula_ros/velodyne/hw_interface_wrapper.hpp b/nebula_ros/include/nebula_ros/velodyne/hw_interface_wrapper.hpp index e919b4367..f35bc59d2 100644 --- a/nebula_ros/include/nebula_ros/velodyne/hw_interface_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/velodyne/hw_interface_wrapper.hpp @@ -33,12 +33,12 @@ class VelodyneHwInterfaceWrapper rclcpp::Node * const parent_node, std::shared_ptr & config); - void OnConfigChange( + void on_config_change( const std::shared_ptr & new_config); - nebula::Status Status(); + nebula::Status status(); - std::shared_ptr HwInterface() const; + std::shared_ptr hw_interface() const; private: std::shared_ptr hw_interface_; diff --git a/nebula_ros/include/nebula_ros/velodyne/hw_monitor_wrapper.hpp b/nebula_ros/include/nebula_ros/velodyne/hw_monitor_wrapper.hpp index c86bfd917..9f1080096 100644 --- a/nebula_ros/include/nebula_ros/velodyne/hw_monitor_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/velodyne/hw_monitor_wrapper.hpp @@ -43,234 +43,234 @@ class VelodyneHwMonitorWrapper const std::shared_ptr & hw_interface, std::shared_ptr & config); - void OnConfigChange( + void on_config_change( const std::shared_ptr & /* new_config */) { } - nebula::Status Status(); + nebula::Status status(); private: - void InitializeVelodyneDiagnostics(); + void initialize_velodyne_diagnostics(); /// @brief Callback of the timer for getting the current lidar status - void OnVelodyneStatusTimer(); + void on_velodyne_status_timer(); /// @brief Callback of the timer for getting the current lidar snapshot - void OnVelodyneSnapshotTimer(); + void on_velodyne_snapshot_timer(); /// @brief Callback of the timer for getting the current lidar status & updating the diagnostics - void OnVelodyneDiagnosticsTimer(); + void on_velodyne_diagnostics_timer(); /// @brief Get value from property_tree /// @param pt property_tree /// @param mtx_pt the mutex associated with `pt` /// @param key Pey string /// @return Value - std::string GetPtreeValue( + std::string get_ptree_value( std::shared_ptr pt, std::mutex & mtx_pt, const std::string & key); /// @brief Making fixed precision string /// @param val Target value /// @param pre Precision /// @return Created string - std::string GetFixedPrecisionString(double val, int pre = 2); + std::string get_fixed_precision_string(double val, int pre = 2); /// @brief Getting top:hv from the current property_tree /// @return tuple - std::tuple VelodyneGetTopHv(); + std::tuple velodyne_get_top_hv(); /// @brief Getting top:ad_temp from the current property_tree (only VLP32) /// @return tuple - std::tuple VelodyneGetTopAdTemp(); // only32 + std::tuple velodyne_get_top_ad_temp(); // only32 /// @brief Getting top:lm20_temp from the current property_tree /// @return tuple - std::tuple VelodyneGetTopLm20Temp(); + std::tuple velodyne_get_top_lm20_temp(); /// @brief Getting top:pwr_5v from the current property_tree /// @return tuple - std::tuple VelodyneGetTopPwr5v(); + std::tuple velodyne_get_top_pwr5v(); /// @brief Getting top:pwr_2_5v from the current property_tree /// @return tuple - std::tuple VelodyneGetTopPwr25v(); + std::tuple velodyne_get_top_pwr25v(); /// @brief Getting top:pwr_3_3v from the current property_tree /// @return tuple - std::tuple VelodyneGetTopPwr33v(); + std::tuple velodyne_get_top_pwr33v(); /// @brief Getting top:pwr_5v_raw from the current property_tree (only VLP16) /// @return tuple - std::tuple VelodyneGetTopPwr5vRaw(); // only16 + std::tuple velodyne_get_top_pwr5v_raw(); // only16 /// @brief Getting top:pwr_raw from the current property_tree (only VLP32) /// @return tuple - std::tuple VelodyneGetTopPwrRaw(); // only32 + std::tuple velodyne_get_top_pwr_raw(); // only32 /// @brief Getting top:pwr_vccint from the current property_tree /// @return tuple - std::tuple VelodyneGetTopPwrVccint(); + std::tuple velodyne_get_top_pwr_vccint(); /// @brief Getting bot:i_out from the current property_tree /// @return tuple - std::tuple VelodyneGetBotIOut(); + std::tuple velodyne_get_bot_i_out(); /// @brief Getting bot:pwr_1_2v from the current property_tree /// @return tuple - std::tuple VelodyneGetBotPwr12v(); + std::tuple velodyne_get_bot_pwr12v(); /// @brief Getting bot:lm20_temp from the current property_tree /// @return tuple - std::tuple VelodyneGetBotLm20Temp(); + std::tuple velodyne_get_bot_lm20_temp(); /// @brief Getting bot:pwr_5v from the current property_tree /// @return tuple - std::tuple VelodyneGetBotPwr5v(); + std::tuple velodyne_get_bot_pwr5v(); /// @brief Getting bot:pwr_2_5v from the current property_tree /// @return tuple - std::tuple VelodyneGetBotPwr25v(); + std::tuple velodyne_get_bot_pwr25v(); /// @brief Getting bot:pwr_3_3v from the current property_tree /// @return tuple - std::tuple VelodyneGetBotPwr33v(); + std::tuple velodyne_get_bot_pwr33v(); /// @brief Getting bot:pwr_v_in from the current property_tree /// @return tuple - std::tuple VelodyneGetBotPwrVIn(); + std::tuple velodyne_get_bot_pwr_v_in(); /// @brief Getting bot:pwr_1_25v from the current property_tree /// @return tuple - std::tuple VelodyneGetBotPwr125v(); + std::tuple velodyne_get_bot_pwr125v(); /// @brief Getting vhv from the current property_tree /// @return tuple - std::tuple VelodyneGetVhv(); + std::tuple velodyne_get_vhv(); /// @brief Getting adc_nf from the current property_tree /// @return tuple - std::tuple VelodyneGetAdcNf(); + std::tuple velodyne_get_adc_nf(); /// @brief Getting adc_stats from the current property_tree /// @return tuple - std::tuple VelodyneGetAdcStats(); + std::tuple velodyne_get_adc_stats(); /// @brief Getting ixe from the current property_tree /// @return tuple - std::tuple VelodyneGetIxe(); + std::tuple velodyne_get_ixe(); /// @brief Getting adctp_stat from the current property_tree /// @return tuple - std::tuple VelodyneGetAdctpStat(); + std::tuple velodyne_get_adctp_stat(); /// @brief Getting gps:pps_state from the current property_tree /// @return tuple - std::tuple VelodyneGetGpsPpsState(); + std::tuple velodyne_get_gps_pps_state(); /// @brief Getting gps:position from the current property_tree /// @return tuple - std::tuple VelodyneGetGpsPosition(); + std::tuple velodyne_get_gps_position(); /// @brief Getting motor:state from the current property_tree /// @return tuple - std::tuple VelodyneGetMotorState(); + std::tuple velodyne_get_motor_state(); /// @brief Getting motor:rpm from the current property_tree /// @return tuple - std::tuple VelodyneGetMotorRpm(); + std::tuple velodyne_get_motor_rpm(); /// @brief Getting motor:lock from the current property_tree /// @return tuple - std::tuple VelodyneGetMotorLock(); + std::tuple velodyne_get_motor_lock(); /// @brief Getting motor:phase from the current property_tree /// @return tuple - std::tuple VelodyneGetMotorPhase(); + std::tuple velodyne_get_motor_phase(); /// @brief Getting laser:state from the current property_tree /// @return tuple - std::tuple VelodyneGetLaserState(); + std::tuple velodyne_get_laser_state(); /// @brief Check top:hv from the current property_tree for diagnostic_updater /// @param diagnostics DiagnosticStatusWrapper - void VelodyneCheckTopHv(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void velodyne_check_top_hv(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); /// @brief Check top:ad_temp from the current property_tree for diagnostic_updater /// @param diagnostics DiagnosticStatusWrapper - void VelodyneCheckTopAdTemp(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void velodyne_check_top_ad_temp(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); /// @brief Check top:lm20_temp from the current property_tree for diagnostic_updater /// @param diagnostics DiagnosticStatusWrapper - void VelodyneCheckTopLm20Temp(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void velodyne_check_top_lm20_temp(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); /// @brief Check top:pwr_5v from the current property_tree for diagnostic_updater /// @param diagnostics DiagnosticStatusWrapper - void VelodyneCheckTopPwr5v(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void velodyne_check_top_pwr5v(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); /// @brief Check top:pwr_2_5v from the current property_tree for diagnostic_updater /// @param diagnostics DiagnosticStatusWrapper - void VelodyneCheckTopPwr25v(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void velodyne_check_top_pwr25v(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); /// @brief Check top:pwr_3_3v from the current property_tree for diagnostic_updater /// @param diagnostics DiagnosticStatusWrapper - void VelodyneCheckTopPwr33v(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void velodyne_check_top_pwr33v(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); /// @brief Check top:pwr_raw from the current property_tree for diagnostic_updater /// @param diagnostics DiagnosticStatusWrapper - void VelodyneCheckTopPwrRaw(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void velodyne_check_top_pwr_raw(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); /// @brief Check top:pwr_vccint from the current property_tree for diagnostic_updater /// @param diagnostics DiagnosticStatusWrapper - void VelodyneCheckTopPwrVccint(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void velodyne_check_top_pwr_vccint(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); /// @brief Check bot:i_out from the current property_tree for diagnostic_updater /// @param diagnostics DiagnosticStatusWrapper - void VelodyneCheckBotIOut(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void velodyne_check_bot_i_out(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); /// @brief Check bot:pwr_1_2v from the current property_tree for diagnostic_updater /// @param diagnostics DiagnosticStatusWrapper - void VelodyneCheckBotPwr12v(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void velodyne_check_bot_pwr12v(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); /// @brief Check bot:lm20_temp from the current property_tree for diagnostic_updater /// @param diagnostics DiagnosticStatusWrapper - void VelodyneCheckBotLm20Temp(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void velodyne_check_bot_lm20_temp(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); /// @brief Check bot:pwr_5v from the current property_tree for diagnostic_updater /// @param diagnostics DiagnosticStatusWrapper - void VelodyneCheckBotPwr5v(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void velodyne_check_bot_pwr5v(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); /// @brief Check bot:pwr_2_5v from the current property_tree for diagnostic_updater /// @param diagnostics DiagnosticStatusWrapper - void VelodyneCheckBotPwr25v(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void velodyne_check_bot_pwr25v(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); /// @brief Check bot:pwr_3_3v from the current property_tree for diagnostic_updater /// @param diagnostics DiagnosticStatusWrapper - void VelodyneCheckBotPwr33v(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void velodyne_check_bot_pwr33v(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); /// @brief Check bot:pwr_v_in from the current property_tree for diagnostic_updater /// @param diagnostics DiagnosticStatusWrapper - void VelodyneCheckBotPwrVIn(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void velodyne_check_bot_pwr_v_in(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); /// @brief Check bot:pwr_1_25v from the current property_tree for diagnostic_updater /// @param diagnostics DiagnosticStatusWrapper - void VelodyneCheckBotPwr125v(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void velodyne_check_bot_pwr125v(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); /// @brief Check vhv from the current property_tree for diagnostic_updater /// @param diagnostics DiagnosticStatusWrapper - void VelodyneCheckVhv(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void velodyne_check_vhv(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); /// @brief Check adc_nf from the current property_tree for diagnostic_updater /// @param diagnostics DiagnosticStatusWrapper - void VelodyneCheckAdcNf(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void velodyne_check_adc_nf(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); /// @brief Check adc_stats from the current property_tree for diagnostic_updater /// @param diagnostics DiagnosticStatusWrapper - void VelodyneCheckAdcStats(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void velodyne_check_adc_stats(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); /// @brief Check ixe from the current property_tree for diagnostic_updater /// @param diagnostics DiagnosticStatusWrapper - void VelodyneCheckIxe(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void velodyne_check_ixe(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); /// @brief Check adctp_stat from the current property_tree for diagnostic_updater /// @param diagnostics DiagnosticStatusWrapper - void VelodyneCheckAdctpStat(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void velodyne_check_adctp_stat(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); rclcpp::TimerBase::SharedPtr diagnostics_status_timer_; - std::shared_ptr current_status_tree; + std::shared_ptr current_status_tree_; /// @brief Check gps:pps_state from the current property_tree for diagnostic_updater /// @param diagnostics DiagnosticStatusWrapper - void VelodyneCheckGpsPpsState(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void velodyne_check_gps_pps_state(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); /// @brief Check gps:position from the current property_tree for diagnostic_updater /// @param diagnostics DiagnosticStatusWrapper - void VelodyneCheckGpsPosition(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void velodyne_check_gps_position(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); /// @brief Check motor:state from the current property_tree for diagnostic_updater /// @param diagnostics DiagnosticStatusWrapper - void VelodyneCheckMotorState(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void velodyne_check_motor_state(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); /// @brief Check motor:rpm from the current property_tree for diagnostic_updater /// @param diagnostics DiagnosticStatusWrapper - void VelodyneCheckMotorRpm(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void velodyne_check_motor_rpm(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); /// @brief Check motor:lock from the current property_tree for diagnostic_updater /// @param diagnostics DiagnosticStatusWrapper - void VelodyneCheckMotorLock(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void velodyne_check_motor_lock(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); /// @brief Check motor:phase from the current property_tree for diagnostic_updater /// @param diagnostics DiagnosticStatusWrapper - void VelodyneCheckMotorPhase(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void velodyne_check_motor_phase(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); /// @brief Check laser:state from the current property_tree for diagnostic_updater /// @param diagnostics DiagnosticStatusWrapper - void VelodyneCheckLaserState(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void velodyne_check_laser_state(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); /// @brief Check the current snapshot for diagnostic_updater /// @param diagnostics DiagnosticStatusWrapper - void VelodyneCheckSnapshot(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void velodyne_check_snapshot(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); /// @brief Check the current states of motor & laser for diagnostic_updater /// @param diagnostics DiagnosticStatusWrapper - void VelodyneCheckStatus(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void velodyne_check_status(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); /// @brief Check the current gps information for diagnostic_updater /// @param diagnostics DiagnosticStatusWrapper - void VelodyneCheckPps(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void velodyne_check_pps(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); /// @brief Check the current temperatures for diagnostic_updater /// @param diagnostics DiagnosticStatusWrapper - void VelodyneCheckTemperature(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void velodyne_check_temperature(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); /// @brief Check the current rpm for diagnostic_updater /// @param diagnostics DiagnosticStatusWrapper - void VelodyneCheckRpm(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void velodyne_check_rpm(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); /// @brief Check the current voltages for diagnostic_updater /// @param diagnostics DiagnosticStatusWrapper - void VelodyneCheckVoltage(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); + void velodyne_check_voltage(diagnostic_updater::DiagnosticStatusWrapper & diagnostics); rclcpp::Logger logger_; diagnostic_updater::Updater diagnostics_updater_; @@ -288,11 +288,11 @@ class VelodyneHwMonitorWrapper rclcpp::TimerBase::SharedPtr diagnostics_update_timer_; rclcpp::TimerBase::SharedPtr diagnostics_diag_timer_; - std::shared_ptr current_snapshot; - std::shared_ptr current_snapshot_tree; - std::shared_ptr current_diag_tree; - std::shared_ptr current_snapshot_time; - uint8_t current_diag_status; + std::shared_ptr current_snapshot_; + std::shared_ptr current_snapshot_tree_; + std::shared_ptr current_diag_tree_; + std::shared_ptr current_snapshot_time_; + uint8_t current_diag_status_; std::mutex mtx_snapshot_; std::mutex mtx_status_; @@ -361,7 +361,7 @@ class VelodyneHwMonitorWrapper static constexpr auto name_status_motor_phase = "Motor Phase"; static constexpr auto name_status_laser_state = "Laser State"; - const std::string message_sep{": "}; + const std::string message_sep_{": "}; static constexpr auto not_supported_message = "Not supported"; static constexpr auto error_message = "Error"; diff --git a/nebula_ros/include/nebula_ros/velodyne/velodyne_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/velodyne/velodyne_ros_wrapper.hpp index 2ed7c4038..732f6b8f1 100644 --- a/nebula_ros/include/nebula_ros/velodyne/velodyne_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/velodyne/velodyne_ros_wrapper.hpp @@ -58,26 +58,26 @@ class VelodyneRosWrapper final : public rclcpp::Node /// @brief Get current status of this driver /// @return Current status - Status GetStatus(); + Status get_status(); /// @brief Start point cloud streaming (Call SensorInterfaceStart of HwInterface) /// @return Resulting status - Status StreamStart(); + Status stream_start(); private: - void ReceiveCloudPacketCallback(std::vector & packet); + void receive_cloud_packet_callback(std::vector & packet); - void ReceiveScanMessageCallback(std::unique_ptr scan_msg); + void receive_scan_message_callback(std::unique_ptr scan_msg); - Status DeclareAndGetSensorConfigParams(); + Status declare_and_get_sensor_config_params(); /// @brief rclcpp parameter callback /// @param parameters Received parameters /// @return SetParametersResult - rcl_interfaces::msg::SetParametersResult OnParameterChange( + rcl_interfaces::msg::SetParametersResult on_parameter_change( const std::vector & p); - Status ValidateAndSetConfig( + Status validate_and_set_config( std::shared_ptr & new_config); Status wrapper_status_; @@ -85,7 +85,7 @@ class VelodyneRosWrapper final : public rclcpp::Node std::shared_ptr sensor_cfg_ptr_{}; /// @brief Stores received packets that have not been processed yet by the decoder thread - mt_queue> packet_queue_; + MtQueue> packet_queue_; /// @brief Thread to isolate decoding from receiving std::thread decoder_thread_; diff --git a/nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp index dede30210..a116733e9 100644 --- a/nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_decoder_wrapper.cpp @@ -37,8 +37,8 @@ ContinentalARS548DecoderWrapper::ContinentalARS548DecoderWrapper( RCLCPP_INFO(logger_, "Starting Decoder"); - InitializeDriver(config_ptr); - status_ = driver_ptr_->GetStatus(); + initialize_driver(config_ptr); + status_ = driver_ptr_->get_status(); if (Status::OK != status_) { throw std::runtime_error( @@ -88,50 +88,50 @@ ContinentalARS548DecoderWrapper::ContinentalARS548DecoderWrapper( }); } -Status ContinentalARS548DecoderWrapper::InitializeDriver( +Status ContinentalARS548DecoderWrapper::initialize_driver( const std::shared_ptr< const nebula::drivers::continental_ars548::ContinentalARS548SensorConfiguration> & config) { driver_ptr_.reset(); driver_ptr_ = std::make_shared(config); - driver_ptr_->RegisterDetectionListCallback(std::bind( - &ContinentalARS548DecoderWrapper::DetectionListCallback, this, std::placeholders::_1)); - driver_ptr_->RegisterObjectListCallback( - std::bind(&ContinentalARS548DecoderWrapper::ObjectListCallback, this, std::placeholders::_1)); - driver_ptr_->RegisterSensorStatusCallback( - std::bind(&ContinentalARS548DecoderWrapper::SensorStatusCallback, this, std::placeholders::_1)); - driver_ptr_->RegisterPacketsCallback( - std::bind(&ContinentalARS548DecoderWrapper::PacketsCallback, this, std::placeholders::_1)); + driver_ptr_->register_detection_list_callback(std::bind( + &ContinentalARS548DecoderWrapper::detection_list_callback, this, std::placeholders::_1)); + driver_ptr_->register_object_list_callback( + std::bind(&ContinentalARS548DecoderWrapper::object_list_callback, this, std::placeholders::_1)); + driver_ptr_->register_sensor_status_callback(std::bind( + &ContinentalARS548DecoderWrapper::sensor_status_callback, this, std::placeholders::_1)); + driver_ptr_->register_packets_callback( + std::bind(&ContinentalARS548DecoderWrapper::packets_callback, this, std::placeholders::_1)); return Status::OK; } -void ContinentalARS548DecoderWrapper::OnConfigChange( +void ContinentalARS548DecoderWrapper::on_config_change( const std::shared_ptr< const nebula::drivers::continental_ars548::ContinentalARS548SensorConfiguration> & new_config_ptr) { std::lock_guard lock(mtx_driver_ptr_); - InitializeDriver(new_config_ptr); + initialize_driver(new_config_ptr); config_ptr_ = new_config_ptr; } -void ContinentalARS548DecoderWrapper::ProcessPacket( +void ContinentalARS548DecoderWrapper::process_packet( std::unique_ptr packet_msg) { - driver_ptr_->ProcessPacket(std::move(packet_msg)); + driver_ptr_->process_packet(std::move(packet_msg)); watchdog_->update(); } -void ContinentalARS548DecoderWrapper::DetectionListCallback( +void ContinentalARS548DecoderWrapper::detection_list_callback( std::unique_ptr msg) { if ( detection_pointcloud_pub_->get_subscription_count() > 0 || detection_pointcloud_pub_->get_intra_process_subscription_count() > 0) { - const auto detection_pointcloud_ptr = ConvertToPointcloud(*msg); + const auto detection_pointcloud_ptr = convert_to_pointcloud(*msg); auto detection_pointcloud_msg_ptr = std::make_unique(); pcl::toROSMsg(*detection_pointcloud_ptr, *detection_pointcloud_msg_ptr); @@ -142,7 +142,7 @@ void ContinentalARS548DecoderWrapper::DetectionListCallback( if ( scan_raw_pub_->get_subscription_count() > 0 || scan_raw_pub_->get_intra_process_subscription_count() > 0) { - auto radar_scan_msg = ConvertToRadarScan(*msg); + auto radar_scan_msg = convert_to_radar_scan(*msg); radar_scan_msg.header = msg->header; scan_raw_pub_->publish(std::move(radar_scan_msg)); } @@ -154,13 +154,13 @@ void ContinentalARS548DecoderWrapper::DetectionListCallback( } } -void ContinentalARS548DecoderWrapper::ObjectListCallback( +void ContinentalARS548DecoderWrapper::object_list_callback( std::unique_ptr msg) { if ( object_pointcloud_pub_->get_subscription_count() > 0 || object_pointcloud_pub_->get_intra_process_subscription_count() > 0) { - const auto object_pointcloud_ptr = ConvertToPointcloud(*msg); + const auto object_pointcloud_ptr = convert_to_pointcloud(*msg); auto object_pointcloud_msg_ptr = std::make_unique(); pcl::toROSMsg(*object_pointcloud_ptr, *object_pointcloud_msg_ptr); @@ -171,7 +171,7 @@ void ContinentalARS548DecoderWrapper::ObjectListCallback( if ( objects_raw_pub_->get_subscription_count() > 0 || objects_raw_pub_->get_intra_process_subscription_count() > 0) { - auto objects_raw_msg = ConvertToRadarTracks(*msg); + auto objects_raw_msg = convert_to_radar_tracks(*msg); objects_raw_msg.header = msg->header; objects_raw_pub_->publish(std::move(objects_raw_msg)); } @@ -179,7 +179,7 @@ void ContinentalARS548DecoderWrapper::ObjectListCallback( if ( objects_markers_pub_->get_subscription_count() > 0 || objects_markers_pub_->get_intra_process_subscription_count() > 0) { - auto marker_array_msg = ConvertToMarkers(*msg); + auto marker_array_msg = convert_to_markers(*msg); objects_markers_pub_->publish(std::move(marker_array_msg)); } @@ -190,7 +190,7 @@ void ContinentalARS548DecoderWrapper::ObjectListCallback( } } -void ContinentalARS548DecoderWrapper::SensorStatusCallback( +void ContinentalARS548DecoderWrapper::sensor_status_callback( const drivers::continental_ars548::ContinentalARS548Status & sensor_status) { diagnostic_msgs::msg::DiagnosticArray diagnostic_array_msg; @@ -304,7 +304,7 @@ void ContinentalARS548DecoderWrapper::SensorStatusCallback( diagnostics_pub_->publish(diagnostic_array_msg); } -void ContinentalARS548DecoderWrapper::PacketsCallback( +void ContinentalARS548DecoderWrapper::packets_callback( std::unique_ptr msg) { if ( @@ -315,7 +315,7 @@ void ContinentalARS548DecoderWrapper::PacketsCallback( } pcl::PointCloud::Ptr -ContinentalARS548DecoderWrapper::ConvertToPointcloud( +ContinentalARS548DecoderWrapper::convert_to_pointcloud( const continental_msgs::msg::ContinentalArs548DetectionList & msg) { pcl::PointCloud::Ptr output_pointcloud( @@ -355,7 +355,7 @@ ContinentalARS548DecoderWrapper::ConvertToPointcloud( } pcl::PointCloud::Ptr -ContinentalARS548DecoderWrapper::ConvertToPointcloud( +ContinentalARS548DecoderWrapper::convert_to_pointcloud( const continental_msgs::msg::ContinentalArs548ObjectList & msg) { pcl::PointCloud::Ptr output_pointcloud( @@ -394,7 +394,7 @@ ContinentalARS548DecoderWrapper::ConvertToPointcloud( return output_pointcloud; } -radar_msgs::msg::RadarScan ContinentalARS548DecoderWrapper::ConvertToRadarScan( +radar_msgs::msg::RadarScan ContinentalARS548DecoderWrapper::convert_to_radar_scan( const continental_msgs::msg::ContinentalArs548DetectionList & msg) { radar_msgs::msg::RadarScan output_msg; @@ -420,20 +420,20 @@ radar_msgs::msg::RadarScan ContinentalARS548DecoderWrapper::ConvertToRadarScan( return output_msg; } -radar_msgs::msg::RadarTracks ContinentalARS548DecoderWrapper::ConvertToRadarTracks( +radar_msgs::msg::RadarTracks ContinentalARS548DecoderWrapper::convert_to_radar_tracks( const continental_msgs::msg::ContinentalArs548ObjectList & msg) { radar_msgs::msg::RadarTracks output_msg; output_msg.tracks.reserve(msg.objects.size()); output_msg.header = msg.header; - constexpr int16_t UNKNOWN_ID = 32000; - constexpr int16_t CAR_ID = 32001; - constexpr int16_t TRUCK_ID = 32002; - constexpr int16_t MOTORCYCLE_ID = 32005; - constexpr int16_t BICYCLE_ID = 32006; - constexpr int16_t PEDESTRIAN_ID = 32007; - constexpr float INVALID_COVARIANCE = 1e6; + constexpr int16_t unknown_id = 32000; + constexpr int16_t car_id = 32001; + constexpr int16_t truck_id = 32002; + constexpr int16_t motorcycle_id = 32005; + constexpr int16_t bicycle_id = 32006; + constexpr int16_t pedestrian_id = 32007; + constexpr float invalid_covariance = 1e6; radar_msgs::msg::RadarTrack track_msg; for (const auto & object : msg.objects) { @@ -449,11 +449,11 @@ radar_msgs::msg::RadarTracks ContinentalARS548DecoderWrapper::ConvertToRadarTrac const int reference_index = std::min(object.position_reference, 8); const double & yaw = object.orientation; track_msg.position.x = object.position.x + - std::cos(yaw) * half_length * reference_to_center_[reference_index][0] - - std::sin(yaw) * half_width * reference_to_center_[reference_index][1]; + std::cos(yaw) * half_length * reference_to_center[reference_index][0] - + std::sin(yaw) * half_width * reference_to_center[reference_index][1]; track_msg.position.y = object.position.y + - std::sin(yaw) * half_length * reference_to_center_[reference_index][0] + - std::cos(yaw) * half_width * reference_to_center_[reference_index][1]; + std::sin(yaw) * half_length * reference_to_center[reference_index][0] + + std::cos(yaw) * half_width * reference_to_center[reference_index][1]; track_msg.position.z = object.position.z; track_msg.velocity = object.absolute_velocity; @@ -463,27 +463,27 @@ radar_msgs::msg::RadarTracks ContinentalARS548DecoderWrapper::ConvertToRadarTrac track_msg.size.z = 1.f; uint8_t max_score = object.classification_unknown; - track_msg.classification = UNKNOWN_ID; + track_msg.classification = unknown_id; if (object.classification_car > max_score) { max_score = object.classification_car; - track_msg.classification = CAR_ID; + track_msg.classification = car_id; } if (object.classification_truck > max_score) { max_score = object.classification_truck; - track_msg.classification = TRUCK_ID; + track_msg.classification = truck_id; } if (object.classification_motorcycle > max_score) { max_score = object.classification_motorcycle; - track_msg.classification = MOTORCYCLE_ID; + track_msg.classification = motorcycle_id; } if (object.classification_bicycle > max_score) { max_score = object.classification_bicycle; - track_msg.classification = BICYCLE_ID; + track_msg.classification = bicycle_id; } if (object.classification_pedestrian > max_score) { max_score = object.classification_pedestrian; - track_msg.classification = PEDESTRIAN_ID; + track_msg.classification = pedestrian_id; } track_msg.position_covariance[0] = @@ -516,12 +516,12 @@ radar_msgs::msg::RadarTracks ContinentalARS548DecoderWrapper::ConvertToRadarTrac track_msg.acceleration_covariance[5] = static_cast(object.absolute_acceleration_std.z * object.absolute_acceleration_std.z); - track_msg.size_covariance[0] = INVALID_COVARIANCE; + track_msg.size_covariance[0] = invalid_covariance; track_msg.size_covariance[1] = 0.f; track_msg.size_covariance[2] = 0.f; - track_msg.size_covariance[3] = INVALID_COVARIANCE; + track_msg.size_covariance[3] = invalid_covariance; track_msg.size_covariance[4] = 0.f; - track_msg.size_covariance[5] = INVALID_COVARIANCE; + track_msg.size_covariance[5] = invalid_covariance; output_msg.tracks.emplace_back(track_msg); } @@ -529,14 +529,14 @@ radar_msgs::msg::RadarTracks ContinentalARS548DecoderWrapper::ConvertToRadarTrac return output_msg; } -visualization_msgs::msg::MarkerArray ContinentalARS548DecoderWrapper::ConvertToMarkers( +visualization_msgs::msg::MarkerArray ContinentalARS548DecoderWrapper::convert_to_markers( const continental_msgs::msg::ContinentalArs548ObjectList & msg) { visualization_msgs::msg::MarkerArray marker_array; marker_array.markers.reserve(4 * msg.objects.size()); - constexpr int LINE_STRIP_CORNERS_NUM = 17; - constexpr std::array, LINE_STRIP_CORNERS_NUM> cube_corners = { + constexpr int line_strip_corners_num = 17; + constexpr std::array, line_strip_corners_num> cube_corners = { {{{-1.0, -1.0, -1.0}}, {{-1.0, -1.0, 1.0}}, {{-1.0, 1.0, 1.0}}, @@ -555,8 +555,8 @@ visualization_msgs::msg::MarkerArray ContinentalARS548DecoderWrapper::ConvertToM {{-1.0, 1.0, -1.0}}, {{1.0, 1.0, -1.0}}}}; - constexpr int PALETTE_SIZE = 32; - constexpr std::array, PALETTE_SIZE> color_array = {{ + constexpr int palette_size = 32; + constexpr std::array, palette_size> color_array = {{ {{1.0, 0.0, 0.0}}, {{0.0, 1.0, 0.0}}, {{0.0, 0.0, 1.0}}, // Red, Green, Blue {{1.0, 1.0, 0.0}}, {{0.0, 1.0, 1.0}}, @@ -586,7 +586,7 @@ visualization_msgs::msg::MarkerArray ContinentalARS548DecoderWrapper::ConvertToM for (const auto & object : msg.objects) { const double half_length = 0.5 * object.shape_length_edge_mean; const double half_width = 0.5 * object.shape_width_edge_mean; - constexpr double DEFAULT_HALF_SIZE = 1.0; + constexpr double default_half_size = 1.0; const int reference_index = std::min(object.position_reference, 8); const double & yaw = object.orientation; current_ids.emplace(object.object_id); @@ -599,18 +599,18 @@ visualization_msgs::msg::MarkerArray ContinentalARS548DecoderWrapper::ConvertToM box_marker.action = visualization_msgs::msg::Marker::ADD; box_marker.type = visualization_msgs::msg::Marker::LINE_STRIP; box_marker.lifetime = rclcpp::Duration::from_seconds(0); - box_marker.color.r = color_array[object.object_id % PALETTE_SIZE][0]; - box_marker.color.g = color_array[object.object_id % PALETTE_SIZE][1]; - box_marker.color.b = color_array[object.object_id % PALETTE_SIZE][2]; + box_marker.color.r = color_array[object.object_id % palette_size][0]; + box_marker.color.g = color_array[object.object_id % palette_size][1]; + box_marker.color.b = color_array[object.object_id % palette_size][2]; box_marker.color.a = 1.0; box_marker.scale.x = 0.1; box_marker.pose.position.x = - object.position.x + std::cos(yaw) * half_length * reference_to_center_[reference_index][0] - - std::sin(yaw) * half_width * reference_to_center_[reference_index][1]; + object.position.x + std::cos(yaw) * half_length * reference_to_center[reference_index][0] - + std::sin(yaw) * half_width * reference_to_center[reference_index][1]; box_marker.pose.position.y = - object.position.y + std::sin(yaw) * half_length * reference_to_center_[reference_index][0] + - std::cos(yaw) * half_width * reference_to_center_[reference_index][1]; + object.position.y + std::sin(yaw) * half_length * reference_to_center[reference_index][0] + + std::cos(yaw) * half_width * reference_to_center[reference_index][1]; box_marker.pose.position.z = object.position.z; box_marker.pose.orientation.w = std::cos(0.5 * yaw); box_marker.pose.orientation.z = std::sin(0.5 * yaw); @@ -619,7 +619,7 @@ visualization_msgs::msg::MarkerArray ContinentalARS548DecoderWrapper::ConvertToM geometry_msgs::msg::Point p; p.x = half_length * corner[0]; p.y = half_width * corner[1]; - p.z = DEFAULT_HALF_SIZE * corner[2]; + p.z = default_half_size * corner[2]; box_marker.points.emplace_back(p); } @@ -697,7 +697,7 @@ visualization_msgs::msg::MarkerArray ContinentalARS548DecoderWrapper::ConvertToM return marker_array; } -nebula::Status ContinentalARS548DecoderWrapper::Status() +nebula::Status ContinentalARS548DecoderWrapper::status() { std::lock_guard lock(mtx_driver_ptr_); @@ -705,7 +705,7 @@ nebula::Status ContinentalARS548DecoderWrapper::Status() return nebula::Status::NOT_INITIALIZED; } - return driver_ptr_->GetStatus(); + return driver_ptr_->get_status(); } } // namespace ros } // namespace nebula diff --git a/nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp index 9e6d3086f..6d539abba 100644 --- a/nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_hw_interface_wrapper.cpp @@ -33,9 +33,9 @@ ContinentalARS548HwInterfaceWrapper::ContinentalARS548HwInterfaceWrapper( status_(Status::NOT_INITIALIZED), config_ptr_(config_ptr) { - hw_interface_->SetLogger( + hw_interface_->set_logger( std::make_shared(parent_node->get_logger().get_child("HwInterface"))); - status_ = hw_interface_->SetSensorConfiguration(config_ptr); + status_ = hw_interface_->set_sensor_configuration(config_ptr); if (status_ != Status::OK) { throw std::runtime_error( @@ -45,10 +45,10 @@ ContinentalARS548HwInterfaceWrapper::ContinentalARS548HwInterfaceWrapper( status_ = Status::OK; } -void ContinentalARS548HwInterfaceWrapper::SensorInterfaceStart() +void ContinentalARS548HwInterfaceWrapper::sensor_interface_start() { if (Status::OK == status_) { - hw_interface_->SensorInterfaceStart(); + hw_interface_->sensor_interface_start(); } if (Status::OK == status_) { @@ -56,70 +56,72 @@ void ContinentalARS548HwInterfaceWrapper::SensorInterfaceStart() parent_node_->create_subscription( "odometry_input", rclcpp::QoS{1}, std::bind( - &ContinentalARS548HwInterfaceWrapper::OdometryCallback, this, std::placeholders::_1)); + &ContinentalARS548HwInterfaceWrapper::odometry_callback, this, std::placeholders::_1)); acceleration_sub_ = parent_node_->create_subscription( "acceleration_input", rclcpp::QoS{1}, std::bind( - &ContinentalARS548HwInterfaceWrapper::AccelerationCallback, this, std::placeholders::_1)); + &ContinentalARS548HwInterfaceWrapper::acceleration_callback, this, + std::placeholders::_1)); steering_angle_sub_ = parent_node_->create_subscription( "steering_angle_input", rclcpp::SensorDataQoS(), std::bind( - &ContinentalARS548HwInterfaceWrapper::SteeringAngleCallback, this, std::placeholders::_1)); + &ContinentalARS548HwInterfaceWrapper::steering_angle_callback, this, + std::placeholders::_1)); set_network_configuration_service_server_ = parent_node_->create_service( "set_network_configuration", std::bind( - &ContinentalARS548HwInterfaceWrapper::SetNetworkConfigurationRequestCallback, this, + &ContinentalARS548HwInterfaceWrapper::set_network_configuration_request_callback, this, std::placeholders::_1, std::placeholders::_2)); set_sensor_mounting_service_server_ = parent_node_->create_service( "set_sensor_mounting", std::bind( - &ContinentalARS548HwInterfaceWrapper::SetSensorMountingRequestCallback, this, + &ContinentalARS548HwInterfaceWrapper::set_sensor_mounting_request_callback, this, std::placeholders::_1, std::placeholders::_2)); set_vehicle_parameters_service_server_ = parent_node_->create_service( "set_vehicle_parameters", std::bind( - &ContinentalARS548HwInterfaceWrapper::SetVehicleParametersRequestCallback, this, + &ContinentalARS548HwInterfaceWrapper::set_vehicle_parameters_request_callback, this, std::placeholders::_1, std::placeholders::_2)); set_radar_parameters_service_server_ = parent_node_->create_service( "set_radar_parameters", std::bind( - &ContinentalARS548HwInterfaceWrapper::SetRadarParametersRequestCallback, this, + &ContinentalARS548HwInterfaceWrapper::set_radar_parameters_request_callback, this, std::placeholders::_1, std::placeholders::_2)); } } -void ContinentalARS548HwInterfaceWrapper::OnConfigChange( +void ContinentalARS548HwInterfaceWrapper::on_config_change( const std::shared_ptr< const nebula::drivers::continental_ars548::ContinentalARS548SensorConfiguration> & new_config_ptr_ptr) { - hw_interface_->SetSensorConfiguration(new_config_ptr_ptr); + hw_interface_->set_sensor_configuration(new_config_ptr_ptr); config_ptr_ = new_config_ptr_ptr; } -Status ContinentalARS548HwInterfaceWrapper::Status() +Status ContinentalARS548HwInterfaceWrapper::status() { return status_; } std::shared_ptr -ContinentalARS548HwInterfaceWrapper::HwInterface() const +ContinentalARS548HwInterfaceWrapper::hw_interface() const { return hw_interface_; } -void ContinentalARS548HwInterfaceWrapper::OdometryCallback( +void ContinentalARS548HwInterfaceWrapper::odometry_callback( const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr msg) { constexpr float speed_to_standstill = 0.5f; @@ -132,44 +134,44 @@ void ContinentalARS548HwInterfaceWrapper::OdometryCallback( } if (standstill_) { - hw_interface_->SetDrivingDirection(0); + hw_interface_->set_driving_direction(0); } else { - hw_interface_->SetDrivingDirection(msg->twist.twist.linear.x > 0.f ? 1 : -1); + hw_interface_->set_driving_direction(msg->twist.twist.linear.x > 0.f ? 1 : -1); } constexpr float ms_to_kmh = 3.6f; - hw_interface_->SetVelocityVehicle(ms_to_kmh * std::abs(msg->twist.twist.linear.x)); + hw_interface_->set_velocity_vehicle(ms_to_kmh * std::abs(msg->twist.twist.linear.x)); constexpr float rad_to_deg = 180.f / M_PI; - hw_interface_->SetYawRate(rad_to_deg * msg->twist.twist.angular.z); + hw_interface_->set_yaw_rate(rad_to_deg * msg->twist.twist.angular.z); } -void ContinentalARS548HwInterfaceWrapper::AccelerationCallback( +void ContinentalARS548HwInterfaceWrapper::acceleration_callback( const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg) { - hw_interface_->SetAccelerationLateralCog(msg->accel.accel.linear.y); - hw_interface_->SetAccelerationLongitudinalCog(msg->accel.accel.linear.x); + hw_interface_->set_acceleration_lateral_cog(msg->accel.accel.linear.y); + hw_interface_->set_acceleration_longitudinal_cog(msg->accel.accel.linear.x); } -void ContinentalARS548HwInterfaceWrapper::SteeringAngleCallback( +void ContinentalARS548HwInterfaceWrapper::steering_angle_callback( const std_msgs::msg::Float32::SharedPtr msg) { constexpr float rad_to_deg = 180.f / M_PI; - hw_interface_->SetSteeringAngleFrontAxle(rad_to_deg * msg->data); + hw_interface_->set_steering_angle_front_axle(rad_to_deg * msg->data); } -void ContinentalARS548HwInterfaceWrapper::SetNetworkConfigurationRequestCallback( +void ContinentalARS548HwInterfaceWrapper::set_network_configuration_request_callback( const std::shared_ptr request, const std::shared_ptr response) { - auto result = hw_interface_->SetSensorIPAddress(request->sensor_ip.data); + auto result = hw_interface_->set_sensor_ip_address(request->sensor_ip.data); response->success = result == Status::OK; response->message = (std::stringstream() << result).str(); } -void ContinentalARS548HwInterfaceWrapper::SetSensorMountingRequestCallback( +void ContinentalARS548HwInterfaceWrapper::set_sensor_mounting_request_callback( const std::shared_ptr request, const std::shared_ptr response) @@ -215,14 +217,14 @@ void ContinentalARS548HwInterfaceWrapper::SetSensorMountingRequestCallback( pitch = rpy.y; } - auto result = hw_interface_->SetSensorMounting( + auto result = hw_interface_->set_sensor_mounting( longitudinal, lateral, vertical, yaw, pitch, request->plug_orientation); response->success = result == Status::OK; response->message = (std::stringstream() << result).str(); } -void ContinentalARS548HwInterfaceWrapper::SetVehicleParametersRequestCallback( +void ContinentalARS548HwInterfaceWrapper::set_vehicle_parameters_request_callback( [[maybe_unused]] const std::shared_ptr< continental_srvs::srv::ContinentalArs548SetVehicleParameters::Request> request, @@ -262,20 +264,20 @@ void ContinentalARS548HwInterfaceWrapper::SetVehicleParametersRequestCallback( vehicle_wheelbase = config_ptr_->configuration_vehicle_wheelbase; } - auto result = hw_interface_->SetVehicleParameters( + auto result = hw_interface_->set_vehicle_parameters( vehicle_length, vehicle_width, vehicle_height, vehicle_wheelbase); response->success = result == Status::OK; response->message = (std::stringstream() << result).str(); } -void ContinentalARS548HwInterfaceWrapper::SetRadarParametersRequestCallback( +void ContinentalARS548HwInterfaceWrapper::set_radar_parameters_request_callback( const std::shared_ptr request, const std::shared_ptr response) { - auto result = hw_interface_->SetRadarParameters( + auto result = hw_interface_->set_radar_parameters( request->maximum_distance, request->frequency_slot, request->cycle_time, request->time_slot, request->country_code, request->powersave_standstill); diff --git a/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp index 7815a8742..011f30891 100644 --- a/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_ars548_ros_wrapper.cpp @@ -30,7 +30,7 @@ ContinentalARS548RosWrapper::ContinentalARS548RosWrapper(const rclcpp::NodeOptio { setvbuf(stdout, NULL, _IONBF, BUFSIZ); - wrapper_status_ = DeclareAndGetSensorConfigParams(); + wrapper_status_ = declare_and_get_sensor_config_params(); if (wrapper_status_ != Status::OK) { throw std::runtime_error( @@ -51,18 +51,19 @@ ContinentalARS548RosWrapper::ContinentalARS548RosWrapper(const rclcpp::NodeOptio decoder_thread_ = std::thread([this]() { while (true) { - decoder_wrapper_->ProcessPacket(packet_queue_.pop()); + decoder_wrapper_->process_packet(packet_queue_.pop()); } }); if (launch_hw_) { - hw_interface_wrapper_->HwInterface()->RegisterPacketCallback( - std::bind(&ContinentalARS548RosWrapper::ReceivePacketCallback, this, std::placeholders::_1)); - StreamStart(); + hw_interface_wrapper_->hw_interface()->register_packet_callback(std::bind( + &ContinentalARS548RosWrapper::receive_packet_callback, this, std::placeholders::_1)); + stream_start(); } else { packets_sub_ = create_subscription( "nebula_packets", rclcpp::SensorDataQoS(), - std::bind(&ContinentalARS548RosWrapper::ReceivePacketsCallback, this, std::placeholders::_1)); + std::bind( + &ContinentalARS548RosWrapper::receive_packets_callback, this, std::placeholders::_1)); RCLCPP_INFO_STREAM( get_logger(), "Hardware connection disabled, listening for packets on " << packets_sub_->get_topic_name()); @@ -71,14 +72,14 @@ ContinentalARS548RosWrapper::ContinentalARS548RosWrapper(const rclcpp::NodeOptio // Register parameter callback after all params have been declared. Otherwise it would be called // once for each declaration parameter_event_cb_ = add_on_set_parameters_callback( - std::bind(&ContinentalARS548RosWrapper::OnParameterChange, this, std::placeholders::_1)); + std::bind(&ContinentalARS548RosWrapper::on_parameter_change, this, std::placeholders::_1)); } -nebula::Status ContinentalARS548RosWrapper::DeclareAndGetSensorConfigParams() +nebula::Status ContinentalARS548RosWrapper::declare_and_get_sensor_config_params() { nebula::drivers::continental_ars548::ContinentalARS548SensorConfiguration config; - config.sensor_model = nebula::drivers::SensorModelFromString( + config.sensor_model = nebula::drivers::sensor_model_from_string( declare_parameter("sensor_model", param_read_only())); config.host_ip = declare_parameter("host_ip", param_read_only()); config.sensor_ip = declare_parameter("sensor_ip", param_read_only()); @@ -107,10 +108,10 @@ nebula::Status ContinentalARS548RosWrapper::DeclareAndGetSensorConfigParams() auto new_config_ptr = std::make_shared< const nebula::drivers::continental_ars548::ContinentalARS548SensorConfiguration>(config); - return ValidateAndSetConfig(new_config_ptr); + return validate_and_set_config(new_config_ptr); } -Status ContinentalARS548RosWrapper::ValidateAndSetConfig( +Status ContinentalARS548RosWrapper::validate_and_set_config( std::shared_ptr & new_config_ptr) { @@ -123,17 +124,17 @@ Status ContinentalARS548RosWrapper::ValidateAndSetConfig( } if (hw_interface_wrapper_) { - hw_interface_wrapper_->OnConfigChange(new_config_ptr); + hw_interface_wrapper_->on_config_change(new_config_ptr); } if (decoder_wrapper_) { - decoder_wrapper_->OnConfigChange(new_config_ptr); + decoder_wrapper_->on_config_change(new_config_ptr); } config_ptr_ = new_config_ptr; return Status::OK; } -void ContinentalARS548RosWrapper::ReceivePacketsCallback( +void ContinentalARS548RosWrapper::receive_packets_callback( std::unique_ptr packets_msg_ptr) { if (hw_interface_wrapper_) { @@ -153,10 +154,10 @@ void ContinentalARS548RosWrapper::ReceivePacketsCallback( } } -void ContinentalARS548RosWrapper::ReceivePacketCallback( +void ContinentalARS548RosWrapper::receive_packet_callback( std::unique_ptr msg_ptr) { - if (!decoder_wrapper_ || decoder_wrapper_->Status() != Status::OK) { + if (!decoder_wrapper_ || decoder_wrapper_->status() != Status::OK) { return; } @@ -165,27 +166,27 @@ void ContinentalARS548RosWrapper::ReceivePacketCallback( } } -Status ContinentalARS548RosWrapper::GetStatus() +Status ContinentalARS548RosWrapper::get_status() { return wrapper_status_; } -Status ContinentalARS548RosWrapper::StreamStart() +Status ContinentalARS548RosWrapper::stream_start() { if (!hw_interface_wrapper_) { return Status::UDP_CONNECTION_ERROR; } - if (hw_interface_wrapper_->Status() != Status::OK) { - return hw_interface_wrapper_->Status(); + if (hw_interface_wrapper_->status() != Status::OK) { + return hw_interface_wrapper_->status(); } - hw_interface_wrapper_->SensorInterfaceStart(); + hw_interface_wrapper_->sensor_interface_start(); - return hw_interface_wrapper_->Status(); + return hw_interface_wrapper_->status(); } -rcl_interfaces::msg::SetParametersResult ContinentalARS548RosWrapper::OnParameterChange( +rcl_interfaces::msg::SetParametersResult ContinentalARS548RosWrapper::on_parameter_change( const std::vector & p) { using rcl_interfaces::msg::SetParametersResult; @@ -217,7 +218,7 @@ rcl_interfaces::msg::SetParametersResult ContinentalARS548RosWrapper::OnParamete auto new_config_ptr = std::make_shared< const nebula::drivers::continental_ars548::ContinentalARS548SensorConfiguration>(new_config); - auto status = ValidateAndSetConfig(new_config_ptr); + auto status = validate_and_set_config(new_config_ptr); if (status != Status::OK) { RCLCPP_WARN_STREAM(get_logger(), "OnParameterChange aborted: " << status); diff --git a/nebula_ros/src/continental/continental_srr520_decoder_wrapper.cpp b/nebula_ros/src/continental/continental_srr520_decoder_wrapper.cpp index e85e6cdca..3fdfa9796 100644 --- a/nebula_ros/src/continental/continental_srr520_decoder_wrapper.cpp +++ b/nebula_ros/src/continental/continental_srr520_decoder_wrapper.cpp @@ -39,8 +39,8 @@ ContinentalSRR520DecoderWrapper::ContinentalSRR520DecoderWrapper( RCLCPP_INFO(logger_, "Starting Decoder"); - InitializeDriver(config); - status_ = driver_ptr_->GetStatus(); + initialize_driver(config); + status_ = driver_ptr_->get_status(); if (Status::OK != status_) { throw std::runtime_error( @@ -97,59 +97,59 @@ ContinentalSRR520DecoderWrapper::ContinentalSRR520DecoderWrapper( }); } -Status ContinentalSRR520DecoderWrapper::InitializeDriver( +Status ContinentalSRR520DecoderWrapper::initialize_driver( const std::shared_ptr< const nebula::drivers::continental_srr520::ContinentalSRR520SensorConfiguration> & config) { driver_ptr_.reset(); driver_ptr_ = std::make_shared(config); - driver_ptr_->RegisterNearDetectionListCallback(std::bind( - &ContinentalSRR520DecoderWrapper::NearDetectionListCallback, this, std::placeholders::_1)); - driver_ptr_->RegisterHRRDetectionListCallback(std::bind( - &ContinentalSRR520DecoderWrapper::HRRDetectionListCallback, this, std::placeholders::_1)); - driver_ptr_->RegisterObjectListCallback( - std::bind(&ContinentalSRR520DecoderWrapper::ObjectListCallback, this, std::placeholders::_1)); - driver_ptr_->RegisterStatusCallback( - std::bind(&ContinentalSRR520DecoderWrapper::StatusCallback, this, std::placeholders::_1)); + driver_ptr_->register_near_detection_list_callback(std::bind( + &ContinentalSRR520DecoderWrapper::near_detection_list_callback, this, std::placeholders::_1)); + driver_ptr_->register_hrr_detection_list_callback(std::bind( + &ContinentalSRR520DecoderWrapper::hrr_detection_list_callback, this, std::placeholders::_1)); + driver_ptr_->register_object_list_callback( + std::bind(&ContinentalSRR520DecoderWrapper::object_list_callback, this, std::placeholders::_1)); + driver_ptr_->register_status_callback( + std::bind(&ContinentalSRR520DecoderWrapper::status_callback, this, std::placeholders::_1)); if (hw_interface_ptr_) { - driver_ptr_->RegisterSyncFollowUpCallback(std::bind( - &ContinentalSRR520DecoderWrapper::SyncFollowUpCallback, this, std::placeholders::_1)); - driver_ptr_->RegisterPacketsCallback( - std::bind(&ContinentalSRR520DecoderWrapper::PacketsCallback, this, std::placeholders::_1)); + driver_ptr_->register_sync_follow_up_callback(std::bind( + &ContinentalSRR520DecoderWrapper::sync_follow_up_callback, this, std::placeholders::_1)); + driver_ptr_->register_packets_callback( + std::bind(&ContinentalSRR520DecoderWrapper::packets_callback, this, std::placeholders::_1)); } - driver_ptr_->SetLogger( + driver_ptr_->set_logger( std::make_shared(parent_node_->get_logger().get_child("Driver"))); return Status::OK; } -void ContinentalSRR520DecoderWrapper::OnConfigChange( +void ContinentalSRR520DecoderWrapper::on_config_change( const std::shared_ptr< const nebula::drivers::continental_srr520::ContinentalSRR520SensorConfiguration> & new_config) { std::lock_guard lock(mtx_driver_ptr_); - InitializeDriver(new_config); + initialize_driver(new_config); sensor_cfg_ = new_config; } -void ContinentalSRR520DecoderWrapper::ProcessPacket( +void ContinentalSRR520DecoderWrapper::process_packet( std::unique_ptr packet_msg) { - driver_ptr_->ProcessPacket(std::move(packet_msg)); + driver_ptr_->process_packet(std::move(packet_msg)); watchdog_->update(); } -void ContinentalSRR520DecoderWrapper::NearDetectionListCallback( +void ContinentalSRR520DecoderWrapper::near_detection_list_callback( std::unique_ptr msg) { if ( near_detection_pointcloud_pub_->get_subscription_count() > 0 || near_detection_pointcloud_pub_->get_intra_process_subscription_count() > 0) { - const auto detection_pointcloud_ptr = ConvertToPointcloud(*msg); + const auto detection_pointcloud_ptr = convert_to_pointcloud(*msg); auto detection_pointcloud_msg_ptr = std::make_unique(); pcl::toROSMsg(*detection_pointcloud_ptr, *detection_pointcloud_msg_ptr); @@ -160,7 +160,7 @@ void ContinentalSRR520DecoderWrapper::NearDetectionListCallback( if ( near_scan_raw_pub_->get_subscription_count() > 0 || near_scan_raw_pub_->get_intra_process_subscription_count() > 0) { - auto radar_scan_msg = ConvertToRadarScan(*msg); + auto radar_scan_msg = convert_to_radar_scan(*msg); radar_scan_msg.header = msg->header; near_scan_raw_pub_->publish(std::move(radar_scan_msg)); } @@ -172,13 +172,13 @@ void ContinentalSRR520DecoderWrapper::NearDetectionListCallback( } } -void ContinentalSRR520DecoderWrapper::HRRDetectionListCallback( +void ContinentalSRR520DecoderWrapper::hrr_detection_list_callback( std::unique_ptr msg) { if ( hrr_detection_pointcloud_pub_->get_subscription_count() > 0 || hrr_detection_pointcloud_pub_->get_intra_process_subscription_count() > 0) { - const auto detection_pointcloud_ptr = ConvertToPointcloud(*msg); + const auto detection_pointcloud_ptr = convert_to_pointcloud(*msg); auto detection_pointcloud_msg_ptr = std::make_unique(); pcl::toROSMsg(*detection_pointcloud_ptr, *detection_pointcloud_msg_ptr); @@ -189,7 +189,7 @@ void ContinentalSRR520DecoderWrapper::HRRDetectionListCallback( if ( hrr_scan_raw_pub_->get_subscription_count() > 0 || hrr_scan_raw_pub_->get_intra_process_subscription_count() > 0) { - auto radar_scan_msg = ConvertToRadarScan(*msg); + auto radar_scan_msg = convert_to_radar_scan(*msg); radar_scan_msg.header = msg->header; hrr_scan_raw_pub_->publish(std::move(radar_scan_msg)); } @@ -201,13 +201,13 @@ void ContinentalSRR520DecoderWrapper::HRRDetectionListCallback( } } -void ContinentalSRR520DecoderWrapper::ObjectListCallback( +void ContinentalSRR520DecoderWrapper::object_list_callback( std::unique_ptr msg) { if ( object_pointcloud_pub_->get_subscription_count() > 0 || object_pointcloud_pub_->get_intra_process_subscription_count() > 0) { - const auto object_pointcloud_ptr = ConvertToPointcloud(*msg); + const auto object_pointcloud_ptr = convert_to_pointcloud(*msg); auto object_pointcloud_msg_ptr = std::make_unique(); pcl::toROSMsg(*object_pointcloud_ptr, *object_pointcloud_msg_ptr); @@ -218,7 +218,7 @@ void ContinentalSRR520DecoderWrapper::ObjectListCallback( if ( objects_raw_pub_->get_subscription_count() > 0 || objects_raw_pub_->get_intra_process_subscription_count() > 0) { - auto objects_raw_msg = ConvertToRadarTracks(*msg); + auto objects_raw_msg = convert_to_radar_tracks(*msg); objects_raw_msg.header = msg->header; objects_raw_pub_->publish(std::move(objects_raw_msg)); } @@ -226,7 +226,7 @@ void ContinentalSRR520DecoderWrapper::ObjectListCallback( if ( objects_markers_pub_->get_subscription_count() > 0 || objects_markers_pub_->get_intra_process_subscription_count() > 0) { - auto marker_array_msg = ConvertToMarkers(*msg); + auto marker_array_msg = convert_to_markers(*msg); objects_markers_pub_->publish(std::move(marker_array_msg)); } @@ -237,14 +237,14 @@ void ContinentalSRR520DecoderWrapper::ObjectListCallback( } } -void ContinentalSRR520DecoderWrapper::StatusCallback( +void ContinentalSRR520DecoderWrapper::status_callback( std::unique_ptr status_msg_ptr) { status_pub_->publish(std::move(status_msg_ptr)); } pcl::PointCloud::Ptr -ContinentalSRR520DecoderWrapper::ConvertToPointcloud( +ContinentalSRR520DecoderWrapper::convert_to_pointcloud( const continental_msgs::msg::ContinentalSrr520DetectionList & msg) { pcl::PointCloud::Ptr output_pointcloud( @@ -278,7 +278,7 @@ ContinentalSRR520DecoderWrapper::ConvertToPointcloud( } pcl::PointCloud::Ptr -ContinentalSRR520DecoderWrapper::ConvertToPointcloud( +ContinentalSRR520DecoderWrapper::convert_to_pointcloud( const continental_msgs::msg::ContinentalSrr520ObjectList & msg) { pcl::PointCloud::Ptr output_pointcloud( @@ -314,7 +314,7 @@ ContinentalSRR520DecoderWrapper::ConvertToPointcloud( return output_pointcloud; } -radar_msgs::msg::RadarScan ContinentalSRR520DecoderWrapper::ConvertToRadarScan( +radar_msgs::msg::RadarScan ContinentalSRR520DecoderWrapper::convert_to_radar_scan( const continental_msgs::msg::ContinentalSrr520DetectionList & msg) { radar_msgs::msg::RadarScan output_msg; @@ -338,15 +338,15 @@ radar_msgs::msg::RadarScan ContinentalSRR520DecoderWrapper::ConvertToRadarScan( return output_msg; } -radar_msgs::msg::RadarTracks ContinentalSRR520DecoderWrapper::ConvertToRadarTracks( +radar_msgs::msg::RadarTracks ContinentalSRR520DecoderWrapper::convert_to_radar_tracks( const continental_msgs::msg::ContinentalSrr520ObjectList & msg) { radar_msgs::msg::RadarTracks output_msg; output_msg.tracks.reserve(msg.objects.size()); output_msg.header = msg.header; - constexpr int16_t UNKNOWN_ID = 32000; - constexpr float INVALID_COVARIANCE = 1e6; + constexpr int16_t unknown_id = 32000; + constexpr float invalid_covariance = 1e6; radar_msgs::msg::RadarTrack track_msg; for (const auto & object : msg.objects) { @@ -373,35 +373,35 @@ radar_msgs::msg::RadarTracks ContinentalSRR520DecoderWrapper::ConvertToRadarTrac track_msg.size.y = object.box_width; track_msg.size.z = 1.f; - track_msg.classification = UNKNOWN_ID; + track_msg.classification = unknown_id; track_msg.position_covariance[0] = object.dist_x_std * object.dist_x_std; - track_msg.position_covariance[1] = INVALID_COVARIANCE; + track_msg.position_covariance[1] = invalid_covariance; track_msg.position_covariance[2] = 0.f; track_msg.position_covariance[3] = object.dist_y_std * object.dist_y_std; track_msg.position_covariance[4] = 0.f; - track_msg.position_covariance[5] = INVALID_COVARIANCE; + track_msg.position_covariance[5] = invalid_covariance; track_msg.velocity_covariance[0] = object.v_abs_x_std * object.v_abs_x_std; - track_msg.velocity_covariance[1] = INVALID_COVARIANCE; + track_msg.velocity_covariance[1] = invalid_covariance; track_msg.velocity_covariance[2] = 0.f; track_msg.velocity_covariance[3] = object.v_abs_y_std * object.v_abs_y_std; track_msg.velocity_covariance[4] = 0.f; - track_msg.velocity_covariance[5] = INVALID_COVARIANCE; + track_msg.velocity_covariance[5] = invalid_covariance; track_msg.acceleration_covariance[0] = object.a_abs_x_std * object.a_abs_x_std; - track_msg.acceleration_covariance[1] = INVALID_COVARIANCE; + track_msg.acceleration_covariance[1] = invalid_covariance; track_msg.acceleration_covariance[2] = 0.f; track_msg.acceleration_covariance[3] = object.a_abs_y_std * object.a_abs_y_std; track_msg.acceleration_covariance[4] = 0.f; - track_msg.acceleration_covariance[5] = INVALID_COVARIANCE; + track_msg.acceleration_covariance[5] = invalid_covariance; - track_msg.size_covariance[0] = INVALID_COVARIANCE; + track_msg.size_covariance[0] = invalid_covariance; track_msg.size_covariance[1] = 0.f; track_msg.size_covariance[2] = 0.f; - track_msg.size_covariance[3] = INVALID_COVARIANCE; + track_msg.size_covariance[3] = invalid_covariance; track_msg.size_covariance[4] = 0.f; - track_msg.size_covariance[5] = INVALID_COVARIANCE; + track_msg.size_covariance[5] = invalid_covariance; output_msg.tracks.emplace_back(track_msg); } @@ -409,14 +409,14 @@ radar_msgs::msg::RadarTracks ContinentalSRR520DecoderWrapper::ConvertToRadarTrac return output_msg; } -visualization_msgs::msg::MarkerArray ContinentalSRR520DecoderWrapper::ConvertToMarkers( +visualization_msgs::msg::MarkerArray ContinentalSRR520DecoderWrapper::convert_to_markers( const continental_msgs::msg::ContinentalSrr520ObjectList & msg) { visualization_msgs::msg::MarkerArray marker_array; marker_array.markers.reserve(4 * msg.objects.size()); - constexpr int LINE_STRIP_CORNERS_NUM = 17; - constexpr std::array, LINE_STRIP_CORNERS_NUM> cube_corners = { + constexpr int line_strip_corners_num = 17; + constexpr std::array, line_strip_corners_num> cube_corners = { {{{-1.0, -1.0, -1.0}}, {{-1.0, -1.0, 1.0}}, {{-1.0, 1.0, 1.0}}, @@ -435,8 +435,8 @@ visualization_msgs::msg::MarkerArray ContinentalSRR520DecoderWrapper::ConvertToM {{-1.0, 1.0, -1.0}}, {{1.0, 1.0, -1.0}}}}; - constexpr int PALETTE_SIZE = 32; - constexpr std::array, PALETTE_SIZE> color_array = {{ + constexpr int palette_size = 32; + constexpr std::array, palette_size> color_array = {{ {{1.0, 0.0, 0.0}}, {{0.0, 1.0, 0.0}}, {{0.0, 0.0, 1.0}}, // Red, Green, Blue {{1.0, 1.0, 0.0}}, {{0.0, 1.0, 1.0}}, @@ -469,7 +469,7 @@ visualization_msgs::msg::MarkerArray ContinentalSRR520DecoderWrapper::ConvertToM const double half_length = 0.5 * object.box_length; const double half_width = 0.5 * object.box_width; - constexpr double DEFAULT_HALF_SIZE = 1.0; + constexpr double default_half_size = 1.0; const double & yaw = object.orientation; current_ids.emplace(object.object_id); @@ -482,15 +482,15 @@ visualization_msgs::msg::MarkerArray ContinentalSRR520DecoderWrapper::ConvertToM box_marker.action = visualization_msgs::msg::Marker::ADD; box_marker.type = visualization_msgs::msg::Marker::LINE_STRIP; box_marker.lifetime = rclcpp::Duration::from_seconds(0); - box_marker.color.r = color_array[object.object_id % PALETTE_SIZE][0]; - box_marker.color.g = color_array[object.object_id % PALETTE_SIZE][1]; - box_marker.color.b = color_array[object.object_id % PALETTE_SIZE][2]; + box_marker.color.r = color_array[object.object_id % palette_size][0]; + box_marker.color.g = color_array[object.object_id % palette_size][1]; + box_marker.color.b = color_array[object.object_id % palette_size][2]; box_marker.color.a = 1.0; box_marker.scale.x = 0.1; box_marker.pose.position.x = object.dist_x; box_marker.pose.position.y = object.dist_y; - box_marker.pose.position.z = DEFAULT_HALF_SIZE; + box_marker.pose.position.z = default_half_size; box_marker.pose.orientation.w = std::cos(0.5 * yaw); box_marker.pose.orientation.z = std::sin(0.5 * yaw); @@ -498,7 +498,7 @@ visualization_msgs::msg::MarkerArray ContinentalSRR520DecoderWrapper::ConvertToM geometry_msgs::msg::Point p; p.x = half_length * corner[0]; p.y = half_width * corner[1]; - p.z = DEFAULT_HALF_SIZE * corner[2]; + p.z = default_half_size * corner[2]; box_marker.points.emplace_back(p); } @@ -574,12 +574,12 @@ visualization_msgs::msg::MarkerArray ContinentalSRR520DecoderWrapper::ConvertToM return marker_array; } -void ContinentalSRR520DecoderWrapper::SyncFollowUpCallback(builtin_interfaces::msg::Time stamp) +void ContinentalSRR520DecoderWrapper::sync_follow_up_callback(builtin_interfaces::msg::Time stamp) { - hw_interface_ptr_->SensorSyncFollowUp(stamp); + hw_interface_ptr_->sensor_sync_follow_up(stamp); } -void ContinentalSRR520DecoderWrapper::PacketsCallback( +void ContinentalSRR520DecoderWrapper::packets_callback( std::unique_ptr msg) { if ( @@ -589,7 +589,7 @@ void ContinentalSRR520DecoderWrapper::PacketsCallback( } } -nebula::Status ContinentalSRR520DecoderWrapper::Status() +nebula::Status ContinentalSRR520DecoderWrapper::status() { std::lock_guard lock(mtx_driver_ptr_); @@ -597,7 +597,7 @@ nebula::Status ContinentalSRR520DecoderWrapper::Status() return nebula::Status::NOT_INITIALIZED; } - return driver_ptr_->GetStatus(); + return driver_ptr_->get_status(); } } // namespace ros } // namespace nebula diff --git a/nebula_ros/src/continental/continental_srr520_hw_interface_wrapper.cpp b/nebula_ros/src/continental/continental_srr520_hw_interface_wrapper.cpp index 42745fa58..3af976097 100644 --- a/nebula_ros/src/continental/continental_srr520_hw_interface_wrapper.cpp +++ b/nebula_ros/src/continental/continental_srr520_hw_interface_wrapper.cpp @@ -33,9 +33,9 @@ ContinentalSRR520HwInterfaceWrapper::ContinentalSRR520HwInterfaceWrapper( status_(Status::NOT_INITIALIZED), config_ptr_(config_ptr) { - hw_interface_->SetLogger( + hw_interface_->set_logger( std::make_shared(parent_node->get_logger().get_child("HwInterface"))); - status_ = hw_interface_->SetSensorConfiguration(config_ptr_); + status_ = hw_interface_->set_sensor_configuration(config_ptr_); if (status_ != Status::OK) { throw std::runtime_error( @@ -45,12 +45,12 @@ ContinentalSRR520HwInterfaceWrapper::ContinentalSRR520HwInterfaceWrapper( status_ = Status::OK; } -void ContinentalSRR520HwInterfaceWrapper::SensorInterfaceStart() +void ContinentalSRR520HwInterfaceWrapper::sensor_interface_start() { using std::chrono_literals::operator""ms; if (Status::OK == status_) { - hw_interface_->SensorInterfaceStart(); + hw_interface_->sensor_interface_start(); } if (Status::OK == status_) { @@ -59,41 +59,42 @@ void ContinentalSRR520HwInterfaceWrapper::SensorInterfaceStart() sync_ptr_ = std::make_shared(ExactTimeSyncPolicy(10), odometry_sub_, acceleration_sub_); - sync_ptr_->registerCallback(&ContinentalSRR520HwInterfaceWrapper::dynamicsCallback, this); + sync_ptr_->registerCallback(&ContinentalSRR520HwInterfaceWrapper::dynamics_callback, this); configure_sensor_service_server_ = parent_node_->create_service( - "configure_sensor", std::bind( - &ContinentalSRR520HwInterfaceWrapper::ConfigureSensorRequestCallback, - this, std::placeholders::_1, std::placeholders::_2)); + "configure_sensor", + std::bind( + &ContinentalSRR520HwInterfaceWrapper::configure_sensor_request_callback, this, + std::placeholders::_1, std::placeholders::_2)); sync_timer_ = rclcpp::create_timer( parent_node_, parent_node_->get_clock(), 100ms, - std::bind(&ContinentalSRR520HwInterfaceWrapper::syncTimerCallback, this)); + std::bind(&ContinentalSRR520HwInterfaceWrapper::sync_timer_callback, this)); } } -void ContinentalSRR520HwInterfaceWrapper::OnConfigChange( +void ContinentalSRR520HwInterfaceWrapper::on_config_change( const std::shared_ptr< const nebula::drivers::continental_srr520::ContinentalSRR520SensorConfiguration> & new_config_ptr) { - hw_interface_->SetSensorConfiguration(new_config_ptr); + hw_interface_->set_sensor_configuration(new_config_ptr); config_ptr_ = new_config_ptr; } -Status ContinentalSRR520HwInterfaceWrapper::Status() +Status ContinentalSRR520HwInterfaceWrapper::status() { return status_; } std::shared_ptr -ContinentalSRR520HwInterfaceWrapper::HwInterface() const +ContinentalSRR520HwInterfaceWrapper::hw_interface() const { return hw_interface_; } -void ContinentalSRR520HwInterfaceWrapper::dynamicsCallback( +void ContinentalSRR520HwInterfaceWrapper::dynamics_callback( const geometry_msgs::msg::TwistWithCovarianceStamped::SharedPtr & odometry_msg, const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr & acceleration_msg) { @@ -106,12 +107,12 @@ void ContinentalSRR520HwInterfaceWrapper::dynamicsCallback( standstill_ = true; } - hw_interface_->SetVehicleDynamics( + hw_interface_->set_vehicle_dynamics( acceleration_msg->accel.accel.linear.x, acceleration_msg->accel.accel.linear.y, odometry_msg->twist.twist.angular.z, odometry_msg->twist.twist.linear.x, standstill_); } -void ContinentalSRR520HwInterfaceWrapper::ConfigureSensorRequestCallback( +void ContinentalSRR520HwInterfaceWrapper::configure_sensor_request_callback( const std::shared_ptr request, const std::shared_ptr @@ -165,7 +166,7 @@ void ContinentalSRR520HwInterfaceWrapper::ConfigureSensorRequestCallback( yaw = std::min(std::max(yaw, -3.14159f), 3.14159f); - auto result = hw_interface_->ConfigureSensor( + auto result = hw_interface_->configure_sensor( request->sensor_id, longitudinal, lateral, vertical, yaw, longitudinal + 0.5 * vehicle_wheelbase, vehicle_wheelbase, request->cover_damping, request->plug_bottom, request->reset_sensor_configuration); @@ -174,9 +175,9 @@ void ContinentalSRR520HwInterfaceWrapper::ConfigureSensorRequestCallback( response->message = (std::stringstream() << result).str(); } -void ContinentalSRR520HwInterfaceWrapper::syncTimerCallback() +void ContinentalSRR520HwInterfaceWrapper::sync_timer_callback() { - hw_interface_->SensorSync(); + hw_interface_->sensor_sync(); } } // namespace ros diff --git a/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp b/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp index e0bed3ca6..f52b4e656 100644 --- a/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp +++ b/nebula_ros/src/continental/continental_srr520_ros_wrapper.cpp @@ -30,7 +30,7 @@ ContinentalSRR520RosWrapper::ContinentalSRR520RosWrapper(const rclcpp::NodeOptio { setvbuf(stdout, NULL, _IONBF, BUFSIZ); - wrapper_status_ = DeclareAndGetSensorConfigParams(); + wrapper_status_ = declare_and_get_sensor_config_params(); if (wrapper_status_ != Status::OK) { throw std::runtime_error( @@ -43,7 +43,7 @@ ContinentalSRR520RosWrapper::ContinentalSRR520RosWrapper(const rclcpp::NodeOptio if (launch_hw_) { hw_interface_wrapper_.emplace(this, config_ptr_); - decoder_wrapper_.emplace(this, config_ptr_, hw_interface_wrapper_->HwInterface()); + decoder_wrapper_.emplace(this, config_ptr_, hw_interface_wrapper_->hw_interface()); } else { decoder_wrapper_.emplace(this, config_ptr_, nullptr); } @@ -52,18 +52,19 @@ ContinentalSRR520RosWrapper::ContinentalSRR520RosWrapper(const rclcpp::NodeOptio decoder_thread_ = std::thread([this]() { while (true) { - decoder_wrapper_->ProcessPacket(packet_queue_.pop()); + decoder_wrapper_->process_packet(packet_queue_.pop()); } }); if (launch_hw_) { - hw_interface_wrapper_->HwInterface()->RegisterPacketCallback( - std::bind(&ContinentalSRR520RosWrapper::ReceivePacketCallback, this, std::placeholders::_1)); - StreamStart(); + hw_interface_wrapper_->hw_interface()->register_packet_callback(std::bind( + &ContinentalSRR520RosWrapper::receive_packet_callback, this, std::placeholders::_1)); + stream_start(); } else { packets_sub_ = create_subscription( "nebula_packets", rclcpp::SensorDataQoS(), - std::bind(&ContinentalSRR520RosWrapper::ReceivePacketsCallback, this, std::placeholders::_1)); + std::bind( + &ContinentalSRR520RosWrapper::receive_packets_callback, this, std::placeholders::_1)); RCLCPP_INFO_STREAM( get_logger(), "Hardware connection disabled, listening for packets on " << packets_sub_->get_topic_name()); @@ -72,14 +73,14 @@ ContinentalSRR520RosWrapper::ContinentalSRR520RosWrapper(const rclcpp::NodeOptio // Register parameter callback after all params have been declared. Otherwise it would be called // once for each declaration parameter_event_cb_ = add_on_set_parameters_callback( - std::bind(&ContinentalSRR520RosWrapper::OnParameterChange, this, std::placeholders::_1)); + std::bind(&ContinentalSRR520RosWrapper::on_parameter_change, this, std::placeholders::_1)); } -nebula::Status ContinentalSRR520RosWrapper::DeclareAndGetSensorConfigParams() +nebula::Status ContinentalSRR520RosWrapper::declare_and_get_sensor_config_params() { nebula::drivers::continental_srr520::ContinentalSRR520SensorConfiguration config; - config.sensor_model = nebula::drivers::SensorModelFromString( + config.sensor_model = nebula::drivers::sensor_model_from_string( declare_parameter("sensor_model", param_read_only())); config.interface = declare_parameter("interface", param_read_only()); config.receiver_timeout_sec = @@ -99,10 +100,10 @@ nebula::Status ContinentalSRR520RosWrapper::DeclareAndGetSensorConfigParams() auto new_config_ptr = std::make_shared< const nebula::drivers::continental_srr520::ContinentalSRR520SensorConfiguration>(config); - return ValidateAndSetConfig(new_config_ptr); + return validate_and_set_config(new_config_ptr); } -Status ContinentalSRR520RosWrapper::ValidateAndSetConfig( +Status ContinentalSRR520RosWrapper::validate_and_set_config( std::shared_ptr & new_config) { @@ -115,17 +116,17 @@ Status ContinentalSRR520RosWrapper::ValidateAndSetConfig( } if (hw_interface_wrapper_) { - hw_interface_wrapper_->OnConfigChange(new_config); + hw_interface_wrapper_->on_config_change(new_config); } if (decoder_wrapper_) { - decoder_wrapper_->OnConfigChange(new_config); + decoder_wrapper_->on_config_change(new_config); } config_ptr_ = new_config; return Status::OK; } -void ContinentalSRR520RosWrapper::ReceivePacketsCallback( +void ContinentalSRR520RosWrapper::receive_packets_callback( std::unique_ptr packets_msg) { if (hw_interface_wrapper_) { @@ -145,10 +146,10 @@ void ContinentalSRR520RosWrapper::ReceivePacketsCallback( } } -void ContinentalSRR520RosWrapper::ReceivePacketCallback( +void ContinentalSRR520RosWrapper::receive_packet_callback( std::unique_ptr msg_ptr) { - if (!decoder_wrapper_ || decoder_wrapper_->Status() != Status::OK) { + if (!decoder_wrapper_ || decoder_wrapper_->status() != Status::OK) { return; } @@ -157,27 +158,27 @@ void ContinentalSRR520RosWrapper::ReceivePacketCallback( } } -Status ContinentalSRR520RosWrapper::GetStatus() +Status ContinentalSRR520RosWrapper::get_status() { return wrapper_status_; } -Status ContinentalSRR520RosWrapper::StreamStart() +Status ContinentalSRR520RosWrapper::stream_start() { if (!hw_interface_wrapper_) { return Status::UDP_CONNECTION_ERROR; } - if (hw_interface_wrapper_->Status() != Status::OK) { - return hw_interface_wrapper_->Status(); + if (hw_interface_wrapper_->status() != Status::OK) { + return hw_interface_wrapper_->status(); } - hw_interface_wrapper_->SensorInterfaceStart(); + hw_interface_wrapper_->sensor_interface_start(); - return hw_interface_wrapper_->Status(); + return hw_interface_wrapper_->status(); } -rcl_interfaces::msg::SetParametersResult ContinentalSRR520RosWrapper::OnParameterChange( +rcl_interfaces::msg::SetParametersResult ContinentalSRR520RosWrapper::on_parameter_change( const std::vector & p) { using rcl_interfaces::msg::SetParametersResult; @@ -202,7 +203,7 @@ rcl_interfaces::msg::SetParametersResult ContinentalSRR520RosWrapper::OnParamete auto new_config_ptr = std::make_shared< const nebula::drivers::continental_srr520::ContinentalSRR520SensorConfiguration>(new_config); - auto status = ValidateAndSetConfig(new_config_ptr); + auto status = validate_and_set_config(new_config_ptr); if (status != Status::OK) { RCLCPP_WARN_STREAM(get_logger(), "OnParameterChange aborted: " << status); diff --git a/nebula_ros/src/hesai/decoder_wrapper.cpp b/nebula_ros/src/hesai/decoder_wrapper.cpp index 8ed584eaf..af7fd09fb 100644 --- a/nebula_ros/src/hesai/decoder_wrapper.cpp +++ b/nebula_ros/src/hesai/decoder_wrapper.cpp @@ -41,7 +41,7 @@ HesaiDecoderWrapper::HesaiDecoderWrapper( RCLCPP_INFO(logger_, "Starting Decoder"); driver_ptr_ = std::make_shared(config, calibration_cfg_ptr_); - status_ = driver_ptr_->GetStatus(); + status_ = driver_ptr_->get_status(); if (Status::OK != status_) { throw std::runtime_error( @@ -76,7 +76,7 @@ HesaiDecoderWrapper::HesaiDecoderWrapper( }); } -void HesaiDecoderWrapper::OnConfigChange( +void HesaiDecoderWrapper::on_config_change( const std::shared_ptr & new_config) { std::lock_guard lock(mtx_driver_ptr_); @@ -85,7 +85,7 @@ void HesaiDecoderWrapper::OnConfigChange( sensor_cfg_ = new_config; } -void HesaiDecoderWrapper::OnCalibrationChange( +void HesaiDecoderWrapper::on_calibration_change( const std::shared_ptr & new_calibration) { std::lock_guard lock(mtx_driver_ptr_); @@ -94,7 +94,7 @@ void HesaiDecoderWrapper::OnCalibrationChange( calibration_cfg_ptr_ = new_calibration; } -void HesaiDecoderWrapper::ProcessCloudPacket( +void HesaiDecoderWrapper::process_cloud_packet( std::unique_ptr packet_msg) { // Accumulate packets for recording only if someone is subscribed to the topic (for performance) @@ -116,7 +116,7 @@ void HesaiDecoderWrapper::ProcessCloudPacket( nebula::drivers::NebulaPointCloudPtr pointcloud = nullptr; { std::lock_guard lock(mtx_driver_ptr_); - pointcloud_ts = driver_ptr_->ParseCloudPacket(packet_msg->data); + pointcloud_ts = driver_ptr_->parse_cloud_packet(packet_msg->data); pointcloud = std::get<0>(pointcloud_ts); } @@ -143,34 +143,34 @@ void HesaiDecoderWrapper::ProcessCloudPacket( auto ros_pc_msg_ptr = std::make_unique(); pcl::toROSMsg(*pointcloud, *ros_pc_msg_ptr); ros_pc_msg_ptr->header.stamp = - rclcpp::Time(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count()); - PublishCloud(std::move(ros_pc_msg_ptr), nebula_points_pub_); + rclcpp::Time(seconds_to_chrono_nano_seconds(std::get<1>(pointcloud_ts)).count()); + publish_cloud(std::move(ros_pc_msg_ptr), nebula_points_pub_); } if ( aw_points_base_pub_->get_subscription_count() > 0 || aw_points_base_pub_->get_intra_process_subscription_count() > 0) { const auto autoware_cloud_xyzi = - nebula::drivers::convertPointXYZIRCAEDTToPointXYZIR(pointcloud); + nebula::drivers::convert_point_xyzircaedt_to_point_xyzir(pointcloud); auto ros_pc_msg_ptr = std::make_unique(); pcl::toROSMsg(*autoware_cloud_xyzi, *ros_pc_msg_ptr); ros_pc_msg_ptr->header.stamp = - rclcpp::Time(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count()); - PublishCloud(std::move(ros_pc_msg_ptr), aw_points_base_pub_); + rclcpp::Time(seconds_to_chrono_nano_seconds(std::get<1>(pointcloud_ts)).count()); + publish_cloud(std::move(ros_pc_msg_ptr), aw_points_base_pub_); } if ( aw_points_ex_pub_->get_subscription_count() > 0 || aw_points_ex_pub_->get_intra_process_subscription_count() > 0) { - const auto autoware_ex_cloud = nebula::drivers::convertPointXYZIRCAEDTToPointXYZIRADT( + const auto autoware_ex_cloud = nebula::drivers::convert_point_xyzircaedt_to_point_xyziradt( pointcloud, std::get<1>(pointcloud_ts)); auto ros_pc_msg_ptr = std::make_unique(); pcl::toROSMsg(*autoware_ex_cloud, *ros_pc_msg_ptr); ros_pc_msg_ptr->header.stamp = - rclcpp::Time(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count()); - PublishCloud(std::move(ros_pc_msg_ptr), aw_points_ex_pub_); + rclcpp::Time(seconds_to_chrono_nano_seconds(std::get<1>(pointcloud_ts)).count()); + publish_cloud(std::move(ros_pc_msg_ptr), aw_points_ex_pub_); } } -void HesaiDecoderWrapper::PublishCloud( +void HesaiDecoderWrapper::publish_cloud( std::unique_ptr pointcloud, const rclcpp::Publisher::SharedPtr & publisher) { @@ -181,7 +181,7 @@ void HesaiDecoderWrapper::PublishCloud( publisher->publish(std::move(pointcloud)); } -nebula::Status HesaiDecoderWrapper::Status() +nebula::Status HesaiDecoderWrapper::status() { std::lock_guard lock(mtx_driver_ptr_); @@ -189,7 +189,7 @@ nebula::Status HesaiDecoderWrapper::Status() return nebula::Status::NOT_INITIALIZED; } - return driver_ptr_->GetStatus(); + return driver_ptr_->get_status(); } } // namespace ros } // namespace nebula diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp index d4fa54b95..149aaa8f3 100644 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -32,7 +32,7 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) { setvbuf(stdout, nullptr, _IONBF, BUFSIZ); - wrapper_status_ = DeclareAndGetSensorConfigParams(); + wrapper_status_ = declare_and_get_sensor_config_params(); if (wrapper_status_ != Status::OK) { throw std::runtime_error( @@ -45,10 +45,10 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) if (launch_hw_) { hw_interface_wrapper_.emplace(this, sensor_cfg_ptr_); - hw_monitor_wrapper_.emplace(this, hw_interface_wrapper_->HwInterface(), sensor_cfg_ptr_); + hw_monitor_wrapper_.emplace(this, hw_interface_wrapper_->hw_interface(), sensor_cfg_ptr_); } - auto calibration_result = GetCalibrationData(sensor_cfg_ptr_->calibration_path); + auto calibration_result = get_calibration_data(sensor_cfg_ptr_->calibration_path); if (!calibration_result.has_value()) { throw std::runtime_error( (std::stringstream() << "No valid calibration found: " << calibration_result.error()).str()); @@ -58,7 +58,7 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) hw_interface_wrapper_ && sensor_cfg_ptr_->sensor_model != drivers::SensorModel::HESAI_PANDARAT128) { auto status = - hw_interface_wrapper_->HwInterface()->checkAndSetLidarRange(*calibration_result.value()); + hw_interface_wrapper_->hw_interface()->checkAndSetLidarRange(*calibration_result.value()); if (status != Status::OK) { throw std::runtime_error( (std::stringstream{} << "Could not set sensor FoV: " << status).str()); @@ -71,18 +71,18 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) decoder_thread_ = std::thread([this]() { while (true) { - decoder_wrapper_->ProcessCloudPacket(packet_queue_.pop()); + decoder_wrapper_->process_cloud_packet(packet_queue_.pop()); } }); if (launch_hw_) { - hw_interface_wrapper_->HwInterface()->RegisterScanCallback( - std::bind(&HesaiRosWrapper::ReceiveCloudPacketCallback, this, std::placeholders::_1)); - StreamStart(); + hw_interface_wrapper_->hw_interface()->RegisterScanCallback( + std::bind(&HesaiRosWrapper::receive_cloud_packet_callback, this, std::placeholders::_1)); + stream_start(); } else { packets_sub_ = create_subscription( "pandar_packets", rclcpp::SensorDataQoS(), - std::bind(&HesaiRosWrapper::ReceiveScanMessageCallback, this, std::placeholders::_1)); + std::bind(&HesaiRosWrapper::receive_scan_message_callback, this, std::placeholders::_1)); RCLCPP_INFO_STREAM( get_logger(), "Hardware connection disabled, listening for packets on " << packets_sub_->get_topic_name()); @@ -91,18 +91,18 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) // Register parameter callback after all params have been declared. Otherwise it would be called // once for each declaration parameter_event_cb_ = add_on_set_parameters_callback( - std::bind(&HesaiRosWrapper::OnParameterChange, this, std::placeholders::_1)); + std::bind(&HesaiRosWrapper::on_parameter_change, this, std::placeholders::_1)); } -nebula::Status HesaiRosWrapper::DeclareAndGetSensorConfigParams() +nebula::Status HesaiRosWrapper::declare_and_get_sensor_config_params() { nebula::drivers::HesaiSensorConfiguration config; auto _sensor_model = declare_parameter("sensor_model", param_read_only()); - config.sensor_model = drivers::SensorModelFromString(_sensor_model); + config.sensor_model = drivers::sensor_model_from_string(_sensor_model); auto _return_mode = declare_parameter("return_mode", param_read_write()); - config.return_mode = drivers::ReturnModeFromStringHesai(_return_mode, config.sensor_model); + config.return_mode = drivers::return_mode_from_string_hesai(_return_mode, config.sensor_model); config.host_ip = declare_parameter("host_ip", param_read_only()); config.sensor_ip = declare_parameter("sensor_ip", param_read_only()); @@ -164,15 +164,15 @@ nebula::Status HesaiRosWrapper::DeclareAndGetSensorConfigParams() declare_parameter("dual_return_distance_threshold", descriptor); } - std::string calibration_parameter_name = getCalibrationParameterName(config.sensor_model); + std::string calibration_parameter_name = get_calibration_parameter_name(config.sensor_model); config.calibration_path = declare_parameter(calibration_parameter_name, param_read_write()); auto _ptp_profile = declare_parameter("ptp_profile", param_read_only()); - config.ptp_profile = drivers::PtpProfileFromString(_ptp_profile); + config.ptp_profile = drivers::ptp_profile_from_string(_ptp_profile); auto _ptp_transport = declare_parameter("ptp_transport_type", param_read_only()); - config.ptp_transport_type = drivers::PtpTransportTypeFromString(_ptp_transport); + config.ptp_transport_type = drivers::ptp_transport_type_from_string(_ptp_transport); if ( config.ptp_transport_type != drivers::PtpTransportType::L2 && @@ -187,7 +187,7 @@ nebula::Status HesaiRosWrapper::DeclareAndGetSensorConfigParams() } auto _ptp_switch = declare_parameter("ptp_switch_type", param_read_only()); - config.ptp_switch_type = drivers::PtpSwitchTypeFromString(_ptp_switch); + config.ptp_switch_type = drivers::ptp_switch_type_from_string(_ptp_switch); { rcl_interfaces::msg::ParameterDescriptor descriptor = param_read_only(); @@ -196,10 +196,10 @@ nebula::Status HesaiRosWrapper::DeclareAndGetSensorConfigParams() } auto new_cfg_ptr = std::make_shared(config); - return ValidateAndSetConfig(new_cfg_ptr); + return validate_and_set_config(new_cfg_ptr); } -Status HesaiRosWrapper::ValidateAndSetConfig( +Status HesaiRosWrapper::validate_and_set_config( std::shared_ptr & new_config) { if (new_config->sensor_model == nebula::drivers::SensorModel::UNKNOWN) { @@ -248,20 +248,20 @@ Status HesaiRosWrapper::ValidateAndSetConfig( } if (hw_interface_wrapper_) { - hw_interface_wrapper_->OnConfigChange(new_config); + hw_interface_wrapper_->on_config_change(new_config); } if (hw_monitor_wrapper_) { - hw_monitor_wrapper_->OnConfigChange(new_config); + hw_monitor_wrapper_->on_config_change(new_config); } if (decoder_wrapper_) { - decoder_wrapper_->OnConfigChange(new_config); + decoder_wrapper_->on_config_change(new_config); } sensor_cfg_ptr_ = new_config; return Status::OK; } -void HesaiRosWrapper::ReceiveScanMessageCallback( +void HesaiRosWrapper::receive_scan_message_callback( std::unique_ptr scan_msg) { if (hw_interface_wrapper_) { @@ -280,25 +280,25 @@ void HesaiRosWrapper::ReceiveScanMessageCallback( } } -Status HesaiRosWrapper::GetStatus() +Status HesaiRosWrapper::get_status() { return wrapper_status_; } -Status HesaiRosWrapper::StreamStart() +Status HesaiRosWrapper::stream_start() { if (!hw_interface_wrapper_) { return Status::UDP_CONNECTION_ERROR; } - if (hw_interface_wrapper_->Status() != Status::OK) { - return hw_interface_wrapper_->Status(); + if (hw_interface_wrapper_->status() != Status::OK) { + return hw_interface_wrapper_->status(); } - return hw_interface_wrapper_->HwInterface()->SensorInterfaceStart(); + return hw_interface_wrapper_->hw_interface()->SensorInterfaceStart(); } -rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::OnParameterChange( +rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::on_parameter_change( const std::vector & p) { using rcl_interfaces::msg::SetParametersResult; @@ -315,7 +315,7 @@ rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::OnParameterChange( std::string _return_mode{}; std::string calibration_parameter_name = - getCalibrationParameterName(sensor_cfg_ptr_->sensor_model); + get_calibration_parameter_name(sensor_cfg_ptr_->sensor_model); bool got_any = get_param(p, "return_mode", _return_mode) | get_param(p, "frame_id", new_cfg.frame_id) | @@ -335,7 +335,7 @@ rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::OnParameterChange( if (_return_mode.length() > 0) new_cfg.return_mode = - nebula::drivers::ReturnModeFromStringHesai(_return_mode, sensor_cfg_ptr_->sensor_model); + nebula::drivers::return_mode_from_string_hesai(_return_mode, sensor_cfg_ptr_->sensor_model); // //////////////////////////////////////// // Get and validate new calibration, if any @@ -356,7 +356,7 @@ rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::OnParameterChange( } // Fail early and do not set the new config if getting calibration data failed. - auto get_calibration_result = GetCalibrationData(new_cfg.calibration_path, true); + auto get_calibration_result = get_calibration_data(new_cfg.calibration_path, true); if (!get_calibration_result.has_value()) { auto result = SetParametersResult(); result.successful = false; @@ -371,7 +371,7 @@ rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::OnParameterChange( } auto new_cfg_ptr = std::make_shared(new_cfg); - auto status = ValidateAndSetConfig(new_cfg_ptr); + auto status = validate_and_set_config(new_cfg_ptr); if (status != Status::OK) { RCLCPP_WARN_STREAM(get_logger(), "OnParameterChange aborted: " << status); auto result = SetParametersResult(); @@ -382,14 +382,15 @@ rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::OnParameterChange( // Set calibration (if any) only once all parameters have been validated if (new_calibration_ptr) { - decoder_wrapper_->OnCalibrationChange(new_calibration_ptr); + decoder_wrapper_->on_calibration_change(new_calibration_ptr); RCLCPP_INFO_STREAM(get_logger(), "Changed calibration to '" << new_cfg.calibration_path << "'"); } if ( new_calibration_ptr && hw_interface_wrapper_ && sensor_cfg_ptr_->sensor_model != drivers::SensorModel::HESAI_PANDARAT128) { - auto status = hw_interface_wrapper_->HwInterface()->checkAndSetLidarRange(*new_calibration_ptr); + auto status = + hw_interface_wrapper_->hw_interface()->checkAndSetLidarRange(*new_calibration_ptr); if (status != Status::OK) { RCLCPP_ERROR_STREAM( get_logger(), "Sensor configuration updated, but setting hardware FoV failed: " << status); @@ -399,9 +400,9 @@ rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::OnParameterChange( return rcl_interfaces::build().successful(true).reason(""); } -void HesaiRosWrapper::ReceiveCloudPacketCallback(std::vector & packet) +void HesaiRosWrapper::receive_cloud_packet_callback(std::vector & packet) { - if (!decoder_wrapper_ || decoder_wrapper_->Status() != Status::OK) { + if (!decoder_wrapper_ || decoder_wrapper_->status() != Status::OK) { return; } @@ -419,7 +420,7 @@ void HesaiRosWrapper::ReceiveCloudPacketCallback(std::vector & packet) } } -std::string HesaiRosWrapper::getCalibrationParameterName(drivers::SensorModel model) const +std::string HesaiRosWrapper::get_calibration_parameter_name(drivers::SensorModel model) const { if (model == drivers::SensorModel::HESAI_PANDARAT128) { return "correction_file"; @@ -428,7 +429,7 @@ std::string HesaiRosWrapper::getCalibrationParameterName(drivers::SensorModel mo return "calibration_file"; } -HesaiRosWrapper::get_calibration_result_t HesaiRosWrapper::GetCalibrationData( +HesaiRosWrapper::get_calibration_result_t HesaiRosWrapper::get_calibration_data( const std::string & calibration_file_path, bool ignore_others) { std::shared_ptr calib; @@ -453,9 +454,9 @@ HesaiRosWrapper::get_calibration_result_t HesaiRosWrapper::GetCalibrationData( // If a sensor is connected, try to download and save its calibration data if (!ignore_others && launch_hw_) { try { - auto raw_data = hw_interface_wrapper_->HwInterface()->GetLidarCalibrationBytes(); + auto raw_data = hw_interface_wrapper_->hw_interface()->GetLidarCalibrationBytes(); RCLCPP_INFO(logger, "Downloaded calibration data from sensor."); - auto status = calib->SaveToFileFromBytes(calibration_file_path_from_sensor, raw_data); + auto status = calib->save_to_file_from_bytes(calibration_file_path_from_sensor, raw_data); if (status != Status::OK) { RCLCPP_ERROR_STREAM(logger, "Could not save calibration data: " << status); } else { @@ -470,7 +471,7 @@ HesaiRosWrapper::get_calibration_result_t HesaiRosWrapper::GetCalibrationData( // If saved calibration data from a sensor exists (either just downloaded above, or previously), // try to load it if (!ignore_others && std::filesystem::exists(calibration_file_path_from_sensor)) { - auto status = calib->LoadFromFile(calibration_file_path_from_sensor); + auto status = calib->load_from_file(calibration_file_path_from_sensor); if (status == Status::OK) { calib->calibration_file = calibration_file_path_from_sensor; return calib; @@ -493,7 +494,7 @@ HesaiRosWrapper::get_calibration_result_t HesaiRosWrapper::GetCalibrationData( } // Try to load the existing fallback calibration file. Return an error if this fails - auto status = calib->LoadFromFile(calibration_file_path); + auto status = calib->load_from_file(calibration_file_path); if (status != Status::OK) { RCLCPP_ERROR_STREAM( logger, "Could not load calibration file at '" << calibration_file_path << "'"); diff --git a/nebula_ros/src/hesai/hw_interface_wrapper.cpp b/nebula_ros/src/hesai/hw_interface_wrapper.cpp index f0ba51102..c56c2f9e8 100644 --- a/nebula_ros/src/hesai/hw_interface_wrapper.cpp +++ b/nebula_ros/src/hesai/hw_interface_wrapper.cpp @@ -62,7 +62,7 @@ HesaiHwInterfaceWrapper::HesaiHwInterfaceWrapper( status_ = Status::OK; } -void HesaiHwInterfaceWrapper::OnConfigChange( +void HesaiHwInterfaceWrapper::on_config_change( const std::shared_ptr & new_config) { hw_interface_->SetSensorConfiguration( @@ -72,12 +72,12 @@ void HesaiHwInterfaceWrapper::OnConfigChange( } } -Status HesaiHwInterfaceWrapper::Status() +Status HesaiHwInterfaceWrapper::status() { return status_; } -std::shared_ptr HesaiHwInterfaceWrapper::HwInterface() const +std::shared_ptr HesaiHwInterfaceWrapper::hw_interface() const { return hw_interface_; } diff --git a/nebula_ros/src/hesai/hw_monitor_wrapper.cpp b/nebula_ros/src/hesai/hw_monitor_wrapper.cpp index 1a81cfdcc..f37575876 100644 --- a/nebula_ros/src/hesai/hw_monitor_wrapper.cpp +++ b/nebula_ros/src/hesai/hw_monitor_wrapper.cpp @@ -65,23 +65,23 @@ HesaiHwMonitorWrapper::HesaiHwMonitorWrapper( info_serial_ = std::string(std::begin(result.sn), std::end(result.sn)); RCLCPP_INFO_STREAM(logger_, "Model: " << info_model_); RCLCPP_INFO_STREAM(logger_, "Serial: " << info_serial_); - InitializeHesaiDiagnostics(); + initialize_hesai_diagnostics(); } -void HesaiHwMonitorWrapper::InitializeHesaiDiagnostics() +void HesaiHwMonitorWrapper::initialize_hesai_diagnostics() { - RCLCPP_INFO_STREAM(logger_, "InitializeHesaiDiagnostics"); + RCLCPP_INFO_STREAM(logger_, "initialize_hesai_diagnostics"); using std::chrono_literals::operator""s; std::ostringstream os; auto hardware_id = info_model_ + ": " + info_serial_; diagnostics_updater_.setHardwareID(hardware_id); RCLCPP_INFO_STREAM(logger_, "Hardware ID: " + hardware_id); - diagnostics_updater_.add("hesai_status", this, &HesaiHwMonitorWrapper::HesaiCheckStatus); - diagnostics_updater_.add("hesai_ptp", this, &HesaiHwMonitorWrapper::HesaiCheckPtp); + diagnostics_updater_.add("hesai_status", this, &HesaiHwMonitorWrapper::hesai_check_status); + diagnostics_updater_.add("hesai_ptp", this, &HesaiHwMonitorWrapper::hesai_check_ptp); diagnostics_updater_.add( - "hesai_temperature", this, &HesaiHwMonitorWrapper::HesaiCheckTemperature); - diagnostics_updater_.add("hesai_rpm", this, &HesaiHwMonitorWrapper::HesaiCheckRpm); + "hesai_temperature", this, &HesaiHwMonitorWrapper::hesai_check_temperature); + diagnostics_updater_.add("hesai_rpm", this, &HesaiHwMonitorWrapper::hesai_check_rpm); current_status_.reset(); current_monitor_.reset(); @@ -91,14 +91,14 @@ void HesaiHwMonitorWrapper::InitializeHesaiDiagnostics() current_monitor_status_ = diagnostic_msgs::msg::DiagnosticStatus::STALE; auto fetch_diag_from_sensor = [this]() { - OnHesaiStatusTimer(); + on_hesai_status_timer(); if (!supports_monitor_) return; if (hw_interface_->UseHttpGetLidarMonitor()) { - OnHesaiLidarMonitorTimerHttp(); + on_hesai_lidar_monitor_timer_http(); } else { - OnHesaiLidarMonitorTimer(); + on_hesai_lidar_monitor_timer(); } }; @@ -106,9 +106,10 @@ void HesaiHwMonitorWrapper::InitializeHesaiDiagnostics() std::chrono::milliseconds(diag_span_), std::move(fetch_diag_from_sensor)); if (hw_interface_->UseHttpGetLidarMonitor()) { - diagnostics_updater_.add("hesai_voltage", this, &HesaiHwMonitorWrapper::HesaiCheckVoltageHttp); + diagnostics_updater_.add( + "hesai_voltage", this, &HesaiHwMonitorWrapper::hesai_check_voltage_http); } else { - diagnostics_updater_.add("hesai_voltage", this, &HesaiHwMonitorWrapper::HesaiCheckVoltage); + diagnostics_updater_.add("hesai_voltage", this, &HesaiHwMonitorWrapper::hesai_check_voltage); } auto on_timer_update = [this] { @@ -142,26 +143,26 @@ void HesaiHwMonitorWrapper::InitializeHesaiDiagnostics() RCLCPP_DEBUG_STREAM(logger_, "add_timer"); } -std::string HesaiHwMonitorWrapper::GetPtreeValue( +std::string HesaiHwMonitorWrapper::get_ptree_value( boost::property_tree::ptree * pt, const std::string & key) { boost::optional value = pt->get_optional(key); if (value) { return value.get(); } else { - return MSG_NOT_SUPPORTED; + return MSG_NOT_SUPPORTED_; } } -std::string HesaiHwMonitorWrapper::GetFixedPrecisionString(double val, int pre) +std::string HesaiHwMonitorWrapper::get_fixed_precision_string(double val, int pre) { std::stringstream ss; ss << std::fixed << std::setprecision(pre) << val; return ss.str(); } -void HesaiHwMonitorWrapper::OnHesaiStatusTimer() +void HesaiHwMonitorWrapper::on_hesai_status_timer() { - RCLCPP_DEBUG_STREAM(logger_, "OnHesaiStatusTimer" << std::endl); + RCLCPP_DEBUG_STREAM(logger_, "on_hesai_status_timer" << std::endl); try { auto result = hw_interface_->GetLidarStatus(); std::scoped_lock lock(mtx_lidar_status_); @@ -169,19 +170,20 @@ void HesaiHwMonitorWrapper::OnHesaiStatusTimer() current_status_.reset(new HesaiLidarStatus(result)); } catch (const std::system_error & error) { RCLCPP_ERROR_STREAM( - rclcpp::get_logger("HesaiHwMonitorWrapper::OnHesaiStatusTimer(std::system_error)"), + rclcpp::get_logger("HesaiHwMonitorWrapper::on_hesai_status_timer(std::system_error)"), error.what()); } catch (const boost::system::system_error & error) { RCLCPP_ERROR_STREAM( - rclcpp::get_logger("HesaiHwMonitorWrapper::OnHesaiStatusTimer(boost::system::system_error)"), + rclcpp::get_logger( + "HesaiHwMonitorWrapper::on_hesai_status_timer(boost::system::system_error)"), error.what()); } - RCLCPP_DEBUG_STREAM(logger_, "OnHesaiStatusTimer END" << std::endl); + RCLCPP_DEBUG_STREAM(logger_, "on_hesai_status_timer END" << std::endl); } -void HesaiHwMonitorWrapper::OnHesaiLidarMonitorTimerHttp() +void HesaiHwMonitorWrapper::on_hesai_lidar_monitor_timer_http() { - RCLCPP_DEBUG_STREAM(logger_, "OnHesaiLidarMonitorTimerHttp"); + RCLCPP_DEBUG_STREAM(logger_, "on_hesai_lidar_monitor_timer_http"); try { hw_interface_->GetLidarMonitorAsyncHttp([this](const std::string & str) { std::scoped_lock lock(mtx_lidar_monitor_); @@ -191,21 +193,22 @@ void HesaiHwMonitorWrapper::OnHesaiLidarMonitorTimerHttp() }); } catch (const std::system_error & error) { RCLCPP_ERROR_STREAM( - rclcpp::get_logger("HesaiHwMonitorWrapper::OnHesaiLidarMonitorTimerHttp(std::system_error)"), + rclcpp::get_logger( + "HesaiHwMonitorWrapper::on_hesai_lidar_monitor_timer_http(std::system_error)"), error.what()); } catch (const boost::system::system_error & error) { RCLCPP_ERROR_STREAM( rclcpp::get_logger( - "HesaiHwMonitorWrapper::OnHesaiLidarMonitorTimerHttp(boost::system::system_" + "HesaiHwMonitorWrapper::on_hesai_lidar_monitor_timer_http(boost::system::system_" "error)"), error.what()); } - RCLCPP_DEBUG_STREAM(logger_, "OnHesaiLidarMonitorTimerHttp END"); + RCLCPP_DEBUG_STREAM(logger_, "on_hesai_lidar_monitor_timer_http END"); } -void HesaiHwMonitorWrapper::OnHesaiLidarMonitorTimer() +void HesaiHwMonitorWrapper::on_hesai_lidar_monitor_timer() { - RCLCPP_DEBUG_STREAM(logger_, "OnHesaiLidarMonitorTimer"); + RCLCPP_DEBUG_STREAM(logger_, "on_hesai_lidar_monitor_timer"); try { auto result = hw_interface_->GetLidarMonitor(); std::scoped_lock lock(mtx_lidar_monitor_); @@ -213,18 +216,19 @@ void HesaiHwMonitorWrapper::OnHesaiLidarMonitorTimer() current_monitor_.reset(new HesaiLidarMonitor(result)); } catch (const std::system_error & error) { RCLCPP_ERROR_STREAM( - rclcpp::get_logger("HesaiHwMonitorWrapper::OnHesaiLidarMonitorTimer(std::system_error)"), + rclcpp::get_logger("HesaiHwMonitorWrapper::on_hesai_lidar_monitor_timer(std::system_error)"), error.what()); } catch (const boost::system::system_error & error) { RCLCPP_ERROR_STREAM( - rclcpp::get_logger("HesaiHwMonitorWrapper::OnHesaiLidarMonitorTimer(boost::system::system_" - "error)"), + rclcpp::get_logger( + "HesaiHwMonitorWrapper::on_hesai_lidar_monitor_timer(boost::system::system_" + "error)"), error.what()); } - RCLCPP_DEBUG_STREAM(logger_, "OnHesaiLidarMonitorTimer END"); + RCLCPP_DEBUG_STREAM(logger_, "on_hesai_lidar_monitor_timer END"); } -void HesaiHwMonitorWrapper::HesaiCheckStatus( +void HesaiHwMonitorWrapper::hesai_check_status( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { std::scoped_lock lock(mtx_lidar_status_); @@ -243,7 +247,8 @@ void HesaiHwMonitorWrapper::HesaiCheckStatus( } } -void HesaiHwMonitorWrapper::HesaiCheckPtp(diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +void HesaiHwMonitorWrapper::hesai_check_ptp( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { std::scoped_lock lock(mtx_lidar_status_); if (current_status_) { @@ -276,7 +281,7 @@ void HesaiHwMonitorWrapper::HesaiCheckPtp(diagnostic_updater::DiagnosticStatusWr } } -void HesaiHwMonitorWrapper::HesaiCheckTemperature( +void HesaiHwMonitorWrapper::hesai_check_temperature( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { std::scoped_lock lock(mtx_lidar_status_); @@ -286,7 +291,7 @@ void HesaiHwMonitorWrapper::HesaiCheckTemperature( for (size_t i = 0; i < std::size(current_status_->temperature); i++) { diagnostics.add( temperature_names_[i], - GetFixedPrecisionString(current_status_->temperature[i].value() * 0.01, 3)); + get_fixed_precision_string(current_status_->temperature[i].value() * 0.01, 3)); } diagnostics.summary(level, boost::algorithm::join(msg, ", ")); } else { @@ -294,7 +299,8 @@ void HesaiHwMonitorWrapper::HesaiCheckTemperature( } } -void HesaiHwMonitorWrapper::HesaiCheckRpm(diagnostic_updater::DiagnosticStatusWrapper & diagnostics) +void HesaiHwMonitorWrapper::hesai_check_rpm( + diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { std::scoped_lock lock(mtx_lidar_status_); if (current_status_) { @@ -308,7 +314,7 @@ void HesaiHwMonitorWrapper::HesaiCheckRpm(diagnostic_updater::DiagnosticStatusWr } } -void HesaiHwMonitorWrapper::HesaiCheckVoltageHttp( +void HesaiHwMonitorWrapper::hesai_check_voltage_http( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { std::scoped_lock lock(mtx_lidar_monitor_); @@ -320,18 +326,18 @@ void HesaiHwMonitorWrapper::HesaiCheckVoltageHttp( std::string mes; key = "lidarInCur"; try { - mes = GetPtreeValue(current_lidar_monitor_tree_.get(), "Body." + key); + mes = get_ptree_value(current_lidar_monitor_tree_.get(), "Body." + key); } catch (boost::bad_lexical_cast & ex) { level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - mes = MSG_ERROR + std::string(ex.what()); + mes = MSG_ERROR_ + std::string(ex.what()); } diagnostics.add(key, mes); key = "lidarInVol"; try { - mes = GetPtreeValue(current_lidar_monitor_tree_.get(), "Body." + key); + mes = get_ptree_value(current_lidar_monitor_tree_.get(), "Body." + key); } catch (boost::bad_lexical_cast & ex) { level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - mes = MSG_ERROR + std::string(ex.what()); + mes = MSG_ERROR_ + std::string(ex.what()); } diagnostics.add(key, mes); @@ -341,7 +347,7 @@ void HesaiHwMonitorWrapper::HesaiCheckVoltageHttp( } } -void HesaiHwMonitorWrapper::HesaiCheckVoltage( +void HesaiHwMonitorWrapper::hesai_check_voltage( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { std::scoped_lock lock(mtx_lidar_monitor_); @@ -350,14 +356,15 @@ void HesaiHwMonitorWrapper::HesaiCheckVoltage( std::vector msg; diagnostics.add( "input_voltage", - GetFixedPrecisionString(current_monitor_->input_voltage.value() * 0.01, 3) + " V"); + get_fixed_precision_string(current_monitor_->input_voltage.value() * 0.01, 3) + " V"); diagnostics.add( - "input_current", GetFixedPrecisionString(current_monitor_->input_current.value() * 0.01, 3) + - " m" - "A"); + "input_current", + get_fixed_precision_string(current_monitor_->input_current.value() * 0.01, 3) + + " m" + "A"); diagnostics.add( "input_power", - GetFixedPrecisionString(current_monitor_->input_power.value() * 0.01, 3) + " W"); + get_fixed_precision_string(current_monitor_->input_power.value() * 0.01, 3) + " W"); diagnostics.summary(level, boost::algorithm::join(msg, ", ")); } else { @@ -365,7 +372,7 @@ void HesaiHwMonitorWrapper::HesaiCheckVoltage( } } -Status HesaiHwMonitorWrapper::Status() +Status HesaiHwMonitorWrapper::status() { return Status::OK; } diff --git a/nebula_ros/src/robosense/decoder_wrapper.cpp b/nebula_ros/src/robosense/decoder_wrapper.cpp index c7cae7d3a..1263ebb05 100644 --- a/nebula_ros/src/robosense/decoder_wrapper.cpp +++ b/nebula_ros/src/robosense/decoder_wrapper.cpp @@ -21,7 +21,7 @@ RobosenseDecoderWrapper::RobosenseDecoderWrapper( calibration_cfg_ptr_(calibration), driver_ptr_(new drivers::RobosenseDriver(config, calibration)) { - status_ = driver_ptr_->GetStatus(); + status_ = driver_ptr_->get_status(); setvbuf(stdout, NULL, _IONBF, BUFSIZ); @@ -58,7 +58,7 @@ RobosenseDecoderWrapper::RobosenseDecoderWrapper( RCLCPP_INFO(logger_, "Initialized decoder wrapper."); } -void RobosenseDecoderWrapper::ProcessCloudPacket( +void RobosenseDecoderWrapper::process_cloud_packet( std::unique_ptr packet_msg) { // Accumulate packets for recording only if someone is subscribed to the topic (for performance) @@ -80,7 +80,7 @@ void RobosenseDecoderWrapper::ProcessCloudPacket( { std::lock_guard lock(mtx_driver_ptr_); - pointcloud_ts = driver_ptr_->ParseCloudPacket(packet_msg->data); + pointcloud_ts = driver_ptr_->parse_cloud_packet(packet_msg->data); pointcloud = std::get<0>(pointcloud_ts); } @@ -102,34 +102,34 @@ void RobosenseDecoderWrapper::ProcessCloudPacket( auto ros_pc_msg_ptr = std::make_unique(); pcl::toROSMsg(*pointcloud, *ros_pc_msg_ptr); ros_pc_msg_ptr->header.stamp = - rclcpp::Time(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count()); - PublishCloud(std::move(ros_pc_msg_ptr), nebula_points_pub_); + rclcpp::Time(seconds_to_chrono_nano_seconds(std::get<1>(pointcloud_ts)).count()); + publish_cloud(std::move(ros_pc_msg_ptr), nebula_points_pub_); } if ( aw_points_base_pub_->get_subscription_count() > 0 || aw_points_base_pub_->get_intra_process_subscription_count() > 0) { const auto autoware_cloud_xyzi = - nebula::drivers::convertPointXYZIRCAEDTToPointXYZIR(pointcloud); + nebula::drivers::convert_point_xyzircaedt_to_point_xyzir(pointcloud); auto ros_pc_msg_ptr = std::make_unique(); pcl::toROSMsg(*autoware_cloud_xyzi, *ros_pc_msg_ptr); ros_pc_msg_ptr->header.stamp = - rclcpp::Time(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count()); - PublishCloud(std::move(ros_pc_msg_ptr), aw_points_base_pub_); + rclcpp::Time(seconds_to_chrono_nano_seconds(std::get<1>(pointcloud_ts)).count()); + publish_cloud(std::move(ros_pc_msg_ptr), aw_points_base_pub_); } if ( aw_points_ex_pub_->get_subscription_count() > 0 || aw_points_ex_pub_->get_intra_process_subscription_count() > 0) { - const auto autoware_ex_cloud = nebula::drivers::convertPointXYZIRCAEDTToPointXYZIRADT( + const auto autoware_ex_cloud = nebula::drivers::convert_point_xyzircaedt_to_point_xyziradt( pointcloud, std::get<1>(pointcloud_ts)); auto ros_pc_msg_ptr = std::make_unique(); pcl::toROSMsg(*autoware_ex_cloud, *ros_pc_msg_ptr); ros_pc_msg_ptr->header.stamp = - rclcpp::Time(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count()); - PublishCloud(std::move(ros_pc_msg_ptr), aw_points_ex_pub_); + rclcpp::Time(seconds_to_chrono_nano_seconds(std::get<1>(pointcloud_ts)).count()); + publish_cloud(std::move(ros_pc_msg_ptr), aw_points_ex_pub_); } } -void RobosenseDecoderWrapper::OnConfigChange( +void RobosenseDecoderWrapper::on_config_change( const std::shared_ptr & new_config) { std::lock_guard lock(mtx_driver_ptr_); @@ -140,12 +140,12 @@ void RobosenseDecoderWrapper::OnConfigChange( /// @brief Get current status of this driver /// @return Current status -nebula::Status RobosenseDecoderWrapper::Status() +nebula::Status RobosenseDecoderWrapper::status() { return status_; } -void RobosenseDecoderWrapper::PublishCloud( +void RobosenseDecoderWrapper::publish_cloud( std::unique_ptr pointcloud, const rclcpp::Publisher::SharedPtr & publisher) { diff --git a/nebula_ros/src/robosense/hw_interface_wrapper.cpp b/nebula_ros/src/robosense/hw_interface_wrapper.cpp index 2abddd285..4f521fe3c 100644 --- a/nebula_ros/src/robosense/hw_interface_wrapper.cpp +++ b/nebula_ros/src/robosense/hw_interface_wrapper.cpp @@ -13,9 +13,9 @@ RobosenseHwInterfaceWrapper::RobosenseHwInterfaceWrapper( logger_(parent_node->get_logger().get_child("HwInterfaceWrapper")), status_(nebula::Status::NOT_INITIALIZED) { - hw_interface_->SetLogger( + hw_interface_->set_logger( std::make_shared(parent_node->get_logger().get_child("HwInterface"))); - status_ = hw_interface_->SetSensorConfiguration(config); + status_ = hw_interface_->set_sensor_configuration(config); if (Status::OK != status_) { throw std::runtime_error( @@ -23,18 +23,18 @@ RobosenseHwInterfaceWrapper::RobosenseHwInterfaceWrapper( } } -void RobosenseHwInterfaceWrapper::OnConfigChange( +void RobosenseHwInterfaceWrapper::on_config_change( const std::shared_ptr & new_config) { - hw_interface_->SetSensorConfiguration(new_config); + hw_interface_->set_sensor_configuration(new_config); } -nebula::Status RobosenseHwInterfaceWrapper::Status() +nebula::Status RobosenseHwInterfaceWrapper::status() { return status_; } -std::shared_ptr RobosenseHwInterfaceWrapper::HwInterface() const +std::shared_ptr RobosenseHwInterfaceWrapper::hw_interface() const { return hw_interface_; } diff --git a/nebula_ros/src/robosense/hw_monitor_wrapper.cpp b/nebula_ros/src/robosense/hw_monitor_wrapper.cpp index d5c43ea7b..9fca97cee 100644 --- a/nebula_ros/src/robosense/hw_monitor_wrapper.cpp +++ b/nebula_ros/src/robosense/hw_monitor_wrapper.cpp @@ -23,7 +23,7 @@ RobosenseHwMonitorWrapper::RobosenseHwMonitorWrapper( diag_span_ = parent_->declare_parameter("diag_span", descriptor); } -void RobosenseHwMonitorWrapper::InitializeRobosenseDiagnostics() +void RobosenseHwMonitorWrapper::initialize_robosense_diagnostics() { std::scoped_lock lock(mtx_config_, mtx_current_sensor_info_); @@ -32,7 +32,7 @@ void RobosenseHwMonitorWrapper::InitializeRobosenseDiagnostics() return; } - auto hw_id = nebula::drivers::SensorModelToString(sensor_cfg_ptr_->sensor_model) + ": " + + auto hw_id = nebula::drivers::sensor_model_to_string(sensor_cfg_ptr_->sensor_model) + ": " + current_sensor_info_["serial_number"]; RCLCPP_INFO(logger_, "InitializeRobosenseDiagnostics"); @@ -41,7 +41,7 @@ void RobosenseHwMonitorWrapper::InitializeRobosenseDiagnostics() RCLCPP_INFO_STREAM(logger_, "hardware_id: " + hw_id); diagnostics_updater_->add( - "robosense_status", this, &RobosenseHwMonitorWrapper::RobosenseCheckStatus); + "robosense_status", this, &RobosenseHwMonitorWrapper::robosense_check_status); auto on_timer_update = [this] { RCLCPP_DEBUG(logger_, "OnUpdateTimer"); @@ -64,7 +64,7 @@ void RobosenseHwMonitorWrapper::InitializeRobosenseDiagnostics() RCLCPP_DEBUG_STREAM(logger_, "add_timer"); } -void RobosenseHwMonitorWrapper::RobosenseCheckStatus( +void RobosenseHwMonitorWrapper::robosense_check_status( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { std::lock_guard lock(mtx_current_sensor_info_); @@ -77,7 +77,7 @@ void RobosenseHwMonitorWrapper::RobosenseCheckStatus( diagnostics.summary(level, "OK"); } -void RobosenseHwMonitorWrapper::DiagnosticsCallback( +void RobosenseHwMonitorWrapper::diagnostics_callback( const std::map & diag_info) { auto current_time = parent_->get_clock()->now(); @@ -89,11 +89,11 @@ void RobosenseHwMonitorWrapper::DiagnosticsCallback( } if (!diagnostics_updater_) { - InitializeRobosenseDiagnostics(); + initialize_robosense_diagnostics(); } } -void RobosenseHwMonitorWrapper::OnConfigChange( +void RobosenseHwMonitorWrapper::on_config_change( const std::shared_ptr & new_config) { std::lock_guard lock(mtx_config_); diff --git a/nebula_ros/src/robosense/robosense_ros_wrapper.cpp b/nebula_ros/src/robosense/robosense_ros_wrapper.cpp index 65c5ec8a9..29b300039 100644 --- a/nebula_ros/src/robosense/robosense_ros_wrapper.cpp +++ b/nebula_ros/src/robosense/robosense_ros_wrapper.cpp @@ -21,7 +21,7 @@ RobosenseRosWrapper::RobosenseRosWrapper(const rclcpp::NodeOptions & options) { setvbuf(stdout, NULL, _IONBF, BUFSIZ); - wrapper_status_ = DeclareAndGetSensorConfigParams(); + wrapper_status_ = declare_and_get_sensor_config_params(); if (wrapper_status_ != Status::OK) { throw std::runtime_error( @@ -42,20 +42,20 @@ RobosenseRosWrapper::RobosenseRosWrapper(const rclcpp::NodeOptions & options) decoder_thread_ = std::thread([this]() { while (true) { - decoder_wrapper_->ProcessCloudPacket(packet_queue_.pop()); + decoder_wrapper_->process_cloud_packet(packet_queue_.pop()); } }); if (launch_hw_) { - hw_interface_wrapper_->HwInterface()->RegisterScanCallback( - std::bind(&RobosenseRosWrapper::ReceiveCloudPacketCallback, this, std::placeholders::_1)); - hw_interface_wrapper_->HwInterface()->RegisterInfoCallback( - std::bind(&RobosenseRosWrapper::ReceiveInfoPacketCallback, this, std::placeholders::_1)); - StreamStart(); + hw_interface_wrapper_->hw_interface()->register_scan_callback( + std::bind(&RobosenseRosWrapper::receive_cloud_packet_callback, this, std::placeholders::_1)); + hw_interface_wrapper_->hw_interface()->register_info_callback( + std::bind(&RobosenseRosWrapper::receive_info_packet_callback, this, std::placeholders::_1)); + stream_start(); } else { packets_sub_ = create_subscription( "robosense_packets", rclcpp::SensorDataQoS(), - std::bind(&RobosenseRosWrapper::ReceiveScanMessageCallback, this, std::placeholders::_1)); + std::bind(&RobosenseRosWrapper::receive_scan_message_callback, this, std::placeholders::_1)); RCLCPP_INFO_STREAM( get_logger(), "Hardware connection disabled, listening for packets on " << packets_sub_->get_topic_name()); @@ -64,18 +64,18 @@ RobosenseRosWrapper::RobosenseRosWrapper(const rclcpp::NodeOptions & options) // Register parameter callback after all params have been declared. Otherwise it would be called // once for each declaration parameter_event_cb_ = add_on_set_parameters_callback( - std::bind(&RobosenseRosWrapper::OnParameterChange, this, std::placeholders::_1)); + std::bind(&RobosenseRosWrapper::on_parameter_change, this, std::placeholders::_1)); } -nebula::Status RobosenseRosWrapper::DeclareAndGetSensorConfigParams() +nebula::Status RobosenseRosWrapper::declare_and_get_sensor_config_params() { nebula::drivers::RobosenseSensorConfiguration config; auto _sensor_model = declare_parameter("sensor_model", param_read_only()); - config.sensor_model = drivers::SensorModelFromString(_sensor_model); + config.sensor_model = drivers::sensor_model_from_string(_sensor_model); auto _return_mode = declare_parameter("return_mode", param_read_write()); - config.return_mode = drivers::ReturnModeFromStringRobosense(_return_mode); + config.return_mode = drivers::return_mode_from_string_robosense(_return_mode); config.host_ip = declare_parameter("host_ip", param_read_only()); config.sensor_ip = declare_parameter("sensor_ip", param_read_only()); @@ -98,10 +98,10 @@ nebula::Status RobosenseRosWrapper::DeclareAndGetSensorConfigParams() } auto new_cfg_ptr = std::make_shared(config); - return ValidateAndSetConfig(new_cfg_ptr); + return validate_and_set_config(new_cfg_ptr); } -Status RobosenseRosWrapper::ValidateAndSetConfig( +Status RobosenseRosWrapper::validate_and_set_config( std::shared_ptr & new_config) { if (new_config->sensor_model == nebula::drivers::SensorModel::UNKNOWN) { @@ -115,20 +115,20 @@ Status RobosenseRosWrapper::ValidateAndSetConfig( } if (hw_interface_wrapper_) { - hw_interface_wrapper_->OnConfigChange(new_config); + hw_interface_wrapper_->on_config_change(new_config); } if (hw_monitor_wrapper_) { - hw_monitor_wrapper_->OnConfigChange(new_config); + hw_monitor_wrapper_->on_config_change(new_config); } if (decoder_wrapper_) { - decoder_wrapper_->OnConfigChange(new_config); + decoder_wrapper_->on_config_change(new_config); } sensor_cfg_ptr_ = new_config; return Status::OK; } -void RobosenseRosWrapper::ReceiveScanMessageCallback( +void RobosenseRosWrapper::receive_scan_message_callback( std::unique_ptr scan_msg) { if (hw_interface_wrapper_) { @@ -148,14 +148,14 @@ void RobosenseRosWrapper::ReceiveScanMessageCallback( } } -void RobosenseRosWrapper::ReceiveInfoPacketCallback(std::vector & packet) +void RobosenseRosWrapper::receive_info_packet_callback(std::vector & packet) { if (!sensor_cfg_ptr_ || !info_driver_) { throw std::runtime_error( "Wrapper already receiving packets despite not being fully initialized yet."); } - auto status = info_driver_->DecodeInfoPacket(packet); + auto status = info_driver_->decode_info_packet(packet); if (status != nebula::Status::OK) { RCLCPP_ERROR_STREAM_THROTTLE( @@ -165,14 +165,14 @@ void RobosenseRosWrapper::ReceiveInfoPacketCallback(std::vector & packe if (!decoder_wrapper_) { auto new_cfg = *sensor_cfg_ptr_; - new_cfg.return_mode = info_driver_->GetReturnMode(); - new_cfg.use_sensor_time = info_driver_->GetSyncStatus(); - auto calib = info_driver_->GetSensorCalibration(); - calib.CreateCorrectedChannels(); + new_cfg.return_mode = info_driver_->get_return_mode(); + new_cfg.use_sensor_time = info_driver_->get_sync_status(); + auto calib = info_driver_->get_sensor_calibration(); + calib.create_corrected_channels(); auto new_cfg_ptr = std::make_shared(new_cfg); - status = ValidateAndSetConfig(new_cfg_ptr); + status = validate_and_set_config(new_cfg_ptr); if (status != nebula::Status::OK) { RCLCPP_ERROR_STREAM_THROTTLE( @@ -184,44 +184,44 @@ void RobosenseRosWrapper::ReceiveInfoPacketCallback(std::vector & packe auto calib_ptr = std::make_shared(std::move(calib)); decoder_wrapper_.emplace( - this, hw_interface_wrapper_ ? hw_interface_wrapper_->HwInterface() : nullptr, sensor_cfg_ptr_, - calib_ptr); + this, hw_interface_wrapper_ ? hw_interface_wrapper_->hw_interface() : nullptr, + sensor_cfg_ptr_, calib_ptr); RCLCPP_INFO_STREAM( - this->get_logger(), "Initialized decoder wrapper: " << decoder_wrapper_->Status()); + this->get_logger(), "Initialized decoder wrapper: " << decoder_wrapper_->status()); } if (!hw_monitor_wrapper_) { return; } - hw_monitor_wrapper_->DiagnosticsCallback(info_driver_->GetSensorInfo()); + hw_monitor_wrapper_->diagnostics_callback(info_driver_->get_sensor_info()); } -Status RobosenseRosWrapper::GetStatus() +Status RobosenseRosWrapper::get_status() { return wrapper_status_; } -Status RobosenseRosWrapper::StreamStart() +Status RobosenseRosWrapper::stream_start() { if (!hw_interface_wrapper_) { return Status::UDP_CONNECTION_ERROR; } - if (hw_interface_wrapper_->Status() != Status::OK) { - return hw_interface_wrapper_->Status(); + if (hw_interface_wrapper_->status() != Status::OK) { + return hw_interface_wrapper_->status(); } - auto info_status = hw_interface_wrapper_->HwInterface()->InfoInterfaceStart(); + auto info_status = hw_interface_wrapper_->hw_interface()->info_interface_start(); if (info_status != Status::OK) { return info_status; } - return hw_interface_wrapper_->HwInterface()->SensorInterfaceStart(); + return hw_interface_wrapper_->hw_interface()->sensor_interface_start(); } -rcl_interfaces::msg::SetParametersResult RobosenseRosWrapper::OnParameterChange( +rcl_interfaces::msg::SetParametersResult RobosenseRosWrapper::on_parameter_change( const std::vector & p) { using rcl_interfaces::msg::SetParametersResult; @@ -250,10 +250,10 @@ rcl_interfaces::msg::SetParametersResult RobosenseRosWrapper::OnParameterChange( } if (_return_mode.length() > 0) - new_cfg.return_mode = nebula::drivers::ReturnModeFromString(_return_mode); + new_cfg.return_mode = nebula::drivers::return_mode_from_string(_return_mode); auto new_cfg_ptr = std::make_shared(new_cfg); - auto status = ValidateAndSetConfig(new_cfg_ptr); + auto status = validate_and_set_config(new_cfg_ptr); if (status != Status::OK) { RCLCPP_WARN_STREAM(get_logger(), "OnParameterChange aborted: " << status); @@ -266,9 +266,9 @@ rcl_interfaces::msg::SetParametersResult RobosenseRosWrapper::OnParameterChange( return rcl_interfaces::build().successful(true).reason(""); } -void RobosenseRosWrapper::ReceiveCloudPacketCallback(std::vector & packet) +void RobosenseRosWrapper::receive_cloud_packet_callback(std::vector & packet) { - if (!decoder_wrapper_ || decoder_wrapper_->Status() != Status::OK) { + if (!decoder_wrapper_ || decoder_wrapper_->status() != Status::OK) { return; } diff --git a/nebula_ros/src/velodyne/decoder_wrapper.cpp b/nebula_ros/src/velodyne/decoder_wrapper.cpp index a98f74e8a..b0191cd72 100644 --- a/nebula_ros/src/velodyne/decoder_wrapper.cpp +++ b/nebula_ros/src/velodyne/decoder_wrapper.cpp @@ -27,7 +27,7 @@ VelodyneDecoderWrapper::VelodyneDecoderWrapper( calibration_file_path_ = parent_node->declare_parameter("calibration_file", param_read_write()); - auto calibration_result = GetCalibrationData(calibration_file_path_); + auto calibration_result = get_calibration_data(calibration_file_path_); if (!calibration_result.has_value()) { throw std::runtime_error( @@ -41,7 +41,7 @@ VelodyneDecoderWrapper::VelodyneDecoderWrapper( RCLCPP_INFO(logger_, "Starting Decoder"); driver_ptr_ = std::make_shared(config, calibration_cfg_ptr_); - status_ = driver_ptr_->GetStatus(); + status_ = driver_ptr_->get_status(); if (Status::OK != status_) { throw std::runtime_error( @@ -76,7 +76,7 @@ VelodyneDecoderWrapper::VelodyneDecoderWrapper( }); } -void VelodyneDecoderWrapper::OnConfigChange( +void VelodyneDecoderWrapper::on_config_change( const std::shared_ptr & new_config) { std::lock_guard lock(mtx_driver_ptr_); @@ -85,7 +85,7 @@ void VelodyneDecoderWrapper::OnConfigChange( sensor_cfg_ = new_config; } -void VelodyneDecoderWrapper::OnCalibrationChange( +void VelodyneDecoderWrapper::on_calibration_change( const std::shared_ptr & new_calibration) { std::lock_guard lock(mtx_driver_ptr_); @@ -95,7 +95,7 @@ void VelodyneDecoderWrapper::OnCalibrationChange( calibration_file_path_ = calibration_cfg_ptr_->calibration_file; } -rcl_interfaces::msg::SetParametersResult VelodyneDecoderWrapper::OnParameterChange( +rcl_interfaces::msg::SetParametersResult VelodyneDecoderWrapper::on_parameter_change( const std::vector & p) { using rcl_interfaces::msg::SetParametersResult; @@ -115,7 +115,7 @@ rcl_interfaces::msg::SetParametersResult VelodyneDecoderWrapper::OnParameterChan return result; } - auto get_calibration_result = GetCalibrationData(calibration_path); + auto get_calibration_result = get_calibration_data(calibration_path); if (!get_calibration_result.has_value()) { auto result = SetParametersResult(); result.successful = false; @@ -126,13 +126,13 @@ rcl_interfaces::msg::SetParametersResult VelodyneDecoderWrapper::OnParameterChan return result; } - OnCalibrationChange(get_calibration_result.value()); + on_calibration_change(get_calibration_result.value()); RCLCPP_INFO_STREAM( logger_, "Changed calibration to '" << calibration_cfg_ptr_->calibration_file << "'"); return rcl_interfaces::build().successful(true).reason(""); } -VelodyneDecoderWrapper::get_calibration_result_t VelodyneDecoderWrapper::GetCalibrationData( +VelodyneDecoderWrapper::get_calibration_result_t VelodyneDecoderWrapper::get_calibration_data( const std::string & calibration_file_path) { auto calib = std::make_shared(); @@ -145,7 +145,7 @@ VelodyneDecoderWrapper::get_calibration_result_t VelodyneDecoderWrapper::GetCali } // Try to load the existing fallback calibration file. Return an error if this fails - auto status = calib->LoadFromFile(calibration_file_path); + auto status = calib->load_from_file(calibration_file_path); if (status != Status::OK) { RCLCPP_ERROR_STREAM( logger_, "Could not load calibration file at '" << calibration_file_path << "'"); @@ -157,7 +157,7 @@ VelodyneDecoderWrapper::get_calibration_result_t VelodyneDecoderWrapper::GetCali return calib; } -void VelodyneDecoderWrapper::ProcessCloudPacket( +void VelodyneDecoderWrapper::process_cloud_packet( std::unique_ptr packet_msg) { // Accumulate packets for recording only if someone is subscribed to the topic (for performance) @@ -179,7 +179,7 @@ void VelodyneDecoderWrapper::ProcessCloudPacket( { std::lock_guard lock(mtx_driver_ptr_); pointcloud_ts = - driver_ptr_->ParseCloudPacket(packet_msg->data, rclcpp::Time(packet_msg->stamp).seconds()); + driver_ptr_->parse_cloud_packet(packet_msg->data, rclcpp::Time(packet_msg->stamp).seconds()); pointcloud = std::get<0>(pointcloud_ts); } @@ -201,34 +201,34 @@ void VelodyneDecoderWrapper::ProcessCloudPacket( auto ros_pc_msg_ptr = std::make_unique(); pcl::toROSMsg(*pointcloud, *ros_pc_msg_ptr); ros_pc_msg_ptr->header.stamp = - rclcpp::Time(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count()); - PublishCloud(std::move(ros_pc_msg_ptr), nebula_points_pub_); + rclcpp::Time(seconds_to_chrono_nano_seconds(std::get<1>(pointcloud_ts)).count()); + publish_cloud(std::move(ros_pc_msg_ptr), nebula_points_pub_); } if ( aw_points_base_pub_->get_subscription_count() > 0 || aw_points_base_pub_->get_intra_process_subscription_count() > 0) { const auto autoware_cloud_xyzi = - nebula::drivers::convertPointXYZIRCAEDTToPointXYZIR(pointcloud); + nebula::drivers::convert_point_xyzircaedt_to_point_xyzir(pointcloud); auto ros_pc_msg_ptr = std::make_unique(); pcl::toROSMsg(*autoware_cloud_xyzi, *ros_pc_msg_ptr); ros_pc_msg_ptr->header.stamp = - rclcpp::Time(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count()); - PublishCloud(std::move(ros_pc_msg_ptr), aw_points_base_pub_); + rclcpp::Time(seconds_to_chrono_nano_seconds(std::get<1>(pointcloud_ts)).count()); + publish_cloud(std::move(ros_pc_msg_ptr), aw_points_base_pub_); } if ( aw_points_ex_pub_->get_subscription_count() > 0 || aw_points_ex_pub_->get_intra_process_subscription_count() > 0) { - const auto autoware_ex_cloud = nebula::drivers::convertPointXYZIRCAEDTToPointXYZIRADT( + const auto autoware_ex_cloud = nebula::drivers::convert_point_xyzircaedt_to_point_xyziradt( pointcloud, std::get<1>(pointcloud_ts)); auto ros_pc_msg_ptr = std::make_unique(); pcl::toROSMsg(*autoware_ex_cloud, *ros_pc_msg_ptr); ros_pc_msg_ptr->header.stamp = - rclcpp::Time(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count()); - PublishCloud(std::move(ros_pc_msg_ptr), aw_points_ex_pub_); + rclcpp::Time(seconds_to_chrono_nano_seconds(std::get<1>(pointcloud_ts)).count()); + publish_cloud(std::move(ros_pc_msg_ptr), aw_points_ex_pub_); } } -void VelodyneDecoderWrapper::PublishCloud( +void VelodyneDecoderWrapper::publish_cloud( std::unique_ptr pointcloud, const rclcpp::Publisher::SharedPtr & publisher) { @@ -239,7 +239,7 @@ void VelodyneDecoderWrapper::PublishCloud( publisher->publish(std::move(pointcloud)); } -nebula::Status VelodyneDecoderWrapper::Status() +nebula::Status VelodyneDecoderWrapper::status() { std::lock_guard lock(mtx_driver_ptr_); @@ -247,7 +247,7 @@ nebula::Status VelodyneDecoderWrapper::Status() return nebula::Status::NOT_INITIALIZED; } - return driver_ptr_->GetStatus(); + return driver_ptr_->get_status(); } } // namespace ros } // namespace nebula diff --git a/nebula_ros/src/velodyne/hw_interface_wrapper.cpp b/nebula_ros/src/velodyne/hw_interface_wrapper.cpp index dafbf0d7e..578f4ce3d 100644 --- a/nebula_ros/src/velodyne/hw_interface_wrapper.cpp +++ b/nebula_ros/src/velodyne/hw_interface_wrapper.cpp @@ -16,16 +16,16 @@ VelodyneHwInterfaceWrapper::VelodyneHwInterfaceWrapper( { setup_sensor_ = parent_node->declare_parameter("setup_sensor", param_read_only()); - hw_interface_->SetLogger( + hw_interface_->set_logger( std::make_shared(parent_node->get_logger().get_child("HwInterface"))); - status_ = hw_interface_->InitializeSensorConfiguration(config); + status_ = hw_interface_->initialize_sensor_configuration(config); if (status_ != Status::OK) { throw std::runtime_error( (std::stringstream{} << "Could not initialize HW interface: " << status_).str()); } - status_ = hw_interface_->InitHttpClient(); + status_ = hw_interface_->init_http_client(); if (status_ != Status::OK) { throw std::runtime_error( @@ -34,7 +34,7 @@ VelodyneHwInterfaceWrapper::VelodyneHwInterfaceWrapper( if (setup_sensor_) { RCLCPP_INFO_STREAM(logger_, "Setting sensor configuration"); - status_ = hw_interface_->SetSensorConfiguration(config); + status_ = hw_interface_->set_sensor_configuration(config); } if (status_ != Status::OK) { @@ -45,22 +45,22 @@ VelodyneHwInterfaceWrapper::VelodyneHwInterfaceWrapper( status_ = Status::OK; } -void VelodyneHwInterfaceWrapper::OnConfigChange( +void VelodyneHwInterfaceWrapper::on_config_change( const std::shared_ptr & new_config) { - hw_interface_->InitializeSensorConfiguration(new_config); - hw_interface_->InitHttpClient(); + hw_interface_->initialize_sensor_configuration(new_config); + hw_interface_->init_http_client(); if (setup_sensor_) { - hw_interface_->SetSensorConfiguration(new_config); + hw_interface_->set_sensor_configuration(new_config); } } -Status VelodyneHwInterfaceWrapper::Status() +Status VelodyneHwInterfaceWrapper::status() { return status_; } -std::shared_ptr VelodyneHwInterfaceWrapper::HwInterface() const +std::shared_ptr VelodyneHwInterfaceWrapper::hw_interface() const { return hw_interface_; } diff --git a/nebula_ros/src/velodyne/hw_monitor_wrapper.cpp b/nebula_ros/src/velodyne/hw_monitor_wrapper.cpp index 94365d025..6ba2a0c0e 100644 --- a/nebula_ros/src/velodyne/hw_monitor_wrapper.cpp +++ b/nebula_ros/src/velodyne/hw_monitor_wrapper.cpp @@ -22,19 +22,19 @@ VelodyneHwMonitorWrapper::VelodyneHwMonitorWrapper( parent_node->declare_parameter("advanced_diagnostics", param_read_only()); std::cout << "Get model name and serial." << std::endl; - auto str = hw_interface_->GetSnapshot(); - current_snapshot_time.reset(new rclcpp::Time(parent_node_->now())); - current_snapshot_tree = - std::make_shared(hw_interface_->ParseJson(str)); - current_diag_tree = - std::make_shared(current_snapshot_tree->get_child("diag")); - current_status_tree = - std::make_shared(current_snapshot_tree->get_child("status")); - current_snapshot.reset(new std::string(str)); + auto str = hw_interface_->get_snapshot(); + current_snapshot_time_.reset(new rclcpp::Time(parent_node_->now())); + current_snapshot_tree_ = + std::make_shared(hw_interface_->parse_json(str)); + current_diag_tree_ = + std::make_shared(current_snapshot_tree_->get_child("diag")); + current_status_tree_ = + std::make_shared(current_snapshot_tree_->get_child("status")); + current_snapshot_.reset(new std::string(str)); try { - info_model_ = GetPtreeValue(current_snapshot_tree, mtx_snapshot_, key_info_model); - info_serial_ = GetPtreeValue(current_snapshot_tree, mtx_snapshot_, key_info_serial); + info_model_ = get_ptree_value(current_snapshot_tree_, mtx_snapshot_, key_info_model); + info_serial_ = get_ptree_value(current_snapshot_tree_, mtx_snapshot_, key_info_serial); RCLCPP_INFO_STREAM(logger_, "Model: " << info_model_); RCLCPP_INFO_STREAM(logger_, "Serial: " << info_serial_); } catch (boost::bad_lexical_cast & ex) { @@ -42,10 +42,10 @@ VelodyneHwMonitorWrapper::VelodyneHwMonitorWrapper( return; } - InitializeVelodyneDiagnostics(); + initialize_velodyne_diagnostics(); } -void VelodyneHwMonitorWrapper::InitializeVelodyneDiagnostics() +void VelodyneHwMonitorWrapper::initialize_velodyne_diagnostics() { RCLCPP_INFO_STREAM(logger_, "InitializeVelodyneDiagnostics"); using std::chrono_literals::operator""s; @@ -57,114 +57,115 @@ void VelodyneHwMonitorWrapper::InitializeVelodyneDiagnostics() if (show_advanced_diagnostics_) { diagnostics_updater_.add( "velodyne_snapshot-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckSnapshot); + &VelodyneHwMonitorWrapper::velodyne_check_snapshot); diagnostics_updater_.add( "velodyne_volt_temp_top_hv-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckTopHv); + &VelodyneHwMonitorWrapper::velodyne_check_top_hv); if (sensor_configuration_->sensor_model != nebula::drivers::SensorModel::VELODYNE_VLP16) { diagnostics_updater_.add( "velodyne_volt_temp_top_ad_temp-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckTopAdTemp); + &VelodyneHwMonitorWrapper::velodyne_check_top_ad_temp); } diagnostics_updater_.add( "velodyne_volt_temp_top_lm20_temp-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckTopLm20Temp); + &VelodyneHwMonitorWrapper::velodyne_check_top_lm20_temp); diagnostics_updater_.add( "velodyne_volt_temp_top_pwr_5v-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckTopPwr5v); + &VelodyneHwMonitorWrapper::velodyne_check_top_pwr5v); diagnostics_updater_.add( "velodyne_volt_temp_top_pwr_2_5v-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckTopPwr25v); + &VelodyneHwMonitorWrapper::velodyne_check_top_pwr25v); diagnostics_updater_.add( "velodyne_volt_temp_top_pwr_3_3v-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckTopPwr33v); + &VelodyneHwMonitorWrapper::velodyne_check_top_pwr33v); diagnostics_updater_.add( "velodyne_volt_temp_top_pwr_raw-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckTopPwrRaw); + &VelodyneHwMonitorWrapper::velodyne_check_top_pwr_raw); diagnostics_updater_.add( "velodyne_volt_temp_top_pwr_vccint-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckTopPwrVccint); + &VelodyneHwMonitorWrapper::velodyne_check_top_pwr_vccint); diagnostics_updater_.add( "velodyne_volt_temp_bot_i_out-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckBotIOut); + &VelodyneHwMonitorWrapper::velodyne_check_bot_i_out); diagnostics_updater_.add( "velodyne_volt_temp_bot_pwr_1_2v-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckBotPwr12v); + &VelodyneHwMonitorWrapper::velodyne_check_bot_pwr12v); diagnostics_updater_.add( "velodyne_volt_temp_bot_lm20_temp-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckBotLm20Temp); + &VelodyneHwMonitorWrapper::velodyne_check_bot_lm20_temp); diagnostics_updater_.add( "velodyne_volt_temp_bot_pwr_5v-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckBotPwr5v); + &VelodyneHwMonitorWrapper::velodyne_check_bot_pwr5v); diagnostics_updater_.add( "velodyne_volt_temp_bot_pwr_2_5v-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckBotPwr25v); + &VelodyneHwMonitorWrapper::velodyne_check_bot_pwr25v); diagnostics_updater_.add( "velodyne_volt_temp_bot_pwr_3_3v-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckBotPwr33v); + &VelodyneHwMonitorWrapper::velodyne_check_bot_pwr33v); diagnostics_updater_.add( "velodyne_volt_temp_bot_pwr_v_in-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckBotPwrVIn); + &VelodyneHwMonitorWrapper::velodyne_check_bot_pwr_v_in); diagnostics_updater_.add( "velodyne_volt_temp_bot_pwr_1_25v-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckBotPwr125v); + &VelodyneHwMonitorWrapper::velodyne_check_bot_pwr125v); diagnostics_updater_.add( "velodyne_vhv-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckVhv); + &VelodyneHwMonitorWrapper::velodyne_check_vhv); diagnostics_updater_.add( "velodyne_adc_nf-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckAdcNf); + &VelodyneHwMonitorWrapper::velodyne_check_adc_nf); diagnostics_updater_.add( "velodyne_adc_stats-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckAdcStats); + &VelodyneHwMonitorWrapper::velodyne_check_adc_stats); diagnostics_updater_.add( "velodyne_ixe-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckIxe); + &VelodyneHwMonitorWrapper::velodyne_check_ixe); diagnostics_updater_.add( "velodyne_adctp_stat-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckAdctpStat); + &VelodyneHwMonitorWrapper::velodyne_check_adctp_stat); diagnostics_updater_.add( "velodyne_status_gps_pps_state-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckGpsPpsState); + &VelodyneHwMonitorWrapper::velodyne_check_gps_pps_state); diagnostics_updater_.add( "velodyne_status_gps_pps_position-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckGpsPosition); + &VelodyneHwMonitorWrapper::velodyne_check_gps_position); diagnostics_updater_.add( "velodyne_status_motor_state-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckMotorState); + &VelodyneHwMonitorWrapper::velodyne_check_motor_state); diagnostics_updater_.add( "velodyne_status_motor_rpm-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckMotorRpm); + &VelodyneHwMonitorWrapper::velodyne_check_motor_rpm); diagnostics_updater_.add( "velodyne_status_motor_lock-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckMotorLock); + &VelodyneHwMonitorWrapper::velodyne_check_motor_lock); diagnostics_updater_.add( "velodyne_status_motor_phase-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckMotorPhase); + &VelodyneHwMonitorWrapper::velodyne_check_motor_phase); diagnostics_updater_.add( "velodyne_status_laser_state-" + sensor_configuration_->frame_id, this, - &VelodyneHwMonitorWrapper::VelodyneCheckLaserState); + &VelodyneHwMonitorWrapper::velodyne_check_laser_state); } - diagnostics_updater_.add("velodyne_status", this, &VelodyneHwMonitorWrapper::VelodyneCheckStatus); - diagnostics_updater_.add("velodyne_pps", this, &VelodyneHwMonitorWrapper::VelodyneCheckPps); diagnostics_updater_.add( - "velodyne_temperature", this, &VelodyneHwMonitorWrapper::VelodyneCheckTemperature); - diagnostics_updater_.add("velodyne_rpm", this, &VelodyneHwMonitorWrapper::VelodyneCheckRpm); + "velodyne_status", this, &VelodyneHwMonitorWrapper::velodyne_check_status); + diagnostics_updater_.add("velodyne_pps", this, &VelodyneHwMonitorWrapper::velodyne_check_pps); diagnostics_updater_.add( - "velodyne_voltage", this, &VelodyneHwMonitorWrapper::VelodyneCheckVoltage); + "velodyne_temperature", this, &VelodyneHwMonitorWrapper::velodyne_check_temperature); + diagnostics_updater_.add("velodyne_rpm", this, &VelodyneHwMonitorWrapper::velodyne_check_rpm); + diagnostics_updater_.add( + "velodyne_voltage", this, &VelodyneHwMonitorWrapper::velodyne_check_voltage); { std::lock_guard lock(mtx_snapshot_); - current_snapshot.reset(new std::string("")); - current_snapshot_time.reset(new rclcpp::Time(parent_node_->now())); + current_snapshot_.reset(new std::string("")); + current_snapshot_time_.reset(new rclcpp::Time(parent_node_->now())); } - current_diag_status = diagnostic_msgs::msg::DiagnosticStatus::STALE; + current_diag_status_ = diagnostic_msgs::msg::DiagnosticStatus::STALE; - auto on_timer_snapshot = [this] { OnVelodyneSnapshotTimer(); }; + auto on_timer_snapshot = [this] { on_velodyne_snapshot_timer(); }; diagnostics_snapshot_timer_ = parent_node_->create_wall_timer( std::chrono::milliseconds(diag_span_), std::move(on_timer_snapshot)); @@ -173,13 +174,13 @@ void VelodyneHwMonitorWrapper::InitializeVelodyneDiagnostics() double dif; { std::lock_guard lock(mtx_snapshot_); - dif = (now - *current_snapshot_time).seconds(); + dif = (now - *current_snapshot_time_).seconds(); } if (diag_span_ * 2.0 < dif * 1000) { - current_diag_status = diagnostic_msgs::msg::DiagnosticStatus::STALE; + current_diag_status_ = diagnostic_msgs::msg::DiagnosticStatus::STALE; RCLCPP_DEBUG_STREAM(logger_, "STALE"); } else { - current_diag_status = diagnostic_msgs::msg::DiagnosticStatus::OK; + current_diag_status_ = diagnostic_msgs::msg::DiagnosticStatus::OK; RCLCPP_DEBUG_STREAM(logger_, "OK"); } diagnostics_updater_.force_update(); @@ -188,37 +189,37 @@ void VelodyneHwMonitorWrapper::InitializeVelodyneDiagnostics() parent_node_->create_wall_timer(std::chrono::milliseconds(1000), std::move(on_timer_update)); } -void VelodyneHwMonitorWrapper::OnVelodyneSnapshotTimer() +void VelodyneHwMonitorWrapper::on_velodyne_snapshot_timer() { - auto str = hw_interface_->GetSnapshot(); - auto ptree = hw_interface_->ParseJson(str); + auto str = hw_interface_->get_snapshot(); + auto ptree = hw_interface_->parse_json(str); { std::lock_guard lock(mtx_snapshot_); - current_snapshot_time.reset(new rclcpp::Time(parent_node_->now())); - current_snapshot_tree = std::make_shared(ptree); - current_diag_tree = - std::make_shared(current_snapshot_tree->get_child("diag")); - current_status_tree = - std::make_shared(current_snapshot_tree->get_child("status")); - current_snapshot.reset(new std::string(str)); + current_snapshot_time_.reset(new rclcpp::Time(parent_node_->now())); + current_snapshot_tree_ = std::make_shared(ptree); + current_diag_tree_ = + std::make_shared(current_snapshot_tree_->get_child("diag")); + current_status_tree_ = + std::make_shared(current_snapshot_tree_->get_child("status")); + current_snapshot_.reset(new std::string(str)); } } -void VelodyneHwMonitorWrapper::OnVelodyneDiagnosticsTimer() +void VelodyneHwMonitorWrapper::on_velodyne_diagnostics_timer() { std::cout << "OnVelodyneDiagnosticsTimer" << std::endl; - auto str = hw_interface_->GetDiag(); + auto str = hw_interface_->get_diag(); { std::lock_guard lock(mtx_diag_); - current_diag_tree = - std::make_shared(hw_interface_->ParseJson(str)); + current_diag_tree_ = + std::make_shared(hw_interface_->parse_json(str)); } diagnostics_updater_.force_update(); } -std::tuple VelodyneHwMonitorWrapper::VelodyneGetTopHv() +std::tuple VelodyneHwMonitorWrapper::velodyne_get_top_hv() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -227,16 +228,16 @@ std::tuple VelodyneHwMonitorWrapper::Ve try { double val = 0.0; val = boost::lexical_cast( - GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_top_hv)); + get_ptree_value(current_diag_tree_, mtx_diag_, key_volt_temp_top_hv)); val = 101.0 * (val * 5.0 / 4096.0 - 5.0); if (val < -150.0) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_top_hv + message_sep + voltage_low_message; + error_mes = name_volt_temp_top_hv + message_sep_ + voltage_low_message; } else if (-132.0 < val) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_top_hv + message_sep + voltage_high_message; + error_mes = name_volt_temp_top_hv + message_sep_ + voltage_high_message; } - mes = GetFixedPrecisionString(val) + " V"; + mes = get_fixed_precision_string(val) + " V"; } catch (boost::bad_lexical_cast & ex) { not_ex = false; level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; @@ -245,7 +246,8 @@ std::tuple VelodyneHwMonitorWrapper::Ve return std::make_tuple(not_ex, level, mes, error_mes); } -std::tuple VelodyneHwMonitorWrapper::VelodyneGetTopAdTemp() +std::tuple +VelodyneHwMonitorWrapper::velodyne_get_top_ad_temp() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -254,9 +256,9 @@ std::tuple VelodyneHwMonitorWrapper::Ve try { double val = 0.0; val = boost::lexical_cast( - GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_top_ad_temp)); + get_ptree_value(current_diag_tree_, mtx_diag_, key_volt_temp_top_ad_temp)); val = val * 5.0 / 4096.0; - mes = GetFixedPrecisionString(val) + " V"; + mes = get_fixed_precision_string(val) + " V"; } catch (boost::bad_lexical_cast & ex) { not_ex = false; level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; @@ -266,7 +268,7 @@ std::tuple VelodyneHwMonitorWrapper::Ve } std::tuple -VelodyneHwMonitorWrapper::VelodyneGetTopLm20Temp() +VelodyneHwMonitorWrapper::velodyne_get_top_lm20_temp() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -275,17 +277,17 @@ VelodyneHwMonitorWrapper::VelodyneGetTopLm20Temp() try { double val = 0.0; val = boost::lexical_cast( - GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_top_lm20_temp)); + get_ptree_value(current_diag_tree_, mtx_diag_, key_volt_temp_top_lm20_temp)); val = -1481.96 + std::sqrt(2.1962e6 + ((1.8639 - val * 5.0 / 4096.0) / 3.88e-6)); if (val < -25.0) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_top_lm20_temp + message_sep + temperature_cold_message; + error_mes = name_volt_temp_top_lm20_temp + message_sep_ + temperature_cold_message; } else if (90.0 < val) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_top_lm20_temp + message_sep + temperature_hot_message; + error_mes = name_volt_temp_top_lm20_temp + message_sep_ + temperature_hot_message; } // mes = boost::lexical_cast(val) + " C"; - mes = GetFixedPrecisionString(val) + " C"; + mes = get_fixed_precision_string(val) + " C"; } catch (boost::bad_lexical_cast & ex) { not_ex = false; level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; @@ -294,7 +296,8 @@ VelodyneHwMonitorWrapper::VelodyneGetTopLm20Temp() return std::make_tuple(not_ex, level, mes, error_mes); } -std::tuple VelodyneHwMonitorWrapper::VelodyneGetTopPwr5v() +std::tuple +VelodyneHwMonitorWrapper::velodyne_get_top_pwr5v() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -303,16 +306,16 @@ std::tuple VelodyneHwMonitorWrapper::Ve try { double val = 0.0; val = boost::lexical_cast( - GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_top_pwr_5v)); + get_ptree_value(current_diag_tree_, mtx_diag_, key_volt_temp_top_pwr_5v)); val = 2.0 * val * 5.0 / 4096.0; if (val < 4.8) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_top_pwr_5v + message_sep + voltage_low_message; + error_mes = name_volt_temp_top_pwr_5v + message_sep_ + voltage_low_message; } else if (5.2 < val) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_top_pwr_5v + message_sep + voltage_high_message; + error_mes = name_volt_temp_top_pwr_5v + message_sep_ + voltage_high_message; } - mes = GetFixedPrecisionString(val) + " V"; + mes = get_fixed_precision_string(val) + " V"; } catch (boost::bad_lexical_cast & ex) { not_ex = false; level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; @@ -321,7 +324,8 @@ std::tuple VelodyneHwMonitorWrapper::Ve return std::make_tuple(not_ex, level, mes, error_mes); } -std::tuple VelodyneHwMonitorWrapper::VelodyneGetTopPwr25v() +std::tuple +VelodyneHwMonitorWrapper::velodyne_get_top_pwr25v() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -330,16 +334,16 @@ std::tuple VelodyneHwMonitorWrapper::Ve try { double val = 0.0; val = boost::lexical_cast( - GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_top_pwr_2_5v)); + get_ptree_value(current_diag_tree_, mtx_diag_, key_volt_temp_top_pwr_2_5v)); val = val * 5.0 / 4096.0; if (val < 2.3) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_top_pwr_2_5v + message_sep + voltage_low_message; + error_mes = name_volt_temp_top_pwr_2_5v + message_sep_ + voltage_low_message; } else if (2.7 < val) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_top_pwr_2_5v + message_sep + voltage_high_message; + error_mes = name_volt_temp_top_pwr_2_5v + message_sep_ + voltage_high_message; } - mes = GetFixedPrecisionString(val) + " V"; + mes = get_fixed_precision_string(val) + " V"; } catch (boost::bad_lexical_cast & ex) { not_ex = false; level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; @@ -348,7 +352,8 @@ std::tuple VelodyneHwMonitorWrapper::Ve return std::make_tuple(not_ex, level, mes, error_mes); } -std::tuple VelodyneHwMonitorWrapper::VelodyneGetTopPwr33v() +std::tuple +VelodyneHwMonitorWrapper::velodyne_get_top_pwr33v() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -357,16 +362,16 @@ std::tuple VelodyneHwMonitorWrapper::Ve try { double val = 0.0; val = boost::lexical_cast( - GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_top_pwr_3_3v)); + get_ptree_value(current_diag_tree_, mtx_diag_, key_volt_temp_top_pwr_3_3v)); val = val * 5.0 / 4096.0; if (val < 3.1) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_top_pwr_3_3v + message_sep + voltage_low_message; + error_mes = name_volt_temp_top_pwr_3_3v + message_sep_ + voltage_low_message; } else if (3.5 < val) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_top_pwr_3_3v + message_sep + voltage_high_message; + error_mes = name_volt_temp_top_pwr_3_3v + message_sep_ + voltage_high_message; } - mes = GetFixedPrecisionString(val) + " V"; + mes = get_fixed_precision_string(val) + " V"; } catch (boost::bad_lexical_cast & ex) { not_ex = false; level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; @@ -376,7 +381,7 @@ std::tuple VelodyneHwMonitorWrapper::Ve } std::tuple -VelodyneHwMonitorWrapper::VelodyneGetTopPwr5vRaw() +VelodyneHwMonitorWrapper::velodyne_get_top_pwr5v_raw() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -385,16 +390,16 @@ VelodyneHwMonitorWrapper::VelodyneGetTopPwr5vRaw() try { double val = 0.0; val = boost::lexical_cast( - GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_top_pwr_5v_raw)); + get_ptree_value(current_diag_tree_, mtx_diag_, key_volt_temp_top_pwr_5v_raw)); val = 2.0 * val * 5.0 / 4096.0; if (val < 2.3) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_top_pwr_5v_raw + message_sep + voltage_low_message; + error_mes = name_volt_temp_top_pwr_5v_raw + message_sep_ + voltage_low_message; } else if (2.7 < val) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_top_pwr_5v_raw + message_sep + voltage_high_message; + error_mes = name_volt_temp_top_pwr_5v_raw + message_sep_ + voltage_high_message; } - mes = GetFixedPrecisionString(val) + " V"; + mes = get_fixed_precision_string(val) + " V"; } catch (boost::bad_lexical_cast & ex) { not_ex = false; level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; @@ -403,7 +408,8 @@ VelodyneHwMonitorWrapper::VelodyneGetTopPwr5vRaw() return std::make_tuple(not_ex, level, mes, error_mes); } -std::tuple VelodyneHwMonitorWrapper::VelodyneGetTopPwrRaw() +std::tuple +VelodyneHwMonitorWrapper::velodyne_get_top_pwr_raw() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -412,16 +418,16 @@ std::tuple VelodyneHwMonitorWrapper::Ve try { double val = 0.0; val = boost::lexical_cast( - GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_top_pwr_raw)); + get_ptree_value(current_diag_tree_, mtx_diag_, key_volt_temp_top_pwr_raw)); val = val * 5.0 / 4096.0; if (val < 1.6) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_top_pwr_raw + message_sep + voltage_low_message; + error_mes = name_volt_temp_top_pwr_raw + message_sep_ + voltage_low_message; } else if (1.9 < val) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_top_pwr_raw + message_sep + voltage_high_message; + error_mes = name_volt_temp_top_pwr_raw + message_sep_ + voltage_high_message; } - mes = GetFixedPrecisionString(val) + " V"; + mes = get_fixed_precision_string(val) + " V"; } catch (boost::bad_lexical_cast & ex) { not_ex = false; level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; @@ -431,7 +437,7 @@ std::tuple VelodyneHwMonitorWrapper::Ve } std::tuple -VelodyneHwMonitorWrapper::VelodyneGetTopPwrVccint() +VelodyneHwMonitorWrapper::velodyne_get_top_pwr_vccint() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -440,17 +446,17 @@ VelodyneHwMonitorWrapper::VelodyneGetTopPwrVccint() try { double val = 0.0; val = boost::lexical_cast( - GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_top_pwr_vccint)); + get_ptree_value(current_diag_tree_, mtx_diag_, key_volt_temp_top_pwr_vccint)); val = val * 5.0 / 4096.0; if (val < 1.0) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_top_pwr_vccint + message_sep + voltage_low_message; + error_mes = name_volt_temp_top_pwr_vccint + message_sep_ + voltage_low_message; } else if (1.4 < val) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_top_pwr_vccint + message_sep + voltage_high_message; + error_mes = name_volt_temp_top_pwr_vccint + message_sep_ + voltage_high_message; } // mes = boost::lexical_cast(val) + " V"; - mes = GetFixedPrecisionString(val) + " V"; + mes = get_fixed_precision_string(val) + " V"; } catch (boost::bad_lexical_cast & ex) { not_ex = false; level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; @@ -459,7 +465,8 @@ VelodyneHwMonitorWrapper::VelodyneGetTopPwrVccint() return std::make_tuple(not_ex, level, mes, error_mes); } -std::tuple VelodyneHwMonitorWrapper::VelodyneGetBotIOut() +std::tuple +VelodyneHwMonitorWrapper::velodyne_get_bot_i_out() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -468,16 +475,16 @@ std::tuple VelodyneHwMonitorWrapper::Ve try { double val = 0.0; val = boost::lexical_cast( - GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_bot_i_out)); + get_ptree_value(current_diag_tree_, mtx_diag_, key_volt_temp_bot_i_out)); val = 10.0 * (val * 5.0 / 4096.0 - 2.5); if (val < 0.3) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_bot_i_out + message_sep + ampere_low_message; + error_mes = name_volt_temp_bot_i_out + message_sep_ + ampere_low_message; } else if (1.0 < val) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_bot_i_out + message_sep + ampere_high_message; + error_mes = name_volt_temp_bot_i_out + message_sep_ + ampere_high_message; } - mes = GetFixedPrecisionString(val) + " A"; + mes = get_fixed_precision_string(val) + " A"; } catch (boost::bad_lexical_cast & ex) { not_ex = false; level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; @@ -486,7 +493,8 @@ std::tuple VelodyneHwMonitorWrapper::Ve return std::make_tuple(not_ex, level, mes, error_mes); } -std::tuple VelodyneHwMonitorWrapper::VelodyneGetBotPwr12v() +std::tuple +VelodyneHwMonitorWrapper::velodyne_get_bot_pwr12v() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -495,16 +503,16 @@ std::tuple VelodyneHwMonitorWrapper::Ve try { double val = 0.0; val = boost::lexical_cast( - GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_bot_pwr_1_2v)); + get_ptree_value(current_diag_tree_, mtx_diag_, key_volt_temp_bot_pwr_1_2v)); val = val * 5.0 / 4096.0; if (val < 1.0) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_bot_pwr_1_2v + message_sep + voltage_low_message; + error_mes = name_volt_temp_bot_pwr_1_2v + message_sep_ + voltage_low_message; } else if (1.4 < val) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_bot_pwr_1_2v + message_sep + voltage_high_message; + error_mes = name_volt_temp_bot_pwr_1_2v + message_sep_ + voltage_high_message; } - mes = GetFixedPrecisionString(val) + " V"; + mes = get_fixed_precision_string(val) + " V"; } catch (boost::bad_lexical_cast & ex) { not_ex = false; level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; @@ -514,7 +522,7 @@ std::tuple VelodyneHwMonitorWrapper::Ve } std::tuple -VelodyneHwMonitorWrapper::VelodyneGetBotLm20Temp() +VelodyneHwMonitorWrapper::velodyne_get_bot_lm20_temp() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -523,17 +531,17 @@ VelodyneHwMonitorWrapper::VelodyneGetBotLm20Temp() try { double val = 0.0; val = boost::lexical_cast( - GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_bot_lm20_temp)); + get_ptree_value(current_diag_tree_, mtx_diag_, key_volt_temp_bot_lm20_temp)); val = -1481.96 + std::sqrt(2.1962e6 + ((1.8639 - val * 5.0 / 4096.0) / 3.88e-6)); if (val < -25.0) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_bot_lm20_temp + message_sep + temperature_cold_message; + error_mes = name_volt_temp_bot_lm20_temp + message_sep_ + temperature_cold_message; } else if (90.0 < val) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_bot_lm20_temp + message_sep + temperature_hot_message; + error_mes = name_volt_temp_bot_lm20_temp + message_sep_ + temperature_hot_message; } // mes = boost::lexical_cast(val) + " C"; - mes = GetFixedPrecisionString(val) + " C"; + mes = get_fixed_precision_string(val) + " C"; } catch (boost::bad_lexical_cast & ex) { not_ex = false; level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; @@ -542,7 +550,8 @@ VelodyneHwMonitorWrapper::VelodyneGetBotLm20Temp() return std::make_tuple(not_ex, level, mes, error_mes); } -std::tuple VelodyneHwMonitorWrapper::VelodyneGetBotPwr5v() +std::tuple +VelodyneHwMonitorWrapper::velodyne_get_bot_pwr5v() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -551,16 +560,16 @@ std::tuple VelodyneHwMonitorWrapper::Ve try { double val = 0.0; val = boost::lexical_cast( - GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_bot_pwr_5v)); + get_ptree_value(current_diag_tree_, mtx_diag_, key_volt_temp_bot_pwr_5v)); val = 2.0 * val * 5.0 / 4096.0; if (val < 4.8) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_bot_pwr_5v + message_sep + voltage_low_message; + error_mes = name_volt_temp_bot_pwr_5v + message_sep_ + voltage_low_message; } else if (5.2 < val) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_bot_pwr_5v + message_sep + voltage_high_message; + error_mes = name_volt_temp_bot_pwr_5v + message_sep_ + voltage_high_message; } - mes = GetFixedPrecisionString(val) + " V"; + mes = get_fixed_precision_string(val) + " V"; } catch (boost::bad_lexical_cast & ex) { not_ex = false; level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; @@ -569,7 +578,8 @@ std::tuple VelodyneHwMonitorWrapper::Ve return std::make_tuple(not_ex, level, mes, error_mes); } -std::tuple VelodyneHwMonitorWrapper::VelodyneGetBotPwr25v() +std::tuple +VelodyneHwMonitorWrapper::velodyne_get_bot_pwr25v() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -578,16 +588,16 @@ std::tuple VelodyneHwMonitorWrapper::Ve try { double val = 0.0; val = boost::lexical_cast( - GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_bot_pwr_2_5v)); + get_ptree_value(current_diag_tree_, mtx_diag_, key_volt_temp_bot_pwr_2_5v)); val = val * 5.0 / 4096.0; if (val < 2.3) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_bot_pwr_2_5v + message_sep + voltage_low_message; + error_mes = name_volt_temp_bot_pwr_2_5v + message_sep_ + voltage_low_message; } else if (2.7 < val) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_bot_pwr_2_5v + message_sep + voltage_high_message; + error_mes = name_volt_temp_bot_pwr_2_5v + message_sep_ + voltage_high_message; } - mes = GetFixedPrecisionString(val) + " V"; + mes = get_fixed_precision_string(val) + " V"; } catch (boost::bad_lexical_cast & ex) { not_ex = false; level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; @@ -596,7 +606,8 @@ std::tuple VelodyneHwMonitorWrapper::Ve return std::make_tuple(not_ex, level, mes, error_mes); } -std::tuple VelodyneHwMonitorWrapper::VelodyneGetBotPwr33v() +std::tuple +VelodyneHwMonitorWrapper::velodyne_get_bot_pwr33v() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -605,16 +616,16 @@ std::tuple VelodyneHwMonitorWrapper::Ve try { double val = 0.0; val = boost::lexical_cast( - GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_bot_pwr_3_3v)); + get_ptree_value(current_diag_tree_, mtx_diag_, key_volt_temp_bot_pwr_3_3v)); val = val * 5.0 / 4096.0; if (val < 3.1) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_bot_pwr_3_3v + message_sep + voltage_low_message; + error_mes = name_volt_temp_bot_pwr_3_3v + message_sep_ + voltage_low_message; } else if (3.5 < val) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_bot_pwr_3_3v + message_sep + voltage_high_message; + error_mes = name_volt_temp_bot_pwr_3_3v + message_sep_ + voltage_high_message; } - mes = GetFixedPrecisionString(val) + " V"; + mes = get_fixed_precision_string(val) + " V"; } catch (boost::bad_lexical_cast & ex) { not_ex = false; level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; @@ -623,7 +634,8 @@ std::tuple VelodyneHwMonitorWrapper::Ve return std::make_tuple(not_ex, level, mes, error_mes); } -std::tuple VelodyneHwMonitorWrapper::VelodyneGetBotPwrVIn() +std::tuple +VelodyneHwMonitorWrapper::velodyne_get_bot_pwr_v_in() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -632,16 +644,16 @@ std::tuple VelodyneHwMonitorWrapper::Ve try { double val = 0.0; val = boost::lexical_cast( - GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_bot_pwr_v_in)); + get_ptree_value(current_diag_tree_, mtx_diag_, key_volt_temp_bot_pwr_v_in)); val = 11.0 * val * 5.0 / 4096.0; if (val < 8.0) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_bot_pwr_v_in + message_sep + voltage_low_message; + error_mes = name_volt_temp_bot_pwr_v_in + message_sep_ + voltage_low_message; } else if (19.0 < val) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_bot_pwr_v_in + message_sep + voltage_high_message; + error_mes = name_volt_temp_bot_pwr_v_in + message_sep_ + voltage_high_message; } - mes = GetFixedPrecisionString(val) + " V"; + mes = get_fixed_precision_string(val) + " V"; } catch (boost::bad_lexical_cast & ex) { not_ex = false; level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; @@ -651,7 +663,7 @@ std::tuple VelodyneHwMonitorWrapper::Ve } std::tuple -VelodyneHwMonitorWrapper::VelodyneGetBotPwr125v() +VelodyneHwMonitorWrapper::velodyne_get_bot_pwr125v() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -660,16 +672,16 @@ VelodyneHwMonitorWrapper::VelodyneGetBotPwr125v() try { double val = 0.0; val = boost::lexical_cast( - GetPtreeValue(current_diag_tree, mtx_diag_, key_volt_temp_bot_pwr_1_25v)); + get_ptree_value(current_diag_tree_, mtx_diag_, key_volt_temp_bot_pwr_1_25v)); val = val * 5.0 / 4096.0; if (val < 1.0) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_bot_pwr_1_25v + message_sep + voltage_low_message; + error_mes = name_volt_temp_bot_pwr_1_25v + message_sep_ + voltage_low_message; } else if (1.4 < val) { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - error_mes = name_volt_temp_bot_pwr_1_25v + message_sep + voltage_high_message; + error_mes = name_volt_temp_bot_pwr_1_25v + message_sep_ + voltage_high_message; } - mes = GetFixedPrecisionString(val) + " V"; + mes = get_fixed_precision_string(val) + " V"; } catch (boost::bad_lexical_cast & ex) { not_ex = false; level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; @@ -678,7 +690,7 @@ VelodyneHwMonitorWrapper::VelodyneGetBotPwr125v() return std::make_tuple(not_ex, level, mes, error_mes); } -std::tuple VelodyneHwMonitorWrapper::VelodyneGetVhv() +std::tuple VelodyneHwMonitorWrapper::velodyne_get_vhv() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -686,7 +698,7 @@ std::tuple VelodyneHwMonitorWrapper::Ve std::string error_mes; try { double val = 0.0; - val = boost::lexical_cast(GetPtreeValue(current_diag_tree, mtx_diag_, key_vhv)); + val = boost::lexical_cast(get_ptree_value(current_diag_tree_, mtx_diag_, key_vhv)); mes = boost::lexical_cast(val); } catch (boost::bad_lexical_cast & ex) { not_ex = false; @@ -696,7 +708,7 @@ std::tuple VelodyneHwMonitorWrapper::Ve return std::make_tuple(not_ex, level, mes, error_mes); } -std::tuple VelodyneHwMonitorWrapper::VelodyneGetAdcNf() +std::tuple VelodyneHwMonitorWrapper::velodyne_get_adc_nf() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -705,7 +717,7 @@ std::tuple VelodyneHwMonitorWrapper::Ve try { std::ostringstream os; boost::optional child = - current_diag_tree->get_child_optional(key_adc_nf); + current_diag_tree_->get_child_optional(key_adc_nf); if (child) { std::ostringstream os; for (auto v = child->begin(); v != child->end(); ++v) { @@ -723,7 +735,8 @@ std::tuple VelodyneHwMonitorWrapper::Ve return std::make_tuple(not_ex, level, mes, error_mes); } -std::tuple VelodyneHwMonitorWrapper::VelodyneGetAdcStats() +std::tuple +VelodyneHwMonitorWrapper::velodyne_get_adc_stats() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -732,7 +745,7 @@ std::tuple VelodyneHwMonitorWrapper::Ve try { std::ostringstream os; boost::optional child = - current_diag_tree->get_child_optional(key_adc_stats); + current_diag_tree_->get_child_optional(key_adc_stats); if (child) { std::ostringstream os; for (auto v = child->begin(); v != child->end(); ++v) { @@ -753,14 +766,14 @@ std::tuple VelodyneHwMonitorWrapper::Ve return std::make_tuple(not_ex, level, mes, error_mes); } -std::tuple VelodyneHwMonitorWrapper::VelodyneGetIxe() +std::tuple VelodyneHwMonitorWrapper::velodyne_get_ixe() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::string mes; std::string error_mes; try { - mes = GetPtreeValue(current_diag_tree, mtx_diag_, key_ixe); + mes = get_ptree_value(current_diag_tree_, mtx_diag_, key_ixe); } catch (boost::bad_lexical_cast & ex) { not_ex = false; level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; @@ -769,7 +782,8 @@ std::tuple VelodyneHwMonitorWrapper::Ve return std::make_tuple(not_ex, level, mes, error_mes); } -std::tuple VelodyneHwMonitorWrapper::VelodyneGetAdctpStat() +std::tuple +VelodyneHwMonitorWrapper::velodyne_get_adctp_stat() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; @@ -778,7 +792,7 @@ std::tuple VelodyneHwMonitorWrapper::Ve try { std::ostringstream os; boost::optional child = - current_diag_tree->get_child_optional(key_adctp_stat); + current_diag_tree_->get_child_optional(key_adctp_stat); if (child) { std::ostringstream os; for (auto v = child->begin(); v != child->end(); ++v) { @@ -797,14 +811,14 @@ std::tuple VelodyneHwMonitorWrapper::Ve } std::tuple -VelodyneHwMonitorWrapper::VelodyneGetGpsPpsState() +VelodyneHwMonitorWrapper::velodyne_get_gps_pps_state() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::string mes; std::string error_mes; try { - mes = GetPtreeValue(current_status_tree, mtx_status_, key_status_gps_pps_state); + mes = get_ptree_value(current_status_tree_, mtx_status_, key_status_gps_pps_state); if (mes == "Absent") { level = diagnostic_msgs::msg::DiagnosticStatus::WARN; error_mes = mes; @@ -821,14 +835,14 @@ VelodyneHwMonitorWrapper::VelodyneGetGpsPpsState() } std::tuple -VelodyneHwMonitorWrapper::VelodyneGetGpsPosition() +VelodyneHwMonitorWrapper::velodyne_get_gps_position() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::string mes; std::string error_mes; try { - mes = GetPtreeValue(current_status_tree, mtx_status_, key_status_gps_pps_position); + mes = get_ptree_value(current_status_tree_, mtx_status_, key_status_gps_pps_position); } catch (boost::bad_lexical_cast & ex) { not_ex = false; level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; @@ -838,14 +852,14 @@ VelodyneHwMonitorWrapper::VelodyneGetGpsPosition() } std::tuple -VelodyneHwMonitorWrapper::VelodyneGetMotorState() +VelodyneHwMonitorWrapper::velodyne_get_motor_state() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::string mes; std::string error_mes; try { - mes = GetPtreeValue(current_status_tree, mtx_status_, key_status_motor_state); + mes = get_ptree_value(current_status_tree_, mtx_status_, key_status_motor_state); } catch (boost::bad_lexical_cast & ex) { not_ex = false; level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; @@ -854,14 +868,15 @@ VelodyneHwMonitorWrapper::VelodyneGetMotorState() return std::make_tuple(not_ex, level, mes, error_mes); } -std::tuple VelodyneHwMonitorWrapper::VelodyneGetMotorRpm() +std::tuple +VelodyneHwMonitorWrapper::velodyne_get_motor_rpm() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::string mes; std::string error_mes; try { - mes = GetPtreeValue(current_status_tree, mtx_status_, key_status_motor_rpm); + mes = get_ptree_value(current_status_tree_, mtx_status_, key_status_motor_rpm); } catch (boost::bad_lexical_cast & ex) { not_ex = false; level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; @@ -870,14 +885,15 @@ std::tuple VelodyneHwMonitorWrapper::Ve return std::make_tuple(not_ex, level, mes, error_mes); } -std::tuple VelodyneHwMonitorWrapper::VelodyneGetMotorLock() +std::tuple +VelodyneHwMonitorWrapper::velodyne_get_motor_lock() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::string mes; std::string error_mes; try { - mes = GetPtreeValue(current_status_tree, mtx_status_, key_status_motor_lock); + mes = get_ptree_value(current_status_tree_, mtx_status_, key_status_motor_lock); } catch (boost::bad_lexical_cast & ex) { not_ex = false; level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; @@ -887,14 +903,14 @@ std::tuple VelodyneHwMonitorWrapper::Ve } std::tuple -VelodyneHwMonitorWrapper::VelodyneGetMotorPhase() +VelodyneHwMonitorWrapper::velodyne_get_motor_phase() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::string mes; std::string error_mes; try { - mes = GetPtreeValue(current_status_tree, mtx_status_, key_status_motor_phase); + mes = get_ptree_value(current_status_tree_, mtx_status_, key_status_motor_phase); } catch (boost::bad_lexical_cast & ex) { not_ex = false; level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; @@ -904,14 +920,14 @@ VelodyneHwMonitorWrapper::VelodyneGetMotorPhase() } std::tuple -VelodyneHwMonitorWrapper::VelodyneGetLaserState() +VelodyneHwMonitorWrapper::velodyne_get_laser_state() { bool not_ex = true; uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::string mes; std::string error_mes; try { - mes = GetPtreeValue(current_status_tree, mtx_status_, key_status_laser_state); + mes = get_ptree_value(current_status_tree_, mtx_status_, key_status_laser_state); } catch (boost::bad_lexical_cast & ex) { not_ex = false; level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; @@ -920,371 +936,313 @@ VelodyneHwMonitorWrapper::VelodyneGetLaserState() return std::make_tuple(not_ex, level, mes, error_mes); } -void VelodyneHwMonitorWrapper::VelodyneCheckTopHv( +void VelodyneHwMonitorWrapper::velodyne_check_top_hv( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { - if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { - auto tpl = VelodyneGetTopHv(); + if (current_diag_tree_ && !current_diag_tree_->empty()) { + auto tpl = velodyne_get_top_hv(); diagnostics.add("sensor", sensor_configuration_->frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckTopAdTemp( +void VelodyneHwMonitorWrapper::velodyne_check_top_ad_temp( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { - if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { - auto tpl = VelodyneGetTopAdTemp(); + if (current_diag_tree_ && !current_diag_tree_->empty()) { + auto tpl = velodyne_get_top_ad_temp(); diagnostics.add("sensor", sensor_configuration_->frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckTopLm20Temp( +void VelodyneHwMonitorWrapper::velodyne_check_top_lm20_temp( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { - if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { - auto tpl = VelodyneGetTopLm20Temp(); + if (current_diag_tree_ && !current_diag_tree_->empty()) { + auto tpl = velodyne_get_top_lm20_temp(); diagnostics.add("sensor", sensor_configuration_->frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckTopPwr5v( +void VelodyneHwMonitorWrapper::velodyne_check_top_pwr5v( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { - if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { - auto tpl = VelodyneGetTopPwr5v(); + if (current_diag_tree_ && !current_diag_tree_->empty()) { + auto tpl = velodyne_get_top_pwr5v(); diagnostics.add("sensor", sensor_configuration_->frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckTopPwr25v( +void VelodyneHwMonitorWrapper::velodyne_check_top_pwr25v( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { - if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { - auto tpl = VelodyneGetTopPwr25v(); + if (current_diag_tree_ && !current_diag_tree_->empty()) { + auto tpl = velodyne_get_top_pwr25v(); diagnostics.add("sensor", sensor_configuration_->frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckTopPwr33v( +void VelodyneHwMonitorWrapper::velodyne_check_top_pwr33v( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { - if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { - auto tpl = VelodyneGetTopPwr33v(); + if (current_diag_tree_ && !current_diag_tree_->empty()) { + auto tpl = velodyne_get_top_pwr33v(); diagnostics.add("sensor", sensor_configuration_->frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckTopPwrRaw( +void VelodyneHwMonitorWrapper::velodyne_check_top_pwr_raw( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { - if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { - auto tpl = VelodyneGetTopPwrRaw(); + if (current_diag_tree_ && !current_diag_tree_->empty()) { + auto tpl = velodyne_get_top_pwr_raw(); diagnostics.add("sensor", sensor_configuration_->frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckTopPwrVccint( +void VelodyneHwMonitorWrapper::velodyne_check_top_pwr_vccint( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { - if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { - auto tpl = VelodyneGetTopPwrVccint(); + if (current_diag_tree_ && !current_diag_tree_->empty()) { + auto tpl = velodyne_get_top_pwr_vccint(); diagnostics.add("sensor", sensor_configuration_->frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckBotIOut( +void VelodyneHwMonitorWrapper::velodyne_check_bot_i_out( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { - if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { - auto tpl = VelodyneGetBotIOut(); + if (current_diag_tree_ && !current_diag_tree_->empty()) { + auto tpl = velodyne_get_bot_i_out(); diagnostics.add("sensor", sensor_configuration_->frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckBotPwr12v( +void VelodyneHwMonitorWrapper::velodyne_check_bot_pwr12v( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { - if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { - auto tpl = VelodyneGetBotPwr12v(); + if (current_diag_tree_ && !current_diag_tree_->empty()) { + auto tpl = velodyne_get_bot_pwr12v(); diagnostics.add("sensor", sensor_configuration_->frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckBotLm20Temp( +void VelodyneHwMonitorWrapper::velodyne_check_bot_lm20_temp( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { - if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { - auto tpl = VelodyneGetBotLm20Temp(); + if (current_diag_tree_ && !current_diag_tree_->empty()) { + auto tpl = velodyne_get_bot_lm20_temp(); diagnostics.add("sensor", sensor_configuration_->frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckBotPwr5v( +void VelodyneHwMonitorWrapper::velodyne_check_bot_pwr5v( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { - if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { - auto tpl = VelodyneGetBotPwr5v(); + if (current_diag_tree_ && !current_diag_tree_->empty()) { + auto tpl = velodyne_get_bot_pwr5v(); diagnostics.add("sensor", sensor_configuration_->frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckBotPwr25v( +void VelodyneHwMonitorWrapper::velodyne_check_bot_pwr25v( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { - if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { - auto tpl = VelodyneGetBotPwr25v(); + if (current_diag_tree_ && !current_diag_tree_->empty()) { + auto tpl = velodyne_get_bot_pwr25v(); diagnostics.add("sensor", sensor_configuration_->frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckBotPwr33v( +void VelodyneHwMonitorWrapper::velodyne_check_bot_pwr33v( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { - if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { - auto tpl = VelodyneGetBotPwr33v(); + if (current_diag_tree_ && !current_diag_tree_->empty()) { + auto tpl = velodyne_get_bot_pwr33v(); diagnostics.add("sensor", sensor_configuration_->frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckBotPwrVIn( +void VelodyneHwMonitorWrapper::velodyne_check_bot_pwr_v_in( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { - if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { - auto tpl = VelodyneGetBotPwrVIn(); + if (current_diag_tree_ && !current_diag_tree_->empty()) { + auto tpl = velodyne_get_bot_pwr_v_in(); diagnostics.add("sensor", sensor_configuration_->frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckBotPwr125v( +void VelodyneHwMonitorWrapper::velodyne_check_bot_pwr125v( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { - if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { - auto tpl = VelodyneGetBotPwr125v(); + if (current_diag_tree_ && !current_diag_tree_->empty()) { + auto tpl = velodyne_get_bot_pwr125v(); diagnostics.add("sensor", sensor_configuration_->frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckVhv( +void VelodyneHwMonitorWrapper::velodyne_check_vhv( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { - if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { - auto tpl = VelodyneGetVhv(); + if (current_diag_tree_ && !current_diag_tree_->empty()) { + auto tpl = velodyne_get_vhv(); diagnostics.add("sensor", sensor_configuration_->frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckAdcNf( +void VelodyneHwMonitorWrapper::velodyne_check_adc_nf( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { - if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { - auto tpl = VelodyneGetAdcNf(); + if (current_diag_tree_ && !current_diag_tree_->empty()) { + auto tpl = velodyne_get_adc_nf(); diagnostics.add("sensor", sensor_configuration_->frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckAdcStats( +void VelodyneHwMonitorWrapper::velodyne_check_adc_stats( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { - if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { - auto tpl = VelodyneGetAdcStats(); + if (current_diag_tree_ && !current_diag_tree_->empty()) { + auto tpl = velodyne_get_adc_stats(); diagnostics.add("sensor", sensor_configuration_->frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckIxe( +void VelodyneHwMonitorWrapper::velodyne_check_ixe( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { - if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { - auto tpl = VelodyneGetIxe(); + if (current_diag_tree_ && !current_diag_tree_->empty()) { + auto tpl = velodyne_get_ixe(); diagnostics.add("sensor", sensor_configuration_->frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckAdctpStat( +void VelodyneHwMonitorWrapper::velodyne_check_adctp_stat( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { - if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { - auto tpl = VelodyneGetAdctpStat(); + if (current_diag_tree_ && !current_diag_tree_->empty()) { + auto tpl = velodyne_get_adctp_stat(); diagnostics.add("sensor", sensor_configuration_->frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::OnVelodyneStatusTimer() +void VelodyneHwMonitorWrapper::on_velodyne_status_timer() { - auto str = hw_interface_->GetStatus(); + auto str = hw_interface_->get_status(); { std::lock_guard lock(mtx_status_); - current_status_tree = - std::make_shared(hw_interface_->ParseJson(str)); + current_status_tree_ = + std::make_shared(hw_interface_->parse_json(str)); } diagnostics_updater_.force_update(); } -void VelodyneHwMonitorWrapper::VelodyneCheckGpsPpsState( +void VelodyneHwMonitorWrapper::velodyne_check_gps_pps_state( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { - if ( - VelodyneHwMonitorWrapper::current_status_tree && - !VelodyneHwMonitorWrapper::current_status_tree->empty()) { - auto tpl = VelodyneGetGpsPpsState(); + if (current_status_tree_ && !current_status_tree_->empty()) { + auto tpl = velodyne_get_gps_pps_state(); diagnostics.add("sensor", sensor_configuration_->frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckGpsPosition( +void VelodyneHwMonitorWrapper::velodyne_check_gps_position( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { - if ( - VelodyneHwMonitorWrapper::current_status_tree && - !VelodyneHwMonitorWrapper::current_status_tree->empty()) { - auto tpl = VelodyneGetGpsPosition(); + if (current_status_tree_ && !current_status_tree_->empty()) { + auto tpl = velodyne_get_gps_position(); diagnostics.add("sensor", sensor_configuration_->frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckMotorState( +void VelodyneHwMonitorWrapper::velodyne_check_motor_state( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { - if ( - VelodyneHwMonitorWrapper::current_status_tree && - !VelodyneHwMonitorWrapper::current_status_tree->empty()) { - auto tpl = VelodyneGetMotorState(); + if (current_status_tree_ && !current_status_tree_->empty()) { + auto tpl = velodyne_get_motor_state(); diagnostics.add("sensor", sensor_configuration_->frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckMotorRpm( +void VelodyneHwMonitorWrapper::velodyne_check_motor_rpm( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { - if ( - VelodyneHwMonitorWrapper::current_status_tree && - !VelodyneHwMonitorWrapper::current_status_tree->empty()) { - auto tpl = VelodyneGetMotorRpm(); + if (current_status_tree_ && !current_status_tree_->empty()) { + auto tpl = velodyne_get_motor_rpm(); diagnostics.add("sensor", sensor_configuration_->frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckMotorLock( +void VelodyneHwMonitorWrapper::velodyne_check_motor_lock( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { - if ( - VelodyneHwMonitorWrapper::current_status_tree && - !VelodyneHwMonitorWrapper::current_status_tree->empty()) { - auto tpl = VelodyneGetMotorLock(); + if (current_status_tree_ && !current_status_tree_->empty()) { + auto tpl = velodyne_get_motor_lock(); diagnostics.add("sensor", sensor_configuration_->frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckMotorPhase( +void VelodyneHwMonitorWrapper::velodyne_check_motor_phase( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { - if ( - VelodyneHwMonitorWrapper::current_status_tree && - !VelodyneHwMonitorWrapper::current_status_tree->empty()) { - auto tpl = VelodyneGetMotorPhase(); + if (current_status_tree_ && !current_status_tree_->empty()) { + auto tpl = velodyne_get_motor_phase(); diagnostics.add("sensor", sensor_configuration_->frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckLaserState( +void VelodyneHwMonitorWrapper::velodyne_check_laser_state( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { - if ( - VelodyneHwMonitorWrapper::current_status_tree && - !VelodyneHwMonitorWrapper::current_status_tree->empty()) { - auto tpl = VelodyneGetLaserState(); + if (current_status_tree_ && !current_status_tree_->empty()) { + auto tpl = velodyne_get_laser_state(); diagnostics.add("sensor", sensor_configuration_->frame_id); diagnostics.summary(std::get<1>(tpl), std::get<2>(tpl)); } } -void VelodyneHwMonitorWrapper::VelodyneCheckSnapshot( +void VelodyneHwMonitorWrapper::velodyne_check_snapshot( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { - uint8_t level = current_diag_status; + uint8_t level = current_diag_status_; diagnostics.add("sensor", sensor_configuration_->frame_id); - diagnostics.summary(level, *current_snapshot); + diagnostics.summary(level, *current_snapshot_); } -void VelodyneHwMonitorWrapper::VelodyneCheckStatus( +void VelodyneHwMonitorWrapper::velodyne_check_status( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { - if ( - VelodyneHwMonitorWrapper::current_status_tree && - !VelodyneHwMonitorWrapper::current_status_tree->empty()) { + if (current_status_tree_ && !current_status_tree_->empty()) { uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::vector msg; - auto tpl = VelodyneGetMotorState(); + auto tpl = velodyne_get_motor_state(); if (std::get<0>(tpl)) { level = std::max(level, std::get<1>(tpl)); if (0 < std::get<3>(tpl).length()) { @@ -1293,7 +1251,7 @@ void VelodyneHwMonitorWrapper::VelodyneCheckStatus( } diagnostics.add(name_status_motor_state, std::get<2>(tpl)); - tpl = VelodyneGetLaserState(); + tpl = velodyne_get_laser_state(); if (std::get<0>(tpl)) { level = std::max(level, std::get<1>(tpl)); if (0 < std::get<3>(tpl).length()) { @@ -1306,16 +1264,14 @@ void VelodyneHwMonitorWrapper::VelodyneCheckStatus( } } -void VelodyneHwMonitorWrapper::VelodyneCheckPps( +void VelodyneHwMonitorWrapper::velodyne_check_pps( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { - if ( - VelodyneHwMonitorWrapper::current_status_tree && - !VelodyneHwMonitorWrapper::current_status_tree->empty()) { + if (current_status_tree_ && !current_status_tree_->empty()) { uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::vector msg; - auto tpl = VelodyneGetGpsPpsState(); + auto tpl = velodyne_get_gps_pps_state(); if (std::get<0>(tpl)) { level = std::max(level, std::get<1>(tpl)); if (0 < std::get<3>(tpl).length()) { @@ -1324,7 +1280,7 @@ void VelodyneHwMonitorWrapper::VelodyneCheckPps( } diagnostics.add(name_status_gps_pps_state, std::get<2>(tpl)); - tpl = VelodyneGetGpsPosition(); + tpl = velodyne_get_gps_position(); if (std::get<0>(tpl)) { level = std::max(level, std::get<1>(tpl)); if (0 < std::get<3>(tpl).length()) { @@ -1337,16 +1293,14 @@ void VelodyneHwMonitorWrapper::VelodyneCheckPps( } } -void VelodyneHwMonitorWrapper::VelodyneCheckTemperature( +void VelodyneHwMonitorWrapper::velodyne_check_temperature( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { - if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + if (current_diag_tree_ && !current_diag_tree_->empty()) { uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::vector msg; - auto tpl = VelodyneGetTopLm20Temp(); + auto tpl = velodyne_get_top_lm20_temp(); if (std::get<0>(tpl)) { level = std::max(level, std::get<1>(tpl)); if (0 < std::get<3>(tpl).length()) { @@ -1355,7 +1309,7 @@ void VelodyneHwMonitorWrapper::VelodyneCheckTemperature( } diagnostics.add(name_volt_temp_top_lm20_temp, std::get<2>(tpl)); - tpl = VelodyneGetBotLm20Temp(); + tpl = velodyne_get_bot_lm20_temp(); if (std::get<0>(tpl)) { level = std::max(level, std::get<1>(tpl)); if (0 < std::get<3>(tpl).length()) { @@ -1368,16 +1322,14 @@ void VelodyneHwMonitorWrapper::VelodyneCheckTemperature( } } -void VelodyneHwMonitorWrapper::VelodyneCheckRpm( +void VelodyneHwMonitorWrapper::velodyne_check_rpm( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { - if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + if (current_diag_tree_ && !current_diag_tree_->empty()) { uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::vector msg; - auto tpl = VelodyneGetMotorRpm(); + auto tpl = velodyne_get_motor_rpm(); if (std::get<0>(tpl)) { level = std::max(level, std::get<1>(tpl)); if (0 < std::get<3>(tpl).length()) { @@ -1386,7 +1338,7 @@ void VelodyneHwMonitorWrapper::VelodyneCheckRpm( } diagnostics.add(name_status_motor_rpm, std::get<2>(tpl)); - tpl = VelodyneGetMotorLock(); + tpl = velodyne_get_motor_lock(); if (std::get<0>(tpl)) { level = std::max(level, std::get<1>(tpl)); if (0 < std::get<3>(tpl).length()) { @@ -1399,16 +1351,14 @@ void VelodyneHwMonitorWrapper::VelodyneCheckRpm( } } -void VelodyneHwMonitorWrapper::VelodyneCheckVoltage( +void VelodyneHwMonitorWrapper::velodyne_check_voltage( diagnostic_updater::DiagnosticStatusWrapper & diagnostics) { - if ( - VelodyneHwMonitorWrapper::current_diag_tree && - !VelodyneHwMonitorWrapper::current_diag_tree->empty()) { + if (current_diag_tree_ && !current_diag_tree_->empty()) { uint8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; std::vector msg; - auto tpl = VelodyneGetTopHv(); + auto tpl = velodyne_get_top_hv(); if (std::get<0>(tpl)) { level = std::max(level, std::get<1>(tpl)); if (0 < std::get<3>(tpl).length()) { @@ -1417,7 +1367,7 @@ void VelodyneHwMonitorWrapper::VelodyneCheckVoltage( } diagnostics.add(name_volt_temp_top_hv, std::get<2>(tpl)); - tpl = VelodyneGetTopPwr5v(); + tpl = velodyne_get_top_pwr5v(); if (std::get<0>(tpl)) { level = std::max(level, std::get<1>(tpl)); if (0 < std::get<3>(tpl).length()) { @@ -1426,7 +1376,7 @@ void VelodyneHwMonitorWrapper::VelodyneCheckVoltage( } diagnostics.add(name_volt_temp_top_pwr_5v, std::get<2>(tpl)); - tpl = VelodyneGetTopPwr25v(); + tpl = velodyne_get_top_pwr25v(); if (std::get<0>(tpl)) { level = std::max(level, std::get<1>(tpl)); if (0 < std::get<3>(tpl).length()) { @@ -1435,7 +1385,7 @@ void VelodyneHwMonitorWrapper::VelodyneCheckVoltage( } diagnostics.add(name_volt_temp_top_pwr_2_5v, std::get<2>(tpl)); - tpl = VelodyneGetTopPwr33v(); + tpl = velodyne_get_top_pwr33v(); if (std::get<0>(tpl)) { level = std::max(level, std::get<1>(tpl)); if (0 < std::get<3>(tpl).length()) { @@ -1445,7 +1395,7 @@ void VelodyneHwMonitorWrapper::VelodyneCheckVoltage( diagnostics.add(name_volt_temp_top_pwr_3_3v, std::get<2>(tpl)); if (sensor_configuration_->sensor_model == nebula::drivers::SensorModel::VELODYNE_VLP16) { - tpl = VelodyneGetTopPwr5vRaw(); + tpl = velodyne_get_top_pwr5v_raw(); if (std::get<0>(tpl)) { level = std::max(level, std::get<1>(tpl)); if (0 < std::get<3>(tpl).length()) { @@ -1454,7 +1404,7 @@ void VelodyneHwMonitorWrapper::VelodyneCheckVoltage( } diagnostics.add(name_volt_temp_top_pwr_5v_raw, std::get<2>(tpl)); } else { - tpl = VelodyneGetTopPwrRaw(); + tpl = velodyne_get_top_pwr_raw(); if (std::get<0>(tpl)) { level = std::max(level, std::get<1>(tpl)); if (0 < std::get<3>(tpl).length()) { @@ -1464,7 +1414,7 @@ void VelodyneHwMonitorWrapper::VelodyneCheckVoltage( diagnostics.add(name_volt_temp_top_pwr_raw, std::get<2>(tpl)); } - tpl = VelodyneGetTopPwrVccint(); + tpl = velodyne_get_top_pwr_vccint(); if (std::get<0>(tpl)) { level = std::max(level, std::get<1>(tpl)); if (0 < std::get<3>(tpl).length()) { @@ -1473,7 +1423,7 @@ void VelodyneHwMonitorWrapper::VelodyneCheckVoltage( } diagnostics.add(name_volt_temp_top_pwr_vccint, std::get<2>(tpl)); - tpl = VelodyneGetBotIOut(); + tpl = velodyne_get_bot_i_out(); if (std::get<0>(tpl)) { level = std::max(level, std::get<1>(tpl)); if (0 < std::get<3>(tpl).length()) { @@ -1482,7 +1432,7 @@ void VelodyneHwMonitorWrapper::VelodyneCheckVoltage( } diagnostics.add(name_volt_temp_bot_i_out, std::get<2>(tpl)); - tpl = VelodyneGetBotPwr12v(); + tpl = velodyne_get_bot_pwr12v(); if (std::get<0>(tpl)) { level = std::max(level, std::get<1>(tpl)); if (0 < std::get<3>(tpl).length()) { @@ -1491,7 +1441,7 @@ void VelodyneHwMonitorWrapper::VelodyneCheckVoltage( } diagnostics.add(name_volt_temp_bot_pwr_1_2v, std::get<2>(tpl)); - tpl = VelodyneGetBotPwr5v(); + tpl = velodyne_get_bot_pwr5v(); if (std::get<0>(tpl)) { level = std::max(level, std::get<1>(tpl)); if (0 < std::get<3>(tpl).length()) { @@ -1500,7 +1450,7 @@ void VelodyneHwMonitorWrapper::VelodyneCheckVoltage( } diagnostics.add(name_volt_temp_bot_pwr_5v, std::get<2>(tpl)); - tpl = VelodyneGetBotPwr25v(); + tpl = velodyne_get_bot_pwr25v(); if (std::get<0>(tpl)) { level = std::max(level, std::get<1>(tpl)); if (0 < std::get<3>(tpl).length()) { @@ -1509,7 +1459,7 @@ void VelodyneHwMonitorWrapper::VelodyneCheckVoltage( } diagnostics.add(name_volt_temp_bot_pwr_2_5v, std::get<2>(tpl)); - tpl = VelodyneGetBotPwr33v(); + tpl = velodyne_get_bot_pwr33v(); if (std::get<0>(tpl)) { level = std::max(level, std::get<1>(tpl)); if (0 < std::get<3>(tpl).length()) { @@ -1518,7 +1468,7 @@ void VelodyneHwMonitorWrapper::VelodyneCheckVoltage( } diagnostics.add(name_volt_temp_bot_pwr_3_3v, std::get<2>(tpl)); - tpl = VelodyneGetBotPwrVIn(); + tpl = velodyne_get_bot_pwr_v_in(); if (std::get<0>(tpl)) { level = std::max(level, std::get<1>(tpl)); if (0 < std::get<3>(tpl).length()) { @@ -1527,7 +1477,7 @@ void VelodyneHwMonitorWrapper::VelodyneCheckVoltage( } diagnostics.add(name_volt_temp_bot_pwr_v_in, std::get<2>(tpl)); - tpl = VelodyneGetBotPwr125v(); + tpl = velodyne_get_bot_pwr125v(); if (std::get<0>(tpl)) { level = std::max(level, std::get<1>(tpl)); if (0 < std::get<3>(tpl).length()) { @@ -1540,7 +1490,7 @@ void VelodyneHwMonitorWrapper::VelodyneCheckVoltage( } } -std::string VelodyneHwMonitorWrapper::GetPtreeValue( +std::string VelodyneHwMonitorWrapper::get_ptree_value( std::shared_ptr pt, std::mutex & mtx_pt, const std::string & key) { std::lock_guard lock(mtx_pt); @@ -1552,14 +1502,14 @@ std::string VelodyneHwMonitorWrapper::GetPtreeValue( } } -std::string VelodyneHwMonitorWrapper::GetFixedPrecisionString(double val, int pre) +std::string VelodyneHwMonitorWrapper::get_fixed_precision_string(double val, int pre) { std::stringstream ss; ss << std::fixed << std::setprecision(pre) << val; return ss.str(); } -Status VelodyneHwMonitorWrapper::Status() +Status VelodyneHwMonitorWrapper::status() { return Status::OK; } diff --git a/nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp b/nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp index 808467e8c..5b28571ac 100644 --- a/nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp +++ b/nebula_ros/src/velodyne/velodyne_ros_wrapper.cpp @@ -19,7 +19,7 @@ VelodyneRosWrapper::VelodyneRosWrapper(const rclcpp::NodeOptions & options) { setvbuf(stdout, NULL, _IONBF, BUFSIZ); - wrapper_status_ = DeclareAndGetSensorConfigParams(); + wrapper_status_ = declare_and_get_sensor_config_params(); if (wrapper_status_ != Status::OK) { throw std::runtime_error( @@ -32,28 +32,28 @@ VelodyneRosWrapper::VelodyneRosWrapper(const rclcpp::NodeOptions & options) if (launch_hw_) { hw_interface_wrapper_.emplace(this, sensor_cfg_ptr_); - hw_monitor_wrapper_.emplace(this, hw_interface_wrapper_->HwInterface(), sensor_cfg_ptr_); + hw_monitor_wrapper_.emplace(this, hw_interface_wrapper_->hw_interface(), sensor_cfg_ptr_); } decoder_wrapper_.emplace( - this, hw_interface_wrapper_ ? hw_interface_wrapper_->HwInterface() : nullptr, sensor_cfg_ptr_); + this, hw_interface_wrapper_ ? hw_interface_wrapper_->hw_interface() : nullptr, sensor_cfg_ptr_); RCLCPP_DEBUG(get_logger(), "Starting stream"); decoder_thread_ = std::thread([this]() { while (true) { - decoder_wrapper_->ProcessCloudPacket(packet_queue_.pop()); + decoder_wrapper_->process_cloud_packet(packet_queue_.pop()); } }); if (launch_hw_) { - hw_interface_wrapper_->HwInterface()->RegisterScanCallback( - std::bind(&VelodyneRosWrapper::ReceiveCloudPacketCallback, this, std::placeholders::_1)); - StreamStart(); + hw_interface_wrapper_->hw_interface()->register_scan_callback( + std::bind(&VelodyneRosWrapper::receive_cloud_packet_callback, this, std::placeholders::_1)); + stream_start(); } else { packets_sub_ = create_subscription( "velodyne_packets", rclcpp::SensorDataQoS(), - std::bind(&VelodyneRosWrapper::ReceiveScanMessageCallback, this, std::placeholders::_1)); + std::bind(&VelodyneRosWrapper::receive_scan_message_callback, this, std::placeholders::_1)); RCLCPP_INFO_STREAM( get_logger(), "Hardware connection disabled, listening for packets on " << packets_sub_->get_topic_name()); @@ -62,18 +62,18 @@ VelodyneRosWrapper::VelodyneRosWrapper(const rclcpp::NodeOptions & options) // Register parameter callback after all params have been declared. Otherwise it would be called // once for each declaration parameter_event_cb_ = add_on_set_parameters_callback( - std::bind(&VelodyneRosWrapper::OnParameterChange, this, std::placeholders::_1)); + std::bind(&VelodyneRosWrapper::on_parameter_change, this, std::placeholders::_1)); } -nebula::Status VelodyneRosWrapper::DeclareAndGetSensorConfigParams() +nebula::Status VelodyneRosWrapper::declare_and_get_sensor_config_params() { nebula::drivers::VelodyneSensorConfiguration config; auto _sensor_model = declare_parameter("sensor_model", param_read_only()); - config.sensor_model = drivers::SensorModelFromString(_sensor_model); + config.sensor_model = drivers::sensor_model_from_string(_sensor_model); auto _return_mode = declare_parameter("return_mode", param_read_write()); - config.return_mode = drivers::ReturnModeFromString(_return_mode); + config.return_mode = drivers::return_mode_from_string(_return_mode); config.host_ip = declare_parameter("host_ip", param_read_only()); config.sensor_ip = declare_parameter("sensor_ip", param_read_only()); @@ -110,10 +110,10 @@ nebula::Status VelodyneRosWrapper::DeclareAndGetSensorConfigParams() } auto new_cfg_ptr = std::make_shared(config); - return ValidateAndSetConfig(new_cfg_ptr); + return validate_and_set_config(new_cfg_ptr); } -Status VelodyneRosWrapper::ValidateAndSetConfig( +Status VelodyneRosWrapper::validate_and_set_config( std::shared_ptr & new_config) { if (new_config->sensor_model == nebula::drivers::SensorModel::UNKNOWN) { @@ -127,20 +127,20 @@ Status VelodyneRosWrapper::ValidateAndSetConfig( } if (hw_interface_wrapper_) { - hw_interface_wrapper_->OnConfigChange(new_config); + hw_interface_wrapper_->on_config_change(new_config); } if (hw_monitor_wrapper_) { - hw_monitor_wrapper_->OnConfigChange(new_config); + hw_monitor_wrapper_->on_config_change(new_config); } if (decoder_wrapper_) { - decoder_wrapper_->OnConfigChange(new_config); + decoder_wrapper_->on_config_change(new_config); } sensor_cfg_ptr_ = new_config; return Status::OK; } -void VelodyneRosWrapper::ReceiveScanMessageCallback( +void VelodyneRosWrapper::receive_scan_message_callback( std::unique_ptr scan_msg) { if (hw_interface_wrapper_) { @@ -160,25 +160,25 @@ void VelodyneRosWrapper::ReceiveScanMessageCallback( } } -Status VelodyneRosWrapper::GetStatus() +Status VelodyneRosWrapper::get_status() { return wrapper_status_; } -Status VelodyneRosWrapper::StreamStart() +Status VelodyneRosWrapper::stream_start() { if (!hw_interface_wrapper_) { return Status::UDP_CONNECTION_ERROR; } - if (hw_interface_wrapper_->Status() != Status::OK) { - return hw_interface_wrapper_->Status(); + if (hw_interface_wrapper_->status() != Status::OK) { + return hw_interface_wrapper_->status(); } - return hw_interface_wrapper_->HwInterface()->SensorInterfaceStart(); + return hw_interface_wrapper_->hw_interface()->sensor_interface_start(); } -rcl_interfaces::msg::SetParametersResult VelodyneRosWrapper::OnParameterChange( +rcl_interfaces::msg::SetParametersResult VelodyneRosWrapper::on_parameter_change( const std::vector & p) { using rcl_interfaces::msg::SetParametersResult; @@ -205,7 +205,7 @@ rcl_interfaces::msg::SetParametersResult VelodyneRosWrapper::OnParameterChange( // Currently, HW interface and monitor wrappers have only read-only parameters, so their update // logic is not implemented if (decoder_wrapper_) { - auto result = decoder_wrapper_->OnParameterChange(p); + auto result = decoder_wrapper_->on_parameter_change(p); if (!result.successful) { return result; } @@ -216,10 +216,10 @@ rcl_interfaces::msg::SetParametersResult VelodyneRosWrapper::OnParameterChange( } if (_return_mode.length() > 0) - new_cfg.return_mode = nebula::drivers::ReturnModeFromString(_return_mode); + new_cfg.return_mode = nebula::drivers::return_mode_from_string(_return_mode); auto new_cfg_ptr = std::make_shared(new_cfg); - auto status = ValidateAndSetConfig(new_cfg_ptr); + auto status = validate_and_set_config(new_cfg_ptr); if (status != Status::OK) { RCLCPP_WARN_STREAM(get_logger(), "OnParameterChange aborted: " << status); @@ -232,9 +232,9 @@ rcl_interfaces::msg::SetParametersResult VelodyneRosWrapper::OnParameterChange( return rcl_interfaces::build().successful(true).reason(""); } -void VelodyneRosWrapper::ReceiveCloudPacketCallback(std::vector & packet) +void VelodyneRosWrapper::receive_cloud_packet_callback(std::vector & packet) { - if (!decoder_wrapper_ || decoder_wrapper_->Status() != Status::OK) { + if (!decoder_wrapper_ || decoder_wrapper_->status() != Status::OK) { return; } diff --git a/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp b/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp index f94989a3f..89962e30a 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp +++ b/nebula_tests/continental/continental_ros_decoder_test_ars548.cpp @@ -44,7 +44,7 @@ ContinentalRosDecoderTest::ContinentalRosDecoderTest( { drivers::continental_ars548::ContinentalARS548SensorConfiguration sensor_configuration; - wrapper_status_ = GetParameters(sensor_configuration); + wrapper_status_ = get_parameters(sensor_configuration); if (Status::OK != wrapper_status_) { RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error: " << wrapper_status_); return; @@ -56,19 +56,19 @@ ContinentalRosDecoderTest::ContinentalRosDecoderTest( sensor_configuration); RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Driver "); - wrapper_status_ = InitializeDriver( + wrapper_status_ = initialize_driver( std::const_pointer_cast( sensor_cfg_ptr_)); - driver_ptr_->RegisterDetectionListCallback( - std::bind(&ContinentalRosDecoderTest::DetectionListCallback, this, std::placeholders::_1)); - driver_ptr_->RegisterObjectListCallback( - std::bind(&ContinentalRosDecoderTest::ObjectListCallback, this, std::placeholders::_1)); + driver_ptr_->register_detection_list_callback( + std::bind(&ContinentalRosDecoderTest::detection_list_callback, this, std::placeholders::_1)); + driver_ptr_->register_object_list_callback( + std::bind(&ContinentalRosDecoderTest::object_list_callback, this, std::placeholders::_1)); RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); } -Status ContinentalRosDecoderTest::InitializeDriver( +Status ContinentalRosDecoderTest::initialize_driver( std::shared_ptr sensor_configuration) { @@ -79,19 +79,19 @@ Status ContinentalRosDecoderTest::InitializeDriver( return Status::OK; } -Status ContinentalRosDecoderTest::GetStatus() +Status ContinentalRosDecoderTest::get_status() { return wrapper_status_; } -Status ContinentalRosDecoderTest::GetParameters( +Status ContinentalRosDecoderTest::get_parameters( drivers::continental_ars548::ContinentalARS548SensorConfiguration & sensor_configuration) { std::filesystem::path bag_root_dir = _SRC_RESOURCES_DIR_PATH; // variable defined in CMakeLists.txt; bag_root_dir /= "continental"; - sensor_configuration.sensor_model = nebula::drivers::SensorModelFromString( + sensor_configuration.sensor_model = nebula::drivers::sensor_model_from_string( declare_parameter("sensor_model", "ARS548", param_read_only())); sensor_configuration.frame_id = declare_parameter("frame_id", "some_sensor_frame", param_read_only()); @@ -115,7 +115,7 @@ Status ContinentalRosDecoderTest::GetParameters( return Status::OK; } -void ContinentalRosDecoderTest::CompareNodes(const YAML::Node & node1, const YAML::Node & node2) +void ContinentalRosDecoderTest::compare_nodes(const YAML::Node & node1, const YAML::Node & node2) { ASSERT_EQ(node1.IsDefined(), node2.IsDefined()); ASSERT_EQ(node1.IsMap(), node2.IsMap()); @@ -125,24 +125,24 @@ void ContinentalRosDecoderTest::CompareNodes(const YAML::Node & node1, const YAM if (node1.IsMap()) { for (YAML::const_iterator it = node1.begin(); it != node1.end(); ++it) { - CompareNodes(it->second, node2[it->first.as()]); + compare_nodes(it->second, node2[it->first.as()]); } } else if (node1.IsScalar()) { ASSERT_EQ(node1.as(), node2.as()); } else if (node1.IsSequence()) { ASSERT_EQ(node1.size(), node2.size()); for (std::size_t i = 0; i < node1.size(); i++) { - CompareNodes(node1[i], node2[i]); + compare_nodes(node1[i], node2[i]); } } } -void ContinentalRosDecoderTest::CheckResult( +void ContinentalRosDecoderTest::check_result( const std::string msg_as_string, const std::string & gt_path) { YAML::Node current_node = YAML::Load(msg_as_string); YAML::Node gt_node = YAML::LoadFile(gt_path); - CompareNodes(gt_node, current_node); + compare_nodes(gt_node, current_node); // To generate the gt // std::ofstream ostream(gt_path); @@ -150,7 +150,7 @@ void ContinentalRosDecoderTest::CheckResult( // ostream.close(); } -void ContinentalRosDecoderTest::DetectionListCallback( +void ContinentalRosDecoderTest::detection_list_callback( std::unique_ptr msg) { EXPECT_EQ(sensor_cfg_ptr_->frame_id, msg->header.frame_id); @@ -162,10 +162,10 @@ void ContinentalRosDecoderTest::DetectionListCallback( auto gt_path = rcpputils::fs::path(bag_path_).parent_path() / detection_path.str(); ASSERT_TRUE(gt_path.exists()); - CheckResult(msg_as_string, gt_path.string()); + check_result(msg_as_string, gt_path.string()); } -void ContinentalRosDecoderTest::ObjectListCallback( +void ContinentalRosDecoderTest::object_list_callback( std::unique_ptr msg) { EXPECT_EQ(sensor_cfg_ptr_->object_frame, msg->header.frame_id); @@ -177,10 +177,10 @@ void ContinentalRosDecoderTest::ObjectListCallback( auto gt_path = rcpputils::fs::path(bag_path_).parent_path() / detection_path.str(); ASSERT_TRUE(gt_path.exists()); - CheckResult(msg_as_string, gt_path.string()); + check_result(msg_as_string, gt_path.string()); } -void ContinentalRosDecoderTest::ReadBag() +void ContinentalRosDecoderTest::read_bag() { rosbag2_storage::StorageOptions storage_options; rosbag2_cpp::ConverterOptions converter_options; @@ -223,7 +223,7 @@ void ContinentalRosDecoderTest::ReadBag() auto extracted_msg_ptr = std::make_unique(extracted_msg.packets[0]); - driver_ptr_->ProcessPacket(std::move(extracted_msg_ptr)); + driver_ptr_->process_packet(std::move(extracted_msg_ptr)); } } } diff --git a/nebula_tests/continental/continental_ros_decoder_test_ars548.hpp b/nebula_tests/continental/continental_ros_decoder_test_ars548.hpp index b509de9aa..46a3fd8a3 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_ars548.hpp +++ b/nebula_tests/continental/continental_ros_decoder_test_ars548.hpp @@ -43,23 +43,24 @@ class ContinentalRosDecoderTest final : public rclcpp::Node //, testing::Test std::shared_ptr sensor_cfg_ptr_; - Status InitializeDriver( + Status initialize_driver( std::shared_ptr sensor_configuration); - Status GetParameters( + Status get_parameters( drivers::continental_ars548::ContinentalARS548SensorConfiguration & sensor_configuration); - void CheckResult(const std::string msg_as_string, const std::string & gt_path); + void check_result(const std::string msg_as_string, const std::string & gt_path); - void DetectionListCallback( + void detection_list_callback( std::unique_ptr msg); - void ObjectListCallback(std::unique_ptr msg); + void object_list_callback( + std::unique_ptr msg); - void CompareNodes(const YAML::Node & node1, const YAML::Node & node2); + void compare_nodes(const YAML::Node & node1, const YAML::Node & node2); - static inline std::chrono::nanoseconds SecondsToChronoNanoSeconds(const double seconds) + static inline std::chrono::nanoseconds seconds_to_chrono_nano_seconds(const double seconds) { return std::chrono::duration_cast( std::chrono::duration(seconds)); @@ -69,9 +70,9 @@ class ContinentalRosDecoderTest final : public rclcpp::Node //, testing::Test explicit ContinentalRosDecoderTest( const rclcpp::NodeOptions & options, const std::string & node_name); - void ReceiveScanMsgCallback(const nebula_msgs::msg::NebulaPackets::SharedPtr scan_msg); - Status GetStatus(); - void ReadBag(); + void receive_scan_msg_callback(const nebula_msgs::msg::NebulaPackets::SharedPtr scan_msg); + Status get_status(); + void read_bag(); private: std::string bag_path_{}; diff --git a/nebula_tests/continental/continental_ros_decoder_test_main_ars548.cpp b/nebula_tests/continental/continental_ros_decoder_test_main_ars548.cpp index 9935e446f..b7581b335 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_main_ars548.cpp +++ b/nebula_tests/continental/continental_ros_decoder_test_main_ars548.cpp @@ -25,7 +25,7 @@ std::shared_ptr continental_driver; TEST(TestDecoder, TestBag) { std::cout << "TEST(TestDecoder, TestBag)" << std::endl; - continental_driver->ReadBag(); + continental_driver->read_bag(); } int main(int argc, char * argv[]) @@ -44,7 +44,7 @@ int main(int argc, char * argv[]) exec.add_node(continental_driver->get_node_base_interface()); RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Get Status"); - nebula::Status driver_status = continental_driver->GetStatus(); + nebula::Status driver_status = continental_driver->get_status(); int result = 0; if (driver_status == nebula::Status::OK) { RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Reading Started"); diff --git a/nebula_tests/continental/continental_ros_decoder_test_main_srr520.cpp b/nebula_tests/continental/continental_ros_decoder_test_main_srr520.cpp index 56b072750..5468b016d 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_main_srr520.cpp +++ b/nebula_tests/continental/continental_ros_decoder_test_main_srr520.cpp @@ -25,7 +25,7 @@ std::shared_ptr continental_driver; TEST(TestDecoder, TestBag) { std::cout << "TEST(TestDecoder, TestBag)" << std::endl; - continental_driver->ReadBag(); + continental_driver->read_bag(); } int main(int argc, char * argv[]) @@ -44,7 +44,7 @@ int main(int argc, char * argv[]) exec.add_node(continental_driver->get_node_base_interface()); RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Get Status"); - nebula::Status driver_status = continental_driver->GetStatus(); + nebula::Status driver_status = continental_driver->get_status(); int result = 0; if (driver_status == nebula::Status::OK) { RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Reading Started"); diff --git a/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp b/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp index 347bdf182..46c142d5c 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp +++ b/nebula_tests/continental/continental_ros_decoder_test_srr520.cpp @@ -44,7 +44,7 @@ ContinentalRosDecoderTest::ContinentalRosDecoderTest( { drivers::continental_srr520::ContinentalSRR520SensorConfiguration sensor_configuration; - wrapper_status_ = GetParameters(sensor_configuration); + wrapper_status_ = get_parameters(sensor_configuration); if (Status::OK != wrapper_status_) { RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error: " << wrapper_status_); return; @@ -56,23 +56,23 @@ ContinentalRosDecoderTest::ContinentalRosDecoderTest( sensor_configuration); RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Driver "); - wrapper_status_ = InitializeDriver( + wrapper_status_ = initialize_driver( std::const_pointer_cast( sensor_cfg_ptr_)); - driver_ptr_->RegisterHRRDetectionListCallback( - std::bind(&ContinentalRosDecoderTest::HRRDetectionListCallback, this, std::placeholders::_1)); - driver_ptr_->RegisterNearDetectionListCallback( - std::bind(&ContinentalRosDecoderTest::NearDetectionListCallback, this, std::placeholders::_1)); - driver_ptr_->RegisterObjectListCallback( - std::bind(&ContinentalRosDecoderTest::ObjectListCallback, this, std::placeholders::_1)); - driver_ptr_->RegisterStatusCallback( - std::bind(&ContinentalRosDecoderTest::StatusCallback, this, std::placeholders::_1)); + driver_ptr_->register_hrr_detection_list_callback(std::bind( + &ContinentalRosDecoderTest::hrr_detection_list_callback, this, std::placeholders::_1)); + driver_ptr_->register_near_detection_list_callback(std::bind( + &ContinentalRosDecoderTest::near_detection_list_callback, this, std::placeholders::_1)); + driver_ptr_->register_object_list_callback( + std::bind(&ContinentalRosDecoderTest::object_list_callback, this, std::placeholders::_1)); + driver_ptr_->register_status_callback( + std::bind(&ContinentalRosDecoderTest::status_callback, this, std::placeholders::_1)); RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); } -Status ContinentalRosDecoderTest::InitializeDriver( +Status ContinentalRosDecoderTest::initialize_driver( std::shared_ptr sensor_configuration) { @@ -83,19 +83,19 @@ Status ContinentalRosDecoderTest::InitializeDriver( return Status::OK; } -Status ContinentalRosDecoderTest::GetStatus() +Status ContinentalRosDecoderTest::get_status() { return wrapper_status_; } -Status ContinentalRosDecoderTest::GetParameters( +Status ContinentalRosDecoderTest::get_parameters( drivers::continental_srr520::ContinentalSRR520SensorConfiguration & sensor_configuration) { std::filesystem::path bag_root_dir = _SRC_RESOURCES_DIR_PATH; // variable defined in CMakeLists.txt; bag_root_dir /= "continental"; - sensor_configuration.sensor_model = nebula::drivers::SensorModelFromString( + sensor_configuration.sensor_model = nebula::drivers::sensor_model_from_string( declare_parameter("sensor_model", "SRR520", param_read_only())); sensor_configuration.frame_id = declare_parameter("frame_id", "some_sensor_frame", param_read_only()); @@ -117,7 +117,7 @@ Status ContinentalRosDecoderTest::GetParameters( return Status::OK; } -void ContinentalRosDecoderTest::CompareNodes(const YAML::Node & node1, const YAML::Node & node2) +void ContinentalRosDecoderTest::compare_nodes(const YAML::Node & node1, const YAML::Node & node2) { ASSERT_EQ(node1.IsDefined(), node2.IsDefined()); ASSERT_EQ(node1.IsMap(), node2.IsMap()); @@ -127,24 +127,24 @@ void ContinentalRosDecoderTest::CompareNodes(const YAML::Node & node1, const YAM if (node1.IsMap()) { for (YAML::const_iterator it = node1.begin(); it != node1.end(); ++it) { - CompareNodes(it->second, node2[it->first.as()]); + compare_nodes(it->second, node2[it->first.as()]); } } else if (node1.IsScalar()) { ASSERT_EQ(node1.as(), node2.as()); } else if (node1.IsSequence()) { ASSERT_EQ(node1.size(), node2.size()); for (std::size_t i = 0; i < node1.size(); i++) { - CompareNodes(node1[i], node2[i]); + compare_nodes(node1[i], node2[i]); } } } -void ContinentalRosDecoderTest::CheckResult( +void ContinentalRosDecoderTest::check_result( const std::string msg_as_string, const std::string & gt_path) { YAML::Node current_node = YAML::Load(msg_as_string); YAML::Node gt_node = YAML::LoadFile(gt_path); - CompareNodes(gt_node, current_node); + compare_nodes(gt_node, current_node); // To generate the gt // std::cout << gt_path << std::endl; @@ -153,7 +153,7 @@ void ContinentalRosDecoderTest::CheckResult( // ostream.close(); } -void ContinentalRosDecoderTest::HRRDetectionListCallback( +void ContinentalRosDecoderTest::hrr_detection_list_callback( std::unique_ptr msg) { hrr_detection_list_count_++; @@ -167,10 +167,10 @@ void ContinentalRosDecoderTest::HRRDetectionListCallback( auto gt_path = rcpputils::fs::path(bag_path_).parent_path() / detection_path.str(); ASSERT_TRUE(gt_path.exists()); - CheckResult(msg_as_string, gt_path.string()); + check_result(msg_as_string, gt_path.string()); } -void ContinentalRosDecoderTest::NearDetectionListCallback( +void ContinentalRosDecoderTest::near_detection_list_callback( std::unique_ptr msg) { near_detection_list_count_++; @@ -184,10 +184,10 @@ void ContinentalRosDecoderTest::NearDetectionListCallback( auto gt_path = rcpputils::fs::path(bag_path_).parent_path() / detection_path.str(); ASSERT_TRUE(gt_path.exists()); - CheckResult(msg_as_string, gt_path.string()); + check_result(msg_as_string, gt_path.string()); } -void ContinentalRosDecoderTest::ObjectListCallback( +void ContinentalRosDecoderTest::object_list_callback( std::unique_ptr msg) { object_list_count_++; @@ -200,10 +200,10 @@ void ContinentalRosDecoderTest::ObjectListCallback( auto gt_path = rcpputils::fs::path(bag_path_).parent_path() / detection_path.str(); ASSERT_TRUE(gt_path.exists()); - CheckResult(msg_as_string, gt_path.string()); + check_result(msg_as_string, gt_path.string()); } -void ContinentalRosDecoderTest::ReadBag() +void ContinentalRosDecoderTest::read_bag() { rosbag2_storage::StorageOptions storage_options; rosbag2_cpp::ConverterOptions converter_options; @@ -244,7 +244,7 @@ void ContinentalRosDecoderTest::ReadBag() for (auto & packet_msg : extracted_msg.packets) { auto extracted_msg_ptr = std::make_unique(packet_msg); - driver_ptr_->ProcessPacket(std::move(extracted_msg_ptr)); + driver_ptr_->process_packet(std::move(extracted_msg_ptr)); } } } diff --git a/nebula_tests/continental/continental_ros_decoder_test_srr520.hpp b/nebula_tests/continental/continental_ros_decoder_test_srr520.hpp index 525360e03..deb5ec146 100644 --- a/nebula_tests/continental/continental_ros_decoder_test_srr520.hpp +++ b/nebula_tests/continental/continental_ros_decoder_test_srr520.hpp @@ -43,30 +43,31 @@ class ContinentalRosDecoderTest final : public rclcpp::Node //, testing::Test std::shared_ptr sensor_cfg_ptr_; - Status InitializeDriver( + Status initialize_driver( std::shared_ptr sensor_configuration); - Status GetParameters( + Status get_parameters( drivers::continental_srr520::ContinentalSRR520SensorConfiguration & sensor_configuration); - void CheckResult(const std::string msg_as_string, const std::string & gt_path); + void check_result(const std::string msg_as_string, const std::string & gt_path); - void HRRDetectionListCallback( + void hrr_detection_list_callback( std::unique_ptr msg); - void NearDetectionListCallback( + void near_detection_list_callback( std::unique_ptr msg); - void StatusCallback([[maybe_unused]] std::unique_ptr msg) + void status_callback([[maybe_unused]] std::unique_ptr msg) { } - void ObjectListCallback(std::unique_ptr msg); + void object_list_callback( + std::unique_ptr msg); - void CompareNodes(const YAML::Node & node1, const YAML::Node & node2); + void compare_nodes(const YAML::Node & node1, const YAML::Node & node2); - static inline std::chrono::nanoseconds SecondsToChronoNanoSeconds(const double seconds) + static inline std::chrono::nanoseconds seconds_to_chrono_nano_seconds(const double seconds) { return std::chrono::duration_cast( std::chrono::duration(seconds)); @@ -76,9 +77,9 @@ class ContinentalRosDecoderTest final : public rclcpp::Node //, testing::Test explicit ContinentalRosDecoderTest( const rclcpp::NodeOptions & options, const std::string & node_name); - void ReceiveScanMsgCallback(const nebula_msgs::msg::NebulaPackets::SharedPtr scan_msg); - Status GetStatus(); - void ReadBag(); + void receive_scan_msg_callback(const nebula_msgs::msg::NebulaPackets::SharedPtr scan_msg); + Status get_status(); + void read_bag(); private: std::string bag_path_{}; diff --git a/nebula_tests/hesai/hesai_common.hpp b/nebula_tests/hesai/hesai_common.hpp index 69554e67e..8145449d0 100644 --- a/nebula_tests/hesai/hesai_common.hpp +++ b/nebula_tests/hesai/hesai_common.hpp @@ -31,7 +31,7 @@ namespace nebula namespace test { -inline void checkPCDs( +inline void check_pcds( nebula::drivers::NebulaPointCloudPtr pc, pcl::PointCloud::Ptr pc_ref) { ASSERT_GT(pc->points.size(), 0); @@ -52,7 +52,7 @@ inline void checkPCDs( } } -inline void checkPCDs( +inline void check_pcds( nebula::drivers::NebulaPointCloudPtr pp1, nebula::drivers::NebulaPointCloudPtr pp2) { EXPECT_EQ(pp1->points.size(), pp2->points.size()); @@ -70,7 +70,7 @@ inline void checkPCDs( } } -inline void printPCD(nebula::drivers::NebulaPointCloudPtr pp) +inline void print_pcd(nebula::drivers::NebulaPointCloudPtr pp) { for (auto p : pp->points) { std::cout << "(" << p.x << ", " << p.y << "," << p.z << "): " << p.intensity << ", " diff --git a/nebula_tests/hesai/hesai_ros_decoder_test.cpp b/nebula_tests/hesai/hesai_ros_decoder_test.cpp index dc423bf43..cc3e86f38 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test.cpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test.cpp @@ -68,10 +68,10 @@ Status HesaiRosDecoderTest::InitializeDriver( driver_ptr_ = std::make_shared( std::static_pointer_cast(sensor_configuration), calibration_configuration); - return driver_ptr_->GetStatus(); + return driver_ptr_->get_status(); } -Status HesaiRosDecoderTest::GetStatus() +Status HesaiRosDecoderTest::get_status() { return wrapper_status_; } @@ -86,8 +86,9 @@ Status HesaiRosDecoderTest::GetParameters( std::filesystem::path bag_root_dir = _SRC_RESOURCES_DIR_PATH; bag_root_dir /= "hesai"; - sensor_configuration.sensor_model = nebula::drivers::SensorModelFromString(params_.sensor_model); - sensor_configuration.return_mode = nebula::drivers::ReturnModeFromStringHesai( + sensor_configuration.sensor_model = + nebula::drivers::sensor_model_from_string(params_.sensor_model); + sensor_configuration.return_mode = nebula::drivers::return_mode_from_string_hesai( params_.return_mode, sensor_configuration.sensor_model); sensor_configuration.frame_id = params_.frame_id; sensor_configuration.sync_angle = params_.sync_angle; @@ -120,7 +121,8 @@ Status HesaiRosDecoderTest::GetParameters( } if (sensor_configuration.sensor_model == drivers::SensorModel::HESAI_PANDARAT128) { - auto cal_status = correction_configuration.LoadFromFile(sensor_configuration.calibration_path); + auto cal_status = + correction_configuration.load_from_file(sensor_configuration.calibration_path); if (cal_status != Status::OK) { RCLCPP_ERROR_STREAM( this->get_logger(), @@ -128,7 +130,8 @@ Status HesaiRosDecoderTest::GetParameters( return cal_status; } } else { - auto cal_status = calibration_configuration.LoadFromFile(sensor_configuration.calibration_path); + auto cal_status = + calibration_configuration.load_from_file(sensor_configuration.calibration_path); if (cal_status != Status::OK) { RCLCPP_ERROR_STREAM( this->get_logger(), @@ -141,7 +144,7 @@ Status HesaiRosDecoderTest::GetParameters( return Status::OK; } -void HesaiRosDecoderTest::ReadBag( +void HesaiRosDecoderTest::read_bag( std::function scan_callback) { rosbag2_storage::StorageOptions storage_options; @@ -184,7 +187,7 @@ void HesaiRosDecoderTest::ReadBag( auto extracted_msg_ptr = std::make_shared(extracted_msg); for (auto & pkt : extracted_msg_ptr->packets) { - auto pointcloud_ts = driver_ptr_->ParseCloudPacket( + auto pointcloud_ts = driver_ptr_->parse_cloud_packet( std::vector(pkt.data.begin(), std::next(pkt.data.begin(), pkt.size))); auto pointcloud = std::get<0>(pointcloud_ts); auto scan_timestamp = std::get<1>(pointcloud_ts); diff --git a/nebula_tests/hesai/hesai_ros_decoder_test.hpp b/nebula_tests/hesai/hesai_ros_decoder_test.hpp index 54e34d60e..bb23feb7d 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test.hpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test.hpp @@ -127,11 +127,11 @@ class HesaiRosDecoderTest final : public rclcpp::Node /// @brief Get current status of this driver /// @return Current status - Status GetStatus(); + Status get_status(); /// @brief Read the specified bag file and compare the constructed point clouds with the /// corresponding PCD files - void ReadBag( + void read_bag( std::function scan_callback); HesaiRosDecoderTestParams params_; diff --git a/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp b/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp index 1c044ff0a..bc584201e 100644 --- a/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp +++ b/nebula_tests/hesai/hesai_ros_decoder_test_main.cpp @@ -63,14 +63,14 @@ TEST_P(DecoderTest, TestPcd) RCLCPP_DEBUG_STREAM(*logger_, "exists: " << target_pcd_path); auto rt = pcd_reader.read(target_pcd_path.string(), *ref_pointcloud); RCLCPP_DEBUG_STREAM(*logger_, rt << " loaded: " << target_pcd_path); - checkPCDs(pointcloud, ref_pointcloud); + check_pcds(pointcloud, ref_pointcloud); check_cnt++; // ref_pointcloud.reset(new nebula::drivers::NebulaPointCloud); ref_pointcloud = std::make_shared>(); } }; - hesai_driver_->ReadBag(scan_callback); + hesai_driver_->read_bag(scan_callback); EXPECT_GT(check_cnt, 0); } @@ -97,7 +97,7 @@ TEST_P(DecoderTest, TestTimezone) tzset(); ASSERT_STREQ(tzname[0], "GMT"); auto gmt = timezone; - hesai_driver_->ReadBag(scan_callback); + hesai_driver_->read_bag(scan_callback); ASSERT_GT(decoded_timestamps.size(), 0U); @@ -113,7 +113,7 @@ TEST_P(DecoderTest, TestTimezone) tzset(); ASSERT_STREQ(tzname[0], "JST"); auto jst = timezone; - hesai_driver_->ReadBag(scan_callback); + hesai_driver_->read_bag(scan_callback); // Wrong timezone settings do not throw an error, they just result in UST+0. // Thus, verify that timezone setting has effect on local timestamp @@ -141,7 +141,7 @@ void DecoderTest::SetUp() hesai_driver_ = std::make_shared(options, node_name, decoder_params); - nebula::Status driver_status = hesai_driver_->GetStatus(); + nebula::Status driver_status = hesai_driver_->get_status(); if (driver_status != nebula::Status::OK) { throw std::runtime_error("Could not initialize driver"); } diff --git a/nebula_tests/hesai/hesai_ros_scan_cutting_test_main.cpp b/nebula_tests/hesai/hesai_ros_scan_cutting_test_main.cpp index 3d26d8103..825d96da5 100644 --- a/nebula_tests/hesai/hesai_ros_scan_cutting_test_main.cpp +++ b/nebula_tests/hesai/hesai_ros_scan_cutting_test_main.cpp @@ -26,7 +26,7 @@ namespace nebula::test { -const nebula::ros::HesaiRosDecoderTestParams TEST_CONFIGS[] = { +const nebula::ros::HesaiRosDecoderTestParams g_test_configs[] = { {"Pandar64", "Dual", "Pandar64.csv", "64/all_points", "hesai", 0, 0.0, 0., 360., 0.3f, 200.f}, {"Pandar64", "Dual", "Pandar64.csv", "64/all_points", "hesai", 0, 180.0, 0., 360., 0.3f, 200.f}, {"Pandar64", "Dual", "Pandar64.csv", "64/all_points", "hesai", 0, 90.0, 90., 270., 0.3f, 200.f}, @@ -97,7 +97,7 @@ TEST_P(ScanCuttingTest, FovAlignment) threshold_angle, drivers::normalize_angle(fov_max_rad, 2 * M_PIf), p.azimuth); } - bool is_at128 = drivers::SensorModelFromString(hesai_driver_->params_.sensor_model) == + bool is_at128 = drivers::sensor_model_from_string(hesai_driver_->params_.sensor_model) == drivers::SensorModel::HESAI_PANDARAT128; // There is a bug in AT128's firm- or hardware that skips a few channels at the beginning of the // defined FoV. for that reason, relax test conditions in the case where that bug happens. @@ -128,7 +128,7 @@ TEST_P(ScanCuttingTest, FovAlignment) check_cnt++; }; - hesai_driver_->ReadBag(scan_callback); + hesai_driver_->read_bag(scan_callback); EXPECT_GT(check_cnt, 0); } @@ -146,7 +146,7 @@ void ScanCuttingTest::SetUp() hesai_driver_ = std::make_shared(options, node_name, decoder_params); - nebula::Status driver_status = hesai_driver_->GetStatus(); + nebula::Status driver_status = hesai_driver_->get_status(); if (driver_status != nebula::Status::OK) { throw std::runtime_error("Could not initialize driver"); } @@ -160,7 +160,7 @@ void ScanCuttingTest::TearDown() } INSTANTIATE_TEST_SUITE_P( - TestMain, ScanCuttingTest, testing::ValuesIn(TEST_CONFIGS), + TestMain, ScanCuttingTest, testing::ValuesIn(g_test_configs), [](const testing::TestParamInfo & p) { return p.param.sensor_model + "__cut" + std::to_string(static_cast(p.param.cut_angle)) + "__fov_start" + std::to_string(static_cast(p.param.fov_start)) + "__fov_end" + diff --git a/nebula_tests/velodyne/velodyne_ros_decoder_test_main_vlp16.cpp b/nebula_tests/velodyne/velodyne_ros_decoder_test_main_vlp16.cpp index f17abef87..df9420846 100644 --- a/nebula_tests/velodyne/velodyne_ros_decoder_test_main_vlp16.cpp +++ b/nebula_tests/velodyne/velodyne_ros_decoder_test_main_vlp16.cpp @@ -13,7 +13,7 @@ std::shared_ptr velodyne_driver; TEST(TestDecoder, TestPcd) { std::cout << "TEST(TestDecoder, TestPcd)" << std::endl; - velodyne_driver->ReadBag(); + velodyne_driver->read_bag(); } int main(int argc, char * argv[]) @@ -32,7 +32,7 @@ int main(int argc, char * argv[]) exec.add_node(velodyne_driver->get_node_base_interface()); RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Get Status"); - nebula::Status driver_status = velodyne_driver->GetStatus(); + nebula::Status driver_status = velodyne_driver->get_status(); int result = 0; if (driver_status == nebula::Status::OK) { RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Reading Started"); diff --git a/nebula_tests/velodyne/velodyne_ros_decoder_test_main_vlp32.cpp b/nebula_tests/velodyne/velodyne_ros_decoder_test_main_vlp32.cpp index e07f1212b..4175cb1a2 100644 --- a/nebula_tests/velodyne/velodyne_ros_decoder_test_main_vlp32.cpp +++ b/nebula_tests/velodyne/velodyne_ros_decoder_test_main_vlp32.cpp @@ -13,7 +13,7 @@ std::shared_ptr velodyne_driver; TEST(TestDecoder, TestPcd) { std::cout << "TEST(TestDecoder, TestPcd)" << std::endl; - velodyne_driver->ReadBag(); + velodyne_driver->read_bag(); } int main(int argc, char * argv[]) @@ -32,7 +32,7 @@ int main(int argc, char * argv[]) exec.add_node(velodyne_driver->get_node_base_interface()); RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Get Status"); - nebula::Status driver_status = velodyne_driver->GetStatus(); + nebula::Status driver_status = velodyne_driver->get_status(); int result = 0; if (driver_status == nebula::Status::OK) { RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Reading Started"); diff --git a/nebula_tests/velodyne/velodyne_ros_decoder_test_main_vls128.cpp b/nebula_tests/velodyne/velodyne_ros_decoder_test_main_vls128.cpp index 0fd0b77e2..b0a675ecf 100644 --- a/nebula_tests/velodyne/velodyne_ros_decoder_test_main_vls128.cpp +++ b/nebula_tests/velodyne/velodyne_ros_decoder_test_main_vls128.cpp @@ -13,7 +13,7 @@ std::shared_ptr velodyne_driver; TEST(TestDecoder, TestPcd) { std::cout << "TEST(TestDecoder, TestPcd)" << std::endl; - velodyne_driver->ReadBag(); + velodyne_driver->read_bag(); } int main(int argc, char * argv[]) @@ -32,7 +32,7 @@ int main(int argc, char * argv[]) exec.add_node(velodyne_driver->get_node_base_interface()); RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Get Status"); - nebula::Status driver_status = velodyne_driver->GetStatus(); + nebula::Status driver_status = velodyne_driver->get_status(); int result = 0; if (driver_status == nebula::Status::OK) { RCLCPP_INFO_STREAM(rclcpp::get_logger(node_name), "Reading Started"); diff --git a/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp16.cpp b/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp16.cpp index 818726bf1..91abe5bea 100644 --- a/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp16.cpp +++ b/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp16.cpp @@ -28,7 +28,7 @@ VelodyneRosDecoderTest::VelodyneRosDecoderTest( drivers::VelodyneCalibrationConfiguration calibration_configuration; drivers::VelodyneSensorConfiguration sensor_configuration; - wrapper_status_ = GetParameters(sensor_configuration, calibration_configuration); + wrapper_status_ = get_parameters(sensor_configuration, calibration_configuration); if (Status::OK != wrapper_status_) { RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error: " << wrapper_status_); return; @@ -42,27 +42,27 @@ VelodyneRosDecoderTest::VelodyneRosDecoderTest( std::make_shared(sensor_configuration); RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Driver "); - wrapper_status_ = InitializeDriver(sensor_cfg_ptr_, calibration_cfg_ptr_); + wrapper_status_ = initialize_driver(sensor_cfg_ptr_, calibration_cfg_ptr_); RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); } -Status VelodyneRosDecoderTest::InitializeDriver( +Status VelodyneRosDecoderTest::initialize_driver( std::shared_ptr sensor_configuration, std::shared_ptr calibration_configuration) { // driver should be initialized here with proper decoder driver_ptr_ = std::make_shared(sensor_configuration, calibration_configuration); - return driver_ptr_->GetStatus(); + return driver_ptr_->get_status(); } -Status VelodyneRosDecoderTest::GetStatus() +Status VelodyneRosDecoderTest::get_status() { return wrapper_status_; } -Status VelodyneRosDecoderTest::GetParameters( +Status VelodyneRosDecoderTest::get_parameters( drivers::VelodyneSensorConfiguration & sensor_configuration, drivers::VelodyneCalibrationConfiguration & calibration_configuration) { @@ -80,7 +80,7 @@ Status VelodyneRosDecoderTest::GetParameters( descriptor.additional_constraints = ""; this->declare_parameter("sensor_model", "VLP16"); sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); + nebula::drivers::sensor_model_from_string(this->get_parameter("sensor_model").as_string()); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -90,7 +90,7 @@ Status VelodyneRosDecoderTest::GetParameters( descriptor.additional_constraints = ""; this->declare_parameter("return_mode", "Dual", descriptor); sensor_configuration.return_mode = - nebula::drivers::ReturnModeFromString(this->get_parameter("return_mode").as_string()); + nebula::drivers::return_mode_from_string(this->get_parameter("return_mode").as_string()); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -198,8 +198,8 @@ Status VelodyneRosDecoderTest::GetParameters( descriptor.additional_constraints = ""; this->declare_parameter( "bag_path", (bag_root_dir / "vlp16" / "1673400471837873222").string(), descriptor); - bag_path = this->get_parameter("bag_path").as_string(); - std::cout << bag_path << std::endl; + bag_path_ = this->get_parameter("bag_path").as_string(); + std::cout << bag_path_ << std::endl; } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -208,7 +208,7 @@ Status VelodyneRosDecoderTest::GetParameters( descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; this->declare_parameter("storage_id", "sqlite3", descriptor); - storage_id = this->get_parameter("storage_id").as_string(); + storage_id_ = this->get_parameter("storage_id").as_string(); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -217,7 +217,7 @@ Status VelodyneRosDecoderTest::GetParameters( descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; this->declare_parameter("format", "cdr", descriptor); - format = this->get_parameter("format").as_string(); + format_ = this->get_parameter("format").as_string(); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -226,7 +226,7 @@ Status VelodyneRosDecoderTest::GetParameters( descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; this->declare_parameter("target_topic", "/velodyne_packets", descriptor); - target_topic = this->get_parameter("target_topic").as_string(); + target_topic_ = this->get_parameter("target_topic").as_string(); } if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { @@ -242,7 +242,7 @@ Status VelodyneRosDecoderTest::GetParameters( return Status::INVALID_CALIBRATION_FILE; } else { auto cal_status = - calibration_configuration.LoadFromFile(calibration_configuration.calibration_file); + calibration_configuration.load_from_file(calibration_configuration.calibration_file); if (cal_status != Status::OK) { RCLCPP_ERROR_STREAM( this->get_logger(), @@ -255,7 +255,7 @@ Status VelodyneRosDecoderTest::GetParameters( return Status::OK; } -void printPCD(nebula::drivers::NebulaPointCloudPtr pp) +void print_pcd(nebula::drivers::NebulaPointCloudPtr pp) { for (auto p : pp->points) { std::cout << "(" << p.x << ", " << p.y << "," << p.z << "): " << p.intensity << ", " @@ -264,7 +264,7 @@ void printPCD(nebula::drivers::NebulaPointCloudPtr pp) } } -void checkPCDs(nebula::drivers::NebulaPointCloudPtr pp1, nebula::drivers::NebulaPointCloudPtr pp2) +void check_pcds(nebula::drivers::NebulaPointCloudPtr pp1, nebula::drivers::NebulaPointCloudPtr pp2) { EXPECT_EQ(pp1->points.size(), pp2->points.size()); for (uint32_t i = 0; i < pp1->points.size(); i++) { @@ -281,7 +281,7 @@ void checkPCDs(nebula::drivers::NebulaPointCloudPtr pp1, nebula::drivers::Nebula } } -void checkPCDs(nebula::drivers::NebulaPointCloudPtr pp1, pcl::PointCloud::Ptr pp2) +void check_pcds(nebula::drivers::NebulaPointCloudPtr pp1, pcl::PointCloud::Ptr pp2) { EXPECT_EQ(pp1->points.size(), pp2->points.size()); for (uint32_t i = 0; i < pp1->points.size(); i++) { @@ -293,17 +293,17 @@ void checkPCDs(nebula::drivers::NebulaPointCloudPtr pp1, pcl::PointCloud serialization; nebula::drivers::NebulaPointCloudPtr pointcloud(new nebula::drivers::NebulaPointCloud); pcl::PointCloud::Ptr ref_pointcloud(new pcl::PointCloud); @@ -329,7 +329,7 @@ void VelodyneRosDecoderTest::ReadBag() std::cout << "Found topic name " << bag_message->topic_name << std::endl; - if (bag_message->topic_name == target_topic) { + if (bag_message->topic_name == target_topic_) { velodyne_msgs::msg::VelodyneScan extracted_msg; rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data); serialization.deserialize_message(&extracted_serialized_msg, &extracted_msg); @@ -339,7 +339,7 @@ void VelodyneRosDecoderTest::ReadBag() auto extracted_msg_ptr = std::make_shared(extracted_msg); for (auto & pkt : extracted_msg.packets) { - auto pointcloud_ts = driver_ptr_->ParseCloudPacket( + auto pointcloud_ts = driver_ptr_->parse_cloud_packet( std::vector(pkt.data.begin(), pkt.data.end()), pkt.stamp.sec); auto pointcloud = std::get<0>(pointcloud_ts); @@ -355,7 +355,7 @@ void VelodyneRosDecoderTest::ReadBag() std::cout << "exists: " << target_pcd_path << std::endl; auto rt = pcd_reader.read(target_pcd_path.string(), *ref_pointcloud); std::cout << rt << " loaded: " << target_pcd_path << std::endl; - checkPCDs(pointcloud, ref_pointcloud); + check_pcds(pointcloud, ref_pointcloud); check_cnt++; ref_pointcloud.reset(new pcl::PointCloud); } diff --git a/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp16.hpp b/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp16.hpp index 47885ab2d..781b42f05 100644 --- a/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp16.hpp +++ b/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp16.hpp @@ -47,7 +47,7 @@ class VelodyneRosDecoderTest final : public rclcpp::Node /// @param sensor_configuration SensorConfiguration for this driver /// @param calibration_configuration CalibrationConfiguration for this driver /// @return Resulting status - Status InitializeDriver( + Status initialize_driver( std::shared_ptr sensor_configuration, std::shared_ptr calibration_configuration); @@ -56,14 +56,14 @@ class VelodyneRosDecoderTest final : public rclcpp::Node /// @param sensor_configuration Output of SensorConfiguration /// @param calibration_configuration Output of CalibrationConfiguration /// @return Resulting status - Status GetParameters( + Status get_parameters( drivers::VelodyneSensorConfiguration & sensor_configuration, drivers::VelodyneCalibrationConfiguration & calibration_configuration); /// @brief Convert seconds to chrono::nanoseconds /// @param seconds /// @return chrono::nanoseconds - static inline std::chrono::nanoseconds SecondsToChronoNanoSeconds(const double seconds) + static inline std::chrono::nanoseconds seconds_to_chrono_nano_seconds(const double seconds) { return std::chrono::duration_cast( std::chrono::duration(seconds)); @@ -75,11 +75,11 @@ class VelodyneRosDecoderTest final : public rclcpp::Node /// @brief Get current status of this driver /// @return Current status - Status GetStatus(); + Status get_status(); /// @brief Read the specified bag file and compare the constructed point clouds with the /// corresponding PCD files - void ReadBag(); + void read_bag(); /* void SetUp() override { // Setup things that should occur before every test instance should go here @@ -91,11 +91,11 @@ class VelodyneRosDecoderTest final : public rclcpp::Node } */ private: - std::string bag_path; - std::string storage_id; - std::string format; - std::string target_topic; - std::string correction_file_path; + std::string bag_path_; + std::string storage_id_; + std::string format_; + std::string target_topic_; + std::string correction_file_path_; }; } // namespace ros diff --git a/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp32.cpp b/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp32.cpp index 871f6dab7..62bcd40ae 100644 --- a/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp32.cpp +++ b/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp32.cpp @@ -26,7 +26,7 @@ VelodyneRosDecoderTest::VelodyneRosDecoderTest( drivers::VelodyneCalibrationConfiguration calibration_configuration; drivers::VelodyneSensorConfiguration sensor_configuration; - wrapper_status_ = GetParameters(sensor_configuration, calibration_configuration); + wrapper_status_ = get_parameters(sensor_configuration, calibration_configuration); if (Status::OK != wrapper_status_) { RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error: " << wrapper_status_); return; @@ -40,27 +40,27 @@ VelodyneRosDecoderTest::VelodyneRosDecoderTest( std::make_shared(sensor_configuration); RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Driver "); - wrapper_status_ = InitializeDriver(sensor_cfg_ptr_, calibration_cfg_ptr_); + wrapper_status_ = initialize_driver(sensor_cfg_ptr_, calibration_cfg_ptr_); RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); } -Status VelodyneRosDecoderTest::InitializeDriver( +Status VelodyneRosDecoderTest::initialize_driver( std::shared_ptr sensor_configuration, std::shared_ptr calibration_configuration) { // driver should be initialized here with proper decoder driver_ptr_ = std::make_shared(sensor_configuration, calibration_configuration); - return driver_ptr_->GetStatus(); + return driver_ptr_->get_status(); } -Status VelodyneRosDecoderTest::GetStatus() +Status VelodyneRosDecoderTest::get_status() { return wrapper_status_; } -Status VelodyneRosDecoderTest::GetParameters( +Status VelodyneRosDecoderTest::get_parameters( drivers::VelodyneSensorConfiguration & sensor_configuration, drivers::VelodyneCalibrationConfiguration & calibration_configuration) { @@ -78,7 +78,7 @@ Status VelodyneRosDecoderTest::GetParameters( descriptor.additional_constraints = ""; this->declare_parameter("sensor_model", "VLP32"); sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); + nebula::drivers::sensor_model_from_string(this->get_parameter("sensor_model").as_string()); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -88,7 +88,7 @@ Status VelodyneRosDecoderTest::GetParameters( descriptor.additional_constraints = ""; this->declare_parameter("return_mode", "SingleStrongest", descriptor); sensor_configuration.return_mode = - nebula::drivers::ReturnModeFromString(this->get_parameter("return_mode").as_string()); + nebula::drivers::return_mode_from_string(this->get_parameter("return_mode").as_string()); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -196,8 +196,8 @@ Status VelodyneRosDecoderTest::GetParameters( descriptor.additional_constraints = ""; this->declare_parameter( "bag_path", (bag_root_dir / "vlp32" / "1713492677464078412").string(), descriptor); - bag_path = this->get_parameter("bag_path").as_string(); - std::cout << bag_path << std::endl; + bag_path_ = this->get_parameter("bag_path").as_string(); + std::cout << bag_path_ << std::endl; } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -206,7 +206,7 @@ Status VelodyneRosDecoderTest::GetParameters( descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; this->declare_parameter("storage_id", "sqlite3", descriptor); - storage_id = this->get_parameter("storage_id").as_string(); + storage_id_ = this->get_parameter("storage_id").as_string(); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -215,7 +215,7 @@ Status VelodyneRosDecoderTest::GetParameters( descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; this->declare_parameter("format", "cdr", descriptor); - format = this->get_parameter("format").as_string(); + format_ = this->get_parameter("format").as_string(); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -225,7 +225,7 @@ Status VelodyneRosDecoderTest::GetParameters( descriptor.additional_constraints = ""; this->declare_parameter( "target_topic", "/sensing/lidar/front/velodyne_packets", descriptor); - target_topic = this->get_parameter("target_topic").as_string(); + target_topic_ = this->get_parameter("target_topic").as_string(); } if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { @@ -241,7 +241,7 @@ Status VelodyneRosDecoderTest::GetParameters( return Status::INVALID_CALIBRATION_FILE; } else { auto cal_status = - calibration_configuration.LoadFromFile(calibration_configuration.calibration_file); + calibration_configuration.load_from_file(calibration_configuration.calibration_file); if (cal_status != Status::OK) { RCLCPP_ERROR_STREAM( this->get_logger(), @@ -254,7 +254,7 @@ Status VelodyneRosDecoderTest::GetParameters( return Status::OK; } -void printPCD(nebula::drivers::NebulaPointCloudPtr pp) +void print_pcd(nebula::drivers::NebulaPointCloudPtr pp) { for (auto p : pp->points) { std::cout << "(" << p.x << ", " << p.y << "," << p.z << "): " << p.intensity << ", " @@ -263,7 +263,7 @@ void printPCD(nebula::drivers::NebulaPointCloudPtr pp) } } -void checkPCDs(nebula::drivers::NebulaPointCloudPtr pp1, nebula::drivers::NebulaPointCloudPtr pp2) +void check_pcds(nebula::drivers::NebulaPointCloudPtr pp1, nebula::drivers::NebulaPointCloudPtr pp2) { EXPECT_EQ(pp1->points.size(), pp2->points.size()); for (uint32_t i = 0; i < pp1->points.size(); i++) { @@ -280,7 +280,7 @@ void checkPCDs(nebula::drivers::NebulaPointCloudPtr pp1, nebula::drivers::Nebula } } -void checkPCDs(nebula::drivers::NebulaPointCloudPtr pp1, pcl::PointCloud::Ptr pp2) +void check_pcds(nebula::drivers::NebulaPointCloudPtr pp1, pcl::PointCloud::Ptr pp2) { EXPECT_EQ(pp1->points.size(), pp2->points.size()); for (uint32_t i = 0; i < pp1->points.size(); i++) { @@ -292,17 +292,17 @@ void checkPCDs(nebula::drivers::NebulaPointCloudPtr pp1, pcl::PointCloud serialization; nebula::drivers::NebulaPointCloudPtr pointcloud(new nebula::drivers::NebulaPointCloud); pcl::PointCloud::Ptr ref_pointcloud(new pcl::PointCloud); @@ -328,7 +328,7 @@ void VelodyneRosDecoderTest::ReadBag() std::cout << "Found topic name " << bag_message->topic_name << std::endl; - if (bag_message->topic_name == target_topic) { + if (bag_message->topic_name == target_topic_) { velodyne_msgs::msg::VelodyneScan extracted_msg; rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data); serialization.deserialize_message(&extracted_serialized_msg, &extracted_msg); @@ -338,7 +338,7 @@ void VelodyneRosDecoderTest::ReadBag() auto extracted_msg_ptr = std::make_shared(extracted_msg); for (auto & pkt : extracted_msg.packets) { - auto pointcloud_ts = driver_ptr_->ParseCloudPacket( + auto pointcloud_ts = driver_ptr_->parse_cloud_packet( std::vector(pkt.data.begin(), pkt.data.end()), pkt.stamp.sec); auto pointcloud = std::get<0>(pointcloud_ts); @@ -355,7 +355,7 @@ void VelodyneRosDecoderTest::ReadBag() std::cout << "exists: " << target_pcd_path << std::endl; auto rt = pcd_reader.read(target_pcd_path.string(), *ref_pointcloud); std::cout << rt << " loaded: " << target_pcd_path << std::endl; - checkPCDs(pointcloud, ref_pointcloud); + check_pcds(pointcloud, ref_pointcloud); check_cnt++; ref_pointcloud.reset(new pcl::PointCloud); } diff --git a/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp32.hpp b/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp32.hpp index 47885ab2d..781b42f05 100644 --- a/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp32.hpp +++ b/nebula_tests/velodyne/velodyne_ros_decoder_test_vlp32.hpp @@ -47,7 +47,7 @@ class VelodyneRosDecoderTest final : public rclcpp::Node /// @param sensor_configuration SensorConfiguration for this driver /// @param calibration_configuration CalibrationConfiguration for this driver /// @return Resulting status - Status InitializeDriver( + Status initialize_driver( std::shared_ptr sensor_configuration, std::shared_ptr calibration_configuration); @@ -56,14 +56,14 @@ class VelodyneRosDecoderTest final : public rclcpp::Node /// @param sensor_configuration Output of SensorConfiguration /// @param calibration_configuration Output of CalibrationConfiguration /// @return Resulting status - Status GetParameters( + Status get_parameters( drivers::VelodyneSensorConfiguration & sensor_configuration, drivers::VelodyneCalibrationConfiguration & calibration_configuration); /// @brief Convert seconds to chrono::nanoseconds /// @param seconds /// @return chrono::nanoseconds - static inline std::chrono::nanoseconds SecondsToChronoNanoSeconds(const double seconds) + static inline std::chrono::nanoseconds seconds_to_chrono_nano_seconds(const double seconds) { return std::chrono::duration_cast( std::chrono::duration(seconds)); @@ -75,11 +75,11 @@ class VelodyneRosDecoderTest final : public rclcpp::Node /// @brief Get current status of this driver /// @return Current status - Status GetStatus(); + Status get_status(); /// @brief Read the specified bag file and compare the constructed point clouds with the /// corresponding PCD files - void ReadBag(); + void read_bag(); /* void SetUp() override { // Setup things that should occur before every test instance should go here @@ -91,11 +91,11 @@ class VelodyneRosDecoderTest final : public rclcpp::Node } */ private: - std::string bag_path; - std::string storage_id; - std::string format; - std::string target_topic; - std::string correction_file_path; + std::string bag_path_; + std::string storage_id_; + std::string format_; + std::string target_topic_; + std::string correction_file_path_; }; } // namespace ros diff --git a/nebula_tests/velodyne/velodyne_ros_decoder_test_vls128.cpp b/nebula_tests/velodyne/velodyne_ros_decoder_test_vls128.cpp index 6075f8d79..f415268fb 100644 --- a/nebula_tests/velodyne/velodyne_ros_decoder_test_vls128.cpp +++ b/nebula_tests/velodyne/velodyne_ros_decoder_test_vls128.cpp @@ -31,7 +31,7 @@ VelodyneRosDecoderTest::VelodyneRosDecoderTest( drivers::VelodyneCalibrationConfiguration calibration_configuration; drivers::VelodyneSensorConfiguration sensor_configuration; - wrapper_status_ = GetParameters(sensor_configuration, calibration_configuration); + wrapper_status_ = get_parameters(sensor_configuration, calibration_configuration); if (Status::OK != wrapper_status_) { RCLCPP_ERROR_STREAM(this->get_logger(), this->get_name() << " Error: " << wrapper_status_); return; @@ -45,27 +45,27 @@ VelodyneRosDecoderTest::VelodyneRosDecoderTest( std::make_shared(sensor_configuration); RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << ". Driver "); - wrapper_status_ = InitializeDriver(sensor_cfg_ptr_, calibration_cfg_ptr_); + wrapper_status_ = initialize_driver(sensor_cfg_ptr_, calibration_cfg_ptr_); RCLCPP_INFO_STREAM(this->get_logger(), this->get_name() << "Wrapper=" << wrapper_status_); } -Status VelodyneRosDecoderTest::InitializeDriver( +Status VelodyneRosDecoderTest::initialize_driver( std::shared_ptr sensor_configuration, std::shared_ptr calibration_configuration) { // driver should be initialized here with proper decoder driver_ptr_ = std::make_shared(sensor_configuration, calibration_configuration); - return driver_ptr_->GetStatus(); + return driver_ptr_->get_status(); } -Status VelodyneRosDecoderTest::GetStatus() +Status VelodyneRosDecoderTest::get_status() { return wrapper_status_; } -Status VelodyneRosDecoderTest::GetParameters( +Status VelodyneRosDecoderTest::get_parameters( drivers::VelodyneSensorConfiguration & sensor_configuration, drivers::VelodyneCalibrationConfiguration & calibration_configuration) { @@ -83,7 +83,7 @@ Status VelodyneRosDecoderTest::GetParameters( descriptor.additional_constraints = ""; this->declare_parameter("sensor_model", "VLS128"); sensor_configuration.sensor_model = - nebula::drivers::SensorModelFromString(this->get_parameter("sensor_model").as_string()); + nebula::drivers::sensor_model_from_string(this->get_parameter("sensor_model").as_string()); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -93,7 +93,7 @@ Status VelodyneRosDecoderTest::GetParameters( descriptor.additional_constraints = ""; this->declare_parameter("return_mode", "Dual", descriptor); sensor_configuration.return_mode = - nebula::drivers::ReturnModeFromString(this->get_parameter("return_mode").as_string()); + nebula::drivers::return_mode_from_string(this->get_parameter("return_mode").as_string()); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -201,8 +201,8 @@ Status VelodyneRosDecoderTest::GetParameters( descriptor.additional_constraints = ""; this->declare_parameter( "bag_path", (bag_root_dir / "vls128" / "1614315746471294674").string(), descriptor); - bag_path = this->get_parameter("bag_path").as_string(); - std::cout << bag_path << std::endl; + bag_path_ = this->get_parameter("bag_path").as_string(); + std::cout << bag_path_ << std::endl; } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -211,7 +211,7 @@ Status VelodyneRosDecoderTest::GetParameters( descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; this->declare_parameter("storage_id", "sqlite3", descriptor); - storage_id = this->get_parameter("storage_id").as_string(); + storage_id_ = this->get_parameter("storage_id").as_string(); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -220,7 +220,7 @@ Status VelodyneRosDecoderTest::GetParameters( descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; this->declare_parameter("format", "cdr", descriptor); - format = this->get_parameter("format").as_string(); + format_ = this->get_parameter("format").as_string(); } { rcl_interfaces::msg::ParameterDescriptor descriptor; @@ -229,7 +229,7 @@ Status VelodyneRosDecoderTest::GetParameters( descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; this->declare_parameter("target_topic", "/velodyne_packets", descriptor); - target_topic = this->get_parameter("target_topic").as_string(); + target_topic_ = this->get_parameter("target_topic").as_string(); } if (sensor_configuration.sensor_model == nebula::drivers::SensorModel::UNKNOWN) { @@ -245,7 +245,7 @@ Status VelodyneRosDecoderTest::GetParameters( return Status::INVALID_CALIBRATION_FILE; } else { auto cal_status = - calibration_configuration.LoadFromFile(calibration_configuration.calibration_file); + calibration_configuration.load_from_file(calibration_configuration.calibration_file); if (cal_status != Status::OK) { RCLCPP_ERROR_STREAM( this->get_logger(), @@ -258,7 +258,7 @@ Status VelodyneRosDecoderTest::GetParameters( return Status::OK; } -void printPCD(nebula::drivers::NebulaPointCloudPtr pp) +void print_pcd(nebula::drivers::NebulaPointCloudPtr pp) { for (auto p : pp->points) { std::cout << "(" << p.x << ", " << p.y << "," << p.z << "): " << p.intensity << ", " @@ -267,7 +267,7 @@ void printPCD(nebula::drivers::NebulaPointCloudPtr pp) } } -void checkPCDs(nebula::drivers::NebulaPointCloudPtr pp1, nebula::drivers::NebulaPointCloudPtr pp2) +void check_pcds(nebula::drivers::NebulaPointCloudPtr pp1, nebula::drivers::NebulaPointCloudPtr pp2) { EXPECT_EQ(pp1->points.size(), pp2->points.size()); for (uint32_t i = 0; i < pp1->points.size(); i++) { @@ -284,7 +284,7 @@ void checkPCDs(nebula::drivers::NebulaPointCloudPtr pp1, nebula::drivers::Nebula } } -void checkPCDs(nebula::drivers::NebulaPointCloudPtr pp1, pcl::PointCloud::Ptr pp2) +void check_pcds(nebula::drivers::NebulaPointCloudPtr pp1, pcl::PointCloud::Ptr pp2) { EXPECT_EQ(pp1->points.size(), pp2->points.size()); for (uint32_t i = 0; i < pp1->points.size(); i++) { @@ -296,17 +296,17 @@ void checkPCDs(nebula::drivers::NebulaPointCloudPtr pp1, pcl::PointCloud serialization; nebula::drivers::NebulaPointCloudPtr pointcloud(new nebula::drivers::NebulaPointCloud); pcl::PointCloud::Ptr ref_pointcloud(new pcl::PointCloud); @@ -331,7 +331,7 @@ void VelodyneRosDecoderTest::ReadBag() std::cout << "Found topic name " << bag_message->topic_name << std::endl; - if (bag_message->topic_name == target_topic) { + if (bag_message->topic_name == target_topic_) { velodyne_msgs::msg::VelodyneScan extracted_msg; rclcpp::SerializedMessage extracted_serialized_msg(*bag_message->serialized_data); serialization.deserialize_message(&extracted_serialized_msg, &extracted_msg); @@ -341,7 +341,7 @@ void VelodyneRosDecoderTest::ReadBag() auto extracted_msg_ptr = std::make_shared(extracted_msg); for (auto & pkt : extracted_msg.packets) { - auto pointcloud_ts = driver_ptr_->ParseCloudPacket( + auto pointcloud_ts = driver_ptr_->parse_cloud_packet( std::vector(pkt.data.begin(), pkt.data.end()), pkt.stamp.sec); auto pointcloud = std::get<0>(pointcloud_ts); @@ -357,7 +357,7 @@ void VelodyneRosDecoderTest::ReadBag() std::cout << "exists: " << target_pcd_path << std::endl; auto rt = pcd_reader.read(target_pcd_path.string(), *ref_pointcloud); std::cout << rt << " loaded: " << target_pcd_path << std::endl; - checkPCDs(pointcloud, ref_pointcloud); + check_pcds(pointcloud, ref_pointcloud); check_cnt++; ref_pointcloud.reset(new pcl::PointCloud); } diff --git a/nebula_tests/velodyne/velodyne_ros_decoder_test_vls128.hpp b/nebula_tests/velodyne/velodyne_ros_decoder_test_vls128.hpp index 8787e8b0d..7777effad 100644 --- a/nebula_tests/velodyne/velodyne_ros_decoder_test_vls128.hpp +++ b/nebula_tests/velodyne/velodyne_ros_decoder_test_vls128.hpp @@ -42,15 +42,15 @@ class VelodyneRosDecoderTest final : public rclcpp::Node std::shared_ptr calibration_cfg_ptr_; std::shared_ptr sensor_cfg_ptr_; - Status InitializeDriver( + Status initialize_driver( std::shared_ptr sensor_configuration, std::shared_ptr calibration_configuration); - Status GetParameters( + Status get_parameters( drivers::VelodyneSensorConfiguration & sensor_configuration, drivers::VelodyneCalibrationConfiguration & calibration_configuration); - static inline std::chrono::nanoseconds SecondsToChronoNanoSeconds(const double seconds) + static inline std::chrono::nanoseconds seconds_to_chrono_nano_seconds(const double seconds) { return std::chrono::duration_cast( std::chrono::duration(seconds)); @@ -60,16 +60,16 @@ class VelodyneRosDecoderTest final : public rclcpp::Node explicit VelodyneRosDecoderTest( const rclcpp::NodeOptions & options, const std::string & node_name); - void ReceiveScanMsgCallback(const velodyne_msgs::msg::VelodyneScan::SharedPtr scan_msg); - Status GetStatus(); - void ReadBag(); + void receive_scan_msg_callback(const velodyne_msgs::msg::VelodyneScan::SharedPtr scan_msg); + Status get_status(); + void read_bag(); private: - std::string bag_path; - std::string storage_id; - std::string format; - std::string target_topic; - std::string correction_file_path; + std::string bag_path_; + std::string storage_id_; + std::string format_; + std::string target_topic_; + std::string correction_file_path_; }; } // namespace ros