diff --git a/CMakeLists.txt b/CMakeLists.txt index cc2ffeb..0dde54d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,6 +4,7 @@ project(mapora) set(CMAKE_CXX_STANDARD 17) add_compile_options(-Wall -Wextra -Wpedantic) +list(APPEND CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake") find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() @@ -30,9 +31,10 @@ include_directories(include set(MAPORA_LIB_SRC src/mapora.cpp src/mapora_rosbag.cpp - src/points_provider_velodyne_vlp16.cpp - src/continuous_packet_parser_vlp16.cpp - src/transform_provider_applanix.cpp + src/points_provider.cpp + src/parsers/velodyne_vlp16.cpp + src/parsers/hesai_xt32.cpp + src/transform_provider.cpp src/utils.cpp) set(MAPORA_LIB_HEADERS @@ -40,12 +42,11 @@ set(MAPORA_LIB_HEADERS include/mapora/mapora_rosbag.hpp include/mapora/date.h include/mapora/csv.hpp - include/mapora/point_xyzi.hpp - include/mapora/point_xyzit.hpp - include/mapora/points_provider_base.hpp - include/mapora/points_provider_velodyne_vlp16.hpp - include/mapora/continuous_packet_parser_vlp16.hpp - include/mapora/transform_provider_applanix.hpp + include/mapora/point_types.hpp + include/mapora/points_provider.hpp + include/mapora/parsers/velodyne_vlp16.hpp + include/mapora/parsers/hesai_xt32.hpp + include/mapora/transform_provider.hpp include/mapora/utils.hpp include/mapora/visibility_control.hpp) @@ -56,9 +57,9 @@ ament_auto_add_library(${PROJECT_NAME} SHARED ${MAPORA_LIB_SRC}) target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES} - ${PcapPlusPlus_LIBRARIES} +# ${PcapPlusPlus_LIBRARIES} Eigen3::Eigen - # PcapPlusPlus::PcapPlusPlus + PcapPlusPlus::PcapPlusPlus TBB::tbb ${GeographicLib_LIBRARIES} ${libLAS_LIBRARIES}) diff --git a/cmake/FindPcapPlusPlus.cmake b/cmake/FindPcapPlusPlus.cmake new file mode 100644 index 0000000..daedc4a --- /dev/null +++ b/cmake/FindPcapPlusPlus.cmake @@ -0,0 +1,53 @@ +# FindPcapPlusPlus.cmake +# +# Finds the PcapPlusPlus library. +# +# This will define the following variables +# +# PcapPlusPlus_FOUND +# PcapPlusPlus_INCLUDE_DIRS +# PcapPlusPlus_LIBRARIES +# PcapPlusPlus_VERSION +# +# and the following imported targets +# +# PcapPlusPlus::PcapPlusPlus +# + +if (PC_PcapPlusPlus_INCLUDEDIR AND PC_PcapPlusPlus_LIBDIR) + set(PcapPlusPlus_FIND_QUIETLY TRUE) +endif () + +find_package(PkgConfig REQUIRED) +pkg_check_modules(PC_PcapPlusPlus REQUIRED PcapPlusPlus) + +set(PcapPlusPlus_VERSION ${PC_PcapPlusPlus_VERSION}) + +mark_as_advanced(PcapPlusPlus_INCLUDE_DIR PcapPlusPlus_LIBRARY) + +foreach (LIB_NAME ${PC_PcapPlusPlus_LIBRARIES}) + find_library(${LIB_NAME}_PATH ${LIB_NAME} HINTS ${PC_PcapPlusPlus_LIBDIR}) + if (${LIB_NAME}_PATH) + list(APPEND PcapPlusPlus_LIBS ${${LIB_NAME}_PATH}) + endif () +endforeach () + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(PcapPlusPlus + REQUIRED_VARS PC_PcapPlusPlus_INCLUDEDIR PC_PcapPlusPlus_LIBDIR + VERSION_VAR PcapPlusPlus_VERSION + ) + +if (PcapPlusPlus_FOUND) + set(PcapPlusPlus_INCLUDE_DIRS ${PC_PcapPlusPlus_INCLUDEDIR}) + set(PcapPlusPlus_LIBRARIES ${PcapPlusPlus_LIBS}) +endif () + +if (PcapPlusPlus_FOUND AND NOT TARGET PcapPlusPlus::PcapPlusPlus) + add_library(PcapPlusPlus::PcapPlusPlus INTERFACE IMPORTED) + set_target_properties(PcapPlusPlus::PcapPlusPlus PROPERTIES + INTERFACE_INCLUDE_DIRECTORIES "${PcapPlusPlus_INCLUDE_DIRS}" + INTERFACE_LINK_LIBRARIES "${PcapPlusPlus_LIBRARIES}" + INTERFACE_COMPILE_FLAGS "${PC_PcapPlusPlus_CFLAGS}" + ) +endif () diff --git a/include/mapora/mapora.hpp b/include/mapora/mapora.hpp index c8154b5..eec5942 100644 --- a/include/mapora/mapora.hpp +++ b/include/mapora/mapora.hpp @@ -17,13 +17,13 @@ #ifndef MAPORA__MAPORA_HPP_ #define MAPORA__MAPORA_HPP_ +#include "mapora/points_provider.hpp" +#include "mapora/transform_provider.hpp" #include "mapora/visibility_control.hpp" -#include "mapora/points_provider_velodyne_vlp16.hpp" -#include "mapora/transform_provider_applanix.hpp" -#include #include -#include #include +#include +#include #include #include @@ -39,7 +39,8 @@ class Mapora : public rclcpp::Node using SharedPtr = std::shared_ptr; using ConstSharedPtr = const std::shared_ptr; using PointCloud2 = sensor_msgs::msg::PointCloud2; - using Points = points_provider::PointsProviderBase::Points; + using Point = point_types::PointXYZITRH; + using Points = std::vector; explicit Mapora(const rclcpp::NodeOptions & options); @@ -55,23 +56,31 @@ class Mapora : public rclcpp::Node double max_point_distance_from_lidar_; double min_point_distance_from_lidar_; std::string las_export_dir; + std::string lidar_model; - std::vector clouds; + std::vector clouds; private: rclcpp::Publisher::SharedPtr pub_ptr_cloud_current_; - rclcpp::Publisher::SharedPtr pub_ptr_path_applanix_; + rclcpp::Publisher::SharedPtr pub_ptr_path; - transform_provider_applanix::TransformProviderApplanix::SharedPtr transform_provider_applanix; - points_provider::PointsProviderVelodyneVlp16::SharedPtr points_provider_velodyne_vlp16; + transform_provider::TransformProvider::SharedPtr + transform_provider; + points_provider::PointsProvider::SharedPtr points_provider_; PointCloud2::SharedPtr points_to_cloud(const Points & points_bad, const std::string & frame_id); void callback_cloud_surround_out(const Points & points_surround); + Points transform_points(Points & cloud); + + std::unique_ptr tf_broadcaster_; - transform_provider_applanix::TransformProviderApplanix::Pose pose; + transform_provider::TransformProvider::Pose pose; + + Point last_point; + int file_counter = 0; }; } // namespace mapora diff --git a/include/mapora/parsers/hesai_xt32.hpp b/include/mapora/parsers/hesai_xt32.hpp new file mode 100644 index 0000000..fa1ccfc --- /dev/null +++ b/include/mapora/parsers/hesai_xt32.hpp @@ -0,0 +1,170 @@ +// +// Created by ataparlar on 11.10.2024. +// + +#ifndef MAPORA__CONTINUOUS_PACKET_PARSER_HESAI_XT32_HPP_ +#define MAPORA__CONTINUOUS_PACKET_PARSER_HESAI_XT32_HPP_ + + +#include "mapora/date.h" +#include "mapora/point_types.hpp" + +#include + +#include +#include +#include +#include +#include +#include + +namespace mapora::points_provider::continuous_packet_parser +{ +class ContinuousPacketParserXt32 { +public: + using SharedPtr = std::shared_ptr; + using ConstSharedPtr = const std::shared_ptr; + using Point = point_types::PointXYZITRH; + using Points = std::vector; + + ContinuousPacketParserXt32(); + + void process_packet_into_cloud( + const pcpp::RawPacket & rawPacket, + const std::function & callback_cloud_surround_out, + const float min_point_distance_from_lidar, + const float max_point_distance_from_lidar); + +private: + using uint8_t = std::uint8_t; + using uint16_t = std::uint16_t; + + struct PreHeader + { + uint8_t start_of_packet_1; + uint8_t start_of_packet_2; + uint8_t protocol_version_major; + uint8_t protocol_version_minor; + uint16_t reserved; + } __attribute__((packed)); + + struct Header + { + uint8_t laser_number; + uint8_t block_number; + uint8_t first_block_return; + uint8_t dis_unit; + uint8_t return_number; + uint8_t udp_seq; + } __attribute__((packed)); + + struct DataPoint + { + uint16_t distance_divided_by_4mm; + uint8_t reflectivity; + uint8_t reserved; + } __attribute__((packed)); + + struct DataBlock // each 130 byte + { + uint16_t azimuth_multiplied_by_100_deg; + DataPoint data_points[32]; + [[nodiscard]] size_t get_size_data_points() const + { + return sizeof(data_points) / sizeof(data_points[0]); + } + } __attribute__((packed)); + + struct Tail + { + uint8_t reserved[9]; + uint8_t high_temp_shutdown; + uint8_t return_mode; + uint16_t motor_speed; + uint8_t year; + uint8_t month; + uint8_t day; + uint8_t hour; + uint8_t minute; + uint8_t second; + uint32_t timestamp; + uint8_t factory_info; + } __attribute__((packed)); + + struct DataPacket { + uint8_t udp_header[42]; + PreHeader pre_header; + Header header; + DataBlock data_blocks[8]; + Tail tail; + uint32_t udp_seq;; + size_t get_size_data_blocks() const + { + return sizeof(data_blocks) / sizeof(data_blocks[0]); + } + }; + + + enum class ReturnMode { Strongest, LastReturn, DualReturn }; + + std::map map_byte_to_return_mode_; + std::map map_return_mode_to_string_; + + enum class HesaiModel + { + Pandar128, // 1 + ET, // 2 + QT, // 3 + AT128, // 4 + PandarXT, // 6 + PandarFT // 7 + }; + + + struct PositionPacket + { + uint8_t udp_header[42]; + uint16_t header; + uint8_t date[6]; + uint8_t time[6]; + uint32_t microsecond_time; + char nmea_sentence[84]; + uint8_t reserved[404]; + uint8_t gps_positioning_status; + uint8_t status_pps; + uint32_t reserved_2; + // uint8_t temp_when_last_shut_from_overheat; + // uint8_t temp_when_booted; + // char nmea_sentence[128]; + // uint8_t reserved_03[178]; + } __attribute__((packed)); + + + HesaiModel hesai_model_; + ReturnMode return_mode_; + + bool factory_bytes_are_read_at_least_once_; + bool has_received_valid_position_package_; + + date::sys_time tp_hours_since_epoch; + + std::map map_byte_to_hesai_model_; + std::map map_hesai_model_to_string_; + + std::vector channel_to_angle_vertical_; + std::vector channel_mod_8_to_azimuth_offsets_; + std::vector ind_block_to_first_channel_; + + bool has_processed_a_packet_; + float angle_deg_azimuth_last_packet_; + uint32_t microseconds_last_packet_; + + Points cloud_; + bool can_publish_again_; + float angle_deg_cut_; +}; +} + + + +#endif // MAPORA__CONTINUOUS_PACKET_PARSER_HESAI_XT32_HPP_ diff --git a/include/mapora/continuous_packet_parser_vlp16.hpp b/include/mapora/parsers/velodyne_vlp16.hpp similarity index 88% rename from include/mapora/continuous_packet_parser_vlp16.hpp rename to include/mapora/parsers/velodyne_vlp16.hpp index 91da05b..92bf930 100644 --- a/include/mapora/continuous_packet_parser_vlp16.hpp +++ b/include/mapora/parsers/velodyne_vlp16.hpp @@ -14,8 +14,8 @@ * limitations under the License. */ -#ifndef MAPORA__CONTINUOUS_PACKET_PARSER_HPP_ -#define MAPORA__CONTINUOUS_PACKET_PARSER_HPP_ +#ifndef MAPORA__CONTINUOUS_PACKET_PARSER_VLP16_HPP_ +#define MAPORA__CONTINUOUS_PACKET_PARSER_VLP16_HPP_ #include #include @@ -23,17 +23,19 @@ #include #include #include -#include "point_xyzit.hpp" +#include "mapora/point_types.hpp" #include "mapora/date.h" #include -namespace mapora::points_provider::continuous_packet_parser_vlp16 +namespace mapora::points_provider::continuous_packet_parser { class ContinuousPacketParserVlp16 { public: - using Point = point_types::PointXYZIT; + using SharedPtr = std::shared_ptr; + using ConstSharedPtr = const std::shared_ptr; + using Point = point_types::PointXYZITRH; using Points = std::vector; ContinuousPacketParserVlp16(); @@ -135,7 +137,7 @@ class ContinuousPacketParserVlp16 bool can_publish_again_; float angle_deg_cut_; }; -} // namespace mapora::point_provider::continuous_packet_parser_vlp16 +} // namespace mapora::point_provider::continuous_packet_parser -#endif // MAPORA__CONTINUOUS_PACKET_PARSER_HPP_ +#endif // MAPORA__CONTINUOUS_PACKET_PARSER_VLP16_HPP_ diff --git a/include/mapora/point_types.hpp b/include/mapora/point_types.hpp new file mode 100644 index 0000000..847b64a --- /dev/null +++ b/include/mapora/point_types.hpp @@ -0,0 +1,81 @@ +#ifndef BUILD_POINT_TYPES_HPP +#define BUILD_POINT_TYPES_HPP + +#include + +#include +#include + +namespace mapora::point_types +{ +struct PointXYZI +{ + float x{0.0F}; + float y{0.0F}; + float z{0.0F}; + float intensity{0.0F}; + friend bool operator==(const PointXYZI & p1, const PointXYZI & p2) + { + using boost::math::epsilon_difference; + return epsilon_difference(p1.x, p2.x) == 0.0F && epsilon_difference(p1.y, p2.y) == 0.0F && + epsilon_difference(p1.z, p2.z) == 0.0F && + epsilon_difference(p1.intensity, p2.intensity) == 0.0F; + } +} __attribute__((packed)); + +struct PointXYZIT +{ + float x{0.0F}; + float y{0.0F}; + float z{0.0F}; + uint32_t intensity{0U}; + uint32_t stamp_unix_seconds{0U}; + uint32_t stamp_nanoseconds{0U}; + friend bool operator==(const PointXYZIT & p1, const PointXYZIT & p2) + { + using boost::math::epsilon_difference; + return epsilon_difference(p1.x, p2.x) == 0.0F && epsilon_difference(p1.y, p2.y) == 0.0F && + epsilon_difference(p1.z, p2.z) == 0.0F && p1.intensity == p2.intensity && + p1.stamp_unix_seconds == p2.stamp_unix_seconds && + p1.stamp_nanoseconds == p2.stamp_nanoseconds; + } +} __attribute__((packed)); + +struct PointXYZIR +{ + float x{0.0F}; + float y{0.0F}; + float z{0.0F}; + uint32_t intensity{0U}; + uint32_t ring{0U}; + friend bool operator==(const PointXYZIR & p1, const PointXYZIR & p2) + { + using boost::math::epsilon_difference; + return epsilon_difference(p1.x, p2.x) == 0.0F && epsilon_difference(p1.y, p2.y) == 0.0F && + epsilon_difference(p1.z, p2.z) == 0.0F && p1.intensity == p2.intensity && + p1.ring == p2.ring; + } +} __attribute__((packed)); + +struct PointXYZITRH +{ + float x{0.0F}; + float y{0.0F}; + float z{0.0F}; + uint32_t intensity{0U}; + uint32_t stamp_unix_seconds{0U}; + uint32_t stamp_nanoseconds{0U}; + uint32_t ring{0U}; + float horizontal_angle{0.0F}; + friend bool operator==(const PointXYZITRH & p1, const PointXYZITRH & p2) + { + using boost::math::epsilon_difference; + return epsilon_difference(p1.x, p2.x) == 0.0F && epsilon_difference(p1.y, p2.y) == 0.0F && + epsilon_difference(p1.z, p2.z) == 0.0F && p1.intensity == p2.intensity && + p1.stamp_unix_seconds == p2.stamp_unix_seconds && + p1.stamp_nanoseconds == p2.stamp_nanoseconds && p1.ring == p2.ring; + } +} __attribute__((packed)); +} // namespace loam_mapper::point_types + +#endif // BUILD_POINT_TYPES_HPP diff --git a/include/mapora/point_xyzi.hpp b/include/mapora/point_xyzi.hpp deleted file mode 100644 index b5d4414..0000000 --- a/include/mapora/point_xyzi.hpp +++ /dev/null @@ -1,45 +0,0 @@ -/* -* Copyright 2023 LeoDrive.ai, Inc. 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 MAPORA__POINT_XYZI_HPP_ -#define MAPORA__POINT_XYZI_HPP_ - -#include -#include -#include - -namespace mapora::point_types -{ -struct PointXYZI -{ - double x; - double y; - double z; - float intensity {0.0F}; - friend bool operator==( - const PointXYZI & p1, - const PointXYZI & p2) - { - using boost::math::epsilon_difference; - return epsilon_difference(p1.x, p2.x) == 0.0F && - epsilon_difference(p1.y, p2.y) == 0.0F && - epsilon_difference(p1.z, p2.z) == 0.0F && - epsilon_difference(p1.intensity, p2.intensity) == 0.0F; - } -} __attribute__((packed)); -} // namespace mapora::point_types - - -#endif // MAPORA__POINT_XYZI_HPP_ diff --git a/include/mapora/point_xyzit.hpp b/include/mapora/point_xyzit.hpp deleted file mode 100644 index 0e213a9..0000000 --- a/include/mapora/point_xyzit.hpp +++ /dev/null @@ -1,51 +0,0 @@ -/* -* Copyright 2023 LeoDrive.ai, Inc. 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 MAPORA__POINT_XYZIT_HPP_ -#define MAPORA__POINT_XYZIT_HPP_ - -#include -#include -#include -#include - -namespace mapora::point_types -{ -struct PointXYZIT -{ - double x; - double y; - double z; - uint32_t intensity {0U}; - uint32_t stamp_unix_seconds{0U}; - uint32_t stamp_nanoseconds{0U}; - friend bool operator==( - const PointXYZIT & p1, - const PointXYZIT & p2) - { - using boost::math::epsilon_difference; - return epsilon_difference(p1.x, p2.x) == 0.0F && - epsilon_difference(p1.y, p2.y) == 0.0F && - epsilon_difference(p1.z, p2.z) == 0.0F && - p1.intensity == p2.intensity && - p1.stamp_unix_seconds == p2.stamp_unix_seconds && - p1.stamp_nanoseconds == p2.stamp_nanoseconds; - } -} __attribute__((packed)); -} // namespace mapora::point_types - - -#endif // MAPORA__POINT_XYZIT_HPP_ diff --git a/include/mapora/points_provider_velodyne_vlp16.hpp b/include/mapora/points_provider.hpp similarity index 72% rename from include/mapora/points_provider_velodyne_vlp16.hpp rename to include/mapora/points_provider.hpp index 8b80600..ed53d1e 100644 --- a/include/mapora/points_provider_velodyne_vlp16.hpp +++ b/include/mapora/points_provider.hpp @@ -23,42 +23,50 @@ #include #include #include -#include "points_provider_base.hpp" -#include "point_xyzit.hpp" +#include "point_types.hpp" #include "mapora/date.h" -#include "mapora/continuous_packet_parser_vlp16.hpp" +#include "mapora/parsers/velodyne_vlp16.hpp" namespace mapora::points_provider { namespace fs = boost::filesystem; -class PointsProviderVelodyneVlp16 : public virtual PointsProviderBase +class PointsProvider { public: - using SharedPtr = std::shared_ptr; + using SharedPtr = std::shared_ptr; using ConstSharedPtr = const SharedPtr; + using Point = point_types::PointXYZITRH; + using Points = std::vector; - explicit PointsProviderVelodyneVlp16( std::string path_folder_pcaps); + explicit PointsProvider( + std::string path_folder_pcaps, std::string sensor_type); - void process() override; + void process(); void process_pcaps_into_clouds( std::function & callback_cloud_surround_out, size_t index_start, size_t count, - float max_point_distance_from_lidar, - float min_point_distance_from_lidar); - std::string info() override; + float min_point_distance_from_lidar, + float max_point_distance_from_lidar); + std::string info(); std::vector paths_pcaps_; + + template void process_pcap_into_clouds( const fs::path & path_pcap, const std::function& callback_cloud_surround_out, - continuous_packet_parser_vlp16::ContinuousPacketParserVlp16& parser, +// continuous_packet_parser::ContinuousPacketParserVlp16& parser, + parser_type &parser, const float min_point_distance_from_lidar, const float max_point_distance_from_lidar); private: fs::path path_folder_pcaps_; + std::string sensor_type_; + + }; } // namespace mapora::points_provider diff --git a/include/mapora/points_provider_base.hpp b/include/mapora/points_provider_base.hpp deleted file mode 100644 index d47128e..0000000 --- a/include/mapora/points_provider_base.hpp +++ /dev/null @@ -1,38 +0,0 @@ -/* -* Copyright 2023 LeoDrive.ai, Inc. 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 MAPORA__POINTS_PROVIDER_BASE_HPP_ -#define MAPORA__POINTS_PROVIDER_BASE_HPP_ - -#include -#include -#include -#include "point_xyzit.hpp" - -namespace mapora::points_provider -{ -class PointsProviderBase -{ -public: - using Point = point_types::PointXYZIT; - using Points = std::vector; - virtual void process() = 0; -// virtual bool get_next_cloud(std::vector & cloud_out) = 0; - virtual std::string info() = 0; -}; -} // namespace mapora::points_provider - -#endif // MAPORA__POINTS_PROVIDER_BASE_HPP_ diff --git a/include/mapora/transform_provider_applanix.hpp b/include/mapora/transform_provider.hpp similarity index 60% rename from include/mapora/transform_provider_applanix.hpp rename to include/mapora/transform_provider.hpp index 66f0ec2..23b2aac 100644 --- a/include/mapora/transform_provider_applanix.hpp +++ b/include/mapora/transform_provider.hpp @@ -22,40 +22,67 @@ #include #include #include +#include -namespace mapora::transform_provider_applanix -{ +namespace mapora::transform_provider { namespace fs = boost::filesystem; -class TransformProviderApplanix -{ +class TransformProvider { public: - using SharedPtr = std::shared_ptr; + using SharedPtr = std::shared_ptr; using ConstSharedPtr = const SharedPtr; - explicit TransformProviderApplanix(const std::string & path_file_ascii_output); + explicit TransformProvider(const std::string & path_file_ascii_output); void process(); + struct Velocity + { + double x{0U}; + double y{0U}; + double z{0U}; + }; struct Pose { uint32_t stamp_unix_seconds{0U}; uint32_t stamp_nanoseconds{0U}; + Velocity velocity; geometry_msgs::msg::PoseWithCovariance pose_with_covariance; + float meridian_convergence; + }; + struct Imu + { + uint32_t stamp_unix_seconds{0U}; + uint32_t stamp_nanoseconds{0U}; + sensor_msgs::msg::Imu imu; }; std::vector poses_; - std::string header_line_string; - std::string time_string; - int data_line_number; - std::string mission_date; + std::vector imu_rotations_; Pose get_pose_at( uint32_t stamp_unix_seconds, uint32_t stamp_nanoseconds); + Imu get_imu_at(uint32_t stamp_unix_seconds, uint32_t stamp_nanoseconds); + + std::vector parse_mgrs_coordinates(const std::string & mgrs_string); + std::string parse_mgrs_zone(const std::string & mgrs_string); + + private: fs::path path_file_ascii_output_; + std::string header_line_string; + std::string time_string; + int data_line_number; + std::string mission_date; + double last_utc_time = 0.0; + bool day_changed_flag = false; + + float compute_meridian_convergence(double lat, double lon); + + int last_index_imu; + int last_index_pose; }; -} // mapora::transform_provider_applanix +} // mapora::transform_provider #endif // MAPORA__TRANSFORM_PROVIDER_APPLANIX_HPP_ diff --git a/include/mapora/utils.hpp b/include/mapora/utils.hpp index b7a0e17..bf17dfe 100644 --- a/include/mapora/utils.hpp +++ b/include/mapora/utils.hpp @@ -17,11 +17,12 @@ #ifndef MAPORA__UTILS_HPP_ #define MAPORA__UTILS_HPP_ -#include -#include -#include +#include #include +#include +#include #include +#include namespace mapora::utils { @@ -41,6 +42,9 @@ class Utils constexpr double multiplier = M_PI / 180.0; return static_cast(deg * multiplier); } + + static Eigen::Matrix3d ned2enu_converter_for_matrices(const Eigen::Matrix3d & matrix3d); + }; } // namespace mapora::utils diff --git a/package.xml b/package.xml index ea48995..c4c27fb 100644 --- a/package.xml +++ b/package.xml @@ -11,6 +11,9 @@ rclcpp rclcpp_components + PcapPlusPlus + GeographicLib + PCL rosbag2_cpp sensor_msgs nav_msgs diff --git a/params/mapora_params.yaml b/params/mapora_params.yaml index 4224d21..f84aa9d 100644 --- a/params/mapora_params.yaml +++ b/params/mapora_params.yaml @@ -1,11 +1,18 @@ /mapora: ros__parameters: - project_namespace: ytu_campus_080423_mapora - pcap_dir_path: src/mapora/example_data/mapora/splitted_pcap - pose_txt_path: src/mapora/example_data/mapora/ytu_campus_080423_pp_istn_utc_leo.txt - las_export_directory: src/mapora/example_data/mapora/output_las/ + + project_namespace: 2024_08_16-bilisim_vadi + + pcap_dir_path: /home/ataparlar/data/task_specific/mapora/pcaps/ + pose_txt_path: /home/ataparlar/data/task_specific/mapora/2024_08_16-bilisim_vadi-map-export.txt + las_export_directory: /home/ataparlar/data/task_specific/mapora/output/ + + # "hesai_xt32" or "velodyne_vlp16" + lidar_model: hesai_xt32 + max_point_distance_from_lidar: 60.0 # in meters min_point_distance_from_lidar: 3.0 # in meters - r_x: 179.59 - r_y: -0.42 - r_z: 0.39 + + r_x: -179.58825 + r_y: 0.44683 + r_z: 179.66740 diff --git a/src/mapora.cpp b/src/mapora.cpp index e8de197..072a872 100644 --- a/src/mapora.cpp +++ b/src/mapora.cpp @@ -1,54 +1,50 @@ /* -* Copyright 2023 LeoDrive.ai, Inc. 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. + * Copyright 2024 LeoDrive.ai, Inc. 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. */ #include "mapora/mapora.hpp" #include -#include +#include #include #include #include + #include +#include + #include -#include "mapora/points_provider_velodyne_vlp16.hpp" -#include "mapora/transform_provider_applanix.hpp" +#include -#include +#include #include #include #include #include - -namespace -{ +namespace { const std::uint32_t QOS_HISTORY_DEPTH = 10; } -size_t file_counter = 0; - -namespace mapora -{ -Mapora::Mapora(const rclcpp::NodeOptions & options) - : Node("mapora", options) -{ - pub_ptr_cloud_current_ = this->create_publisher("cloud_current", 10); - pub_ptr_path_applanix_ = this->create_publisher("path_applanix", 10); - tf_broadcaster_ = - std::make_unique(*this); +namespace mapora { +Mapora::Mapora(const rclcpp::NodeOptions &options) : Node("mapora") { + pub_ptr_cloud_current_ = + this->create_publisher("cloud_current", 10); + pub_ptr_path = + this->create_publisher("path", 10); + tf_broadcaster_ = std::make_unique(*this); this->declare_parameter("r_x", 0.0); this->declare_parameter("r_y", 0.0); @@ -59,6 +55,7 @@ Mapora::Mapora(const rclcpp::NodeOptions & options) this->declare_parameter("max_point_distance_from_lidar", 60.0); this->declare_parameter("min_point_distance_from_lidar", 3.0); this->declare_parameter("las_export_directory", "/projects/mapora_ws/src"); + this->declare_parameter("lidar_model", "hesai_xt32"); imu2lidar_roll = this->get_parameter("r_x").as_double(); imu2lidar_pitch = this->get_parameter("r_y").as_double(); @@ -66,169 +63,123 @@ Mapora::Mapora(const rclcpp::NodeOptions & options) pcap_dir_path_ = this->get_parameter("pcap_dir_path").as_string(); pose_txt_path_ = this->get_parameter("pose_txt_path").as_string(); project_namespace_ = this->get_parameter("project_namespace").as_string(); - max_point_distance_from_lidar_ = this->get_parameter("max_point_distance_from_lidar").as_double(); - min_point_distance_from_lidar_ = this->get_parameter("min_point_distance_from_lidar").as_double(); + max_point_distance_from_lidar_ = + this->get_parameter("max_point_distance_from_lidar").as_double(); + min_point_distance_from_lidar_ = + this->get_parameter("min_point_distance_from_lidar").as_double(); las_export_dir = this->get_parameter("las_export_directory").as_string(); - - // Applanix Pose extraction - RCLCPP_INFO(this->get_logger(), "Applanix Transform Provider started with the .txt file in: %s", pose_txt_path_.c_str()); - transform_provider_applanix = std::make_shared( - pose_txt_path_); - transform_provider_applanix->process(); - RCLCPP_INFO(this->get_logger(), "Applanix Transform Provider is DONE."); - - // Velodyne Point Cloud Extraction - RCLCPP_INFO(this->get_logger(), "Velodyne Point Cloud Provider started with the PCAP files in : %s", pcap_dir_path_.c_str()); - points_provider_velodyne_vlp16 = std::make_shared( - pcap_dir_path_); - points_provider_velodyne_vlp16->process(); - - std::function callback = - std::bind(&Mapora::callback_cloud_surround_out, this, std::placeholders::_1); + lidar_model = this->get_parameter("lidar_model").as_string(); + + // Pose extraction + RCLCPP_INFO(this->get_logger(), + "Transform Provider started with the .txt file in: %s", + pose_txt_path_.c_str()); + transform_provider = + std::make_shared( + pose_txt_path_); + transform_provider->process(); + RCLCPP_INFO(this->get_logger(), "Transform Provider is DONE."); + + // Point Cloud Extraction + RCLCPP_INFO( + this->get_logger(), + "Point Cloud Provider started with the PCAP files in : %s", + pcap_dir_path_.c_str()); + points_provider_ = std::make_shared( + pcap_dir_path_, lidar_model); + points_provider_->process(); + + std::function callback = std::bind( + &Mapora::callback_cloud_surround_out, this, std::placeholders::_1); // time auto whole_process_start = std::chrono::high_resolution_clock::now(); - for (int i = 0; i < points_provider_velodyne_vlp16->paths_pcaps_.size(); i++) - { + for (int i = 0; i < points_provider_->paths_pcaps_.size(); i++) { auto start = std::chrono::high_resolution_clock::now(); RCLCPP_INFO(this->get_logger(), "Processing the %d. pcap file.", i); - points_provider_velodyne_vlp16->process_pcaps_into_clouds( - callback, i, 1,min_point_distance_from_lidar_, max_point_distance_from_lidar_); -// future_process = std::async(std::launch::async, &Mapora::process, this); + points_provider_->process_pcaps_into_clouds(callback, i, 1, + min_point_distance_from_lidar_, + max_point_distance_from_lidar_); + // future_process = std::async(std::launch::async, &Mapora::process, + // this); process(); clouds.erase(clouds.begin(), clouds.end()); file_counter++; auto stop = std::chrono::high_resolution_clock::now(); - auto duration = std::chrono::duration_cast(stop - start); - RCLCPP_INFO(this->get_logger(), "%zu. cloud process took %ld seconds.", file_counter, duration.count()/1000); - RCLCPP_INFO(this->get_logger(), "-------------------------------------------------------------------------"); + auto duration = + std::chrono::duration_cast(stop - start); + RCLCPP_INFO(this->get_logger(), "%zu. cloud process took %ld seconds.", + file_counter, duration.count() / 1000); + RCLCPP_INFO(this->get_logger(), "------------------------------------------" + "-------------------------------"); } auto whole_process_stop = std::chrono::high_resolution_clock::now(); - auto whole_process_duration = std::chrono::duration_cast(whole_process_stop - whole_process_start); - RCLCPP_INFO(this->get_logger(), "Whole process took %ld seconds.", whole_process_duration.count()/1000); + auto whole_process_duration = + std::chrono::duration_cast( + whole_process_stop - whole_process_start); + RCLCPP_INFO(this->get_logger(), "Whole process took %ld seconds.", + whole_process_duration.count() / 1000); rclcpp::shutdown(); } -void Mapora::process() -{ - RCLCPP_INFO(this->get_logger(), "Calibration angles, r_x: %f, r_y: %f, r_z: %f degrees", +void Mapora::process() { + RCLCPP_INFO(this->get_logger(), + "Calibration angles, r_x: %f, r_y: %f, r_z: %f degrees", imu2lidar_roll, imu2lidar_pitch, imu2lidar_yaw); RCLCPP_INFO(this->get_logger(), "Mapora begins."); std::string path = las_export_dir; - std::string name_las = path + project_namespace_ + "_global_" + std::to_string(file_counter) + ".las"; + std::string name_las = path + project_namespace_ + "_global_" + + std::to_string(file_counter) + ".las"; std::ofstream ofs; ofs.open(name_las, std::ios::out | std::ios::binary); liblas::Header las_header; - las_header.SetScale(0.001, 0.01, 0.001); + las_header.SetScale(0.001, 0.001, 0.001); las_header.SetDataFormatId(liblas::ePointFormat0); liblas::Writer writer(ofs, las_header); - auto thing_to_cloud = []( - const Points & points_bad, - const std::string & frame_id) { - using CloudModifier = point_cloud_msg_wrapper::PointCloud2Modifier; + auto thing_to_cloud = [](const points_provider::PointsProvider::Points + &points_bad, + const std::string &frame_id) { + using CloudModifier = + point_cloud_msg_wrapper::PointCloud2Modifier; PointCloud2::SharedPtr cloud_ptr_current = std::make_shared(); CloudModifier cloud_modifier_current(*cloud_ptr_current, frame_id); cloud_modifier_current.resize(points_bad.size()); - std::transform( - std::execution::par, - points_bad.cbegin(), - points_bad.cend(), - cloud_modifier_current.begin(), - [](const points_provider::PointsProviderVelodyneVlp16::Point & point_bad) { - return point_types::PointXYZI{ - point_bad.x, point_bad.y, point_bad.z, static_cast(point_bad.intensity)}; - }); + std::transform(std::execution::par, points_bad.cbegin(), points_bad.cend(), + cloud_modifier_current.begin(), + [](const points_provider::PointsProvider::Point &point_bad) { + return point_types::PointXYZI{ + point_bad.x, point_bad.y, point_bad.z, + static_cast(point_bad.intensity)}; + }); return cloud_ptr_current; }; - nav_msgs::msg::Path path_applanix; - path_applanix.header.frame_id = "map"; - - points_provider::PointsProviderVelodyneVlp16::Points cloud_all; - - path_applanix.poses.resize(transform_provider_applanix->poses_.size()); - std::cout << "transform_provider_applanix->poses_.size(): " << transform_provider_applanix->poses_.size() << std::endl; - - for (int i=0; iposes_.size(); i++) - path_applanix.poses[i].pose = transform_provider_applanix->poses_[i].pose_with_covariance.pose; - pub_ptr_path_applanix_->publish(path_applanix); - - std::cout << "Point Cloud: " << file_counter << std::endl; - auto process_cloud_single = [&](const points_provider::PointsProviderVelodyneVlp16::Points& cloud){ - - points_provider::PointsProviderVelodyneVlp16::Points cloud_trans; - cloud_trans.resize(cloud.size()); - - std::transform( - std::execution::par, - cloud.cbegin(), - cloud.cend(), - cloud_trans.begin(), - [this, &path_applanix](const points_provider::PointsProviderVelodyneVlp16::Point & point) { - points_provider::PointsProviderVelodyneVlp16::Point point_trans; - - // position from applanix data is taken into pose below according to the stamps . - pose = transform_provider_applanix->get_pose_at(point.stamp_unix_seconds, point.stamp_nanoseconds); - - geometry_msgs::msg::PoseStamped pose_stamped; - pose_stamped.pose = pose.pose_with_covariance.pose; - pose_stamped.header.frame_id = path_applanix.header.frame_id; - pose_stamped.header.stamp.sec = point.stamp_unix_seconds; - pose_stamped.header.stamp.nanosec = point.stamp_nanoseconds; - - const auto & pose_ori = pose.pose_with_covariance.pose.orientation; - Eigen::Quaterniond quat_ins_to_map(pose_ori.w, pose_ori.x, pose_ori.y, pose_ori.z); - Eigen::Affine3d affine_imu2lidar(Eigen::Affine3d::Identity()); - affine_imu2lidar.matrix().topLeftCorner<3, 3>() = - Eigen::AngleAxisd(utils::Utils::deg_to_rad(-imu2lidar_yaw), Eigen::Vector3d::UnitZ()) - .toRotationMatrix() * - Eigen::AngleAxisd(utils::Utils::deg_to_rad(imu2lidar_pitch), Eigen::Vector3d::UnitY()) - .toRotationMatrix() * - Eigen::AngleAxisd(utils::Utils::deg_to_rad(-imu2lidar_roll), Eigen::Vector3d::UnitX()) - .toRotationMatrix(); - - // sensor to map rotation is created to add translations and get the right rotation. - Eigen::Affine3d affine_sensor2map(Eigen::Affine3d::Identity()); - - affine_sensor2map.matrix().topLeftCorner<3, 3>() = quat_ins_to_map.toRotationMatrix().inverse() * affine_imu2lidar.rotation().inverse(); - - // pose is added to the transformation matrix. - these were completed for every point in the pointclouds. - auto & pose_pos = pose_stamped.pose.position; - affine_sensor2map.matrix().topRightCorner<3, 1>() << pose_pos.x, pose_pos.y, pose_pos.z; - - // get the point's position w.r.t. the point cloud origin. - Eigen::Vector4d vec_point_in_first(point.x, - point.y, - point.z, - 1.0); - // create a 3D vector for transformed point to the map position and rotation. - Eigen::Vector4d vec_point_trans = affine_sensor2map.matrix() * vec_point_in_first; - - point_trans.x = static_cast(vec_point_trans(0)); - point_trans.y = static_cast(vec_point_trans(1)); - point_trans.z = static_cast(vec_point_trans(2)); - point_trans.intensity = point.intensity; - - return point_trans; - }); - - geometry_msgs::msg::TransformStamped t; - t.transform.translation.x = pose.pose_with_covariance.pose.position.x; - t.transform.translation.y = pose.pose_with_covariance.pose.position.y; - t.transform.translation.z = pose.pose_with_covariance.pose.position.z; - t.transform.rotation.x = pose.pose_with_covariance.pose.orientation.x; - t.transform.rotation.y = pose.pose_with_covariance.pose.orientation.y; - t.transform.rotation.z = pose.pose_with_covariance.pose.orientation.z; - t.transform.rotation.w = pose.pose_with_covariance.pose.orientation.w; - t.header.frame_id = "map"; - t.child_frame_id = "sensor_kit"; + nav_msgs::msg::Path path_; + path_.header.frame_id = "map"; + path_.header.stamp = this->get_clock()->now(); + path_.poses.resize(transform_provider->poses_.size()); + + for (auto &pose : transform_provider->poses_) { + geometry_msgs::msg::PoseStamped poseStamped; + poseStamped.header.frame_id = "map"; + poseStamped.header.stamp = this->get_clock()->now(); + poseStamped.pose = pose.pose_with_covariance.pose; + path_.poses.push_back(poseStamped); + } + pub_ptr_path->publish(path_); - tf_broadcaster_->sendTransform(t); + points_provider::PointsProvider::Points cloud_all; + for (auto &cloud : clouds) { + Points cloud_trans = transform_points(cloud); auto cloud_ptr_current = thing_to_cloud(cloud_trans, "map"); + pub_ptr_cloud_current_->publish(*cloud_ptr_current); + cloud_all.insert(cloud_all.end(), cloud_trans.begin(), cloud_trans.end()); - for (auto & point : cloud_trans) { + // std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + for (auto &point : cloud_trans) { liblas::Point las_point(&las_header); las_point.SetX(point.x); las_point.SetY(point.y); @@ -236,39 +187,132 @@ void Mapora::process() las_point.SetIntensity(point.intensity); writer.WritePoint(las_point); } - pub_ptr_cloud_current_->publish(*cloud_ptr_current); - }; - -// section 2 - for (auto & cloud : clouds) { - process_cloud_single(cloud); } + + cloud_all.clear(); } -void Mapora::callback_cloud_surround_out(const Mapora::Points & points_surround) -{ +void Mapora::callback_cloud_surround_out( + const Mapora::Points &points_surround) { + // pub_ptr_basic_cloud_current_->publish(*points_to_cloud(points_surround, + // "map")); std::this_thread::sleep_for(std::chrono::milliseconds(150)); clouds.push_back(points_surround); } -sensor_msgs::msg::PointCloud2::SharedPtr Mapora::points_to_cloud( - const Mapora::Points & points_bad, const std::string & frame_id) -{ - using CloudModifier = point_cloud_msg_wrapper::PointCloud2Modifier; +sensor_msgs::msg::PointCloud2::SharedPtr +Mapora::points_to_cloud(const Mapora::Points &points_bad, + const std::string &frame_id) { + using CloudModifier = + point_cloud_msg_wrapper::PointCloud2Modifier; PointCloud2::SharedPtr cloud_ptr_current = std::make_shared(); CloudModifier cloud_modifier_current(*cloud_ptr_current, frame_id); cloud_modifier_current.resize(points_bad.size()); std::transform( - std::execution::par, - points_bad.cbegin(), - points_bad.cend(), - cloud_modifier_current.begin(), - [](const points_provider::PointsProviderVelodyneVlp16::Point & point_bad) { - return point_types::PointXYZI{ - point_bad.x, point_bad.y, point_bad.z, static_cast(point_bad.intensity)}; - }); + std::execution::par, points_bad.cbegin(), points_bad.cend(), + cloud_modifier_current.begin(), + [](const points_provider::PointsProvider::Point &point_bad) { + return point_types::PointXYZIR{ + point_bad.x, point_bad.y, point_bad.z, + static_cast(point_bad.intensity), + // static_cast(point_bad.stamp_nanoseconds), + // static_cast(point_bad.horizontal_angle), + static_cast(point_bad.ring)}; + }); return cloud_ptr_current; } -} // namespace mapora +Mapora::Points Mapora::transform_points(Mapora::Points &cloud) { + Points filtered_points; + // filtered_points.resize(cloud.size()); + + auto last_pose = transform_provider->poses_.back(); + std::copy_if( + cloud.cbegin(), cloud.cend(), std::back_inserter(filtered_points), + [&last_pose](const points_provider::PointsProvider::Point &point) { + return !(point.stamp_unix_seconds > last_pose.stamp_unix_seconds || + (point.stamp_unix_seconds == last_pose.stamp_unix_seconds && + point.stamp_nanoseconds > last_pose.stamp_nanoseconds)); + }); + + Points cloud_trans; + cloud_trans.resize(filtered_points.size()); + + std::transform( + std::execution::par, filtered_points.cbegin(), filtered_points.cend(), + cloud_trans.begin(), + [this](const points_provider::PointsProvider::Point &point) { + if ((point.stamp_unix_seconds > + transform_provider->poses_.back().stamp_unix_seconds) || + (point.stamp_unix_seconds == + transform_provider->poses_.back() + .stamp_unix_seconds && + point.stamp_nanoseconds > + transform_provider->poses_.back() + .stamp_nanoseconds)) { + return last_point; + } + + points_provider::PointsProvider::Point point_trans; + + mapora::transform_provider::TransformProvider::Pose + pose = this->transform_provider->get_pose_at( + point.stamp_unix_seconds, point.stamp_nanoseconds); + + geometry_msgs::msg::PoseStamped pose_stamped; + pose_stamped.pose = pose.pose_with_covariance.pose; + pose_stamped.header.frame_id = "map"; + + const auto &pose_ori = pose.pose_with_covariance.pose.orientation; + Eigen::Quaterniond quat(pose_ori.w, pose_ori.x, pose_ori.y, pose_ori.z); + + Eigen::Affine3d affine_sensor2map(Eigen::Affine3d::Identity()); + + Eigen::Affine3d affine_imu2lidar(Eigen::Affine3d::Identity()); + affine_imu2lidar.matrix().topLeftCorner<3, 3>() = + Eigen::AngleAxisd(utils::Utils::deg_to_rad( + -(imu2lidar_yaw - pose.meridian_convergence)), + Eigen::Vector3d::UnitZ()) + .toRotationMatrix() * + Eigen::AngleAxisd(utils::Utils::deg_to_rad(imu2lidar_pitch), + Eigen::Vector3d::UnitY()) + .toRotationMatrix() * + Eigen::AngleAxisd(utils::Utils::deg_to_rad(imu2lidar_roll), + Eigen::Vector3d::UnitX()) + .toRotationMatrix(); + + Eigen::Affine3d affine_imu2lidar_enu; + affine_imu2lidar_enu.matrix().topLeftCorner<3, 3>() = + utils::Utils::ned2enu_converter_for_matrices( + affine_imu2lidar.matrix().topLeftCorner<3, 3>()); + + affine_sensor2map.matrix().topLeftCorner<3, 3>() = + quat.toRotationMatrix() * affine_imu2lidar_enu.rotation(); + + auto &pose_pos = pose_stamped.pose.position; + affine_sensor2map.matrix().topRightCorner<3, 1>() << pose_pos.x, + pose_pos.y, pose_pos.z; + + Eigen::Vector4d vec_point_in(point.x, point.y, point.z, 1.0); + Eigen::Vector4d vec_point_trans = + affine_sensor2map.matrix() * vec_point_in; + + point_trans.x = static_cast(vec_point_trans(0)); + point_trans.y = static_cast(vec_point_trans(1)); + point_trans.z = static_cast(vec_point_trans(2)); + point_trans.ring = point.ring; + point_trans.horizontal_angle = point.horizontal_angle; + point_trans.intensity = point.intensity; + point_trans.stamp_unix_seconds = point.stamp_unix_seconds; + point_trans.stamp_nanoseconds = point.stamp_nanoseconds; + + last_point = point_trans; + + return point_trans; + }); + + return cloud_trans; +} + +} // namespace mapora #include "rclcpp_components/register_node_macro.hpp" RCLCPP_COMPONENTS_REGISTER_NODE(mapora::Mapora) diff --git a/src/parsers/hesai_xt32.cpp b/src/parsers/hesai_xt32.cpp new file mode 100644 index 0000000..bf5b99a --- /dev/null +++ b/src/parsers/hesai_xt32.cpp @@ -0,0 +1,399 @@ +// +// Created by ataparlar on 11.10.2024. +// + +#include "mapora/parsers/hesai_xt32.hpp" +#include "mapora/utils.hpp" + +#include + + +namespace mapora::points_provider::continuous_packet_parser +{ + +ContinuousPacketParserXt32::ContinuousPacketParserXt32() + : factory_bytes_are_read_at_least_once_{false}, + has_received_valid_position_package_{false}, + has_processed_a_packet_{false}, + angle_deg_azimuth_last_packet_{0.0f}, + microseconds_last_packet_{0U}, + can_publish_again_{true}, + angle_deg_cut_{90.0f} +{ + map_byte_to_return_mode_.insert(std::make_pair(55, ReturnMode::Strongest)); + map_byte_to_return_mode_.insert(std::make_pair(56, ReturnMode::LastReturn)); + map_byte_to_return_mode_.insert(std::make_pair(57, ReturnMode::DualReturn)); + // map_byte_to_return_mode_.insert(std::make_pair(59, ReturnMode::DualReturnWithConfidence)); + + map_return_mode_to_string_.insert(std::make_pair(ReturnMode::Strongest, "Strongest")); + map_return_mode_to_string_.insert(std::make_pair(ReturnMode::LastReturn, "LastReturn")); + map_return_mode_to_string_.insert(std::make_pair(ReturnMode::DualReturn, "DualReturn")); + // map_return_mode_to_string_.insert( + // std::make_pair(ReturnMode::DualReturnWithConfidence, "DualReturnWithConfidence")); + + map_byte_to_hesai_model_.insert(std::make_pair(1, HesaiModel::Pandar128)); + map_byte_to_hesai_model_.insert(std::make_pair(2, HesaiModel::ET)); + map_byte_to_hesai_model_.insert(std::make_pair(3, HesaiModel::QT)); + map_byte_to_hesai_model_.insert(std::make_pair(4, HesaiModel::AT128)); + map_byte_to_hesai_model_.insert(std::make_pair(6, HesaiModel::PandarXT)); + map_byte_to_hesai_model_.insert(std::make_pair(7, HesaiModel::PandarFT)); + + map_hesai_model_to_string_.insert(std::make_pair(HesaiModel::Pandar128, "Pandar128")); + map_hesai_model_to_string_.insert(std::make_pair(HesaiModel::ET, "ET")); + map_hesai_model_to_string_.insert(std::make_pair(HesaiModel::QT, "QT")); + map_hesai_model_to_string_.insert(std::make_pair(HesaiModel::AT128, "AT128")); + map_hesai_model_to_string_.insert(std::make_pair(HesaiModel::PandarXT, "PandarXT")); + map_hesai_model_to_string_.insert(std::make_pair(HesaiModel::PandarFT, "PandarFT")); + + channel_to_angle_vertical_ = std::vector{ + 15.0F, 14.0F, 13.0F, 12.0F, 11.0F, 10.0F, 9.0F, 8.0F, 7.0F, 6.0F, 5.0F, + 4.0F, 3.0F, 2.0F, 1.0F, 0.0F, -1.0F, -2.0F, -3.0F, -4.0F, -5.0F, -6.0F, + -7.0F, -8.0F, -9.0F, -10.0F, -11.0F, -12.0F, -13.0F, -14.0F, -15.0F, -16.0F}; + assert(channel_to_angle_vertical_.size() == 32); +} + +void ContinuousPacketParserXt32::process_packet_into_cloud( + const pcpp::RawPacket & rawPacket, + const std::function & callback_cloud_surround_out, + const float min_point_distance_from_lidar, + const float max_point_distance_from_lidar) +{ + switch (rawPacket.getFrameLength()) { + case 554: { + if (has_received_valid_position_package_) { + break; + } + + auto * position_packet = reinterpret_cast(rawPacket.getRawData()); + + std::string nmea_sentence(position_packet->nmea_sentence); + auto segments_with_nullstuff = utils::Utils::string_to_vec_split_by(nmea_sentence, '\r'); + + auto segments_with_crc = + utils::Utils::string_to_vec_split_by(segments_with_nullstuff.front(), '*'); + auto segments = utils::Utils::string_to_vec_split_by(segments_with_crc.front(), ','); + + if (13 > segments.size() || 14 < segments.size()) { + throw std::length_error( + "nmea sentence should have 13 elements, it has " + std::to_string(segments.size())); + } + + // Receiver status: A= Active, V= Void + if (segments.at(2) != "A") { + std::cout << "Receiver Status != Active" << std::endl; + break; + } + const auto & str_time = segments.at(1); + int hours_raw = std::stoi(str_time.substr(0, 2)); + + const auto & str_date = segments.at(9); + int days_raw = std::stoi(str_date.substr(0, 2)); + int months_raw = std::stoi(str_date.substr(2, 2)); + int years_raw = 2000 + std::stoi(str_date.substr(4, 2)); + + date::year_month_day date_current_ = date::year{years_raw} / months_raw / days_raw; + tp_hours_since_epoch = date::sys_days(date_current_) + std::chrono::hours(hours_raw); + +// for (auto segment : segments) { +// std::cout << segment << std::endl; +// } + + has_received_valid_position_package_ = true; + break; + } + case 1122: { + if (!has_received_valid_position_package_) { + // Ignore until first valid Position Packet is received + break; + } + + const auto * data_packet_with_header = + reinterpret_cast(rawPacket.getRawData()); + + date::hh_mm_ss microseconds_since_toh = date::make_time( + // std::chrono::hours(data_packet_with_header->tail.hour) + + std::chrono::minutes(data_packet_with_header->tail.minute) + + std::chrono::seconds(data_packet_with_header->tail.second) + + std::chrono::microseconds(data_packet_with_header->tail.timestamp)); + double microseconds_toh = data_packet_with_header->tail.minute * 60000000 + + data_packet_with_header->tail.second * 1000000 + + data_packet_with_header->tail.timestamp; + + HesaiModel hesai_model; + auto it = map_byte_to_hesai_model_.find(data_packet_with_header->pre_header.protocol_version_major); + if (it == map_byte_to_hesai_model_.end()) { + // Key not found, break the loop + break; + } else { + hesai_model = it->second; + // Proceed with your logic using hesai_model + } + + if (hesai_model != HesaiModel::PandarXT) { + throw std::runtime_error( + "hesai_model was expected to be: " + map_hesai_model_to_string_.at(HesaiModel::PandarXT) + + " but it was: " + map_hesai_model_to_string_.at(hesai_model)); + } + + + ReturnMode return_mode; + auto it2 = map_byte_to_return_mode_.find(data_packet_with_header->tail.return_mode); + if (it2 == map_byte_to_return_mode_.end()) { + // Key not found, break the loop + break; + } else { + return_mode = it2->second; + // Proceed with your logic using hesai_model + } + + if (!factory_bytes_are_read_at_least_once_) { + hesai_model_ = hesai_model; + return_mode_ = return_mode; + factory_bytes_are_read_at_least_once_ = true; + } else { + if (hesai_model != hesai_model_) { + throw std::runtime_error( + "hesai_model was expected to be: " + map_hesai_model_to_string_.at(hesai_model) + + " but it was: " + map_hesai_model_to_string_.at(hesai_model)); + } + if (return_mode != return_mode_) { + throw std::runtime_error( + "return_mode was expected to be: " + map_return_mode_to_string_.at(return_mode_) + + " but it was: " + map_return_mode_to_string_.at(return_mode)); + } + } + + double speed_deg_per_microseconds_angle_azimuth; + float angle_deg_azimuth_last; + + if (return_mode == ReturnMode::DualReturn) { + for (size_t ind_block = 0; ind_block < data_packet_with_header->get_size_data_blocks(); + ind_block += 2) { + const auto & data_block_first = data_packet_with_header->data_blocks[ind_block]; + const auto & data_block_second = data_packet_with_header->data_blocks[ind_block + 1]; + + float angle_deg_azimuth_of_block_first = + static_cast(data_block_first.azimuth_multiplied_by_100_deg) / 100.0f; + float angle_deg_azimuth_of_block_second = + static_cast(data_block_second.azimuth_multiplied_by_100_deg) / 100.0f; + if (angle_deg_azimuth_of_block_first != angle_deg_azimuth_of_block_second) continue; + + if (!has_processed_a_packet_) { + angle_deg_azimuth_last_packet_ = angle_deg_azimuth_of_block_first; + microseconds_last_packet_ = microseconds_toh; + has_processed_a_packet_ = true; + break; + } else if (ind_block == 0) { + float angle_deg_azimuth_increased = angle_deg_azimuth_of_block_first; + if (angle_deg_azimuth_of_block_first < angle_deg_azimuth_last_packet_) { + angle_deg_azimuth_increased += 360.0f; + } + float angle_deg_angle_delta = + angle_deg_azimuth_increased - angle_deg_azimuth_last_packet_; + + // Compensate for ToH microseconds rollover + uint32_t microseconds_toh_current_increased = microseconds_toh; + if (microseconds_toh < microseconds_last_packet_) { + microseconds_toh_current_increased += 3600000000U; + // Increase internal epoch hour time point + tp_hours_since_epoch += std::chrono::hours(1); + } + uint32_t microseconds_delta = + microseconds_toh_current_increased - microseconds_last_packet_; + + speed_deg_per_microseconds_angle_azimuth = + static_cast(angle_deg_angle_delta) / microseconds_delta; + + angle_deg_azimuth_last_packet_ = angle_deg_azimuth_of_block_first; + microseconds_last_packet_ = microseconds_toh; + } + + for (int ind_point = 0; ind_point < data_block_first.get_size_data_points(); + ind_point++) { + const auto & data_point_second = data_block_second.data_points[ind_point]; + + float timing_offset_from_first_block; + if (ind_block == 0 || ind_block == 1) { + timing_offset_from_first_block = 3.28 - (50 * 3); + } else if (ind_block == 2 || ind_block == 3) { + timing_offset_from_first_block = 3.28 - (50 * 2); + } else if (ind_block == 4 || ind_block == 5) { + timing_offset_from_first_block = 3.28 - (50 * 1); + } else if (ind_block == 6 || ind_block == 7) { + timing_offset_from_first_block = 3.28; + } + float timing_offset_from_first_firing = 1.512 * (ind_point - 1) + 0.28; + + auto timing_offset_from_first_block_dur = + std::chrono::duration(timing_offset_from_first_block); + auto timing_offset_from_first_firing_dur = + std::chrono::duration(timing_offset_from_first_firing); + auto timing_offset_from_first_block_time = + std::chrono::duration_cast( + timing_offset_from_first_block_dur); + auto timing_offset_from_first_firing_time = + std::chrono::duration_cast( + timing_offset_from_first_firing_dur); + + float angle_deg_azimuth_point = + angle_deg_azimuth_of_block_first + + static_cast( + speed_deg_per_microseconds_angle_azimuth * timing_offset_from_first_firing); + + if (angle_deg_azimuth_point >= 360.0f) { + angle_deg_azimuth_point -= 360.0f; + } + + angle_deg_azimuth_last = angle_deg_azimuth_point; + float angle_rad_azimuth_point = utils::Utils::deg_to_rad(angle_deg_azimuth_point); + float angle_deg_vertical = channel_to_angle_vertical_.at(ind_point); + float angle_rad_vertical = utils::Utils::deg_to_rad(angle_deg_vertical); + float dist_m = + static_cast(data_point_second.distance_divided_by_4mm * 4) / 1000.0f; + + float dist_xy = dist_m * std::cos(angle_rad_vertical); + Point point; + point.x = dist_xy * std::sin(angle_rad_azimuth_point); + point.y = dist_xy * std::cos(angle_rad_azimuth_point); + point.z = dist_m * std::sin(angle_rad_vertical); + point.intensity = data_point_second.reflectivity; + point.ring = ind_point; + point.horizontal_angle = angle_deg_azimuth_point; + point.stamp_unix_seconds = + std::chrono::seconds( + tp_hours_since_epoch.time_since_epoch() + microseconds_since_toh.minutes() + + microseconds_since_toh.seconds()) + .count(); + point.stamp_nanoseconds = + std::chrono::nanoseconds( + microseconds_since_toh.subseconds() + timing_offset_from_first_block_time + + timing_offset_from_first_firing_time) + .count(); + + if (dist_m != 0 && dist_m <= max_point_distance_from_lidar && + dist_m >= min_point_distance_from_lidar) { + cloud_.push_back(point); + } + } + } + } else if (return_mode == ReturnMode::LastReturn || return_mode == ReturnMode::Strongest) { + for (size_t ind_block = 0; ind_block < data_packet_with_header->get_size_data_blocks(); + ind_block++) { + const auto & data_block = data_packet_with_header->data_blocks[ind_block]; + float angle_deg_azimuth_of_block = + static_cast(data_block.azimuth_multiplied_by_100_deg) / 100.0f; + + if (!has_processed_a_packet_) { + angle_deg_azimuth_last_packet_ = angle_deg_azimuth_of_block; + microseconds_last_packet_ = microseconds_toh; + has_processed_a_packet_ = true; + break; + } else if (ind_block == 0) { + // Compensate for azimuth angular rollover + float angle_deg_azimuth_increased = angle_deg_azimuth_of_block; + if (angle_deg_azimuth_of_block < angle_deg_azimuth_last_packet_) { + angle_deg_azimuth_increased += 360.0f; + } + float angle_deg_angle_delta = + angle_deg_azimuth_increased - angle_deg_azimuth_last_packet_; + + // Compensate for ToH microseconds rollover + + uint32_t microseconds_toh_current_increased = microseconds_toh; + if (microseconds_toh < microseconds_last_packet_) { + microseconds_toh_current_increased += 3600000000U; + // Increase internal epoch hour time point + tp_hours_since_epoch += std::chrono::hours(1); + } + uint32_t microseconds_delta = + microseconds_toh_current_increased - microseconds_last_packet_; + + speed_deg_per_microseconds_angle_azimuth = + static_cast(angle_deg_angle_delta) / microseconds_delta; + + angle_deg_azimuth_last_packet_ = angle_deg_azimuth_of_block; + microseconds_last_packet_ = microseconds_toh; + } + + for (size_t ind_point = 0; ind_point < data_block.get_size_data_points(); ind_point++) { + const auto & data_point = data_block.data_points[ind_point]; + + double timing_offset_from_first_block = 3.28 - (50 * (8 - ind_block + 1)); + double timing_offset_from_first_firing = 1.512 * (ind_point - 1) + 0.28; + // } + + auto timing_offset_from_first_block_dur = + std::chrono::duration(timing_offset_from_first_block); + auto timing_offset_from_first_firing_dur = + std::chrono::duration(timing_offset_from_first_firing); + auto timing_offset_from_first_block_time = + std::chrono::duration_cast( + timing_offset_from_first_block_dur); + auto timing_offset_from_first_firing_time = + std::chrono::duration_cast( + timing_offset_from_first_firing_dur); + + float angle_deg_azimuth_point = + angle_deg_azimuth_of_block + + static_cast( + speed_deg_per_microseconds_angle_azimuth * timing_offset_from_first_firing); + + if (angle_deg_azimuth_point >= 360.0f) { + angle_deg_azimuth_point -= 360.0f; + } + + angle_deg_azimuth_last = angle_deg_azimuth_point; + float angle_rad_azimuth_point = utils::Utils::deg_to_rad(angle_deg_azimuth_point); + float angle_deg_vertical = channel_to_angle_vertical_.at(ind_point); + float angle_rad_vertical = utils::Utils::deg_to_rad(angle_deg_vertical); + float dist_m = static_cast(data_point.distance_divided_by_4mm * 4) / 1000.0f; + + float dist_xy = dist_m * std::cos(angle_rad_vertical); + Point point; + point.x = dist_xy * std::sin(angle_rad_azimuth_point); + point.y = dist_xy * std::cos(angle_rad_azimuth_point); + point.z = dist_m * std::sin(angle_rad_vertical); + point.intensity = data_point.reflectivity; + point.ring = ind_point; + point.horizontal_angle = angle_deg_azimuth_point; + point.stamp_unix_seconds = + std::chrono::seconds( + tp_hours_since_epoch.time_since_epoch() + microseconds_since_toh.minutes() + + microseconds_since_toh.seconds()) + .count(); + point.stamp_nanoseconds = + std::chrono::nanoseconds( + microseconds_since_toh.subseconds() + timing_offset_from_first_block_time + + timing_offset_from_first_firing_time) + .count(); + + if (dist_m != 0 && dist_m <= max_point_distance_from_lidar && + dist_m >= min_point_distance_from_lidar ) { + cloud_.push_back(point); + } + } + } + } + + bool is_close_to_cut_area = std::abs(angle_deg_azimuth_last - angle_deg_cut_) < 1.0f; + + if (!is_close_to_cut_area) { + can_publish_again_ = true; + } + + if (can_publish_again_ && is_close_to_cut_area) { + callback_cloud_surround_out(cloud_); + cloud_.clear(); + can_publish_again_ = false; + } + + break; + } + default: { + // std::cerr << "Unknown package with length: " << rawPacket.getFrameLength() << + // std::endl; + break; + } + } +} +} \ No newline at end of file diff --git a/src/continuous_packet_parser_vlp16.cpp b/src/parsers/velodyne_vlp16.cpp similarity index 96% rename from src/continuous_packet_parser_vlp16.cpp rename to src/parsers/velodyne_vlp16.cpp index 255b551..12265cb 100644 --- a/src/continuous_packet_parser_vlp16.cpp +++ b/src/parsers/velodyne_vlp16.cpp @@ -14,12 +14,13 @@ * limitations under the License. */ -#include "mapora/continuous_packet_parser_vlp16.hpp" +#include "mapora/parsers/velodyne_vlp16.hpp" #include +#include #include #include "mapora/utils.hpp" -namespace mapora::points_provider::continuous_packet_parser_vlp16 { +namespace mapora::points_provider::continuous_packet_parser { ContinuousPacketParserVlp16::ContinuousPacketParserVlp16() : factory_bytes_are_read_at_least_once_{false}, has_received_valid_position_package_{false}, @@ -265,6 +266,12 @@ void ContinuousPacketParserVlp16::process_packet_into_cloud( point.stamp_nanoseconds = std::chrono::nanoseconds(microseconds_since_toh.subseconds()).count(); + std::chrono::duration point_duration = + std::chrono::seconds( + tp_hours_since_epoch.time_since_epoch() + microseconds_since_toh.minutes() + + microseconds_since_toh.seconds()) + + std::chrono::nanoseconds(microseconds_since_toh.subseconds()); + if ( std::sqrt(std::pow(point.x, 2) + std::pow(point.y, 2) + std::pow(point.z, 2)) < min_point_distance_from_lidar) { @@ -276,7 +283,6 @@ void ContinuousPacketParserVlp16::process_packet_into_cloud( continue; } - cloud_.push_back(point); } } @@ -301,4 +307,4 @@ void ContinuousPacketParserVlp16::process_packet_into_cloud( } } } -} // namespace mapora::point_provider::continuous_packet_parser_vlp16 +} // namespace mapora::point_provider::continuous_packet_parser diff --git a/src/points_provider_velodyne_vlp16.cpp b/src/points_provider.cpp similarity index 61% rename from src/points_provider_velodyne_vlp16.cpp rename to src/points_provider.cpp index cd57aea..01a42b3 100644 --- a/src/points_provider_velodyne_vlp16.cpp +++ b/src/points_provider.cpp @@ -17,35 +17,38 @@ #include #include -#include +#include "mapora/parsers/hesai_xt32.hpp" +#include "mapora/parsers/velodyne_vlp16.hpp" +#include "mapora/point_types.hpp" +#include "mapora/points_provider.hpp" +#include #include #include -#include +#include #include #include -#include "mapora/point_xyzit.hpp" -#include "mapora/points_provider_velodyne_vlp16.hpp" -#include "mapora/continuous_packet_parser_vlp16.hpp" namespace mapora::points_provider { namespace fs = boost::filesystem; -using Point = PointsProviderBase::Point; -using Points = PointsProviderBase::Points; +using Point = point_types::PointXYZITRH; +using Points = std::vector; -PointsProviderVelodyneVlp16::PointsProviderVelodyneVlp16(std::string path_folder_pcaps) +PointsProvider::PointsProvider( + std::string path_folder_pcaps, std::string sensor_type) : path_folder_pcaps_{path_folder_pcaps} { if (!fs::is_directory(path_folder_pcaps_)) { throw std::runtime_error(path_folder_pcaps_.string() + " is not a directory."); } + sensor_type_ = sensor_type; } -void PointsProviderVelodyneVlp16::process() { +void PointsProvider::process() { paths_pcaps_.clear(); for (const auto &path_pcap : boost::make_iterator_range(fs::directory_iterator(path_folder_pcaps_))) { if (fs::is_directory(path_pcap.path())) { continue; } if (path_pcap.path().extension() != ".pcap") { continue; } - std::cout << "pcap: " << path_pcap.path().string() << std::endl; +// std::cout << "pcap: " << path_pcap.path().string() << std::endl; paths_pcaps_.push_back(path_pcap); } @@ -60,30 +63,40 @@ void PointsProviderVelodyneVlp16::process() { } } -void PointsProviderVelodyneVlp16::process_pcaps_into_clouds( +void PointsProvider::process_pcaps_into_clouds( std::function &callback_cloud_surround_out, const size_t index_start, const size_t count, - const float max_point_distance_from_lidar, - const float min_point_distance_from_lidar) { + const float min_point_distance_from_lidar, + const float max_point_distance_from_lidar) { if (index_start >= paths_pcaps_.size() || index_start + count > paths_pcaps_.size()) { throw std::range_error("index is outside paths_pcaps_ range."); } - continuous_packet_parser_vlp16::ContinuousPacketParserVlp16 packet_parser; - for (size_t i = index_start; i < index_start + count; ++i) { - process_pcap_into_clouds(paths_pcaps_.at(i), callback_cloud_surround_out, - packet_parser, max_point_distance_from_lidar, min_point_distance_from_lidar); + if (sensor_type_ == "velodyne_vlp16") { + continuous_packet_parser::ContinuousPacketParserVlp16 packet_parser; + for (size_t i = index_start; i < index_start + count; ++i) { + process_pcap_into_clouds(paths_pcaps_.at(i), callback_cloud_surround_out, + packet_parser, min_point_distance_from_lidar, max_point_distance_from_lidar); + } + } else if (sensor_type_ == "hesai_xt32") { + continuous_packet_parser::ContinuousPacketParserXt32 packet_parser; + for (size_t i = index_start; i < index_start + count; ++i) { + process_pcap_into_clouds(paths_pcaps_.at(i), callback_cloud_surround_out, + packet_parser, min_point_distance_from_lidar, max_point_distance_from_lidar); + } } } -void PointsProviderVelodyneVlp16::process_pcap_into_clouds( +template +void PointsProvider::process_pcap_into_clouds( const fs::path &path_pcap, const std::function &callback_cloud_surround_out, - continuous_packet_parser_vlp16::ContinuousPacketParserVlp16 &parser, +// continuous_packet_parser::ContinuousPacketParserVlp16 &parser, + parser_type &parser, const float min_point_distance_from_lidar, const float max_point_distance_from_lidar) { - std::cout << "processing: " << path_pcap << std::endl; +// std::cout << "processing: " << path_pcap << std::endl; pcpp::IFileReaderDevice *reader = pcpp::IFileReaderDevice::getReader(path_pcap.string()); if (reader == nullptr) { printf("Cannot determine reader for file type\n"); @@ -97,7 +110,7 @@ void PointsProviderVelodyneVlp16::process_pcap_into_clouds( pcpp::RawPacket rawPacket; while (reader->getNextPacket(rawPacket)) { - parser.process_packet_into_cloud(rawPacket,callback_cloud_surround_out, + parser.process_packet_into_cloud(rawPacket, callback_cloud_surround_out, min_point_distance_from_lidar, max_point_distance_from_lidar); } @@ -105,5 +118,5 @@ void PointsProviderVelodyneVlp16::process_pcap_into_clouds( delete reader; } -std::string PointsProviderVelodyneVlp16::info() { return ""; } +std::string PointsProvider::info() { return ""; } } // namespace mapora::points_provider diff --git a/src/transform_provider_applanix.cpp b/src/transform_provider.cpp similarity index 63% rename from src/transform_provider_applanix.cpp rename to src/transform_provider.cpp index f8666c7..0f61ad1 100644 --- a/src/transform_provider_applanix.cpp +++ b/src/transform_provider.cpp @@ -14,20 +14,23 @@ * limitations under the License. */ -#include "mapora/transform_provider_applanix.hpp" +#include "mapora/transform_provider.hpp" #include "mapora/csv.hpp" -#include +#include "mapora/date.h" +#include "mapora/utils.hpp" +#include +#include +#include +#include +#include #include -#include #include -#include +#include #include -#include -#include "mapora/date.h" -#include "mapora/utils.hpp" +#include -namespace mapora::transform_provider_applanix { -TransformProviderApplanix::TransformProviderApplanix( +namespace mapora::transform_provider { +TransformProvider::TransformProvider( const std::string &path_file_ascii_output) : path_file_ascii_output_(path_file_ascii_output) { if (!fs::exists(path_file_ascii_output_)) { @@ -40,7 +43,7 @@ TransformProviderApplanix::TransformProviderApplanix( } } -void TransformProviderApplanix::process() { +void TransformProvider::process() { io::LineReader lines(path_file_ascii_output_.string()); double line_number_for_header = 1; while (char *line = lines.next_line()) @@ -49,7 +52,7 @@ void TransformProviderApplanix::process() { if (line_number_for_header == 16) { // 16 is the mission date line in the applanix export file mission_date += line; mission_date.erase(0, 20); // erases 20 characters from 0. - std::cout << "mission_date: " << mission_date << std::endl; +// std::cout << "mission_date: " << mission_date << std::endl; break; } @@ -61,7 +64,7 @@ void TransformProviderApplanix::process() { line_number_for_header++; } - std::string path_applanix_modified = "temp_applanix_modified.csv"; + std::string path_applanix_modified = "temp_modified.csv"; { std::ifstream filein(path_file_ascii_output_.string()); //File to read from std::ofstream fileout(path_applanix_modified); //Temporary file @@ -222,20 +225,63 @@ void TransformProviderApplanix::process() { in.pitch_std, in.heading_std)) { + double x, y, z; int zone; bool northp; int prec=8; + GeographicLib::UTMUPS::Forward(in.latitude, in.longitude, zone, northp, x, y); + std::string mgrs_string; + GeographicLib::MGRS::Forward(zone, northp, x, y, prec, mgrs_string); + // std::cout << mgrs_string << std::endl; + std::vector coords = parse_mgrs_coordinates(mgrs_string); + x = coords.at(0); + y = coords.at(1); + z = in.ellipsoid_height; + +// x = in.easting - origin_x; +// y = in.northing - origin_y; +// z = in.ellipsoid_height - origin_z; + + Pose pose; - pose.pose_with_covariance.pose.position.set__x(in.easting); - pose.pose_with_covariance.pose.position.set__y(in.northing); - pose.pose_with_covariance.pose.position.set__z(in.ellipsoid_height); - Eigen::AngleAxisd angle_axis_x(utils::Utils::deg_to_rad(-in.roll+180), Eigen::Vector3d::UnitX()); - Eigen::AngleAxisd angle_axis_y(utils::Utils::deg_to_rad(in.pitch), Eigen::Vector3d::UnitY()); - Eigen::AngleAxisd angle_axis_z(utils::Utils::deg_to_rad(in.heading-90), Eigen::Vector3d::UnitZ()); - - Eigen::Quaterniond q = (angle_axis_x * angle_axis_y * angle_axis_z); + Imu imu; + pose.pose_with_covariance.pose.position.set__x(x); + pose.pose_with_covariance.pose.position.set__y(y); + pose.pose_with_covariance.pose.position.set__z(z); +// pose.pose_with_covariance.pose.position.set__x(in.easting); +// pose.pose_with_covariance.pose.position.set__y(in.northing); +// pose.pose_with_covariance.pose.position.set__z(in.ellipsoid_height); + Eigen::AngleAxisd angle_axis_x(utils::Utils::deg_to_rad(in.roll), Eigen::Vector3d::UnitY()); + Eigen::AngleAxisd angle_axis_y(utils::Utils::deg_to_rad(in.pitch), Eigen::Vector3d::UnitX()); + Eigen::AngleAxisd angle_axis_z(utils::Utils::deg_to_rad(-in.heading), Eigen::Vector3d::UnitZ()); + pose.velocity.x = in.east_vel; + pose.velocity.y = in.north_vel; + pose.velocity.z = in.up_vel; + + Eigen::Quaterniond q = (angle_axis_z * angle_axis_y * angle_axis_x); pose.pose_with_covariance.pose.orientation.set__x(q.x()); pose.pose_with_covariance.pose.orientation.set__y(q.y()); pose.pose_with_covariance.pose.orientation.set__z(q.z()); pose.pose_with_covariance.pose.orientation.set__w(q.w()); + imu.imu.linear_acceleration.set__x(in.y_acceleration); + imu.imu.linear_acceleration.set__y(in.x_acceleration); + imu.imu.linear_acceleration.set__z(-in.z_acceleration); + std::array linear_acc_variances{ + std::pow(in.east_std, 2), std::pow(-in.north_std, 2), std::pow(-in.height_std, 2), + }; + for (size_t i = 0; i < 3; ++i) { + imu.imu.linear_acceleration_covariance.at(i*4) = linear_acc_variances.at(i); + } + + imu.imu.angular_velocity.set__x(in.y_angular_rate); + imu.imu.angular_velocity.set__y(in.x_angular_rate); + imu.imu.angular_velocity.set__z(-in.z_angular_rate); + std::array angular_rate_variances{ + std::pow(in.pitch_std, 2), std::pow(in.roll_std, 2), std::pow(-in.heading_std, 2), + }; + for (size_t i = 0; i < 3; ++i) { + imu.imu.linear_acceleration_covariance.at(i*4) = linear_acc_variances.at(i); + } + + auto segments = utils::Utils::string_to_vec_split_by(mission_date, '/'); int days_raw = std::stoi(segments.at(0)); @@ -254,6 +300,9 @@ void TransformProviderApplanix::process() { pose.stamp_unix_seconds = std::chrono::seconds(tp.time_since_epoch()).count(); pose.stamp_nanoseconds = std::chrono::nanoseconds(time_since_midnight.subseconds()).count(); + imu.stamp_unix_seconds = pose.stamp_unix_seconds; + imu.stamp_nanoseconds = pose.stamp_nanoseconds; + std::array variances{ std::pow(in.north_std, 2), std::pow(in.east_std, 2), @@ -273,6 +322,9 @@ void TransformProviderApplanix::process() { for (size_t i = 0; i < 6; ++i) { pose.pose_with_covariance.covariance.at(i * 7) = variances.at(i); } + pose.meridian_convergence = compute_meridian_convergence(in.latitude, in.longitude); + + imu_rotations_.push_back(imu); poses_.push_back(pose); } } catch (const std::exception &ex) { @@ -280,7 +332,8 @@ void TransformProviderApplanix::process() { } } -TransformProviderApplanix::Pose TransformProviderApplanix::get_pose_at( +TransformProvider::Pose +TransformProvider::get_pose_at( uint32_t stamp_unix_seconds, uint32_t stamp_nanoseconds) { Pose pose_search; @@ -297,4 +350,80 @@ TransformProviderApplanix::Pose TransformProviderApplanix::get_pose_at( size_t index = std::distance(poses_.begin(), iter_result); return poses_.at(index); } + +TransformProvider::Imu +TransformProvider::get_imu_at( + uint32_t stamp_unix_seconds, uint32_t stamp_nanoseconds) +{ + Imu imu_search; + + imu_search.stamp_unix_seconds = stamp_unix_seconds; + imu_search.stamp_nanoseconds = stamp_nanoseconds; + auto iter_result = std::lower_bound( + imu_rotations_.begin(), imu_rotations_.end(), imu_search, [](const Imu & p1, const Imu & p2) { + if (p1.stamp_unix_seconds == p2.stamp_unix_seconds) { + return p1.stamp_nanoseconds < p2.stamp_nanoseconds; + } + return p1.stamp_unix_seconds < p2.stamp_unix_seconds; + }); + + size_t index; + + if ( + iter_result == imu_rotations_.end() || + iter_result->stamp_unix_seconds > stamp_unix_seconds + 1 || + iter_result->stamp_nanoseconds > stamp_nanoseconds + 50000000) { + // std::cerr << "Imu not found for timestamp: " + // << stamp_unix_seconds << "." << stamp_nanoseconds << std::endl; + // Handle the error as needed, e.g., throw an exception, return a default pose, etc. + // throw std::runtime_error("Imu not found"); + index = last_index_imu; + } else { + index = std::distance(imu_rotations_.begin(), iter_result); + last_index_imu = index; + } + + + // std::cout << "ind: " << index << std::endl; + return imu_rotations_.at(index); +} +std::vector +TransformProvider::parse_mgrs_coordinates(const std::string & mgrs_string) { + std::string mgrs_grid = mgrs_string.substr(0, 5); + std::string mgrs_x_str = mgrs_string.substr(5, 8); + std::string mgrs_y_str = mgrs_string.substr(13, 8); + + double mgrs_x = std::stod(mgrs_x_str); + mgrs_x /= 1000; + + double mgrs_y = std::stod(mgrs_y_str); + mgrs_y /= 1000; + + return std::vector{mgrs_x, mgrs_y}; +} + +std::string TransformProvider::parse_mgrs_zone(const std::string & mgrs_string) { + return mgrs_string.substr(0, 5); +} + +float TransformProvider::compute_meridian_convergence(double lat, double lon) { + + double x, y; + int zone; + bool northp; + + GeographicLib::UTMUPS::Forward(lat, lon, zone, northp, x, y); + double lambda0 = (zone - 1) * 6 - 180 + 3; + + GeographicLib::TransverseMercatorExact tm( + GeographicLib::Constants::WGS84_a(), + GeographicLib::Constants::WGS84_f(), + lambda0); + + double gamma, k; + tm.Forward(lambda0, lat, lon, x, y, gamma, k); + + return gamma; +} + } // mapora::transform_provider \ No newline at end of file diff --git a/src/utils.cpp b/src/utils.cpp index ceef7ad..44d3aa0 100644 --- a/src/utils.cpp +++ b/src/utils.cpp @@ -15,9 +15,10 @@ */ #include "mapora/utils.hpp" +#include #include -#include #include +#include #include namespace mapora::utils @@ -55,6 +56,24 @@ std::vector Utils::string_to_vec_split_by( return seglist; } +Eigen::Matrix3d Utils::ned2enu_converter_for_matrices(const Eigen::Matrix3d & matrix3d) +{ + Eigen::Matrix3d ned2enu; + ned2enu.matrix().topLeftCorner<3, 3>() = + Eigen::AngleAxisd(utils::Utils::deg_to_rad(-90.0), Eigen::Vector3d::UnitZ()) + .toRotationMatrix() * + Eigen::AngleAxisd(utils::Utils::deg_to_rad(0.0), Eigen::Vector3d::UnitY()) + .toRotationMatrix() * + Eigen::AngleAxisd(utils::Utils::deg_to_rad(180.0), Eigen::Vector3d::UnitX()) + .toRotationMatrix(); + + Eigen::Matrix3d output_matrix; + output_matrix = matrix3d.matrix() * ned2enu.matrix(); + + return output_matrix; +} + + } // namespace mapora::utils