diff --git a/.cspell.json b/.cspell.json index 3574bbfd3..275633662 100644 --- a/.cspell.json +++ b/.cspell.json @@ -9,6 +9,8 @@ "block_id", "gprmc", "Hesai", + "idxs", + "manc", "memcpy", "mkdoxy", "nohup", @@ -21,6 +23,7 @@ "Pdelay", "QT", "rclcpp", + "Robosense", "schedutil", "STD_COUT", "stds", diff --git a/nebula_common/include/nebula_common/nebula_common.hpp b/nebula_common/include/nebula_common/nebula_common.hpp index 3199efe24..e328165b1 100644 --- a/nebula_common/include/nebula_common/nebula_common.hpp +++ b/nebula_common/include/nebula_common/nebula_common.hpp @@ -356,6 +356,7 @@ enum class SensorModel { ROBOSENSE_BPEARL, ROBOSENSE_BPEARL_V3, ROBOSENSE_BPEARL_V4, + ROBOSENSE_M1 }; /// @brief not used? @@ -446,6 +447,9 @@ inline std::ostream & operator<<(std::ostream & os, nebula::drivers::SensorModel case SensorModel::ROBOSENSE_BPEARL_V4: os << "BPEARL V4.0"; break; + case SensorModel::ROBOSENSE_M1: + os << "M1"; + break; case SensorModel::UNKNOWN: os << "Sensor Unknown"; break; @@ -520,6 +524,7 @@ inline SensorModel SensorModelFromString(const std::string & sensor_model) if (sensor_model == "Bpearl") return SensorModel::ROBOSENSE_BPEARL; if (sensor_model == "Bpearl_V3") return SensorModel::ROBOSENSE_BPEARL_V3; if (sensor_model == "Bpearl_V4") return SensorModel::ROBOSENSE_BPEARL_V4; + if (sensor_model == "M1") return SensorModel::ROBOSENSE_M1; return SensorModel::UNKNOWN; } @@ -567,6 +572,8 @@ inline std::string SensorModelToString(const SensorModel & sensor_model) return "Bpearl_V3"; case SensorModel::ROBOSENSE_BPEARL_V4: return "Bpearl_V4"; + case SensorModel::ROBOSENSE_M1: + return "M1"; default: return "UNKNOWN"; } diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/m1.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/m1.hpp new file mode 100644 index 000000000..56eebd820 --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_robosense/decoders/m1.hpp @@ -0,0 +1,312 @@ +#pragma once + +#include "nebula_decoders/nebula_decoders_common/sensor.hpp" +#include "nebula_decoders/nebula_decoders_common/sensor_mixins/angles.hpp" +#include "nebula_decoders/nebula_decoders_common/sensor_mixins/channel.hpp" +#include "nebula_decoders/nebula_decoders_common/sensor_mixins/distance.hpp" +#include "nebula_decoders/nebula_decoders_common/sensor_mixins/intensity.hpp" +#include "nebula_decoders/nebula_decoders_common/sensor_mixins/return_mode.hpp" +#include "nebula_decoders/nebula_decoders_common/sensor_mixins/scan_completion.hpp" +#include "nebula_decoders/nebula_decoders_common/sensor_mixins/timestamp.hpp" +#include "nebula_decoders/nebula_decoders_common/sensor_mixins/validity.hpp" +#include "nebula_decoders/nebula_decoders_common/util.hpp" +#include "nebula_decoders/nebula_decoders_robosense/decoders/robosense_packet.hpp" +#include "nebula_decoders/nebula_decoders_robosense/decoders/sensor_info.hpp" + +#include "boost/endian/buffers.hpp" + +#include +#include +#include +#include + +using namespace boost::endian; +using namespace nebula::drivers::sensor_mixins; + +namespace nebula +{ +namespace drivers +{ +namespace robosense_packet +{ +namespace m1 +{ +#pragma pack(push, 1) + +struct Header +{ + big_uint32_buf_t header_id; + big_uint16_buf_t sequence_number; + big_uint16_buf_t protocol_version; + uint8_t wave_mode; + uint8_t time_sync_mode; + Timestamp timestamp; + uint8_t reserved_1[10]; + uint8_t lidar_type; + uint8_t mems_tmp; +}; + +template +struct M1Block +{ + uint8_t time_offset; + uint8_t return_seq; + UnitT units[UnitN]; + typedef UnitT unit_t; +}; + +struct M1Unit +{ + big_uint16_buf_t distance; + big_uint16_buf_t elevation; + big_uint16_buf_t azimuth; + big_uint8_buf_t reflectivity; + big_uint16_buf_t reserved; +}; + +struct Packet : public PacketBase<25, 5, 2, 100> +{ + typedef Body, Packet::N_BLOCKS> body_t; + Header header; + body_t body; + big_uint24_buf_t tail; +}; + +struct InfoPacket +{ + uint8_t header[8]; + uint8_t reserved_1; + uint8_t frame_rate; + + IpAddress lidar_ip; + IpAddress dest_pc_ip; + MacAddress mac_addr; + big_uint16_buf_t pc_dest_msop_port; + big_uint16_buf_t pc_dest_difop_port; + + big_uint16_buf_t h_fov_start; + big_uint16_buf_t h_fov_end; + big_uint16_buf_t v_fov_start; + big_uint16_buf_t v_fov_end; + + FirmwareVersion mb_programmable_logic_fw_version; + FirmwareVersion mb_programming_system_fw_version; + + SerialNumber serial_number; + + uint8_t return_mode; + + uint8_t time_sync_mode; + uint8_t sync_status; + Timestamp time; + + uint8_t operating_status[31]; + uint8_t diagnostics_reserved[29]; + + uint8_t reserved_2[60]; + uint8_t reserved_3[71]; +}; + +#pragma pack(pop) +} // namespace m1 +} // namespace robosense_packet + +using namespace sensor_mixins; + +class M1 : public SensorBase, + public PointTimestampMixin, + public robosense_packet::RobosensePacketTimestampMixin, + public ReturnModeMixin, + public AnglesInUnitMixin, + public BasicReflectivityMixin, + public DistanceMixin, + public ChannelIsUnitMixin, + public NonZeroDistanceIsValidMixin, + public ScanCompletionMixin, + public AngleCorrectorMixin +{ +private: + uint32_t last_sequence_number_{}; + + std::array sin_{}; + std::array cos_{}; + + float internalAngleToRad(int32_t angle) const + { + return static_cast(angle - 32768) / 100.0f * M_PI / 180.0f; + } + +public: + static constexpr float MIN_RANGE = 0.2f; + static constexpr float MAX_RANGE = 150.f; + static constexpr size_t MAX_SCAN_BUFFER_POINTS = 1152000; + + static constexpr std::array RETURN_GROUP_STRIDE{1, 0, 0}; + + M1() + { + for (size_t i = 0; i < 65536; ++i) { + sin_[i] = std::sin(internalAngleToRad(i)); + cos_[i] = std::cos(internalAngleToRad(i)); + } + } + + size_t getDecodeGroupIndex(const uint8_t * const raw_packet) const override + { + constexpr size_t header_offset = offsetof(robosense_packet::m1::Packet, header); + constexpr size_t wave_mode_offset = offsetof(robosense_packet::m1::Header, wave_mode); + constexpr size_t seq_num_offset = offsetof(robosense_packet::m1::Header, sequence_number); + // In single return mode, the return index is always 0 + if (raw_packet[header_offset + wave_mode_offset] != 0x00) { + return 0; + } + + // From the datasheet: in dual return mode, the odd-numbered packets store the first return + // [...] the even numbered packets store the second return + bool is_odd = raw_packet[header_offset + seq_num_offset] % 2 == 1; + return is_odd ? 0 : 1; + } + + bool checkScanCompleted(const packet_t & packet, const size_t /* block_id */) override + { + const uint32_t current_sequence_number = getFieldValue(packet.header.sequence_number); + bool completed = current_sequence_number < last_sequence_number_; + last_sequence_number_ = current_sequence_number; + return completed; + } + + int32_t getPacketRelativeTimestamp( + const packet_t & packet, const size_t block_id, const size_t /* channel_id */, + const ReturnMode /* return_mode */) const override + { + const auto * block = getBlock(packet, block_id); + return static_cast(getFieldValue(block->time_offset)) * 1000; + }; + + double getDistanceUnit(const packet_t & /* packet */) const override { return 0.005; } + + /// @brief Get the distance value of the given unit in meters. + double getDistance( + const packet_t & packet, const size_t block_id, const size_t unit_id) const override + { + const auto * unit = getUnit(packet, block_id, unit_id); + return getFieldValue(unit->distance) * getDistanceUnit(packet); + } + + sensor_mixins::CorrectedAngleData getCorrectedAngleData( + int32_t raw_azimuth, int32_t raw_elevation) const override + { + sensor_mixins::CorrectedAngleData data; + data.azimuth_rad = internalAngleToRad(raw_azimuth); + data.elevation_rad = internalAngleToRad(raw_elevation); + data.sin_azimuth = sin_[raw_azimuth]; + data.cos_azimuth = cos_[raw_azimuth]; + data.sin_elevation = sin_[raw_elevation]; + data.cos_elevation = cos_[raw_elevation]; + return data; + } + + ReturnMode getReturnMode( + const packet_t & packet, const SensorConfigurationBase & /* config */) const override + { + switch (getFieldValue(packet.header.wave_mode)) { + case 0x00: + return ReturnMode::DUAL; + case 0x04: + return ReturnMode::SINGLE_STRONGEST; + case 0x05: + return ReturnMode::SINGLE_LAST; + case 0x06: + return ReturnMode::SINGLE_FIRST; + default: + return ReturnMode::UNKNOWN; + } + } +}; + +class M1Info : public SensorInfoBase +{ +public: + typedef typename robosense_packet::m1::InfoPacket packet_t; + + ::std::map getSensorInfo( + const robosense_packet::m1::InfoPacket & info_packet) const override + { + std::map sensor_info; + + sensor_info["lidar_ip"] = info_packet.lidar_ip.to_string(); + sensor_info["dest_pc_ip"] = info_packet.dest_pc_ip.to_string(); + sensor_info["mac_addr"] = info_packet.mac_addr.to_string(); + sensor_info["pc_dest_msop_port"] = std::to_string(info_packet.pc_dest_msop_port.value()); + sensor_info["pc_dest_difop_port"] = std::to_string(info_packet.pc_dest_difop_port.value()); + sensor_info["h_fov_start"] = robosense_packet::get_float_value(info_packet.h_fov_start.value()); + sensor_info["h_fov_end"] = robosense_packet::get_float_value(info_packet.h_fov_end.value()); + sensor_info["mb_programmable_logic_fw_version"] = + info_packet.mb_programmable_logic_fw_version.to_string(); + sensor_info["mb_programming_system_fw_version"] = + info_packet.mb_programming_system_fw_version.to_string(); + sensor_info["serial_number"] = info_packet.serial_number.to_string(); + + switch (getReturnMode(info_packet)) { + case ReturnMode::DUAL: + sensor_info["return_mode"] = "dual"; + break; + case ReturnMode::SINGLE_STRONGEST: + sensor_info["return_mode"] = "strongest"; + break; + case ReturnMode::SINGLE_LAST: + sensor_info["return_mode"] = "last"; + break; + case ReturnMode::SINGLE_FIRST: + sensor_info["return_mode"] = "first"; + break; + default: + sensor_info["return_mode"] = "n/a"; + break; + } + + switch (info_packet.time_sync_mode) { + case 0: + sensor_info["time_sync_mode"] = "gps"; + break; + case 1: + sensor_info["time_sync_mode"] = "e2e"; + break; + case 2: + sensor_info["time_sync_mode"] = "p2p"; + break; + case 3: + sensor_info["time_sync_mode"] = "gptp"; + break; + } + + switch (info_packet.sync_status) { + case 0: + sensor_info["sync_status"] = "time_sync_invalid"; + break; + case 1: + sensor_info["sync_status"] = "gps_time_sync_successful"; + break; + case 2: + sensor_info["sync_status"] = "ptp_time_sync_successful"; + break; + } + + sensor_info["time"] = std::to_string(info_packet.time.get_time_in_ns()); + + return sensor_info; + } + + std::optional getSensorCalibration( + const robosense_packet::m1::InfoPacket & /* info_packet */) const override + { + return {}; // M1 has no calibration + } + + bool getSyncStatus(const robosense_packet::m1::InfoPacket & /* info_packet */) const override { + return false; //TODO(mojomex) + } +}; + +} // namespace drivers +} // namespace nebula \ No newline at end of file diff --git a/nebula_decoders/src/nebula_decoders_robosense/robosense_driver.cpp b/nebula_decoders/src/nebula_decoders_robosense/robosense_driver.cpp index 90384db36..706b757cf 100644 --- a/nebula_decoders/src/nebula_decoders_robosense/robosense_driver.cpp +++ b/nebula_decoders/src/nebula_decoders_robosense/robosense_driver.cpp @@ -32,6 +32,10 @@ RobosenseDriver::RobosenseDriver( std::shared_ptr sensor = std::make_shared(sensor_configuration, calibration_configuration); scan_decoder_.reset(new RobosenseDecoder(sensor_configuration, sensor)); } break; + case SensorModel::ROBOSENSE_M1: { + std::shared_ptr sensor = std::make_shared(); + scan_decoder_.reset(new RobosenseDecoder(sensor_configuration, sensor)); + } break; default: driver_status_ = nebula::Status::NOT_INITIALIZED; throw std::runtime_error("Driver not Implemented for selected sensor."); diff --git a/nebula_decoders/src/nebula_decoders_robosense/robosense_info_driver.cpp b/nebula_decoders/src/nebula_decoders_robosense/robosense_info_driver.cpp index 161a791e0..6ddf6cf5d 100644 --- a/nebula_decoders/src/nebula_decoders_robosense/robosense_info_driver.cpp +++ b/nebula_decoders/src/nebula_decoders_robosense/robosense_info_driver.cpp @@ -23,7 +23,9 @@ RobosenseInfoDriver::RobosenseInfoDriver( case SensorModel::ROBOSENSE_HELIOS: info_decoder_.reset(new RobosenseInfoDecoder()); break; - + case SensorModel::ROBOSENSE_M1: + info_decoder_.reset(new RobosenseInfoDecoder()); + break; default: driver_status_ = nebula::Status::NOT_INITIALIZED; throw std::runtime_error("Driver not Implemented for selected sensor."); diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_robosense/robosense_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_robosense/robosense_hw_interface.hpp index cd45984af..6a0e4cdea 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_robosense/robosense_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_robosense/robosense_hw_interface.hpp @@ -30,6 +30,8 @@ constexpr uint16_t HELIOS_PACKET_SIZE = 1248; constexpr uint16_t HELIOS_INFO_PACKET_SIZE = 1248; constexpr uint16_t BPEARL_PACKET_SIZE = 1248; constexpr uint16_t BPEARL_INFO_PACKET_SIZE = 1248; +constexpr uint16_t M1_PACKET_SIZE = 1210; +constexpr uint16_t M1_INFO_PACKET_SIZE = 256; /// @brief Hardware interface of Robosense driver class RobosenseHwInterface : NebulaHwInterfaceBase diff --git a/nebula_hw_interfaces/src/nebula_robosense_hw_interfaces/robosense_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_robosense_hw_interfaces/robosense_hw_interface.cpp index b917cbe2a..3af0f14f9 100644 --- a/nebula_hw_interfaces/src/nebula_robosense_hw_interfaces/robosense_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_robosense_hw_interfaces/robosense_hw_interface.cpp @@ -50,16 +50,20 @@ void RobosenseHwInterface::ReceiveCloudPacketCallback(const std::vector scan_cloud_ptr_->packets.emplace_back(msop_packet); - int current_phase{}; bool comp_flg = false; - const auto & data = scan_cloud_ptr_->packets.back().data; - current_phase = (data[azimuth_index_ + 1] & 0xff) + ((data[azimuth_index_] & 0xff) << 8); + if (sensor_configuration_->sensor_model != SensorModel::ROBOSENSE_M1) { + int current_phase; + const auto & data = scan_cloud_ptr_->packets.back().data; + current_phase = (data[azimuth_index_ + 1] & 0xff) + ((data[azimuth_index_] & 0xff) << 8); - current_phase = (static_cast(current_phase) + 36000 - scan_phase) % 36000; + current_phase = (static_cast(current_phase) + 36000 - scan_phase) % 36000; - if (current_phase >= prev_phase_ || scan_cloud_ptr_->packets.size() < 2) { - prev_phase_ = current_phase; + if (current_phase >= prev_phase_ || scan_cloud_ptr_->packets.size() < 2) { + prev_phase_ = current_phase; + } else { + comp_flg = true; + } } else { comp_flg = true; } @@ -164,21 +168,28 @@ Status RobosenseHwInterface::SetSensorConfiguration( case SensorModel::ROBOSENSE_BPEARL: case SensorModel::ROBOSENSE_BPEARL_V3: case SensorModel::ROBOSENSE_BPEARL_V4: - azimuth_index_ = 44; - is_valid_packet_ = [](size_t packet_size) { return (packet_size == BPEARL_PACKET_SIZE); }; - is_valid_info_packet_ = [](size_t packet_size) { - return (packet_size == BPEARL_INFO_PACKET_SIZE); - }; + azimuth_index_ = 44; + is_valid_packet_ = [](size_t packet_size) { return (packet_size == BPEARL_PACKET_SIZE); }; + is_valid_info_packet_ = [](size_t packet_size) { + return (packet_size == BPEARL_INFO_PACKET_SIZE); + }; break; case SensorModel::ROBOSENSE_HELIOS: - azimuth_index_ = 44; - is_valid_packet_ = [](size_t packet_size) { return (packet_size == HELIOS_PACKET_SIZE); }; - is_valid_info_packet_ = [](size_t packet_size) { - return (packet_size == HELIOS_INFO_PACKET_SIZE); - }; + azimuth_index_ = 44; + is_valid_packet_ = [](size_t packet_size) { return (packet_size == HELIOS_PACKET_SIZE); }; + is_valid_info_packet_ = [](size_t packet_size) { + return (packet_size == HELIOS_INFO_PACKET_SIZE); + }; + break; + case SensorModel::ROBOSENSE_M1: + azimuth_index_ = 0; // unused + is_valid_packet_ = [](size_t packet_size) { return (packet_size == M1_PACKET_SIZE); }; + is_valid_info_packet_ = [](size_t packet_size) { + return (packet_size == M1_INFO_PACKET_SIZE); + }; break; default: - status = Status::INVALID_SENSOR_MODEL; + status = Status::INVALID_SENSOR_MODEL; break; } diff --git a/nebula_ros/config/robosense/M1.yaml b/nebula_ros/config/robosense/M1.yaml new file mode 100644 index 000000000..02a44355e --- /dev/null +++ b/nebula_ros/config/robosense/M1.yaml @@ -0,0 +1,10 @@ +/**: + ros__parameters: + sensor_model: "M1" # See readme for supported models + sensor_ip: "192.168.1.200" # Lidar Sensor IP + host_ip: "192.168.1.102" # Broadcast IP from Sensor + frame_id: "robosense" + data_port: 6699 # LiDAR Data Port (MSOP) + gnss_port: 7788 # LiDAR GNSS Port (DIFOP) + diag_span: 1000 # milliseconds + dual_return_distance_threshold: 0.1 diff --git a/nebula_ros/launch/nebula_launch.py b/nebula_ros/launch/nebula_launch.py index f84c0004b..22d8509bc 100644 --- a/nebula_ros/launch/nebula_launch.py +++ b/nebula_ros/launch/nebula_launch.py @@ -20,7 +20,7 @@ def get_lidar_make(sensor_name): return "Hesai", ".csv" elif sensor_name[:3].lower() in ["hdl", "vlp", "vls"]: return "Velodyne", ".yaml" - elif sensor_name.lower() in ["helios", "bpearl"]: + elif sensor_name.lower() in ["helios", "bpearl", "m1"]: return "Robosense", None return "unrecognized_sensor_model" @@ -144,7 +144,7 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("container", ""), add_launch_arg("config_file", ""), add_launch_arg("sensor_model", ""), - add_launch_arg("sensor_ip", "192.168.1.201"), + add_launch_arg("sensor_ip", "192.168.1.200"), add_launch_arg("return_mode", "Dual"), add_launch_arg("launch_hw", "true"), add_launch_arg("setup_sensor", "true"), diff --git a/nebula_ros/launch/robosense_launch.all_hw.xml b/nebula_ros/launch/robosense_launch.all_hw.xml index 4d36c99ff..da5e632bf 100644 --- a/nebula_ros/launch/robosense_launch.all_hw.xml +++ b/nebula_ros/launch/robosense_launch.all_hw.xml @@ -1,6 +1,6 @@ - + diff --git a/nebula_ros/src/robosense/robosense_decoder_ros_wrapper.cpp b/nebula_ros/src/robosense/robosense_decoder_ros_wrapper.cpp index e2e997f08..666d6bfeb 100644 --- a/nebula_ros/src/robosense/robosense_decoder_ros_wrapper.cpp +++ b/nebula_ros/src/robosense/robosense_decoder_ros_wrapper.cpp @@ -46,7 +46,7 @@ RobosenseDriverRosWrapper::RobosenseDriverRosWrapper(const rclcpp::NodeOptions & void RobosenseDriverRosWrapper::ReceiveScanMsgCallback( const robosense_msgs::msg::RobosenseScan::SharedPtr scan_msg) { - if (!driver_ptr_) { + if (!driver_ptr_) { if (sensor_cfg_ptr_->sensor_model == drivers::SensorModel::ROBOSENSE_BPEARL) { if (scan_msg->packets.back().data[32] == drivers::BPEARL_V4_FLAG) { sensor_cfg_ptr_->sensor_model = drivers::SensorModel::ROBOSENSE_BPEARL_V4;