diff --git a/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp b/nebula_common/include/nebula_common/velodyne/velodyne_common.hpp index 505f95dbd..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,7 +62,7 @@ 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; @@ -72,7 +72,7 @@ struct VelodyneCalibrationConfiguration : CalibrationConfigurationBase /// @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/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_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 7ca6fdf5e..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 @@ -65,12 +65,12 @@ class VelodyneDriver : NebulaDriverBase /// @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_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 01901b606..72a2ba00a 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/velodyne_driver.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/velodyne_driver.cpp @@ -49,7 +49,7 @@ Status VelodyneDriver::set_calibration_configuration( 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/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/velodyne/velodyne_ros_offline_extract_bag_pcd.cpp b/nebula_examples/src/velodyne/velodyne_ros_offline_extract_bag_pcd.cpp index f62f85ec6..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) { @@ -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_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_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/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 cdc4877c8..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_; diff --git a/nebula_ros/src/velodyne/decoder_wrapper.cpp b/nebula_ros/src/velodyne/decoder_wrapper.cpp index 1c6d0f18f..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,8 +201,8 @@ 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 || @@ -212,8 +212,8 @@ void VelodyneDecoderWrapper::ProcessCloudPacket( 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 || @@ -223,12 +223,12 @@ void VelodyneDecoderWrapper::ProcessCloudPacket( 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 1d398d70b..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,10 +62,10 @@ 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; @@ -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; } @@ -219,7 +219,7 @@ rcl_interfaces::msg::SetParametersResult VelodyneRosWrapper::OnParameterChange( 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/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 760f2a711..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) { @@ -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 6f7a1d28a..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) { @@ -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 6fd113d13..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) { @@ -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