diff --git a/aem/env.sh b/aem/env.sh index a87d7a8af7..6666900f6f 100755 --- a/aem/env.sh +++ b/aem/env.sh @@ -33,8 +33,8 @@ export APOLLO_ENV_BACKEND="${APOLLO_ENV_BACKEND:-docker}" export APOLLO_ENV_CONTAINER_PREFIX=${APOLLO_ENV_CONTAINER_PREFIX:-apollo_neo_dev_} export APOLLO_ENV_CONTAINER_REPO_X86='registry.baidubce.com/apollo/apollo-env-gpu' export APOLLO_ENV_CONTAINER_REPO_ARM='registry.baidubce.com/apollo/apollo-env-arm' -export APOLLO_ENV_CONTAINER_TAG_X86='9.0-u22-preview' -export APOLLO_ENV_CONTAINER_TAG_ARM='9.0-preview' +export APOLLO_ENV_CONTAINER_TAG_X86='10.0-u22' +export APOLLO_ENV_CONTAINER_TAG_ARM='10.0-u20' export APOLLO_ENV_CONTAINER_NAME="${APOLLO_ENV_CONTAINER_NAME:-${APOLLO_ENV_CONTAINER_PREFIX}${APOLLO_ENV_NAME}}" export APOLLO_ENV_CONTAINER_USER="${APOLLO_ENV_CONTAINER_USER:-${USER}}" export APOLLO_ENV_CONTAINER_GROUP="${APOLLO_ENV_CONTAINER_GROUP:-$(id -gn)}" diff --git a/aem/funcs.sh b/aem/funcs.sh index 20d0f1667d..e9afa203ff 100755 --- a/aem/funcs.sh +++ b/aem/funcs.sh @@ -712,6 +712,10 @@ apollo_create_container_env_options() { # shell history env_opts+=('-e' "HISTFILE=${APOLLO_ENV_WORKROOT}/.cache/.bash_history") + # eplite + cat /etc/bash.bashrc | grep AIPE_WITH_UNIX_DOMAIN_SOCKET >/dev/null 2>&1 + [[ $? == 0 ]] && env_opts+=('-e' "AIPE_WITH_UNIX_DOMAIN_SOCKET=ON") + echo "${env_opts[*]}" } export -f apollo_create_container_env_options diff --git a/cyber/base/BUILD b/cyber/base/BUILD index d7c1d97727..d7d163f96c 100644 --- a/cyber/base/BUILD +++ b/cyber/base/BUILD @@ -6,7 +6,7 @@ package(default_visibility = ["//visibility:public"]) apollo_cc_library( name = "cyber_base", hdrs = [ - # "arena_queue.h", + "arena_queue.h", "atomic_hash_map.h", "atomic_rw_lock.h", "bounded_queue.h", diff --git a/cyber/base/arena_queue.h b/cyber/base/arena_queue.h new file mode 100644 index 0000000000..5abdb50849 --- /dev/null +++ b/cyber/base/arena_queue.h @@ -0,0 +1,262 @@ +/****************************************************************************** + * Copyright 2024 The Apollo Authors. All Rights Reserved. + * + * 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 CYBER_BASE_ARENA_QUEUE_H_ +#define CYBER_BASE_ARENA_QUEUE_H_ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "cyber/base/macros.h" +#include "cyber/base/wait_strategy.h" + +namespace apollo { +namespace cyber { +namespace base { + +template +class ArenaQueue { + public: + using value_type = T; + using size_type = uint64_t; + + public: + ArenaQueue() {} + ArenaQueue& operator=(const ArenaQueue& other) = delete; + ArenaQueue(const ArenaQueue& other) = delete; + ~ArenaQueue(); + bool Init(uint64_t size); + bool Init(uint64_t size, google::protobuf::Arena* arena); + + T* AddBack(); + T* PopFront(); + T* GetBack(); + T* GetFront(); + + uint64_t Size(); + bool Empty(); + uint64_t Head() { return head_.load(); } + uint64_t Tail() { return tail_.load(); } + uint64_t Commit() { return commit_.load(); } + bool NextIndex(uint64_t& index) { + if (Empty()) { + return false; + } + if (arena_) { + if (GetIndex(index) < Tail() - 1) { + index = GetIndex(index + 1); + return true; + } + return false; + } else { + if (index < Size() - 1) { + index = index + 1; + return true; + } + return false; + } + } + bool GetHeadIndex(uint64_t& index) { + if (Empty()) { + return false; + } + if (arena_) { + index = GetIndex(head_ + 1); + return true; + } else { + index = 0; + return true; + } + } + bool GetTailIndex(uint64_t& index) { + if (Empty()) { + return false; + } + if (arena_) { + index = GetIndex(tail_ - 1); + return true; + } else { + index = Size() - 1; + return true; + } + } + bool GetEleByIndex(uint64_t i, T*& ptr) { + if (Empty()) { + return false; + } + if (arena_) { + ptr = pool_[GetIndex(i)]; + return true; + } else { + if (i > Size() - 1) { + return false; + } + ptr = &normal_queue[i]; + return true; + } + } + bool IsArenaEnable() { return arena_; } + + private: + uint64_t GetIndex(uint64_t num); + + alignas(CACHELINE_SIZE) std::atomic head_ = {0}; + alignas(CACHELINE_SIZE) std::atomic tail_ = {1}; + alignas(CACHELINE_SIZE) std::atomic commit_ = {1}; + + uint64_t pool_size_ = 0; + std::vector pool_; + bool arena_; + std::deque normal_queue; +}; + +template +ArenaQueue::~ArenaQueue() {} + +template +inline bool ArenaQueue::Init(uint64_t size) { + arena_ = false; + return true; +} + +template +inline bool ArenaQueue::Init(uint64_t size, google::protobuf::Arena* arena) { + pool_size_ = size + 2; + if (pool_.size() == pool_size_) { + return true; + } + pool_.clear(); + for (uint64_t i = 0; i < pool_size_; ++i) { + pool_.push_back(google::protobuf::Arena::CreateMessage(arena)); + } + arena_ = true; + return true; +} + +template +T* ArenaQueue::GetBack() { + if (Empty()) { + return nullptr; + } + if (arena_) { + return pool_[GetIndex(tail_ - 1)]; + } else { + return &normal_queue.back(); + } +} + +template +T* ArenaQueue::GetFront() { + if (Empty()) { + return nullptr; + } + if (arena_) { + return pool_[GetIndex(head_ + 1)]; + } else { + return &normal_queue.front(); + } +} + +template +T* ArenaQueue::AddBack() { + if (arena_) { + uint64_t new_tail = 0; + uint64_t old_commit = 0; + uint64_t old_tail = tail_.load(std::memory_order_acquire); + do { + new_tail = old_tail + 1; + if (GetIndex(new_tail) == + GetIndex(head_.load(std::memory_order_acquire))) { + return nullptr; + } + } while (!tail_.compare_exchange_weak(old_tail, new_tail, + std::memory_order_acq_rel, + std::memory_order_relaxed)); + do { + old_commit = old_tail; + } while (cyber_unlikely(!commit_.compare_exchange_weak( + old_commit, new_tail, std::memory_order_acq_rel, + std::memory_order_relaxed))); + return pool_[GetIndex(old_tail)]; + } else { + T instance; + normal_queue.push_back(instance); + return &normal_queue.back(); + } +} + +template +T* ArenaQueue::PopFront() { + if (Empty()) { + return nullptr; + } + if (arena_) { + uint64_t new_head = 0; + uint64_t old_head = head_.load(std::memory_order_acquire); + do { + new_head = old_head + 1; + if (new_head == commit_.load(std::memory_order_acquire)) { + return nullptr; + } + } while (!head_.compare_exchange_weak(old_head, new_head, + std::memory_order_acq_rel, + std::memory_order_relaxed)); + return pool_[GetIndex(new_head)]; + } else { + normal_queue.pop_front(); + return nullptr; + } +} + +template +inline uint64_t ArenaQueue::Size() { + if (arena_) { + return tail_ - head_ - 1; + } else { + return normal_queue.size(); + } +} + +template +inline bool ArenaQueue::Empty() { + if (arena_) { + return Size() == 0; + } else { + return normal_queue.empty(); + } +} + +template +inline uint64_t ArenaQueue::GetIndex(uint64_t num) { + return num - (num / pool_size_) * pool_size_; // faster than % +} + +} // namespace base +} // namespace cyber +} // namespace apollo + +#endif // CYBER_BASE_ARENA_QUEUE_H_ diff --git a/cyber/node/writer_base.h b/cyber/node/writer_base.h index 6e2fa22d6f..389cb82baa 100644 --- a/cyber/node/writer_base.h +++ b/cyber/node/writer_base.h @@ -83,6 +83,13 @@ class WriterBase { return role_attr_.channel_name(); } + /** + * @brief Get Writer's Channel id + * + * @return const uint64_t& const reference to the channel id + */ + const uint64_t GetChannelId() const { return role_attr_.channel_id(); } + /** * @brief Is Writer initialized? * diff --git a/cyber/proto/transport_conf.proto b/cyber/proto/transport_conf.proto index 3f31019b56..b6db252815 100644 --- a/cyber/proto/transport_conf.proto +++ b/cyber/proto/transport_conf.proto @@ -21,6 +21,7 @@ message ArenaChannelConf { // 2^31 - 128 * 1024 * 1024, which is hardcode in the underlying implementation optional uint64 max_msg_size = 2 [default = 33554432]; optional uint64 max_pool_size = 3 [default = 32]; + optional uint64 shared_buffer_size = 4 [default = 0]; }; message ArenaShmConf { diff --git a/cyber/tools/cyber_recorder/main.cc b/cyber/tools/cyber_recorder/main.cc index 9b703bd0eb..f208ba3f08 100644 --- a/cyber/tools/cyber_recorder/main.cc +++ b/cyber/tools/cyber_recorder/main.cc @@ -15,13 +15,15 @@ *****************************************************************************/ #include +#include #include +#include #include #include #include #include -#include +#include "cyber/common/environment.h" #include "cyber/common/file.h" #include "cyber/common/time_conversion.h" #include "cyber/init.h" @@ -31,9 +33,10 @@ #include "cyber/tools/cyber_recorder/recoverer.h" #include "cyber/tools/cyber_recorder/spliter.h" -#include "gperftools/profiler.h" +#include "gflags/gflags.h" #include "gperftools/heap-profiler.h" #include "gperftools/malloc_extension.h" +#include "gperftools/profiler.h" using apollo::cyber::common::GetFileName; using apollo::cyber::common::StringToUnixSeconds; @@ -154,8 +157,8 @@ void DisplayUsage(const std::string& binary, const std::string& command, std::cout << "\t-h, --help\t\t\t\tshow help message" << std::endl; break; case 'H': - std::cout << "\t-H, --heap-profule\t\t\t\tprofile heap info" \ - << std::endl; + std::cout << "\t-H, --heap-profule\t\t\t\tprofile heap info" + << std::endl; break; case 'C': std::cout << "\t-C, --cpu-profule\t\t\t\tprofile cpu info" << std::endl; @@ -232,10 +235,30 @@ int main(int argc, char** argv) { enable_heap_profile = true; break; case 'f': - opt_file_vec.emplace_back(std::string(optarg)); + if (apollo::cyber::common::PathIsAbsolute(std::string(optarg))) { + auto opt_file_abs_path = + apollo::cyber::common::GetEnv("PWD") + "/" + std::string(optarg); + if (apollo::cyber::common::PathExists(opt_file_abs_path)) { + opt_file_vec.emplace_back(opt_file_abs_path); + } else { + opt_file_vec.emplace_back(std::string(optarg)); + } + } else { + opt_file_vec.emplace_back(std::string(optarg)); + } for (int i = optind; i < argc; i++) { if (*argv[i] != '-') { - opt_file_vec.emplace_back(std::string(argv[i])); + if (apollo::cyber::common::PathIsAbsolute(std::string(argv[i]))) { + auto opt_file_abs_path = apollo::cyber::common::GetEnv("PWD") + + "/" + std::string(argv[i]); + if (apollo::cyber::common::PathExists(opt_file_abs_path)) { + opt_file_vec.emplace_back(opt_file_abs_path); + } else { + opt_file_vec.emplace_back(std::string(argv[i])); + } + } else { + opt_file_vec.emplace_back(std::string(argv[i])); + } } else { break; } @@ -262,7 +285,13 @@ int main(int argc, char** argv) { } break; case 'o': - opt_output_vec.push_back(std::string(optarg)); + if (apollo::cyber::common::PathIsAbsolute(std::string(optarg))) { + auto opt_output_file_abs_path = + apollo::cyber::common::GetEnv("PWD") + "/" + std::string(optarg); + opt_output_vec.push_back(opt_output_file_abs_path); + } else { + opt_output_vec.push_back(std::string(optarg)); + } break; case 'a': opt_all = true; @@ -382,6 +411,13 @@ int main(int argc, char** argv) { std::cout << "usage: cyber_recorder info file" << std::endl; return -1; } + if (apollo::cyber::common::PathIsAbsolute(file_path)) { + auto file_path_abs = + apollo::cyber::common::GetEnv("PWD") + "/" + std::string(file_path); + if (std::filesystem::exists(file_path_abs)) { + file_path = file_path_abs; + } + } ::apollo::cyber::Init(argv[0]); Info info; bool info_result = info.Display(file_path); @@ -410,7 +446,7 @@ int main(int argc, char** argv) { std::cout << "MUST specify file option (-f)." << std::endl; return -1; } - ::apollo::cyber::Init(argv[0]); + ::apollo::cyber::Init(argv[0], "cyber_recorder"); PlayParam play_param; play_param.is_play_all_channels = opt_all || opt_white_channels.empty(); play_param.is_loop_playback = opt_loop; @@ -441,6 +477,7 @@ int main(int argc, char** argv) { } if (opt_output_vec.empty()) { std::string default_output_file = + apollo::cyber::common::GetEnv("PWD") + "/" + UnixSecondsToString(time(nullptr), "%Y%m%d%H%M%S") + ".record"; opt_output_vec.push_back(default_output_file); } @@ -448,7 +485,7 @@ int main(int argc, char** argv) { auto recorder = std::make_shared(opt_output_vec[0], opt_all, opt_white_channels, opt_black_channels, opt_header); - std::signal(SIGTERM, [](int sig){ + std::signal(SIGTERM, [](int sig) { apollo::cyber::OnShutdown(sig); if (enable_cpu_profile) { ProfilerStop(); @@ -459,7 +496,7 @@ int main(int argc, char** argv) { } }); - std::signal(SIGINT, [](int sig){ + std::signal(SIGINT, [](int sig) { apollo::cyber::OnShutdown(sig); if (enable_cpu_profile) { ProfilerStop(); diff --git a/cyber/transport/shm/protobuf_arena_manager.cc b/cyber/transport/shm/protobuf_arena_manager.cc index 8cbd82cc93..30db9b772b 100644 --- a/cyber/transport/shm/protobuf_arena_manager.cc +++ b/cyber/transport/shm/protobuf_arena_manager.cc @@ -72,9 +72,12 @@ bool ArenaSegment::Init(uint64_t message_size, uint64_t block_num) { } bool ArenaSegment::OpenOrCreate(uint64_t message_size, uint64_t block_num) { + auto arena_conf = + cyber::common::GlobalData::Instance()->GetChannelArenaConf(channel_id_); + auto shared_buffer_size = arena_conf.shared_buffer_size(); auto size = sizeof(ArenaSegmentState) + sizeof(ArenaSegmentBlockDescriptor) * block_num + - message_size * block_num; + message_size * block_num + shared_buffer_size; auto shmid = shmget(static_cast(key_id_), size, 0644 | IPC_CREAT | IPC_EXCL); if (shmid == -1) { @@ -96,6 +99,18 @@ bool ArenaSegment::OpenOrCreate(uint64_t message_size, uint64_t block_num) { } arenas_.resize(block_num, nullptr); + if (shared_buffer_size == 0) { + shared_buffer_arena_ = nullptr; + } else { + google::protobuf::ArenaOptions options; + options.start_block_size = shared_buffer_size; + options.max_block_size = shared_buffer_size; + options.initial_block = reinterpret_cast( + reinterpret_cast(shm_address_) + sizeof(ArenaSegmentState) + + block_num * sizeof(ArenaSegmentBlock) + block_num * message_size); + options.initial_block_size = shared_buffer_size; + shared_buffer_arena_ = std::make_shared(options); + } for (size_t i = 0; i < block_num; i++) { arena_block_address_.push_back( reinterpret_cast(shm_address_) + sizeof(ArenaSegmentState) + @@ -120,6 +135,9 @@ bool ArenaSegment::OpenOrCreate(uint64_t message_size, uint64_t block_num) { } bool ArenaSegment::Open(uint64_t message_size, uint64_t block_num) { + auto arena_conf = + cyber::common::GlobalData::Instance()->GetChannelArenaConf(channel_id_); + auto shared_buffer_size = arena_conf.shared_buffer_size(); auto shmid = shmget(static_cast(key_id_), 0, 0644); if (shmid == -1) { // shm not exist @@ -137,6 +155,18 @@ bool ArenaSegment::Open(uint64_t message_size, uint64_t block_num) { reinterpret_cast(shm_address_) + sizeof(ArenaSegmentState)); arenas_.resize(block_num, nullptr); + if (shared_buffer_size == 0) { + shared_buffer_arena_ = nullptr; + } else { + google::protobuf::ArenaOptions options; + options.start_block_size = shared_buffer_size; + options.max_block_size = shared_buffer_size; + options.initial_block = reinterpret_cast( + reinterpret_cast(shm_address_) + sizeof(ArenaSegmentState) + + block_num * sizeof(ArenaSegmentBlock) + block_num * message_size); + options.initial_block_size = shared_buffer_size; + shared_buffer_arena_ = std::make_shared(options); + } for (size_t i = 0; i < block_num; i++) { arena_block_address_.push_back( reinterpret_cast(shm_address_) + sizeof(ArenaSegmentState) + @@ -170,10 +200,9 @@ bool ArenaSegment::AddBlockWriteLock(uint64_t block_index) { // blocks_[block_index].writing_ref_count_.fetch_add(1); auto& block = blocks_[block_index]; int32_t rw_lock_free = ArenaSegmentBlock::kRWLockFree; - if (!block.lock_num_.compare_exchange_weak(rw_lock_free, - ArenaSegmentBlock::kWriteExclusive, - std::memory_order_acq_rel, - std::memory_order_relaxed)) { + if (!block.lock_num_.compare_exchange_weak( + rw_lock_free, ArenaSegmentBlock::kWriteExclusive, + std::memory_order_acq_rel, std::memory_order_relaxed)) { ADEBUG << "lock num: " << block.lock_num_.load(); return false; } @@ -204,8 +233,8 @@ bool ArenaSegment::AddBlockReadLock(uint64_t block_index) { int32_t try_times = 0; while (!block.lock_num_.compare_exchange_weak(lock_num, lock_num + 1, - std::memory_order_acq_rel, - std::memory_order_relaxed)) { + std::memory_order_acq_rel, + std::memory_order_relaxed)) { ++try_times; if (try_times == ArenaSegmentBlock::kMaxTryLockTimes) { AINFO << "fail to add read lock num, curr num: " << lock_num; @@ -416,6 +445,10 @@ bool ProtobufArenaManager::Enable() { bool ProtobufArenaManager::EnableSegment(uint64_t channel_id) { if (segments_.find(channel_id) != segments_.end()) { + if (arena_buffer_callbacks_.find(channel_id) != + arena_buffer_callbacks_.end()) { + arena_buffer_callbacks_[channel_id](); + } return true; } @@ -443,6 +476,10 @@ bool ProtobufArenaManager::EnableSegment(uint64_t channel_id) { channel_id, arena_conf.max_msg_size(), arena_conf.max_pool_size(), reinterpret_cast(segment_shm_address)); segments_[channel_id] = segment; + if (arena_buffer_callbacks_.find(channel_id) != + arena_buffer_callbacks_.end()) { + arena_buffer_callbacks_[channel_id](); + } return true; } @@ -454,6 +491,9 @@ bool ProtobufArenaManager::Destroy() { for (auto& segment : segments_) { address_allocator_->Deallocate(segment.first); } + for (auto& buffer : non_arena_buffers_) { + delete buffer.second; + } segments_.clear(); init_ = false; diff --git a/cyber/transport/shm/protobuf_arena_manager.h b/cyber/transport/shm/protobuf_arena_manager.h index b423c2fda7..d250ccc7e7 100644 --- a/cyber/transport/shm/protobuf_arena_manager.h +++ b/cyber/transport/shm/protobuf_arena_manager.h @@ -16,16 +16,17 @@ #ifndef CYBER_TRANSPORT_SHM_PROTOBUF_ARENA_MANAGER_H_ #define CYBER_TRANSPORT_SHM_PROTOBUF_ARENA_MANAGER_H_ -#include +#include #include #include -#include +#include #include +#include "cyber/base/arena_queue.h" #include "cyber/base/pthread_rw_lock.h" -#include "cyber/common/macros.h" #include "cyber/common/global_data.h" +#include "cyber/common/macros.h" #include "cyber/message/arena_manager_base.h" #include "cyber/message/arena_message_wrapper.h" #include "cyber/transport/shm/arena_address_allocator.h" @@ -113,6 +114,8 @@ class ArenaSegment { uint64_t key_id_; void* base_address_; void* shm_address_; + std::shared_ptr shared_buffer_arena_; + void* arena_buffer_address_ = nullptr; uint64_t message_capacity_; }; @@ -175,10 +178,61 @@ class ProtobufArenaManager : public message::ArenaManagerBase { const void* message) override; void* GetMessage(message::ArenaMessageWrapper* wrapper) override; - template::value, - M>::type* = nullptr> + void* GetAvailableBuffer(uint64_t channel_id) { + auto segment = this->GetSegment(channel_id); + if (!segment) { + if (non_arena_buffers_.find(channel_id) == non_arena_buffers_.end()) { + return nullptr; + } + return non_arena_buffers_[channel_id]; + } + if (segment->arena_buffer_address_ != nullptr) { + return segment->arena_buffer_address_; + } + return non_arena_buffers_[channel_id]; + } + + template + bool RegisterQueue(uint64_t channel_id, uint64_t size) { + if (non_arena_buffers_.find(channel_id) == non_arena_buffers_.end() || + arena_buffer_callbacks_.find(channel_id) == + arena_buffer_callbacks_.end()) { + auto non_arena_buffer_ptr = new apollo::cyber::base::ArenaQueue(); + non_arena_buffer_ptr->Init(size); + non_arena_buffers_[channel_id] = non_arena_buffer_ptr; + arena_buffer_callbacks_[channel_id] = [this, channel_id, size]() { + auto segment = this->GetSegment(channel_id); + if (!segment) { + ADEBUG << "channel id '" << channel_id << "' not enable"; + ADEBUG << "fallback to use nomarl queue"; + return; + } + if (segment->shared_buffer_arena_ == nullptr) { + ADEBUG << "Not enable arena shared buffer in channel id '" + << channel_id << "'"; + ADEBUG << "fallback to use nomarl queue"; + return; + } + if (segment->arena_buffer_address_ == nullptr) { + auto ptr = google::protobuf::Arena::Create>( + segment->shared_buffer_arena_.get()); + ptr->Init(size, segment->shared_buffer_arena_.get()); + segment->arena_buffer_address_ = reinterpret_cast(ptr); + } + }; + } + // try enable arena buffer + auto segment = GetSegment(channel_id); + if (segment) { + arena_buffer_callbacks_[channel_id](); + } + return true; + } + + template ::value, + M>::type* = nullptr> void AcquireArenaMessage(uint64_t channel_id, std::shared_ptr& ret_msg) { auto arena_conf = cyber::common::GlobalData::Instance()->GetChannelArenaConf(channel_id); @@ -199,32 +253,33 @@ class ProtobufArenaManager : public message::ArenaManagerBase { if (!segment->AcquireBlockToWrite(size, &wb)) { return; } - options.initial_block = reinterpret_cast( - segment->arena_block_address_[wb.block_index_]); + options.initial_block = + reinterpret_cast(segment->arena_block_address_[wb.block_index_]); options.initial_block_size = segment->message_capacity_; if (segment->arenas_[wb.block_index_] != nullptr) { segment->arenas_[wb.block_index_] = nullptr; } - segment->arenas_[wb.block_index_] = \ + segment->arenas_[wb.block_index_] = std::make_shared(options); // deconstructor do nothing to avoid proto // instance deconstructed before arena allocator - ret_msg = std::shared_ptr(google::protobuf::Arena::CreateMessage( - segment->arenas_[wb.block_index_].get()), [segment, wb](M* ptr){ - int32_t lock_num = segment->blocks_[ - wb.block_index_].lock_num_.load(); - if (lock_num < ArenaSegmentBlock::kRWLockFree) { - segment->ReleaseWrittenBlock(wb); - } - }); + ret_msg = std::shared_ptr( + google::protobuf::Arena::CreateMessage( + segment->arenas_[wb.block_index_].get()), + [segment, wb](M* ptr) { + int32_t lock_num = segment->blocks_[wb.block_index_].lock_num_.load(); + if (lock_num < ArenaSegmentBlock::kRWLockFree) { + segment->ReleaseWrittenBlock(wb); + } + }); return; } - template::value, - M>::type* = nullptr> + template ::value, + M>::type* = nullptr> void AcquireArenaMessage(uint64_t channel_id, std::shared_ptr& ret_msg) { return; } @@ -232,6 +287,8 @@ class ProtobufArenaManager : public message::ArenaManagerBase { private: bool init_; std::unordered_map> segments_; + std::unordered_map non_arena_buffers_; + std::unordered_map> arena_buffer_callbacks_; std::mutex segments_mutex_; std::shared_ptr address_allocator_; diff --git a/cyber/transport/transmitter/hybrid_transmitter.h b/cyber/transport/transmitter/hybrid_transmitter.h index aab09b431d..fe176d485c 100644 --- a/cyber/transport/transmitter/hybrid_transmitter.h +++ b/cyber/transport/transmitter/hybrid_transmitter.h @@ -168,9 +168,9 @@ bool HybridTransmitter::Transmit(const MessagePtr& msg, history_->Add(msg, msg_info); bool return_val = false; for (auto& item : transmitters_) { - return_val |= item.second->Transmit(msg, msg_info); + item.second->Transmit(msg, msg_info); } - return return_val; + return true; } template diff --git a/modules/dreamview_plus/conf/dreamview.conf b/modules/dreamview_plus/conf/dreamview.conf index 41e22281b4..d95b3618b4 100644 --- a/modules/dreamview_plus/conf/dreamview.conf +++ b/modules/dreamview_plus/conf/dreamview.conf @@ -3,7 +3,6 @@ --default_data_collection_config_path=modules/dreamview_plus/conf/data_collection_table.pb.txt --default_preprocess_config_path=modules/dreamview_plus/conf/preprocess_table.pb.txt --data_handler_config_path=modules/dreamview_plus/conf/data_handler.conf ---global_components_config_path=modules/dreamview_plus/conf/global_components_config.pb.txt --vehicle_data_config_filename=modules/dreamview_plus/conf/vehicle_data.pb.txt --default_hmi_mode=Default --server_ports=8888 diff --git a/modules/drivers/lidar/common/lidar_component_base_impl.h b/modules/drivers/lidar/common/lidar_component_base_impl.h index a77307c314..45bfd562ef 100644 --- a/modules/drivers/lidar/common/lidar_component_base_impl.h +++ b/modules/drivers/lidar/common/lidar_component_base_impl.h @@ -22,7 +22,9 @@ #include "modules/common_msgs/sensor_msgs/pointcloud.pb.h" #include "modules/drivers/lidar/common/proto/lidar_config_base.pb.h" +#include "cyber/base/arena_queue.h" #include "cyber/cyber.h" +#include "cyber/transport/shm/protobuf_arena_manager.h" #include "modules/drivers/lidar/common/sync_buffering.h" namespace apollo { @@ -52,6 +54,8 @@ class LidarComponentBaseImpl : public apollo::cyber::Component { virtual bool WritePointCloud(const std::shared_ptr& point_cloud); + virtual std::shared_ptr> + GetPointQueue(); // static std::shared_ptr PcdDefaultAllocator( // std::shared_ptr>& pcd_writer); @@ -124,6 +128,29 @@ bool LidarComponentBaseImpl::WriteScan( return scan_writer_->Write(scan_message); } +template +std::shared_ptr> +LidarComponentBaseImpl::GetPointQueue() { + std::shared_ptr> ret; + auto channel_id = pcd_writer_->GetChannelId(); + + auto arena_manager = + apollo::cyber::transport::ProtobufArenaManager::Instance(); + if (!arena_manager->Enable()) { + ADEBUG << "arena manager enable failed."; + } else { + apollo::cyber::base::ArenaQueue* p; + arena_manager->RegisterQueue(channel_id, + 170000); + ret = std::shared_ptr>( + reinterpret_cast< + apollo::cyber::base::ArenaQueue*>( + arena_manager->GetAvailableBuffer(channel_id)), + [](apollo::cyber::base::ArenaQueue* ptr) {}); + } + return ret; +} + template std::shared_ptr LidarComponentBaseImpl::AllocatePointCloud() { diff --git a/modules/drivers/lidar/livox/component/livox_component.cpp b/modules/drivers/lidar/livox/component/livox_component.cpp index 05ce899655..e3b8a98f65 100644 --- a/modules/drivers/lidar/livox/component/livox_component.cpp +++ b/modules/drivers/lidar/livox/component/livox_component.cpp @@ -24,233 +24,272 @@ namespace apollo { namespace drivers { namespace lidar { -void LivoxLidarComponent::BinaryDataProcess( - const unsigned char* data, - int data_type, - int point_size, - uint64_t pkt_timestamp, - uint32_t time_interval) { - if (data_type == kLivoxLidarCartesianCoordinateHighData) { - AINFO << "high data received, dot num = " << point_size; - auto* p_point_data - = reinterpret_cast( - data); - - for (uint32_t i = 0; i < point_size; ++i) { - PointXYZIT point; - point.set_x(p_point_data[i].x * 0.001); - point.set_y(p_point_data[i].y * 0.001); - point.set_z(p_point_data[i].z * 0.001); - point.set_intensity(p_point_data[i].reflectivity); - uint64_t point_timestamp = pkt_timestamp + i * time_interval; - point.set_timestamp(point_timestamp); - integral_queue_.push_back(std::move(point)); - } - - uint64_t integral_timestamp_diff = integral_time_ * 1e9; - int cnt = 0; - AINFO << "old front t = " << integral_queue_.front().timestamp() - << " old back t = " << integral_queue_.back().timestamp(); - while (!integral_queue_.empty() - && integral_queue_.front().timestamp() - < integral_queue_.back().timestamp() - - integral_timestamp_diff) { - integral_queue_.pop_front(); - cnt++; - } - AINFO << "integral point cnt = " << integral_queue_.size() - << ", point pop size = " << cnt; - AINFO << "new front t = " << integral_queue_.front().timestamp() - << " new back t = " << integral_queue_.back().timestamp(); - - } else if (data_type == kLivoxLidarCartesianCoordinateLowData) { - AINFO << "low data received, dot num = " << point_size; - auto* p_point_data - = reinterpret_cast(data); - // todo 后增加 - - } else if (data_type == kLivoxLidarSphericalCoordinateData) { - AINFO << "spherical data received, dot num = " << point_size; - auto* p_point_data - = reinterpret_cast(data); - // todo 后增加 - } -} - -void LivoxLidarComponent::PointCloudCallback( - uint32_t handle, - const uint8_t dev_type, - LivoxLidarEthernetPacket* data, - void* client_data) { - if (data == nullptr) { - return; +void LivoxLidarComponent::BinaryDataProcess(const unsigned char* data, + const int& data_type, + const int& point_size, + const uint64_t& pkt_timestamp, + const uint32_t& time_interval) { + if (data_type == kLivoxLidarCartesianCoordinateHighData) { + ADEBUG << "high data received, dot num = " << point_size; + auto* p_point_data = + reinterpret_cast(data); + + auto integral_queue = LidarComponentBase::GetPointQueue(); + for (uint32_t i = 0; i < point_size; ++i) { + auto point = integral_queue->AddBack(); + point->set_x(p_point_data[i].x * 0.001); + point->set_y(p_point_data[i].y * 0.001); + point->set_z(p_point_data[i].z * 0.001); + point->set_intensity(p_point_data[i].reflectivity); + uint64_t point_timestamp = pkt_timestamp + i * time_interval; + point->set_timestamp(point_timestamp); } - AINFO << boost::format( - "point cloud handle: %d, data_num: %d, data_type: %d, " - "length: " - "%d, frame_counter: %d\n") - % handle % data->dot_num % data->data_type % data->length - % data->frame_cnt; - - size_t byte_size = GetEthPacketByteSize(data); - uint64_t pkt_timestamp = GetEthPacketTimestamp( - data->time_type, data->timestamp, sizeof(data->timestamp)); - uint32_t time_interval = data->time_interval * 100; // 0.1us -> 100ns - - BinaryDataProcess( - data->data, - data->data_type, - data->dot_num, - pkt_timestamp, - time_interval); - - if (byte_size > 0) { - std::shared_ptr scan_message - = std::make_shared(); - scan_message->set_data_type(data->data_type); - scan_message->set_timestamp(pkt_timestamp); - auto* data_addr = static_cast(data->data); - scan_message->set_data(data_addr, byte_size); - scan_message->set_point_size(data->dot_num); - LidarComponentBase::WriteScan(scan_message); + uint64_t integral_timestamp_diff = integral_time_ * 1e9; + int cnt = 0; + ADEBUG << "old front t = " << integral_queue->GetFront()->timestamp() + << " old back t = " << integral_queue->GetBack()->timestamp(); + while (!integral_queue->Empty() && + integral_queue->GetFront()->timestamp() < + integral_queue->GetBack()->timestamp() - + integral_timestamp_diff) { + integral_queue->PopFront(); + cnt++; } + ADEBUG << "integral point cnt = " << integral_queue->Size() + << ", point pop size = " << cnt; + ADEBUG << "new front t = " << integral_queue->GetFront()->timestamp() + << " new back t = " << integral_queue->GetBack()->timestamp(); + + } else if (data_type == kLivoxLidarCartesianCoordinateLowData) { + ADEBUG << "low data received, dot num = " << point_size; + auto* p_point_data = + reinterpret_cast(data); + // todo 后增加 + + } else if (data_type == kLivoxLidarSphericalCoordinateData) { + ADEBUG << "spherical data received, dot num = " << point_size; + auto* p_point_data = reinterpret_cast(data); + // todo 后增加 + } +} - CheckTimestampAndPublishPointCloud(); +void LivoxLidarComponent::PointCloudCallback(uint32_t handle, + const uint8_t dev_type, + LivoxLidarEthernetPacket* data, + void* client_data) { + if (data == nullptr) { + return; + } + + ADEBUG << boost::format( + "point cloud handle: %d, data_num: %d, data_type: %d, " + "length: " + "%d, frame_counter: %d\n") % + handle % data->dot_num % data->data_type % data->length % + data->frame_cnt; + + size_t byte_size = GetEthPacketByteSize(data); + uint64_t pkt_timestamp = GetEthPacketTimestamp( + data->time_type, data->timestamp, sizeof(data->timestamp), data->dot_num, + config_.use_lidar_clock()); + uint32_t time_interval = 100; // 0.1us -> 100ns + + BinaryDataProcess(data->data, data->data_type, data->dot_num, pkt_timestamp, + time_interval); + + if (byte_size > 0) { + std::shared_ptr scan_message = + std::make_shared(); + scan_message->set_data_type(data->data_type); + scan_message->set_timestamp(pkt_timestamp); + auto* data_addr = static_cast(data->data); + scan_message->set_data(data_addr, byte_size); + scan_message->set_point_size(data->dot_num); + LidarComponentBase::WriteScan(scan_message); + } + + CheckTimestampAndPublishPointCloud(); } size_t LivoxLidarComponent::GetEthPacketByteSize( - LivoxLidarEthernetPacket* data) { - size_t byte_size = 0; - if (data == nullptr || data->data == nullptr) { - return 0; - } - switch (data->data_type) { + LivoxLidarEthernetPacket* data) { + size_t byte_size = 0; + if (data == nullptr || data->data == nullptr) { + return 0; + } + switch (data->data_type) { case kLivoxLidarCartesianCoordinateHighData: - byte_size = sizeof(LivoxLidarCartesianHighRawPoint) * data->dot_num; - break; + byte_size = sizeof(LivoxLidarCartesianHighRawPoint) * data->dot_num; + break; case kLivoxLidarCartesianCoordinateLowData: - byte_size = sizeof(LivoxLidarCartesianLowRawPoint) * data->dot_num; - break; + byte_size = sizeof(LivoxLidarCartesianLowRawPoint) * data->dot_num; + break; case kLivoxLidarSphericalCoordinateData: - byte_size = sizeof(LivoxLidarSpherPoint) * data->dot_num; - break; + byte_size = sizeof(LivoxLidarSpherPoint) * data->dot_num; + break; default: - byte_size = 0; - break; - } - return byte_size; + byte_size = 0; + break; + } + return byte_size; } void LivoxLidarComponent::PreparePointsMsg(PointCloud& msg) { - msg.set_height(1); - msg.set_width(msg.point_size() / msg.height()); - msg.set_is_dense(false); - const auto timestamp - = msg.point(static_cast(msg.point_size()) - 1).timestamp(); - msg.set_measurement_time( - GetSecondTimestampFromNanosecondTimestamp(timestamp)); - - double lidar_time = GetSecondTimestampFromNanosecondTimestamp(timestamp); - double diff_time = msg.header().timestamp_sec() - lidar_time; - if (diff_time > 0.2) { - AINFO << "timestamp difference too large " << std::fixed - << std::setprecision(16) - << "system time: " << msg.header().timestamp_sec() - << ", lidar time: " << lidar_time << ", diff is:" << diff_time; - } - - msg.mutable_header()->set_lidar_timestamp(timestamp); + msg.set_height(1); + msg.set_width(msg.point_size() / msg.height()); + msg.set_is_dense(false); + const auto timestamp = + msg.point(static_cast(msg.point_size()) - 1).timestamp(); + msg.set_measurement_time( + GetSecondTimestampFromNanosecondTimestamp(timestamp)); + + double lidar_time = GetSecondTimestampFromNanosecondTimestamp(timestamp); + double diff_time = msg.header().timestamp_sec() - lidar_time; + if (diff_time > 0.2) { + AINFO << "timestamp difference too large " << std::fixed + << std::setprecision(16) + << "system time: " << msg.header().timestamp_sec() + << ", lidar time: " << lidar_time << ", diff is:" << diff_time; + } + + msg.mutable_header()->set_lidar_timestamp(timestamp); } bool LivoxLidarComponent::Init() { - GetProtoConfig(&config_); - RETURN_VAL_IF( - !LidarComponentBase::InitBase( - config_.config_base()), - false); - - integral_time_ = config_.integral_time(); - - if (config_.config_base().source_type() - == LidarConfigBase_SourceType_ONLINE_LIDAR) { - if (!config_.has_enable_sdk_console_log() - || !config_.enable_sdk_console_log()) { - DisableLivoxSdkConsoleLogger(); - } - if (!LivoxLidarSdkInit(config_.lidar_config_file_path().c_str())) { - AERROR << "livox sdk init fail, maybe init by other component"; - // LivoxLidarSdkUninit(); - } - - std::string lidar_ip = config_.lidar_ip(); - uint32_t lidar_handle = 0; - if (!LivoxDispatcher::GetLivoxDispatcherInstance().GetHandleFromIP( - lidar_ip, lidar_handle)) { - AERROR << "livox ip address format error, component init fail"; - return false; - } + GetProtoConfig(&config_); + RETURN_VAL_IF( + !LidarComponentBase::InitBase(config_.config_base()), + false); + + integral_time_ = config_.integral_time(); + + if (config_.config_base().source_type() == + LidarConfigBase_SourceType_ONLINE_LIDAR) { + if (!config_.has_enable_sdk_console_log() || + !config_.enable_sdk_console_log()) { + DisableLivoxSdkConsoleLogger(); + } + if (!LivoxLidarSdkInit(config_.lidar_config_file_path().c_str())) { + AERROR << "livox sdk init fail, maybe init by other component"; + // LivoxLidarSdkUninit(); + } - AINFO << "init lidar, handle = " << lidar_handle; - - LivoxDispatcher::GetLivoxDispatcherInstance() - .RegisterHandleDispatchCallback( - lidar_handle, - std::bind( - &LivoxLidarComponent::PointCloudCallback, - this, - std::placeholders::_1, - std::placeholders::_2, - std::placeholders::_3, - std::placeholders::_4)); - - SetLivoxLidarPointCloudCallBack( - GlobalPointCloudCallback, - reinterpret_cast( - &LivoxDispatcher::GetLivoxDispatcherInstance())); - - AINFO << "livox lidar init success"; + std::string lidar_ip = config_.lidar_ip(); + uint32_t lidar_handle = 0; + if (!LivoxDispatcher::GetLivoxDispatcherInstance().GetHandleFromIP( + lidar_ip, lidar_handle)) { + AERROR << "livox ip address format error, component init fail"; + return false; } - return true; + + AINFO << "init lidar, handle = " << lidar_handle; + + LivoxDispatcher::GetLivoxDispatcherInstance() + .RegisterHandleDispatchCallback( + lidar_handle, + std::bind(&LivoxLidarComponent::PointCloudCallback, this, + std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3, std::placeholders::_4)); + + SetLivoxLidarPointCloudCallBack( + GlobalPointCloudCallback, + reinterpret_cast( + &LivoxDispatcher::GetLivoxDispatcherInstance())); + + AINFO << "livox lidar init success"; + } + return true; } void LivoxLidarComponent::ReadScanCallback( - const std::shared_ptr& scan_message) { - auto data_addr = (unsigned char*)scan_message->data().c_str(); - BinaryDataProcess( - data_addr, - scan_message->data_type(), - scan_message->point_size(), - scan_message->timestamp(), - scan_message->time_interval()); - - CheckTimestampAndPublishPointCloud(); + const std::shared_ptr& scan_message) { + auto data_addr = (unsigned char*)scan_message->data().c_str(); + BinaryDataProcess(data_addr, scan_message->data_type(), + scan_message->point_size(), scan_message->timestamp(), + scan_message->time_interval()); + + CheckTimestampAndPublishPointCloud(); } void LivoxLidarComponent::CheckTimestampAndPublishPointCloud() { - if (!integral_queue_.empty()) { - uint64_t timestamp_now = cyber::Time::Now().ToNanosecond(); - uint64_t timestamp_dist - = timestamp_now - last_pointcloud_pub_timestamp_; - - uint64_t tolerable_timestamp = (1e9 / pointcloud_freq_); - - if (timestamp_dist > tolerable_timestamp) { - auto pcd_frame = LidarComponentBase< - livox::LivoxScan>::AllocatePointCloud(); - - for (auto it = integral_queue_.begin(); it != integral_queue_.end(); - ++it) { - auto point = pcd_frame->add_point(); - point->CopyFrom(*it); - } - - PreparePointsMsg(*pcd_frame); - LidarComponentBase::WritePointCloud(pcd_frame); - AINFO << "pcd frame write, publish timestamp = " << timestamp_now; - last_pointcloud_pub_timestamp_ = timestamp_now; + auto integral_queue = LidarComponentBase::GetPointQueue(); + if (integral_queue->Empty()) { + return; + } + uint64_t timestamp_now = cyber::Time::Now().ToNanosecond(); + uint64_t timestamp_dist = timestamp_now - last_pointcloud_pub_timestamp_; + + uint64_t tolerable_timestamp = (1e9 / pointcloud_freq_); + + uint64_t custom_integral_min_timestamp = 0; + if (config_.has_custom_integral()) { + custom_integral_min_timestamp = + integral_queue->GetBack()->timestamp() - + config_.custom_integral().integral_time() * 1e9; + } + if (timestamp_dist > tolerable_timestamp) { + auto pcd_frame = LidarComponentBase::AllocatePointCloud(); + uint64_t tail_index, head_index, i; + integral_queue->GetHeadIndex(head_index); + integral_queue->GetTailIndex(tail_index); + i = head_index; + while (true) { + PointXYZIT* p; + integral_queue->GetEleByIndex(i, p); + if (custom_integral_min_timestamp > 0) { + if (std::isnan(p->x()) || std::isnan(p->y()) || std::isnan(p->z())) { + if (i == tail_index) { + break; + } + if (!integral_queue->NextIndex(i)) { + break; + } + continue; } + // in custom integral area + if (p->timestamp() < custom_integral_min_timestamp && + (config_.custom_integral().has_min_x() && + p->x() > config_.custom_integral().min_x()) && + (config_.custom_integral().has_max_x() && + p->x() < config_.custom_integral().max_x()) && + (config_.custom_integral().has_min_y() && + p->y() > config_.custom_integral().min_y()) && + (config_.custom_integral().has_max_y() && + p->y() < config_.custom_integral().max_y())) { + if (i == tail_index) { + break; + } + if (!integral_queue->NextIndex(i)) { + break; + } + continue; + } + } + if (pcd_frame->GetArena() != nullptr && integral_queue->IsArenaEnable()) { + pcd_frame->mutable_point()->UnsafeArenaAddAllocated(p); + } else { + auto point = pcd_frame->add_point(); + point->CopyFrom(*p); + } + if (i == tail_index) { + break; + } + if (!integral_queue->NextIndex(i)) { + break; + } + } + + if (pcd_frame->point_size() == 0) { + AWARN << "pcd frame is empty"; + return; } + + PreparePointsMsg(*pcd_frame); + LidarComponentBase::WritePointCloud(pcd_frame); + ADEBUG << "pcd frame write, publish timestamp = " << timestamp_now; + last_pointcloud_pub_timestamp_ = timestamp_now; + } } } // namespace lidar diff --git a/modules/drivers/lidar/livox/component/livox_component.h b/modules/drivers/lidar/livox/component/livox_component.h index 91b68bd6dc..f0ff29cb7b 100644 --- a/modules/drivers/lidar/livox/component/livox_component.h +++ b/modules/drivers/lidar/livox/component/livox_component.h @@ -36,36 +36,30 @@ namespace lidar { class LivoxLidarComponent final : public LidarComponentBase { public: - void BinaryDataProcess( - const unsigned char* data, - int data_type, - int point_size, - uint64_t pkt_timestamp, - uint32_t time_interval); + void BinaryDataProcess(const unsigned char* data, const int& data_type, + const int& point_size, const uint64_t& pkt_timestamp, + const uint32_t& time_interval); - void PointCloudCallback( - uint32_t handle, - const uint8_t dev_type, - LivoxLidarEthernetPacket* data, - void* client_data); + void PointCloudCallback(uint32_t handle, const uint8_t dev_type, + LivoxLidarEthernetPacket* data, void* client_data); - size_t GetEthPacketByteSize(LivoxLidarEthernetPacket* data); + size_t GetEthPacketByteSize(LivoxLidarEthernetPacket* data); - void PreparePointsMsg(PointCloud& msg); + void PreparePointsMsg(PointCloud& msg); - bool Init() override; + bool Init() override; - void ReadScanCallback( - const std::shared_ptr& scan_message) override; + void ReadScanCallback( + const std::shared_ptr& scan_message) override; - void CheckTimestampAndPublishPointCloud(); + void CheckTimestampAndPublishPointCloud(); - livox::Config config_; - std::deque integral_queue_; + livox::Config config_; + std::deque integral_queue_; - uint64_t last_pointcloud_pub_timestamp_{0}; - double pointcloud_freq_ = {10.0}; // Hz - double integral_time_ = {0.4}; // second + uint64_t last_pointcloud_pub_timestamp_{0}; + double pointcloud_freq_ = {10.0}; // Hz + double integral_time_ = {0.1}; // second }; CYBER_REGISTER_COMPONENT(LivoxLidarComponent) diff --git a/modules/drivers/lidar/livox/component/livox_dispatcher.cpp b/modules/drivers/lidar/livox/component/livox_dispatcher.cpp index 7e46153995..357124e5f9 100644 --- a/modules/drivers/lidar/livox/component/livox_dispatcher.cpp +++ b/modules/drivers/lidar/livox/component/livox_dispatcher.cpp @@ -23,44 +23,42 @@ namespace drivers { namespace lidar { bool LivoxDispatcher::GetHandleFromIP(const std::string& ip, uint32_t& handle) { - const std::regex ip_regex( - "(\\d{1,3})\\.(\\d{1,3})\\.(\\d{1,3})\\.(\\d{1,3})"); - std::smatch ip_match; - if (!std::regex_match(ip, ip_match, ip_regex)) { - AERROR << "ip = " << ip << "; is not a valid ip address"; - return false; - } + const std::regex ip_regex( + "(\\d{1,3})\\.(\\d{1,3})\\.(\\d{1,3})\\.(\\d{1,3})"); + std::smatch ip_match; + if (!std::regex_match(ip, ip_match, ip_regex)) { + AERROR << "ip = " << ip << "; is not a valid ip address"; + return false; + } - if (ip_match.size() != 5) { - AERROR << "ip = " << ip << "; match error"; - return false; - } - handle = 0; - for (int i = 1; i <= 4; ++i) { - int dig = stoi(ip_match[i]); - handle = handle + pow(256, i - 1) * dig; - } - return true; + if (ip_match.size() != 5) { + AERROR << "ip = " << ip << "; match error"; + return false; + } + handle = 0; + for (int i = 1; i <= 4; ++i) { + int dig = stoi(ip_match[i]); + handle = handle + pow(256, i - 1) * dig; + } + return true; } void LivoxDispatcher::RegisterHandleDispatchCallback( - uint32_t handle, - PointCloudCallbackType cb) { - AINFO << "register handle " << handle; - std::lock_guard lg(mtx); - handle_callback_functions[handle] = cb; + uint32_t handle, PointCloudCallbackType cb) { + AINFO << "register handle " << handle; + std::lock_guard lg(mtx); + handle_callback_functions[handle] = cb; } -void LivoxDispatcher::LivoxPointCloudCallback( - uint32_t handle, - const uint8_t dev_type, - LivoxLidarEthernetPacket* data, - void* client_data) { - std::lock_guard lg(mtx); - if (handle_callback_functions.count(handle)) { - PointCloudCallbackType cb = handle_callback_functions[handle]; - cb(handle, dev_type, data, client_data); - } +void LivoxDispatcher::LivoxPointCloudCallback(uint32_t handle, + const uint8_t dev_type, + LivoxLidarEthernetPacket* data, + void* client_data) { + std::lock_guard lg(mtx); + if (handle_callback_functions.count(handle)) { + PointCloudCallbackType cb = handle_callback_functions[handle]; + cb(handle, dev_type, data, client_data); + } } LivoxDispatcher LivoxDispatcher::dispatcher_instance_; diff --git a/modules/drivers/lidar/livox/conf/livox.pb.txt b/modules/drivers/lidar/livox/conf/livox.pb.txt index ff3f8f22de..f97181903e 100644 --- a/modules/drivers/lidar/livox/conf/livox.pb.txt +++ b/modules/drivers/lidar/livox/conf/livox.pb.txt @@ -7,7 +7,8 @@ config_base { source_type: ONLINE_LIDAR } -lidar_config_file_path: "modules/drivers/lidar/livox/conf/mid360_config.json" +lidar_config_file_path: "/apollo/modules/drivers/lidar/livox/conf/mid360_config.json" enable_sdk_console_log: false +use_lidar_clock: false integral_time: 0.1 lidar_ip: "192.168.10.238" diff --git a/modules/drivers/lidar/livox/conf/mid360_config.json b/modules/drivers/lidar/livox/conf/mid360_config.json index 0a024e1a8a..ed36cea1df 100644 --- a/modules/drivers/lidar/livox/conf/mid360_config.json +++ b/modules/drivers/lidar/livox/conf/mid360_config.json @@ -1,23 +1,24 @@ { "MID360": { - "lidar_net_info" : { - "cmd_data_port" : 56100, - "push_msg_port" : 56200, + "lidar_net_info": { + "cmd_data_port": 56100, + "push_msg_port": 56200, "point_data_port": 56300, - "imu_data_port" : 56400, - "log_data_port" : 56500 + "imu_data_port": 56400, + "log_data_port": 56500 }, - "host_net_info" : [ + "host_net_info": [ { - "lidar_ip" : ["192.168.10.138"], - "host_ip" : "192.168.10.6", - "multicast_ip" : "224.1.1.5", - "cmd_data_port" : 56101, - "push_msg_port" : 56201, + "lidar_ip": [ + "192.168.10.138" + ], + "host_ip": "192.168.10.6", + "cmd_data_port": 56101, + "push_msg_port": 56201, "point_data_port": 56301, - "imu_data_port" : 56401, - "log_data_port" : 56501 + "imu_data_port": 56401, + "log_data_port": 56501 } ] } -} +} \ No newline at end of file diff --git a/modules/drivers/lidar/livox/conf/mid360_multi_host_lidar_config.json b/modules/drivers/lidar/livox/conf/mid360_multi_host_lidar_config.json index 2b4298d3b6..7e8153d89b 100644 --- a/modules/drivers/lidar/livox/conf/mid360_multi_host_lidar_config.json +++ b/modules/drivers/lidar/livox/conf/mid360_multi_host_lidar_config.json @@ -9,7 +9,10 @@ }, "host_net_info": [ { - "lidar_ip": ["192.168.10.138", "192.168.10.139"], + "lidar_ip": [ + "192.168.10.138", + "192.168.10.139" + ], "host_ip": "192.168.10.6", "cmd_data_port": 56101, "push_msg_port": 56201, @@ -18,7 +21,9 @@ "log_data_port": 56501 }, { - "lidar_ip": ["192.168.10.238"], + "lidar_ip": [ + "192.168.10.238" + ], "host_ip": "192.168.10.8", "cmd_data_port": 56101, "push_msg_port": 56201, @@ -28,4 +33,4 @@ } ] } -} +} \ No newline at end of file diff --git a/modules/drivers/lidar/livox/dag/livox.dag b/modules/drivers/lidar/livox/dag/livox.dag index edc2c75d46..26703546fd 100644 --- a/modules/drivers/lidar/livox/dag/livox.dag +++ b/modules/drivers/lidar/livox/dag/livox.dag @@ -4,7 +4,7 @@ module_config { class_name : "LivoxLidarComponent" config { name : "Livox_Driver" - config_file_path : "modules/drivers/lidar/livox/conf/livox.pb.txt" + config_file_path : "/apollo/modules/drivers/lidar/livox/conf/livox.pb.txt" } } } \ No newline at end of file diff --git a/modules/drivers/lidar/livox/driver/livox_util.cpp b/modules/drivers/lidar/livox/driver/livox_util.cpp index 29691a8665..8f2e452dbc 100644 --- a/modules/drivers/lidar/livox/driver/livox_util.cpp +++ b/modules/drivers/lidar/livox/driver/livox_util.cpp @@ -19,21 +19,21 @@ namespace apollo { namespace drivers { namespace lidar { -uint64_t GetEthPacketTimestamp( - uint8_t timestamp_type, - uint8_t* time_stamp, - uint8_t size) { +uint64_t GetEthPacketTimestamp(const uint8_t& timestamp_type, + const uint8_t* time_stamp, const uint8_t& size, + const uint32_t& point_size, + const bool use_lidar_clock) { + if (use_lidar_clock && (timestamp_type == kTimestampTypeGptpOrPtp || + timestamp_type == kTimestampTypeGps)) { LdsStamp time; memcpy(time.stamp_bytes, time_stamp, size); - - if (timestamp_type == kTimestampTypeGptpOrPtp - || timestamp_type == kTimestampTypeGps) { - return time.stamp; - AERROR_EVERY(1000000) << "use package timestamp"; - } else { - AERROR_EVERY(1000000) << "use system timestamp"; - } - return cyber::Time::Now().ToNanosecond(); + AERROR_EVERY(1000000) << "use package timestamp"; + return time.stamp; + } else { + AERROR_EVERY(1000000) << "use system timestamp"; + } + // 0.1us a point + return cyber::Time::Now().ToNanosecond() - 100 * point_size; } } // namespace lidar diff --git a/modules/drivers/lidar/livox/driver/livox_util.h b/modules/drivers/lidar/livox/driver/livox_util.h index f0bcad21f1..67ae2076df 100644 --- a/modules/drivers/lidar/livox/driver/livox_util.h +++ b/modules/drivers/lidar/livox/driver/livox_util.h @@ -22,26 +22,26 @@ namespace drivers { namespace lidar { typedef enum { - kTimestampTypeNoSync = 0, /**< No sync signal mode. */ - kTimestampTypeGptpOrPtp = 1, /**< gPTP or PTP sync mode */ - kTimestampTypeGps = 2 /**< GPS sync mode. */ + kTimestampTypeNoSync = 0, /**< No sync signal mode. */ + kTimestampTypeGptpOrPtp = 1, /**< gPTP or PTP sync mode */ + kTimestampTypeGps = 2 /**< GPS sync mode. */ } TimestampType; /** 8bytes stamp to uint64_t stamp */ typedef union { - struct { - uint32_t low; - uint32_t high; - } stamp_word; + struct { + uint32_t low; + uint32_t high; + } stamp_word; - uint8_t stamp_bytes[8]; - int64_t stamp; + uint8_t stamp_bytes[8]; + int64_t stamp; } LdsStamp; -uint64_t GetEthPacketTimestamp( - uint8_t timestamp_type, - uint8_t* time_stamp, - uint8_t size); +uint64_t GetEthPacketTimestamp(const uint8_t& timestamp_type, + const uint8_t* time_stamp, const uint8_t& size, + const uint32_t& point_size, + const bool use_lidar_clock = false); } // namespace lidar } // namespace drivers diff --git a/modules/drivers/lidar/livox/proto/livox.proto b/modules/drivers/lidar/livox/proto/livox.proto index 56d00d20b6..d716ab90cb 100644 --- a/modules/drivers/lidar/livox/proto/livox.proto +++ b/modules/drivers/lidar/livox/proto/livox.proto @@ -4,12 +4,22 @@ package apollo.drivers.livox; import "modules/drivers/lidar/common/proto/lidar_config_base.proto"; +message CustomIntegral { + required double integral_time = 1; + optional double min_x = 2; + optional double max_x = 3; + optional double min_y = 4; + optional double max_y = 5; +} + message Config { required apollo.drivers.lidar.LidarConfigBase config_base = 1; required string lidar_ip = 2; optional string lidar_config_file_path = 3; optional double integral_time = 4 [default = 0.1]; optional bool enable_sdk_console_log = 5 [default = false]; + optional bool use_lidar_clock = 6 [default = false]; + optional CustomIntegral custom_integral = 7; } message LivoxScan { diff --git a/scripts/model_download.sh b/scripts/model_download.sh index a8d5189b1c..6412e1a122 100644 --- a/scripts/model_download.sh +++ b/scripts/model_download.sh @@ -15,28 +15,29 @@ # See the License for the specific language governing permissions and # limitations under the License. ############################################################################### +TOP_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd -P)" declare -A dic -models_dir="/apollo/modules/perception/data/models/" +models_dir="${TOP_DIR}/modules/perception/data/models/" -dic=([3d-r4-half_caffe.zip]="https://apollo-perception.bj.bcebos.com/core_model/3d-r4-half_caffe.zip?authorization=bce-auth-v1/e3384375161a482d8fa77e1ef2d32e05/2023-10-27T09%3A39%3A46Z/-1/host/35f44fc343144e9a6f2d9f19a1daef32ab32bdbdb21f9bfab13ba235e3118c97" \ - [cnnseg128_caffe.zip]="https://apollo-perception.bj.bcebos.com/core_model/cnnseg128_caffe.zip?authorization=bce-auth-v1/e3384375161a482d8fa77e1ef2d32e05/2023-10-27T10%3A49%3A05Z/-1/host/17850b8a5f73333291bbdedcb0103222a70b143516d86f8af79649f4f4100543" \ - [cnnseg64_caffe.zip]="https://apollo-perception.bj.bcebos.com/core_model/cnnseg64_caffe.zip?authorization=bce-auth-v1/e3384375161a482d8fa77e1ef2d32e05/2023-10-27T10%3A49%3A21Z/-1/host/390c40b2946bfba35910c6c3de738eaafdc1b11b15c9131c4a437f72a026deb4" \ - [cnnseg16_caffe.zip]="https://apollo-perception.bj.bcebos.com/core_model/cnnseg16_caffe.zip?authorization=bce-auth-v1/e3384375161a482d8fa77e1ef2d32e05/2023-10-27T10%3A49%3A40Z/-1/host/a8996774dd80355a16e47949d1488b785faf8d2093252d2ccd41263a0a74a44b" \ - [horizontal_caffe.zip]="https://apollo-perception.bj.bcebos.com/core_model/horizontal_caffe.zip?authorization=bce-auth-v1/e3384375161a482d8fa77e1ef2d32e05/2023-10-27T10%3A51%3A41Z/-1/host/c1c84568d35ac87f99f4a27a00baf00e0a19a5034be32a02b0e4cd584ace4ef0" \ - [quadrate_caffe.zip]="https://apollo-perception.bj.bcebos.com/core_model/quadrate_caffe.zip?authorization=bce-auth-v1/e3384375161a482d8fa77e1ef2d32e05/2023-10-27T11%3A03%3A53Z/-1/host/7aa8d34acdc71de4acae2fa7ee46267b7e504751f36c0fcff8978d292322bde3" \ - [vertical_caffe.zip]="https://apollo-perception.bj.bcebos.com/core_model/vertical_caffe.zip?authorization=bce-auth-v1/e3384375161a482d8fa77e1ef2d32e05/2023-10-27T11%3A04%3A55Z/-1/host/6762735da1dbd5095efc72b3e32ee46d9d25671c3bc68dc8930c9f10eed865d9" \ - [tl_detection_caffe.zip]="https://apollo-perception.bj.bcebos.com/core_model/tl_detection_caffe.zip?authorization=bce-auth-v1/e3384375161a482d8fa77e1ef2d32e05/2023-10-27T11%3A04%3A40Z/-1/host/f00446d0d66465d7aa55723605a29c6061f1ff7dec34e7d926b6f5d63592106b" \ - [smoke_torch.zip]="https://apollo-perception.bj.bcebos.com/core_model/smoke_torch.zip?authorization=bce-auth-v1/e3384375161a482d8fa77e1ef2d32e05/2023-10-27T11%3A04%3A22Z/-1/host/b9aed27fd7480add4225a7fe8f83b3f81aecde39035b057837ef3ff9ccc93055" \ - [yolox3d_onnx.zip]="https://apollo-perception.bj.bcebos.com/core_model/yolox3d_onnx.zip?authorization=bce-auth-v1/e3384375161a482d8fa77e1ef2d32e05/2023-12-07T08%3A13%3A44Z/-1/host/49df2b47683c2f9666005c556a858ff44df08e53d98257909b116e621b76a220" \ - [center_point_paddle.zip]="https://apollo-perception.bj.bcebos.com/core_model/center_point_paddle.zip?authorization=bce-auth-v1/e3384375161a482d8fa77e1ef2d32e05/2023-10-31T09%3A29%3A10Z/-1/host/8f45ff099aa79c6043c744cf9ff72fb432335ac60aa3fe9235b9ec275e003aab" \ - [point_pillars_radar4d_torch.zip]="https://apollo-perception.bj.bcebos.com/core_model/point_pillars_radar4d_torch.zip?authorization=bce-auth-v1/e3384375161a482d8fa77e1ef2d32e05/2023-12-07T08%3A11%3A00Z/-1/host/2275614dd6c8999c7cb6b171b889bd0d637997fac52e826597cfe76309efb4ac" \ - [point_pillars_torch.zip]="https://apollo-perception.bj.bcebos.com/core_model/point_pillars_torch.zip?authorization=bce-auth-v1/e3384375161a482d8fa77e1ef2d32e05/2023-11-02T09%3A28%3A04Z/-1/host/8c5356cf4f38c3590017fe552acb9764dac8696f92324aee414ce7eaa029b57a" \ - [mask_pillars_torch.zip]="https://apollo-perception.bj.bcebos.com/core_model/mask_pillars_torch.zip?authorization=bce-auth-v1/e3384375161a482d8fa77e1ef2d32e05/2023-11-02T09%3A28%3A22Z/-1/host/129833d8aacecc3a95e729f73d73d6b44bfcb5c8a23eb1e5051b71b4b9f8a075" \ - [apollo_bevnet_onnx.zip]="https://apollo-perception.bj.bcebos.com/core_model/apollo_bevnet_onnx.zip?authorization=bce-auth-v1/ALTAKg5yEiU4seODUEbcZVtvIv/2024-10-22T13%3A01%3A45Z/-1/host/20d8d79aedbf84a29b52e05348d783dc5094164618346de3d665c0b2f0f94861" \ +dic=([3d-r4-half_caffe.zip]="http://apollo-perception.bj.bcebos.com/core_model/3d-r4-half_caffe.zip?authorization=bce-auth-v1%2FALTAK6fleneBT08Sn61Gseah1T%2F2024-11-19T03%3A05%3A17Z%2F-1%2Fhost%2F5575ba1e6b5c33e5e989b3b379e16aa9e6546ab28e6f45a54162fadc5dce0ffa" \ + [cnnseg128_caffe.zip]="http://apollo-perception.bj.bcebos.com/core_model/cnnseg128_caffe.zip?authorization=bce-auth-v1%2FALTAK6fleneBT08Sn61Gseah1T%2F2024-11-19T03%3A06%3A36Z%2F-1%2Fhost%2Faec61e93a98ba3a70edfde89fbc5635294df057dc2f5774a59060332f182067a" \ + [cnnseg64_caffe.zip]="http://apollo-perception.bj.bcebos.com/core_model/cnnseg64_caffe.zip?authorization=bce-auth-v1%2FALTAK6fleneBT08Sn61Gseah1T%2F2024-11-19T03%3A07%3A45Z%2F-1%2Fhost%2F4c567da65ebd7e3a20bbb4b9419db6e17f5f233c5867de6d17e70d4bd9f07057" \ + [cnnseg16_caffe.zip]="http://apollo-perception.bj.bcebos.com/core_model/cnnseg16_caffe.zip?authorization=bce-auth-v1%2FALTAK6fleneBT08Sn61Gseah1T%2F2024-11-19T03%3A07%3A59Z%2F-1%2Fhost%2F05cda136284eea1ba84c552c2e5cdd4d6d054c9b23ef209fa87623b746bed185" \ + [horizontal_caffe.zip]="http://apollo-perception.bj.bcebos.com/core_model/horizontal_caffe.zip?authorization=bce-auth-v1%2FALTAK6fleneBT08Sn61Gseah1T%2F2024-11-19T03%3A08%3A14Z%2F-1%2Fhost%2Ffafa1d10174f763828b10ec251b0359da311e8bb4b37824c56e03fcdea61f9f7" \ + [quadrate_caffe.zip]="http://apollo-perception.bj.bcebos.com/core_model/quadrate_caffe.zip?authorization=bce-auth-v1%2FALTAK6fleneBT08Sn61Gseah1T%2F2024-11-19T03%3A08%3A29Z%2F-1%2Fhost%2F10bc4ce80bd0497ea92188aff6d28a9c35dbbd12ae12cd836e9b19e4ab0a79d3" \ + [vertical_caffe.zip]="http://apollo-perception.bj.bcebos.com/core_model/vertical_caffe.zip?authorization=bce-auth-v1%2FALTAK6fleneBT08Sn61Gseah1T%2F2024-11-19T03%3A08%3A41Z%2F-1%2Fhost%2F6ea0744dbc5b5e83a673e241c07ad8d3f5930763b1bc9c05084cac5c16b07d02" \ + [tl_detection_caffe.zip]="http://apollo-perception.bj.bcebos.com/core_model/tl_detection_caffe.zip?authorization=bce-auth-v1%2FALTAK6fleneBT08Sn61Gseah1T%2F2024-11-19T03%3A08%3A55Z%2F-1%2Fhost%2F524a2c191d2a332d500232600fd0dd30db21591adc83f728dc8ae5daef4ef335" \ + [smoke_torch.zip]="http://apollo-perception.bj.bcebos.com/core_model/smoke_torch.zip?authorization=bce-auth-v1%2FALTAK6fleneBT08Sn61Gseah1T%2F2024-11-19T03%3A09%3A10Z%2F-1%2Fhost%2Ffb046eae4495ef593e8cc468e6b62c9a504223d862c27596a878a8b250b756c5" \ + [yolox3d_onnx.zip]="http://apollo-perception.bj.bcebos.com/core_model/yolox3d_onnx.zip?authorization=bce-auth-v1%2FALTAK6fleneBT08Sn61Gseah1T%2F2024-11-19T03%3A09%3A24Z%2F-1%2Fhost%2F16ce6949d86c2521e4ac1d4e3f3d6d6d266b7aece8d68e18d12ee295a77de5fa" \ + [center_point_paddle.zip]="http://apollo-perception.bj.bcebos.com/core_model/center_point_paddle.zip?authorization=bce-auth-v1%2FALTAK6fleneBT08Sn61Gseah1T%2F2024-11-19T03%3A09%3A36Z%2F-1%2Fhost%2F109777339cc3fab17f5f54760711e04139dcdba62f2aa75e955d91d8d21cd963" \ + [point_pillars_radar4d_torch.zip]="http://apollo-perception.bj.bcebos.com/core_model/point_pillars_radar4d_torch.zip?authorization=bce-auth-v1%2FALTAK6fleneBT08Sn61Gseah1T%2F2024-11-19T03%3A09%3A49Z%2F-1%2Fhost%2Ff7bfb72f9cdbb866d5965a16828fa42275d97844b9f546237bfa000de252063d" \ + [point_pillars_torch.zip]="http://apollo-perception.bj.bcebos.com/core_model/point_pillars_torch.zip?authorization=bce-auth-v1%2FALTAK6fleneBT08Sn61Gseah1T%2F2024-11-19T03%3A10%3A07Z%2F-1%2Fhost%2F4d0ad2d272b36fcccb7c0a8280e3af1d665cc1dff393a877ce90b7ca0ea5a5ff" \ + [mask_pillars_torch.zip]="http://apollo-perception.bj.bcebos.com/core_model/mask_pillars_torch.zip?authorization=bce-auth-v1%2FALTAK6fleneBT08Sn61Gseah1T%2F2024-11-19T03%3A10%3A20Z%2F-1%2Fhost%2Feb4e3afe11c4a58be4421cb5271d780e4f648feb2c18b1b9d022aed04da1d838" \ + [apollo_bevnet_onnx.zip]="http://apollo-perception.bj.bcebos.com/core_model/apollo_bevnet_onnx.zip?authorization=bce-auth-v1%2FALTAK6fleneBT08Sn61Gseah1T%2F2024-11-19T03%3A19%3A47Z%2F-1%2Fhost%2Fcff6ed21d22bb27395182790e07582b95edbed4059f611211db1224aad93acc7" \ ) -cd "/apollo" +pushd "${TOP_DIR}" for key in $(echo ${!dic[*]}); do download_link=${dic[$key]} @@ -48,3 +49,5 @@ for key in $(echo ${!dic[*]}); do done rm -rf ${key::-4} ${key} done + +popd \ No newline at end of file