File bpearl_v4.hpp
File List > decoders > bpearl_v4.hpp
Go to the documentation of this file
// Copyright 2024 TIER IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#pragma once
#include "nebula_decoders/nebula_decoders_robosense/decoders/robosense_packet.hpp"
#include "nebula_decoders/nebula_decoders_robosense/decoders/robosense_sensor.hpp"
#include "boost/endian/buffers.hpp"
#include <bitset>
#include <cstddef>
#include <cstdint>
#include <map>
#include <memory>
#include <string>
using namespace boost::endian; // NOLINT(build/namespaces)
namespace nebula
{
namespace drivers
{
namespace robosense_packet
{
namespace bpearl_v4
{
#pragma pack(push, 1)
struct Header
{
big_uint64_buf_t header_id;
uint8_t reserved_first[4];
big_uint32_buf_t packet_count;
uint8_t reserved_second[4];
Timestamp timestamp;
uint8_t reserved_third[1];
big_uint8_buf_t lidar_type;
big_uint8_buf_t lidar_model;
uint8_t reserved_fourth[9];
};
struct Packet : public PacketBase<12, 32, 2, 100>
{
typedef Body<Block<Unit, Packet::N_CHANNELS>, Packet::N_BLOCKS> body_t;
Header header;
body_t body;
big_uint48_buf_t tail;
};
struct OperatingStatus
{
big_uint8_buf_t reserved_first;
big_uint16_buf_t machine_current;
big_uint24_buf_t reserved_second;
big_uint16_buf_t machine_voltage;
uint8_t reserved_third[16];
};
struct FaultDiagnosis
{
big_uint16_buf_t startup_times;
big_uint32_buf_t reserved;
big_uint8_buf_t gps_status;
big_uint16_buf_t machine_temp;
uint8_t reserved_first[11];
big_uint16_buf_t phase;
big_uint16_buf_t rotation_speed;
};
struct InfoPacket
{
big_uint64_buf_t header;
big_uint16_buf_t motor_speed_setting;
Ethernet ethernet;
FovSetting fov_setting;
big_uint16_buf_t tcp_msop_port;
big_uint16_buf_t phase_lock;
FirmwareVersion mainboard_firmware_version;
FirmwareVersion bottom_firmware_version;
FirmwareVersion app_software_version;
FirmwareVersion motor_firmware_version;
uint8_t reserved_first[228];
big_uint8_buf_t baud_rate;
uint8_t reserved_second[3];
SerialNumber serial_number;
uint8_t reserved_third[2];
big_uint8_buf_t return_mode;
big_uint8_buf_t time_sync_mode;
big_uint8_buf_t time_sync_state;
Timestamp time;
OperatingStatus operating_status;
big_uint8_buf_t rotation_direction;
big_uint32_buf_t running_time;
uint8_t reserved_fourth[9];
FaultDiagnosis fault_diagnosis;
uint8_t reserved_fifth[7];
big_uint8_buf_t gprmc[86];
SensorCalibration sensor_calibration;
uint8_t reserved_sixth[586];
big_uint16_buf_t tail;
};
#pragma pack(pop)
} // namespace bpearl_v4
template <>
inline double get_dis_unit<bpearl_v4::Packet>(const bpearl_v4::Packet & /* packet */)
{
return 0.0025;
}
} // namespace robosense_packet
class BpearlV4 : public RobosenseSensor<
robosense_packet::bpearl_v4::Packet, robosense_packet::bpearl_v4::InfoPacket>
{
private:
static constexpr int firing_time_offset_ns_single_[12][32] = {
{0, 167, 334, 500, 667, 834, 1001, 1168, 1334, 1501, 1668,
1835, 2002, 2168, 2335, 2502, 2669, 2836, 3002, 3169, 3336, 3503,
3670, 3836, 4003, 4170, 4337, 4504, 4670, 4837, 5004, 5171},
{5555, 5722, 5889, 6055, 6222, 6389, 6556, 6723, 6889, 7056, 7223,
7390, 7557, 7723, 7890, 8057, 8224, 8391, 8557, 8724, 8891, 9058,
9225, 9391, 9558, 9725, 9892, 10059, 10225, 10392, 10559, 10726},
{11110, 11277, 11444, 11610, 11777, 11944, 12111, 12278, 12444, 12611, 12778,
12945, 13112, 13278, 13445, 13612, 13779, 13946, 14112, 14279, 14446, 14613,
14780, 14946, 15113, 15280, 15447, 15614, 15780, 15947, 16114, 16281},
{16665, 16832, 16999, 17165, 17332, 17499, 17666, 17833, 17999, 18166, 18333,
18500, 18667, 18833, 19000, 19167, 19334, 19501, 19667, 19834, 20001, 20168,
20335, 20501, 20668, 20835, 21002, 21169, 21335, 21502, 21669, 21836},
{22220, 22387, 22554, 22720, 22887, 23054, 23221, 23388, 23554, 23721, 23888,
24055, 24222, 24388, 24555, 24722, 24889, 25056, 25222, 25389, 25556, 25723,
25890, 26056, 26223, 26390, 26557, 26724, 26890, 27057, 27224, 27391},
{27775, 27942, 28109, 28275, 28442, 28609, 28776, 28943, 29109, 29276, 29443,
29610, 29777, 29943, 30110, 30277, 30444, 30611, 30777, 30944, 31111, 31278,
31445, 31611, 31778, 31945, 32112, 32279, 32445, 32612, 32779, 32946},
{33330, 33497, 33664, 33830, 33997, 34164, 34331, 34498, 34664, 34831, 34998,
35165, 35332, 35498, 35665, 35832, 35999, 36166, 36332, 36499, 36666, 36833,
37000, 37166, 37333, 37500, 37667, 37834, 38000, 38167, 38334, 38501},
{38885, 39052, 39219, 39385, 39552, 39719, 39886, 40053, 40219, 40386, 40553,
40720, 40887, 41053, 41220, 41387, 41554, 41721, 41887, 42054, 42221, 42388,
42555, 42721, 42888, 43055, 43222, 43389, 43555, 43722, 43889, 44056},
{44440, 44607, 44774, 44940, 45107, 45274, 45441, 45608, 45774, 45941, 46108,
46275, 46442, 46608, 46775, 46942, 47109, 47276, 47442, 47609, 47776, 47943,
48110, 48276, 48443, 48610, 48777, 48944, 49110, 49277, 49444, 49611},
{49995, 50162, 50329, 50495, 50662, 50829, 50996, 51163, 51329, 51496, 51663,
51830, 51997, 52163, 52330, 52497, 52664, 52831, 52997, 53164, 53331, 53498,
53665, 53831, 53998, 54165, 54332, 54499, 54665, 54832, 54999, 55166},
{55550, 55717, 55884, 56050, 56217, 56384, 56551, 56718, 56884, 57051, 57218,
57385, 57552, 57718, 57885, 58052, 58219, 58386, 58552, 58719, 58886, 59053,
59220, 59386, 59553, 59720, 59887, 60054, 60220, 60387, 60554, 60721},
{61105, 61272, 61439, 61605, 61772, 61939, 62106, 62273, 62439, 62606, 62773,
62940, 63107, 63273, 63440, 63607, 63774, 63941, 64107, 64274, 64441, 64608,
64775, 64941, 65108, 65275, 65442, 65609, 65775, 65942, 66109, 66276}};
static constexpr int firing_time_offset_ns_dual_[12][32]{
{0, 167, 334, 500, 667, 834, 1001, 1168, 1334, 1501, 1668,
1835, 2002, 2168, 2335, 2502, 2669, 2836, 3002, 3169, 3336, 3503,
3670, 3836, 4003, 4170, 4337, 4504, 4670, 4837, 5004, 5171},
{0, 167, 334, 500, 667, 834, 1001, 1168, 1334, 1501, 1668,
1835, 2002, 2168, 2335, 2502, 2669, 2836, 3002, 3169, 3336, 3503,
3670, 3836, 4003, 4170, 4337, 4504, 4670, 4837, 5004, 5171},
{5555, 5722, 5889, 6055, 6222, 6389, 6556, 6723, 6889, 7056, 7223,
7390, 7557, 7723, 7890, 8057, 8224, 8391, 8557, 8724, 8891, 9058,
9225, 9391, 9558, 9725, 9892, 10059, 10225, 10392, 10559, 10726},
{5555, 5722, 5889, 6055, 6222, 6389, 6556, 6723, 6889, 7056, 7223,
7390, 7557, 7723, 7890, 8057, 8224, 8391, 8557, 8724, 8891, 9058,
9225, 9391, 9558, 9725, 9892, 10059, 10225, 10392, 10559, 10726},
{11110, 11277, 11444, 11610, 11777, 11944, 12111, 12278, 12444, 12611, 12778,
12945, 13112, 13278, 13445, 13612, 13779, 13946, 14112, 14279, 14446, 14613,
14780, 14946, 15113, 15280, 15447, 15614, 15780, 15947, 16114, 16281},
{11110, 11277, 11444, 11610, 11777, 11944, 12111, 12278, 12444, 12611, 12778,
12945, 13112, 13278, 13445, 13612, 13779, 13946, 14112, 14279, 14446, 14613,
14780, 14946, 15113, 15280, 15447, 15614, 15780, 15947, 16114, 16281},
{16665, 16832, 16999, 17165, 17332, 17499, 17666, 17833, 17999, 18166, 18333,
18500, 18667, 18833, 19000, 19167, 19334, 19501, 19667, 19834, 20001, 20168,
20335, 20501, 20668, 20835, 21002, 21169, 21335, 21502, 21669, 21836},
{16665, 16832, 16999, 17165, 17332, 17499, 17666, 17833, 17999, 18166, 18333,
18500, 18667, 18833, 19000, 19167, 19334, 19501, 19667, 19834, 20001, 20168,
20335, 20501, 20668, 20835, 21002, 21169, 21335, 21502, 21669, 21836},
{22220, 22387, 22554, 22720, 22887, 23054, 23221, 23388, 23554, 23721, 23888,
24055, 24222, 24388, 24555, 24722, 24889, 25056, 25222, 25389, 25556, 25723,
25890, 26056, 26223, 26390, 26557, 26724, 26890, 27057, 27224, 27391},
{22220, 22387, 22554, 22720, 22887, 23054, 23221, 23388, 23554, 23721, 23888,
24055, 24222, 24388, 24555, 24722, 24889, 25056, 25222, 25389, 25556, 25723,
25890, 26056, 26223, 26390, 26557, 26724, 26890, 27057, 27224, 27391},
{27775, 27942, 28109, 28275, 28442, 28609, 28776, 28943, 29109, 29276, 29443,
29610, 29777, 29943, 30110, 30277, 30444, 30611, 30777, 30944, 31111, 31278,
31445, 31611, 31778, 31945, 32112, 32279, 32445, 32612, 32779, 32946},
{27775, 27942, 28109, 28275, 28442, 28609, 28776, 28943, 29109, 29276, 29443,
29610, 29777, 29943, 30110, 30277, 30444, 30611, 30777, 30944, 31111, 31278,
31445, 31611, 31778, 31945, 32112, 32279, 32445, 32612, 32779, 32946}};
static constexpr uint8_t DUAL_RETURN_FLAG = 0x00;
static constexpr uint8_t STRONGEST_RETURN_FLAG = 0x04;
static constexpr uint8_t LAST_RETURN_FLAG = 0x05;
static constexpr uint8_t FIRST_RETURN_FLAG = 0x06;
static constexpr uint8_t SYNC_MODE_GPS_FLAG = 0x00;
static constexpr uint8_t SYNC_MODE_E2E_FLAG = 0x01;
static constexpr uint8_t SYNC_MODE_P2P_FLAG = 0x02;
static constexpr uint8_t SYNC_MODE_GPTP_FLAG = 0x03;
static constexpr uint8_t SYNC_STATUS_INVALID_FLAG = 0x00;
static constexpr uint8_t SYNC_STATUS_GPS_SUCCESS_FLAG = 0x01;
static constexpr uint8_t SYNC_STATUS_PTP_SUCCESS_FLAG = 0x02;
public:
static constexpr float MIN_RANGE = 0.1f;
static constexpr float MAX_RANGE = 30.f;
static constexpr size_t MAX_SCAN_BUFFER_POINTS = 1152000;
int getPacketRelativePointTimeOffset(
const uint32_t block_id, const uint32_t channel_id,
const std::shared_ptr<const RobosenseSensorConfiguration> & sensor_configuration) override
{
if (sensor_configuration->return_mode == ReturnMode::DUAL)
return firing_time_offset_ns_dual_[block_id][channel_id];
else
return firing_time_offset_ns_single_[block_id][channel_id];
}
ReturnMode getReturnMode(const robosense_packet::bpearl_v4::InfoPacket & info_packet) override
{
switch (info_packet.return_mode.value()) {
case DUAL_RETURN_FLAG:
return ReturnMode::DUAL;
case STRONGEST_RETURN_FLAG:
return ReturnMode::SINGLE_STRONGEST;
case LAST_RETURN_FLAG:
return ReturnMode::SINGLE_LAST;
case FIRST_RETURN_FLAG:
return ReturnMode::SINGLE_FIRST;
default:
return ReturnMode::UNKNOWN;
}
}
RobosenseCalibrationConfiguration getSensorCalibration(
const robosense_packet::bpearl_v4::InfoPacket & info_packet) override
{
return info_packet.sensor_calibration.getCalibration();
}
bool getSyncStatus(const robosense_packet::bpearl_v4::InfoPacket & info_packet) override
{
switch (info_packet.time_sync_state.value()) {
case SYNC_STATUS_INVALID_FLAG:
return false;
case SYNC_STATUS_GPS_SUCCESS_FLAG:
return true;
case SYNC_STATUS_PTP_SUCCESS_FLAG:
return true;
default:
return false;
}
}
std::map<std::string, std::string> getSensorInfo(
const robosense_packet::bpearl_v4::InfoPacket & info_packet) override
{
std::map<std::string, std::string> sensor_info;
sensor_info["motor_speed"] = std::to_string(info_packet.motor_speed_setting.value());
sensor_info["lidar_ip"] = info_packet.ethernet.lidar_ip.to_string();
sensor_info["dest_pc_ip"] = info_packet.ethernet.dest_pc_ip.to_string();
sensor_info["mac_addr"] = info_packet.ethernet.mac_addr.to_string();
sensor_info["lidar_out_msop_port"] =
std::to_string(info_packet.ethernet.lidar_out_msop_port.value());
sensor_info["lidar_out_difop_port"] =
std::to_string(info_packet.ethernet.lidar_out_difop_port.value());
sensor_info["fov_start"] =
robosense_packet::get_float_value(info_packet.fov_setting.fov_start.value());
sensor_info["fov_end"] =
robosense_packet::get_float_value(info_packet.fov_setting.fov_end.value());
sensor_info["tcp_msop_port"] = std::to_string(info_packet.tcp_msop_port.value());
sensor_info["phase_lock"] = std::to_string(info_packet.phase_lock.value());
sensor_info["mainboard_firmware_version"] = info_packet.mainboard_firmware_version.to_string();
sensor_info["bottom_firmware_version"] = info_packet.bottom_firmware_version.to_string();
sensor_info["app_software_version"] = info_packet.app_software_version.to_string();
sensor_info["motor_firmware_version"] = info_packet.motor_firmware_version.to_string();
sensor_info["baud_rate"] = std::to_string(info_packet.baud_rate.value());
sensor_info["serial_number"] = info_packet.serial_number.to_string();
switch (info_packet.return_mode.value()) {
case DUAL_RETURN_FLAG:
sensor_info["return_mode"] = "dual";
break;
case STRONGEST_RETURN_FLAG:
sensor_info["return_mode"] = "strongest";
break;
case LAST_RETURN_FLAG:
sensor_info["return_mode"] = "last";
break;
case FIRST_RETURN_FLAG:
sensor_info["return_mode"] = "first";
break;
default:
sensor_info["return_mode"] = "n/a";
break;
}
switch (info_packet.time_sync_mode.value()) {
case SYNC_MODE_GPS_FLAG:
sensor_info["time_sync_mode"] = "gps";
break;
case SYNC_MODE_E2E_FLAG:
sensor_info["time_sync_mode"] = "e2e";
break;
case SYNC_MODE_P2P_FLAG:
sensor_info["time_sync_mode"] = "p2p";
break;
case SYNC_MODE_GPTP_FLAG:
sensor_info["time_sync_mode"] = "gptp";
break;
default:
sensor_info["time_sync_mode"] = "n/a";
break;
}
switch (info_packet.time_sync_state.value()) {
case SYNC_STATUS_INVALID_FLAG:
sensor_info["time_sync_state"] = "time_sync_invalid";
break;
case SYNC_STATUS_GPS_SUCCESS_FLAG:
sensor_info["time_sync_state"] = "gps_time_sync_successful";
break;
case SYNC_STATUS_PTP_SUCCESS_FLAG:
sensor_info["time_sync_state"] = "ptp_time_sync_successful";
break;
default:
sensor_info["time_sync_state"] = "n/a";
}
sensor_info["time"] = std::to_string(info_packet.time.get_time_in_ns());
sensor_info["machine_current"] =
robosense_packet::get_float_value(info_packet.operating_status.machine_current.value());
sensor_info["machine_voltage"] =
robosense_packet::get_float_value(info_packet.operating_status.machine_voltage.value());
sensor_info["rotation_direction"] = std::to_string(info_packet.rotation_direction.value());
sensor_info["running_time"] = std::to_string(info_packet.running_time.value());
sensor_info["startup_times"] =
std::to_string(info_packet.fault_diagnosis.startup_times.value());
const std::bitset<8> gps_st_bits{info_packet.fault_diagnosis.gps_status.value()};
if (gps_st_bits[0])
sensor_info["pps_lock"] = "valid";
else
sensor_info["pps_lock"] = "invalid";
if (gps_st_bits[1])
sensor_info["gprmc_lock"] = "valid";
else
sensor_info["gprmc_lock"] = "invalid";
if (gps_st_bits[2])
sensor_info["utc_lock"] = "synchronized";
else
sensor_info["utc_lock"] = "not_synchronized";
if (gps_st_bits[3])
sensor_info["gprmc_input_status"] = "received_gprmc";
else
sensor_info["gprmc_input_status"] = "no_gprmc";
if (gps_st_bits[4])
sensor_info["pps_input_status"] = "received_pps";
else
sensor_info["pps_input_status"] = "no_pps";
sensor_info["machine_temp"] =
robosense_packet::get_float_value(info_packet.fault_diagnosis.machine_temp.value());
sensor_info["phase"] = std::to_string(info_packet.fault_diagnosis.phase.value());
sensor_info["rotation_speed"] =
std::to_string(info_packet.fault_diagnosis.rotation_speed.value());
std::string gprmc_string;
for (auto i : info_packet.gprmc) {
gprmc_string += static_cast<char>(i.value());
}
sensor_info["gprmc_string"] = gprmc_string;
return sensor_info;
}
};
} // namespace drivers
} // namespace nebula