@@ -29,6 +29,25 @@ constexpr uint16_t kMaxIntensity = 30000;
constexpr uint32_t kMaxPointsPerRev = 12800 ; // 320° at the finest 0.025° step
constexpr int kConnectTimeoutMs = 2000 ;
// The head measures over a fixed window of every turn — from 20° to 340° in
// device angles, 0° at the rear — and is blind over the remaining 40°. Both
// the revolution's size and every packet's position are anchored to that
// window (the vendor ROS driver hard-codes the same two numbers). Consistent
// with MODEL_ESPE_LGA60's -160…160 preset once angle_offset_deg (-180) is
// applied.
constexpr uint16_t kSweepStartDeg = 20 ;
constexpr uint16_t kSweepEndDeg = 340 ;
constexpr float kSweepSpanDeg = static_cast < float > ( kSweepEndDeg - kSweepStartDeg ) ;
constexpr float kFullTurnDeg = 360.f ;
// Rotation periods above this are stalls/reconnects, not a spin rate (the
// device runs at 10 or 20 Hz): don't publish timing derived from them.
constexpr float kMaxRevPeriodS = 1.f ;
// A point no packet ever delivered — "invalid", distinct from the infinity
// that means the device looked and got no return.
const float kMissingPoint = std : : numeric_limits < float > : : quiet_NaN ( ) ;
uint16_t be16 ( const uint8_t * p ) {
return static_cast < uint16_t > ( ( p [ 0 ] < < 8 ) | p [ 1 ] ) ;
}
@@ -69,8 +88,11 @@ ErrorCode EspeDriver::open() {
}
recv_buf_ . clear ( ) ;
pending_ranges_ . clear ( ) ;
pending_intensities_ . clear ( ) ;
angle_inc_deg_ = 0.f ;
points_total_ = 0 ;
pending_time _= 0 ;
have_last_rev _ = false ;
scan_ready_ = false ;
espe_error_status_ . reset ( ) ;
latest_diag_ = Diagnostics { } ;
@@ -147,6 +169,7 @@ bool EspeDriver::parse_buffer() {
recv_buf_ . erase ( 0 , sizeof ( kRangeMagic ) ) ; // bogus header — resync
continue ;
}
// Both counters describe this packet; the clamp is the vendor's.
if ( data_size > measure_size ) data_size = measure_size ;
size_t frame_size = kRangeHeaderSize + static_cast < size_t > ( data_size ) * 4 ;
@@ -161,39 +184,86 @@ bool EspeDriver::parse_buffer() {
}
}
// Range frame: "HISN", then big-endian u16 start_angle, end_angle (deg),
// d ata_size (points in this frame), data_position (cumulative points incl.
// this frame), measu re_ size (points per revolution), time; then data_size ×
// 4 B little-endian (u16 distance mm, u16 intensity).
void EspeDriver : : handle_range_frame ( const uint8_t * frame , uint16_t data_size ) {
uint16_t start_angle = be16 ( frame + 4 ) ;
uint16_t end_angle = be16 ( frame + 6 ) ;
uint16_t data_position = be16 ( frame + 10 ) ;
uint16_t measure_size = be16 ( frame + 12 ) ;
pending_time_ = be16 ( frame + 14 ) ;
// Latch the angular step and size the revolution around it. Kept stable once
// l atched: the wire angles are whole degrees, so a step re-derived from a
// mid-sweep packet jitters, and re- sizing would drop the sweep in flight.
void EspeDriver : : set_resolution ( float inc_deg ) {
if ( ! ( inc_deg > 0.f ) ) return ;
const long total = std : : lround ( kSweepSpanDeg / inc_deg ) ;
if ( total < 2 | | total > static_cast < long > ( kMaxPointsPerRev ) ) return ; // implausible step
if ( angle_inc_deg_ > 0.f & & static_cast < uint32_t > ( total ) = = points_total_ ) return ;
// First frame of a revolution (or geometry changed) → start a new one.
if ( points_total_ ! = measure_size | | data_position < = data_size ) {
points_total_ = measure_size ;
rev_start_deg_ = static_cast < float > ( start_angle ) ;
angle_inc_deg_ = static_cast < float > ( end_angle - start_angle ) / measure_size ;
pending_ranges_ . assign ( points_total_ , 0.f ) ;
angle_inc_deg_ = inc_deg ;
points_total_ = static_cast < uint32_t > ( total ) ;
begin_revolution ( ) ;
}
void EspeDriver : : begin_revolution ( ) {
if ( points_total_ = = 0 ) return ;
pending_ranges_ . assign ( points_total_ , kMissingPoint ) ;
pending_intensities_ . assign ( points_total_ , 0.f ) ;
}
if ( angle_inc_deg_ < = 0.f ) { points_total_ = 0 ; return ; }
// start_ angl e is normally constant across the revolution, so this is just
// the cumulative position; the angle term covers firmware that advances it.
int32_t begin = static_cast < int32_t > ( std : : lround (
( static_cast < float > ( start_angle ) - rev_start_deg_ ) / angle_inc_deg_ ) )
// R ange frame: "HISN", then big-endian u16 start_angle, end_angle (the
// angular window THIS packet covers, whole degrees), data_size (points in
// this packet's payload), data_position and measure_size (the vendor's
// "position"/"count" of the current packet's points), time; then data_size ×
// 4 B little-endian (u16 distance mm, u16 intensity).
//
// A packet is a slice of the sweep, not a revolution: the device splits every
// 20°→340° sweep into several of them, the first opening at 20° and the last
// closing at 340°. So the revolution holds 320°/step points, NOT measure_size
// — reading measure_size as the revolution size (as this driver first did)
// emits one scan per packet, each covering only that packet's few degrees.
void EspeDriver : : handle_range_frame ( const uint8_t * frame , uint16_t data_size ) {
const uint16_t start_angle = be16 ( frame + 4 ) ;
const uint16_t end_angle = be16 ( frame + 6 ) ;
const uint16_t data_position = be16 ( frame + 10 ) ;
const uint16_t measure_size = be16 ( frame + 12 ) ;
// frame + 14 is a 16-bit device counter the vendor header marks as "time
// flag (not enabled)"; see finish_scan() for why it is not a timestamp.
// Step = this packet's angular span / its point count. Taken from the
// packet that opens a sweep — the one the vendor driver trusts — or from
// whatever arrives first while nothing is latched yet.
if ( start_angle = = kSweepStartDeg | | angle_inc_deg_ < = 0.f ) {
const float span = static_cast < float > ( end_angle ) - static_cast < float > ( start_angle ) ;
if ( span > 0.f & & measure_size > 0 )
set_resolution ( span / static_cast < float > ( measure_size ) ) ;
}
if ( angle_inc_deg_ < = 0.f ) return ; // step still unknown — nowhere to put the points
// First packet of a sweep: drop anything a lost closing packet left behind.
if ( start_angle = = kSweepStartDeg & & data_position < = data_size ) begin_revolution ( ) ;
// Index of this packet's first point within the sweep, verbatim from the
// vendor driver: the angular offset from 20°, plus what the header's own
// counters carry. Exactly one of the two terms moves, whichever way the
// firmware numbers its packets — either start_angle walks the sweep while
// data_position stays at this packet's own count, or start_angle stays at
// 20° while data_position accumulates — so the sum is the packet's true
// start index in both cases.
const int32_t begin =
static_cast < int32_t > ( std : : lround (
( static_cast < float > ( start_angle ) - static_cast < float > ( kSweepStartDeg ) ) / angle_inc_deg_ ) )
+ static_cast < int32_t > ( data_position ) - static_cast < int32_t > ( data_size ) ;
// Integer wire angles make 320°/step land a point or two short of what the
// device actually streams; grow rather than clip the tail (the vendor
// driver does the same).
const size_t needed = static_cast < size_t > ( begin > 0 ? begin : 0 ) + data_size ;
if ( needed > pending_ranges_ . size ( ) & & needed < = kMaxPointsPerRev ) {
pending_ranges_ . resize ( needed , kMissingPoint ) ;
pending_intensities_ . resize ( needed , 0.f ) ;
points_total_ = static_cast < uint32_t > ( needed ) ;
}
const uint8_t * p = frame + kRangeHeaderSize ;
for ( uint16_t i = 0 ; i < data_size ; + + i , p + = 4 ) {
int32_t idx = begin + i ;
if ( idx < 0 | | idx > = static_cast < int32_t > ( points_total_ ) ) continue ;
uint16_t dist = le16 ( p + 0 ) ;
uint16_t inten = le16 ( p + 2 ) ;
const int32_t idx = begin + i ;
if ( idx < 0 | | idx > = static_cast < int32_t > ( pending_ranges_ . size ( ) ) ) continue ;
const uint16_t dist = le16 ( p + 0 ) ;
const uint16_t inten = le16 ( p + 2 ) ;
pending_ranges_ [ idx ] = ( dist > kMaxDistanceMm )
? std : : numeric_limits < float > : : infinity ( )
: static_cast < float > ( dist ) * 1e-3 f ; // mm -> m
@@ -203,22 +273,53 @@ void EspeDriver::handle_range_frame(const uint8_t* frame, uint16_t data_size) {
* ( 255.f / kMaxIntensity ) ;
}
if ( data_position > = points_total_ ) finish_scan ( ) ;
// The packet that closes the sweep at 340°, with its point counter full,
// ends the revolution — the vendor driver's condition unchanged. It holds
// whichever way the firmware numbers packets: per-packet counters make
// data_position == measure_size true on every packet (so the 340° edge
// decides), cumulative ones make it true only on the sweep's last packet.
if ( end_angle = = kSweepEndDeg & & data_position = = measure_size ) finish_scan ( ) ;
}
void EspeDriver : : finish_scan ( ) {
if ( pending_ranges_ . size ( ) < 2 | | angle_inc_deg_ < = 0.f ) return ;
LaserScan & scan = ready_result_ . scan ;
scan = LaserScan { } ;
scan . timestamp_ms = pending_time_ ; // header "time" field, unit unverified
// The header's 16-bit "time" field is a device counter of unverified unit
// (the vendor header calls it "not enabled" and its ROS driver never
// stamps a scan with it), while this field is contracted to be a device
// clock in ms — leave it at 0 and report timing from the rotation below.
scan . timestamp_ms = 0 ;
scan . ranges = std : : move ( pending_ranges_ ) ;
scan . intensities = std : : move ( pending_intensities_ ) ;
scan . angle_min = ( rev_start_deg_ + cfg_ . angle_offset_deg ) * kDeg2Rad ;
scan . angle_min = ( static_cast < float > ( kSweepStartDeg ) + cfg_ . angle_offset_deg ) * kDeg2Rad ;
scan . angle_increment = angle_inc_deg_ * kDeg2Rad ;
scan . angle_max = scan . angle_min +
scan . angle_increment * static_cast < float > ( scan . ranges . size ( ) - 1 ) ;
scan . range_min = cfg_ . range_min_m ;
scan . range_max = cfg_ . range_max_m ;
// Timing: revolutions complete one rotation period apart, but the points
// in one only span the 320° the head measures — the remaining 40° is dead
// time before the next sweep starts. scan_time is what a consumer
// subtracts from the arrival time to date the FIRST point, so it must be
// the sweep, not the period (the vendor ROS driver stamps with the same
// 320/360 factor). Consequence: a spin rate read back as 1/scan_time is
// 360/320 higher than the mechanical one. The first revolution has
// nothing to measure against — leave the fields at 0 and let the consumer
// fall back.
const auto now = std : : chrono : : steady_clock : : now ( ) ;
if ( have_last_rev_ ) {
const float period = std : : chrono : : duration < float > ( now - last_rev_end_ ) . count ( ) ;
if ( period > 0.f & & period < kMaxRevPeriodS ) {
scan . scan_time = period * ( kSweepSpanDeg / kFullTurnDeg ) ;
scan . time_increment = scan . scan_time / static_cast < float > ( scan . ranges . size ( ) - 1 ) ;
}
}
last_rev_end_ = now ;
have_last_rev_ = true ;
finalize_scan ( scan , cfg_ , inverted_ ) ;
ExtraInfo & info = ready_result_ . info ;
@@ -227,12 +328,9 @@ void EspeDriver::finish_scan() {
info . espe_error_status = espe_error_status_ ;
latest_diag_ = decode_diagnostics ( info ) ;
latest_diag_ . device_timestamp_ms = scan . timestamp_ms ;
mark_scan_decoded ( ) ;
pend ing _ranges_ . clear ( ) ;
pending_intensities_ . clear ( ) ;
points_total_ = 0 ;
beg in_revolution ( ) ; // the vectors above were moved out — restore them
scan_ready_ = true ;
}
