forked from autowarefoundation/autoware.universe
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
feat: add robosense support (autowarefoundation#77)
* Added Helios and Bpearl as supported models * Created Robosense hardware interface * Created hardware interface ROS 2 wrapper for Robosense * Creating Robosense decoder * Created Robosense angle corrector * Created Robosense decoder * Created decoder ROS 2 wrapper * Fix pragma pack and packet * Delete forgotten parenthesis * Fix byte order issue * Calculate timestamp for each point * Consider return type * Use scan phase and get distance unit from packet * Format code and convert header guards to #pragma once * Added time offsets for dual return mode * Save calibration file * Get calibration from sensor * Fix stringstream new line issue * Load calibration from sensor by default * Close info packet UDP driver after receiving for the first time * Fix calibration file format * Created hardware monitor * Receive DIFOP packet at the same time * Refactor hardware monitor * Create calibration file name dynamically * Get return mode from DIFOP packet * Delete comments * Get n_returns from sensor config * Added dual return distance threshold * Created LoadFromStream method * Added Bpearl * Get hardware info of Bpearl * Added time offsets for Bpearl * Remove calibration configuration from Bpearl hw monitor * Fix the format of hardware monitor * Check calibration format while loading it * Wait for first DIFOP packet before continuing * Tidied up the code * Fix Bpearl return mode issue * Created robosense_msgs * Use robosense_msgs on hw interface and decoder * Fix package name * Publish both MSOP and DIFOP packets from hw interface ros wrapper * Subscribe packets on decoder ros wrapper * Subscribe to robosense difop packets on hw monitor * Updated MAX_SCAN_BUFFER_POINTS * Fixed point return type * Fixed return mode diagnostic * Fixed calibration sign issue * Fixed return mode types * Deleted unused code * Added lidar model to the packet message * Deleted hw_interface from decoder ros wrapper * Checking model type in decoder ros wrapper * Initialize driver for different Bpearl models * Added Bpearl v4.0 * Added Bpearl v4 diag * Added Bpearl V4 to hw monitor * Fixed packet structs * Fix Bpearl v3 timestamp * Handle sensor timestamp usage * Deleted unnecessary params * Fix info driver initialization * Fixed diagnostic format * Added Robosense to python launch file * Deleted unnecessary parts * Fix Helios range resolution * Fix diagnostic info for Bpearl v3 * Added generic float function * Fixed bpearl diag * Fixed Helios diag * Fixed getSyncStatus * Fix axes * Change Helios 5515 name to only Helios * Add robosense words to .cspell.json * refactor(nebula_common.hpp): convert if chain to switch-case * refactor(nebula_decoders): add "using namespace" to simplify big endian types * refactor(nebula_decoders): use switch statements over if-else * fix(nebula_decoders): add necessary header * refactor(nebula_decoders): get rid of magic numbers * refactor(nebula_decoders): add using namespace for boost:endian * refactor: get rid of magic numbers * chore: add formulas for unpacking difop packet * refactor(robosense_hw_monitor): remove unnecessary log * fix(robosense_packet.hpp): change nanoseconds to microseconds * refactor(robosense_packet.hpp): throw unknown resolution error * chore(cspell): add robosense current and voltage words * fix(robosense): create corrected channel numbers * fix(robosense): handle ptp synchronization
- Loading branch information
Showing
41 changed files
with
4,352 additions
and
6 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -31,6 +31,13 @@ | |
| "Vccint", | ||
| "XT", | ||
| "XTM", | ||
| "DHAVE" | ||
| "DHAVE", | ||
| "Bpearl", | ||
| "Helios", | ||
| "Msop", | ||
| "Difop", | ||
| "gptp", | ||
| "Idat", | ||
| "Vdat" | ||
| ] | ||
| } | ||
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
202 changes: 202 additions & 0 deletions
202
nebula_common/include/nebula_common/robosense/robosense_common.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,202 @@ | ||
| #pragma once | ||
|
|
||
| #include "nebula_common/nebula_common.hpp" | ||
| #include "nebula_common/nebula_status.hpp" | ||
|
|
||
| #include <bitset> | ||
| #include <cmath> | ||
| #include <fstream> | ||
| #include <iostream> | ||
| #include <optional> | ||
| #include <sstream> | ||
| #include <vector> | ||
|
|
||
| namespace nebula | ||
| { | ||
| namespace drivers | ||
| { | ||
|
|
||
| // Flag for detecting Bpearl version | ||
| constexpr uint8_t BPEARL_V4_FLAG = 0x04; | ||
|
|
||
| /// @brief struct for Robosense sensor configuration | ||
| struct RobosenseSensorConfiguration : SensorConfigurationBase | ||
| { | ||
| uint16_t gnss_port{}; // difop | ||
| double scan_phase{}; // start/end angle | ||
| double dual_return_distance_threshold{}; | ||
| uint16_t rotation_speed; | ||
| uint16_t cloud_min_angle; | ||
| uint16_t cloud_max_angle; | ||
| }; | ||
|
|
||
| /// @brief Convert RobosenseSensorConfiguration to string (Overloading the << operator) | ||
| /// @param os | ||
| /// @param arg | ||
| /// @return stream | ||
| inline std::ostream & operator<<(std::ostream & os, RobosenseSensorConfiguration const & arg) | ||
| { | ||
| os << (SensorConfigurationBase)(arg) << ", GnssPort: " << arg.gnss_port | ||
| << ", ScanPhase:" << arg.scan_phase << ", RotationSpeed:" << arg.rotation_speed | ||
| << ", FOV(Start):" << arg.cloud_min_angle << ", FOV(End):" << arg.cloud_max_angle; | ||
| return os; | ||
| } | ||
|
|
||
| /// @brief Convert return mode name to ReturnMode enum (Robosense-specific ReturnModeFromString) | ||
| /// @param return_mode Return mode name (Upper and lower case letters must match) | ||
| /// @return Corresponding ReturnMode | ||
| inline ReturnMode ReturnModeFromStringRobosense(const std::string & return_mode) | ||
| { | ||
| if (return_mode == "Dual") return ReturnMode::DUAL; | ||
| if (return_mode == "Strongest") return ReturnMode::SINGLE_STRONGEST; | ||
| if (return_mode == "Last") return ReturnMode::SINGLE_LAST; | ||
| if (return_mode == "First") return ReturnMode::SINGLE_FIRST; | ||
|
|
||
| return ReturnMode::UNKNOWN; | ||
| } | ||
|
|
||
| size_t GetChannelSize(const SensorModel & model) | ||
| { | ||
| switch (model) { | ||
| case SensorModel::ROBOSENSE_BPEARL_V3: | ||
| return 32; | ||
| case SensorModel::ROBOSENSE_HELIOS: | ||
| return 32; | ||
| } | ||
| } | ||
|
|
||
| struct ChannelCorrection | ||
| { | ||
| float azimuth{NAN}; | ||
| float elevation{NAN}; | ||
| uint16_t channel{}; | ||
|
|
||
| [[nodiscard]] bool has_value() const { return !std::isnan(azimuth) && !std::isnan(elevation); } | ||
| }; | ||
|
|
||
| /// @brief struct for Robosense calibration configuration | ||
| struct RobosenseCalibrationConfiguration : CalibrationConfigurationBase | ||
| { | ||
| std::vector<ChannelCorrection> calibration; | ||
|
|
||
| void SetChannelSize(const size_t channel_num) { calibration.resize(channel_num); } | ||
|
|
||
| template <typename stream_t> | ||
| inline nebula::Status LoadFromStream(stream_t & stream) | ||
| { | ||
| std::string header; | ||
| std::getline(stream, header); | ||
|
|
||
| char sep; | ||
| int laser_id; | ||
| float elevation; | ||
| float azimuth; | ||
| Status load_status = Status::OK; | ||
| for (size_t i = 0; i < calibration.size(); ++i) { | ||
| stream >> laser_id >> sep >> elevation >> sep >> azimuth; | ||
|
|
||
| if (laser_id <= 0 || laser_id > calibration.size()) { | ||
| std::cout << "Invalid laser id: " << laser_id << std::endl; | ||
| load_status = Status::INVALID_CALIBRATION_FILE; | ||
| } | ||
| if (std::isnan(elevation) || std::isnan(azimuth)) { | ||
| std::cout << "Invalid calibration data" << laser_id << "," << elevation << "," << azimuth | ||
| << std::endl; | ||
| load_status = Status::INVALID_CALIBRATION_FILE; | ||
| } | ||
| if ( | ||
| calibration[laser_id - 1].has_value() && calibration[laser_id - 1].elevation != elevation && | ||
| calibration[laser_id - 1].azimuth != azimuth) { | ||
| std::cout << "Duplicate calibration data for laser id: " << laser_id << std::endl; | ||
| load_status = Status::INVALID_CALIBRATION_FILE; | ||
| } | ||
|
|
||
| ChannelCorrection correction{azimuth, elevation}; | ||
| calibration[laser_id - 1] = correction; | ||
| } | ||
|
|
||
| for (auto & calib : calibration) { | ||
| if (!calib.has_value()) { | ||
| std::cout << calib.elevation << "," << calib.azimuth << std::endl; | ||
| std::cout << "Missing calibration data" << std::endl; | ||
| load_status = Status::INVALID_CALIBRATION_FILE; | ||
| } | ||
| } | ||
|
|
||
| if (load_status != Status::OK) { | ||
| for (auto & correction : calibration) { | ||
| correction.elevation = NAN; | ||
| correction.azimuth = NAN; | ||
| } | ||
| } | ||
|
|
||
| return load_status; | ||
| } | ||
|
|
||
| inline nebula::Status LoadFromFile(const std::string & calibration_file) | ||
| { | ||
| std::ifstream ifs(calibration_file); | ||
| if (!ifs) { | ||
| return Status::INVALID_CALIBRATION_FILE; | ||
| } | ||
|
|
||
| const auto status = LoadFromStream(ifs); | ||
| ifs.close(); | ||
| return status; | ||
| } | ||
|
|
||
| /// @brief Loading calibration data (not used) | ||
| /// @param calibration_content | ||
| /// @return Resulting status | ||
| inline nebula::Status LoadFromString(const std::string & calibration_content) | ||
| { | ||
| std::stringstream ss; | ||
| ss << calibration_content; | ||
|
|
||
| const auto status = LoadFromStream(ss); | ||
| return status; | ||
| } | ||
|
|
||
| // inline nebula::Status LoadFromDifop(const std::string & calibration_file) | ||
|
|
||
| /// @brief Saving calibration data (not used) | ||
| /// @param calibration_file | ||
| /// @return Resulting status | ||
| inline nebula::Status SaveFile(const std::string & calibration_file) | ||
| { | ||
| std::ofstream ofs(calibration_file); | ||
| if (!ofs) { | ||
| return Status::CANNOT_SAVE_FILE; | ||
| } | ||
| ofs << "Laser id,Elevation,Azimuth" << std::endl; | ||
|
|
||
| for (size_t i = 0; i < calibration.size(); ++i) { | ||
| auto laser_id = i + 1; | ||
| float elevation = calibration[i].elevation; | ||
| float azimuth = calibration[i].azimuth; | ||
| ofs << laser_id << "," << elevation << "," << azimuth << std::endl; | ||
| } | ||
|
|
||
| ofs.close(); | ||
| return Status::OK; | ||
| } | ||
|
|
||
| [[nodiscard]] inline ChannelCorrection GetCorrection(const size_t channel_id) const | ||
| { | ||
| return calibration[channel_id]; | ||
| } | ||
|
|
||
| void CreateCorrectedChannels() | ||
| { | ||
| for(auto& correction : calibration) { | ||
| uint16_t channel = 0; | ||
| for(const auto& compare:calibration) { | ||
| if(compare.elevation < correction.elevation) ++channel; | ||
| } | ||
| correction.channel = channel; | ||
| } | ||
| } | ||
| }; | ||
|
|
||
| } // namespace drivers | ||
| } // namespace nebula |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.