-
Notifications
You must be signed in to change notification settings - Fork 79
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
* Simplify lidar scan assembly code As double-buffering is used for the scan assemnly (in contrast to the ouster example) we can simply zero out our buffer for the new frame. We then no longer need to special-case zeroing-out missing packets. * Initialize FullRotationAccumulator with batchReady == false * Implement buffered packet processing This commit splits the processing of lidar data into two seperate threads. Previously, the code receiving sensor data from the wire was blocked by the lidar or imu processors, causing dataloss on slower systems (Issue #96). * Modifies sensor interface so receiving functions get passed a pointer to a buffer to write their data to * Implements ringbuffers for lidar and imu data in ouster_driver * Splits processing into two seperate threads: * receiveData receives from the wire and fills ringbuffers * processData takes packets from the queue, assembles the lidar scan and runs the processors * Threads are implemented as pure threads instead of ros timers, as timing inconsistencies for the receiver thread were also leading to data loss * Revert "Simplify lidar scan assembly code" This reverts commit 581b35f. This modification to the client code is not necessary for the buffered packet processing, and conflicts with changes since made in the ouster_examples repository. * Lock against race on [lidar|imu]_packet_head Moves the modification of the head indices to before the mutex unlock. This is only necessary for the warning about buffer overruns to work correctly. We don't need to lock against the actual data in the buffer. * Move ringbuffer code to it's own header * Fix linting issues Co-authored-by: Mirko Kugelmeier <[email protected]>
- Loading branch information
Showing
9 changed files
with
261 additions
and
80 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
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
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
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,114 @@ | ||
| // Copyright 2022, Mirko Kugelmeier | ||
| // 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. | ||
|
|
||
| #ifndef ROS2_OUSTER__RINGBUFFER_HPP_ | ||
| #define ROS2_OUSTER__RINGBUFFER_HPP_ | ||
|
|
||
| #include <atomic> | ||
| #include <memory> | ||
|
|
||
| namespace ros2_ouster | ||
| { | ||
|
|
||
| /** | ||
| * @class ros2_ouster::RingBuffer | ||
| * @brief A simple circular buffer implementation used for queued processing of incoming lidar / imu data | ||
| */ | ||
| class RingBuffer | ||
| { | ||
| public: | ||
| /** | ||
| * Create a new RingBuffer instance | ||
| * @param element_size The size (in bytes) of each element | ||
| * @param num_elements Number of maximum elements in the buffer. | ||
| */ | ||
| RingBuffer(std::size_t element_size, std::size_t num_elements) | ||
| : _element_size(element_size), | ||
| _num_elements(num_elements), | ||
| _head(0), | ||
| _tail(0), | ||
| _buf(new uint8_t[element_size * _num_elements]) | ||
| {} | ||
|
|
||
| /** | ||
| * @brief Check whether there is any data to be read from the buffer at head() | ||
| * @return true if there are no elements in the buffer | ||
| */ | ||
| bool empty() | ||
| { | ||
| return _head == _tail; | ||
| } | ||
|
|
||
| /** | ||
| * @brief Check if the ringbuffer is full | ||
| * If the buffer is full, adding new data to it will overwrite or corrupt data at the head() position. | ||
| * This function is only safe to call in the producing thread, i.e. no calls to push() may occur simultaneously | ||
| * @return true if the buffer is full | ||
| */ | ||
| bool full() | ||
| { | ||
| // Make sure we have a consistent value for the head index here. | ||
| // This makes sure we do not return 'false positives' when the buffer is not actually | ||
| // full due to a race on _head between the conditions. | ||
| bool head = _head; | ||
|
|
||
| // This ringbuffer implementation lets both head and tail loop twice around the actual number of | ||
| // elements in the buffer to encode the difference between the empty() and full() states. The | ||
| // buffer is full if head and tail refer to the same element but on different 'iterations' | ||
| // of the loop | ||
| return ((_tail * 2) % _num_elements) == head && head != _tail; | ||
| } | ||
|
|
||
| /// @return A pointer to the current element to read from | ||
| uint8_t * head() | ||
| { | ||
| return &_buf[(_head % _num_elements) * _element_size]; | ||
| } | ||
|
|
||
| /// @return A pointer to the current element to write to | ||
| uint8_t * tail() | ||
| { | ||
| return &_buf[(_tail % _num_elements) * _element_size]; | ||
| } | ||
|
|
||
| /// @brief Remove the current head element from the queue | ||
| void pop() | ||
| { | ||
| _head = (_head + 1) % (_num_elements * 2); | ||
| } | ||
|
|
||
| /// @brief Add a new element (with data already filled at the location pointed to by tail()) | ||
| /// to the buffer | ||
| void push() | ||
| { | ||
| _tail = (_tail + 1) % (_num_elements * 2); | ||
| } | ||
|
|
||
| protected: | ||
| // The size of each individual element in bytes and the max number of elements in the buffer | ||
| const std::size_t _element_size; | ||
| const std::size_t _num_elements; | ||
|
|
||
| // Since the ringbuffer is used across threads that produce / consume the data, we use | ||
| // std::atomic for head and tail to prevent re-ordering of assignments to these variables | ||
| std::atomic<std::size_t> _head; | ||
| std::atomic<std::size_t> _tail; | ||
|
|
||
| // Defining the backing buffer as a unique_ptr gives us our dtor and correct copy/move | ||
| // semantics for free | ||
| std::unique_ptr<uint8_t[]> _buf; | ||
| }; | ||
|
|
||
| } // namespace ros2_ouster | ||
|
|
||
| #endif // ROS2_OUSTER__RINGBUFFER_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
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.