forked from tier4/nebula
-
Notifications
You must be signed in to change notification settings - Fork 1
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Prototype (outputs pointclouds but nothing more)
- Loading branch information
Showing
29 changed files
with
2,101 additions
and
15 deletions.
There are no files selected for viewing
Large diffs are not rendered by default.
Oops, something went wrong.
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 |
---|---|---|
|
@@ -5,6 +5,7 @@ | |
"words": [ | ||
"adctp", | ||
"Adctp", | ||
"aeva", | ||
"applicate", | ||
"AT", | ||
"autosar", | ||
|
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,151 @@ | ||
// 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 <pcl/impl/point_types.hpp> | ||
|
||
#include <cmath> | ||
#include <cstddef> | ||
#include <cstdint> | ||
#include <string> | ||
#include <type_traits> | ||
#include <vector> | ||
namespace nebula::drivers::aeva | ||
{ | ||
|
||
#pragma pack(push, 1) | ||
|
||
template <typename storage_t, size_t n_fractional_bits> | ||
struct FixedPoint | ||
{ | ||
[[nodiscard]] float value() const { return value_ * std::pow(2., -1. * n_fractional_bits); } | ||
|
||
private: | ||
storage_t value_; | ||
}; | ||
|
||
struct SomeIpHeader | ||
{ | ||
uint16_t service_id; | ||
uint16_t method_id; | ||
uint32_t message_length; | ||
uint16_t client_id; | ||
uint16_t session_id; | ||
uint8_t protocol; | ||
uint8_t interface_version; | ||
uint8_t message_type; | ||
uint8_t return_code; | ||
uint32_t tp_header_offset : 28; | ||
uint32_t tp_header : 4; | ||
}; | ||
|
||
struct MessageHeader | ||
{ | ||
uint8_t major_version : 4; | ||
uint8_t minor_version : 4; | ||
uint8_t message_type; | ||
uint16_t sequence_id; | ||
uint32_t message_length; | ||
uint64_t acquisition_time_ns; | ||
uint64_t publish_time_ns; | ||
}; | ||
|
||
struct PointcloudMsgSubheaderAndMetadata | ||
{ | ||
uint16_t aeva_marker; | ||
uint8_t platform; | ||
uint8_t reserved_1; | ||
uint16_t ns_per_index; | ||
uint64_t first_point_timestamp_ns; | ||
int32_t frame_sync_index; | ||
uint32_t period_ns; | ||
uint32_t n_entries; | ||
uint32_t capacity; | ||
uint16_t num_beams : 4; | ||
uint16_t num_peaks : 2; | ||
uint16_t range_scale : 10; | ||
uint8_t line_index; | ||
uint8_t max_line_index; | ||
uint32_t face_index : 4; | ||
uint32_t n_faces : 4; | ||
uint32_t reserved_2 : 3; | ||
uint32_t sensitivity_mode : 3; | ||
uint32_t frame_parity : 1; | ||
uint32_t reserved_3 : 1; | ||
uint32_t window_measurement : 1; | ||
uint32_t reserved_5 : 1; | ||
uint32_t discard_line : 1; | ||
uint32_t ping_pong : 1; | ||
uint32_t reserved_6 : 12; | ||
FixedPoint<int16_t, 9> mirror_rps; | ||
uint16_t dither_step : 5; | ||
uint16_t max_dither_step : 5; | ||
uint16_t reserved_7 : 6; | ||
uint8_t reserved_8[12]; | ||
}; | ||
|
||
struct PointCloudPoint | ||
{ | ||
FixedPoint<int16_t, 15> azimuth; | ||
FixedPoint<int16_t, 15> elevation; | ||
FixedPoint<uint16_t, 7> range; | ||
FixedPoint<int16_t, 8> velocity; | ||
uint8_t intensity; | ||
uint8_t signal_quality; | ||
uint32_t beam_id : 3; | ||
uint32_t peak_id : 2; | ||
uint32_t line_transition : 1; | ||
uint32_t valid : 1; | ||
uint32_t dynamic : 1; | ||
uint32_t reserved_1 : 1; | ||
uint32_t up_sweep : 1; | ||
uint32_t in_ambiguity_region : 1; | ||
uint32_t reserved_2 : 9; | ||
uint32_t peak_width : 4; | ||
uint32_t is_single_detection : 1; | ||
uint32_t reserved_3 : 7; | ||
}; | ||
|
||
struct PointCloudMessageFragment | ||
{ | ||
std::vector<PointCloudPoint> points; | ||
}; | ||
|
||
#pragma pack(pop) | ||
|
||
struct EIGEN_ALIGN16 PointXYZVIRCAEDT | ||
{ | ||
float x; | ||
float y; | ||
float z; | ||
float v; | ||
uint8_t intensity; | ||
uint8_t return_type; | ||
uint16_t channel; | ||
float azimuth; | ||
float elevation; | ||
float distance; | ||
uint32_t time_stamp; | ||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
}; | ||
|
||
} // namespace nebula::drivers::aeva | ||
|
||
POINT_CLOUD_REGISTER_POINT_STRUCT( // NOLINT | ||
nebula::drivers::aeva::PointXYZVIRCAEDT, | ||
(float, x, x)(float, y, y)(float, z, z)(float, v, v)(std::uint8_t, intensity, intensity)( | ||
std::uint8_t, return_type, | ||
return_type)(std::uint16_t, channel, channel)(float, azimuth, azimuth)( | ||
float, elevation, elevation)(float, distance, distance)(std::uint32_t, time_stamp, time_stamp)) |
57 changes: 57 additions & 0 deletions
57
nebula_common/include/nebula_common/loggers/console_logger.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
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,57 @@ | ||
// 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_common/loggers/logger.hpp" | ||
|
||
#include <cstdio> | ||
#include <iostream> | ||
#include <istream> | ||
#include <memory> | ||
#include <ostream> | ||
#include <sstream> | ||
#include <string> | ||
#include <utility> | ||
|
||
namespace nebula::drivers::loggers | ||
{ | ||
|
||
class ConsoleLogger : public Logger | ||
{ | ||
public: | ||
explicit ConsoleLogger(std::string name) : name_(std::move(name)) {} | ||
|
||
void debug(const std::string & message) override { printTagged(std::cout, "DEBUG", message); } | ||
void info(const std::string & message) override { printTagged(std::cout, "INFO", message); } | ||
void warn(const std::string & message) override { printTagged(std::cerr, "WARN", message); } | ||
void error(const std::string & message) override { printTagged(std::cerr, "ERROR", message); } | ||
|
||
std::shared_ptr<Logger> child(const std::string & name) override | ||
{ | ||
return std::make_shared<ConsoleLogger>(name_ + "." + name); | ||
} | ||
|
||
private: | ||
const std::string name_; | ||
|
||
void printTagged(std::ostream & os, const std::string & severity, const std::string & message) | ||
{ | ||
// In multithreaded logging, building the string first (... + ...) and then shifting to the | ||
// stream will ensure that no other logger outputs between string fragments | ||
os << ("[" + name_ + "][" + severity + "] " + message) << std::endl; | ||
} | ||
}; | ||
|
||
} // namespace nebula::drivers::loggers |
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,41 @@ | ||
// 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 <memory> | ||
#include <string> | ||
|
||
namespace nebula::drivers::loggers | ||
{ | ||
|
||
class Logger | ||
{ | ||
public: | ||
Logger() = default; | ||
Logger(const Logger &) = default; | ||
Logger(Logger &&) = delete; | ||
Logger & operator=(const Logger &) = default; | ||
Logger & operator=(Logger &&) = delete; | ||
virtual ~Logger() = default; | ||
|
||
virtual void debug(const std::string & message) = 0; | ||
virtual void info(const std::string & message) = 0; | ||
virtual void warn(const std::string & message) = 0; | ||
virtual void error(const std::string & message) = 0; | ||
|
||
virtual std::shared_ptr<Logger> child(const std::string & name) = 0; | ||
}; | ||
|
||
} // namespace nebula::drivers::loggers |
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,13 @@ | ||
// 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. |
Oops, something went wrong.