From f8e1dcd2fd525895e303801d2c227bd9d4459f1f Mon Sep 17 00:00:00 2001 From: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com> Date: Fri, 3 Dec 2021 10:54:54 +0900 Subject: [PATCH] feat: add gnss packages (#23) * rename gnss package (#714) Signed-off-by: Yamato Ando * Avoid setting CMAKE_BUILD_TYPE=Release in each CMakeLists.txt (#720) * remove set CMAKE_BUILD_TYPE Release in each CMakeLists.txt * remove set CMAKE_BUILD_TYPE Release in ndt_pcl_modified * set compile options for debug in ndt_omp * Fix indent * add warning if -DCMAKE_BUILD_TYPE=Release is not set in ndt_omp Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> * remove ROS1 packages temporarily Signed-off-by: mitsudome-r * Revert "remove ROS1 packages temporarily" This reverts commit bc523d679e6737ec37be4fdcbdedca141411bd9d. Signed-off-by: mitsudome-r * add COLCON_IGNORE to ros1 packages Signed-off-by: mitsudome-r * ROS2 Porting: geo_pos_conv (#8) * Convert to ROS2 package - Update changelog -> 2.0.0 - Update package xml to use the ROS2 xml schema - Update CMakelist with ament cmake commands - Add ROS2 compile options * Convert CMakelist for foxy * Clean up - Remove comments * Remove colcon ignore * Rename launch files to launch.xml (#28) * ROS2 Porting: gnss_poser (#18) * Fix CMakelist - Add ublox_msgs package dependency - Rename header files to use the .hpp extention - Remove colcon ignore - Fix Cmake ref to ublox in dependency * Fix compilation issues with convert.hpp - Add sensor_msgs::NavSatFix header which was missing before (?) - Remove logging for now - Contains warnings - geographic lib error polymorphous error - Fix geographic lib -Wcatch-value warnings - Add ifndef guard in gnss_stat.hpp * First pass - successful compilation - Fix CMakelist geographic library - Remove references to main ROS functions - Convert msg references - Add ifndef guards - Convert tf2 methods - Add functionality back - Fix type errors * Add publishers and subscribers - Add parameters * Convert launch files to ROS2 format * Add logging * Convert to component * Fix define guards to follow ROS2/Google style * Add -Werror compile flag * Clean up * Fix CMakelist to allow for the node to be found via ros2 launch - Revert filenames so it is easier to review * Address PR Comments: - Fix CMakelist and package.xml - Remove redundant depend/exec_depend calls - Remove redundant find_package calls - Explicitly add headers to the library * Address PR comments: - Revert removal of GNSSPoser namespace - Use std::make_shared in place of new - Reorder includes to conform with standard * Address PR Comments: - Rename variable to conform with the google style guide - Use the node clock in logging method calls * Address PR Comment: - Convert transform broadcaster to shared pointer * Address PR comment: - Remove -Werror compile flag post discussion * fix duration unit for RCLCPP_*_THROTTLE (#75) Signed-off-by: Takamasa Horibe * [gnss_poser] switch broadcaster to full member... (#136) instead of `shared_ptr` to avoid undefined behavior when executing the node because it used `shared_from_this()` in the constructor * Adjust copyright notice on 532 out of 699 source files (#143) * Use quotes for includes where appropriate (#144) * Use quotes for includes where appropriate * Fix lint tests * Make tests pass hopefully * Run uncrustify on the entire Pilot.Auto codebase (#151) * Run uncrustify on the entire Pilot.Auto codebase * Exclude open PRs * ROS2 Linting: geo_pos_conv (#165) * Add linters and fix linter issues - convert Cmake to use ament_cmake_auto * Address PR comments: - Remove unneeded cast - Throw error instead of setting default values to zero * ROS2 Linting: gnss_poser (#166) * Add linting and fix litner errors * Address PR comment: - Use author name instead of user - Remove ROS2 maintainer (remnants of older porting) * apply env_var to use_sim_time (#222) * [gnss_poser] fix namespace for component (#295) Signed-off-by: Takamasa Horibe * [gnss_poser] fix link to Geographiclib (#296) * [gnss_poser] fix link to Geographiclib * Fix mismatching spaces inside () after command Signed-off-by: wep21 Co-authored-by: Autoware Co-authored-by: wep21 * Ros2 v0.8.0 gnss poser (#287) * [gnss poser]: Remove std_msgs (#341) Signed-off-by: wep21 * add use_sim-time option (#454) * Format launch files (#1219) Signed-off-by: Kenji Miyake * Fix for rolling (#1226) * Replace doc by description Signed-off-by: Kenji Miyake * Replace ns by push-ros-namespace Signed-off-by: Kenji Miyake * Remove use_sim_time for set_parameter (#1260) Signed-off-by: wep21 * Add pre-commit (#1560) * add pre-commit * add pre-commit-config * add additional settings for private repository * use default pre-commit-config * update pre-commit setting * Ignore whitespace for line breaks in markdown * Update .github/workflows/pre-commit.yml Co-authored-by: Kazuki Miyahara * exclude svg * remove pretty-format-json * add double-quote-string-fixer * consider COLCON_IGNORE file when seaching modified package * format file * pre-commit fixes * Update pre-commit.yml * Update .pre-commit-config.yaml Co-authored-by: Kazuki Miyahara Co-authored-by: pre-commit Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> * Add markdownlint and prettier (#1661) * Add markdownlint and prettier Signed-off-by: Kenji Miyake * Ignore .param.yaml Signed-off-by: Kenji Miyake * Apply format Signed-off-by: Kenji Miyake * suppress warnings for sensing packages (#1729) * add Werror for tier4_pcl_extensions * use std::type instead of pcl::type * use std::isfinite instead of pcl_isfinite * fix getFieldIndex * add Werror * fix uninitialized * add Werror * fix string to char* * Fix -Wunused-parameter (#1836) * Fix -Wunused-parameter Signed-off-by: Kenji Miyake * Fix mistake Signed-off-by: Kenji Miyake * fix spell * Fix lint issues Signed-off-by: Kenji Miyake * Ignore flake8 warnings Signed-off-by: Kenji Miyake Co-authored-by: Hiroki OTA * Fix clang warnings (#1859) * Fix -Wreturn-std-move Signed-off-by: Kenji Miyake * Fix -Wunused-private-field Signed-off-by: Kenji Miyake * Ignore -Wnonportable-include-path for mussp Signed-off-by: Kenji Miyake * Fix -Wunused-const-variable Signed-off-by: Kenji Miyake * Fix "can not be used when making a shared object" Signed-off-by: Kenji Miyake * add sort-package-xml hook in pre-commit (#1881) * add sort xml hook in pre-commit * change retval to exit_status * rename * add prettier plugin-xml * use early return * add license note * add tier4 license * restore prettier * change license order * move local hooks to public repo * move prettier-xml to pre-commit-hooks-ros * update version for bug-fix * apply pre-commit * fix gnss_poser transform (#1493) (#1623) * fix gnss_poser transform (#1493) * fix gnss_poser transform * change transformation method from doTransform to tf2::Transform multiplication * rename tf variables * correct variable spelling Signed-off-by: wep21 * Fix lint Signed-off-by: wep21 Co-authored-by: hiro-ya-iv <30652835+hiro-ya-iv@users.noreply.github.com> * Change formatter to clang-format and black (#2332) * Revert "Temporarily comment out pre-commit hooks" This reverts commit 748e9cdb145ce12f8b520bcbd97f5ff899fc28a3. * Replace ament_lint_common with autoware_lint_common Signed-off-by: Kenji Miyake * Remove ament_cmake_uncrustify and ament_clang_format Signed-off-by: Kenji Miyake * Apply Black Signed-off-by: Kenji Miyake * Apply clang-format Signed-off-by: Kenji Miyake * Fix build errors Signed-off-by: Kenji Miyake * Fix for cpplint * Fix include double quotes to angle brackets Signed-off-by: Kenji Miyake * Apply clang-format Signed-off-by: Kenji Miyake * Fix build errors Signed-off-by: Kenji Miyake * Add COLCON_IGNORE (#500) Signed-off-by: Kenji Miyake * remove COLCON_IGNORE (#515) * doc gnss poser (#580) * add minimal doc * fix document title * doc geo conv pos (#573) * add minimal doc * fix document title * Remove template comments from node documents (#608) * rename document file * remove template comment Co-authored-by: YamatoAndo Co-authored-by: Daichi Murakami Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: mitsudome-r Co-authored-by: Jilada Eccleston Co-authored-by: Nikolai Morin Co-authored-by: Takamasa Horibe Co-authored-by: Frederik Beaujean <72439809+fred-apex-ai@users.noreply.github.com> Co-authored-by: Kosuke Murakami Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Co-authored-by: Autoware Co-authored-by: wep21 Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: tkimura4 Co-authored-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> Co-authored-by: Kazuki Miyahara Co-authored-by: pre-commit Co-authored-by: Hiroki OTA Co-authored-by: hiro-ya-iv <30652835+hiro-ya-iv@users.noreply.github.com> Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> --- sensing/geo_pos_conv/CHANGELOG.rst | 203 ++++++++++ sensing/geo_pos_conv/CMakeLists.txt | 28 ++ sensing/geo_pos_conv/README.md | 21 ++ .../include/geo_pos_conv/geo_pos_conv.hpp | 56 +++ sensing/geo_pos_conv/package.xml | 22 ++ sensing/geo_pos_conv/src/geo_pos_conv.cpp | 303 +++++++++++++++ sensing/gnss_poser/CMakeLists.txt | 55 +++ sensing/gnss_poser/README.md | 51 +++ .../gnss_poser/include/gnss_poser/convert.hpp | 134 +++++++ .../include/gnss_poser/gnss_poser_core.hpp | 96 +++++ .../include/gnss_poser/gnss_stat.hpp | 52 +++ .../gnss_poser/launch/gnss_poser.launch.xml | 43 +++ .../launch/ubloxfix2mgrs.launch.xml | 13 + sensing/gnss_poser/package.xml | 31 ++ sensing/gnss_poser/src/gnss_poser_core.cpp | 354 ++++++++++++++++++ 15 files changed, 1462 insertions(+) create mode 100644 sensing/geo_pos_conv/CHANGELOG.rst create mode 100644 sensing/geo_pos_conv/CMakeLists.txt create mode 100644 sensing/geo_pos_conv/README.md create mode 100644 sensing/geo_pos_conv/include/geo_pos_conv/geo_pos_conv.hpp create mode 100644 sensing/geo_pos_conv/package.xml create mode 100644 sensing/geo_pos_conv/src/geo_pos_conv.cpp create mode 100644 sensing/gnss_poser/CMakeLists.txt create mode 100644 sensing/gnss_poser/README.md create mode 100644 sensing/gnss_poser/include/gnss_poser/convert.hpp create mode 100644 sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp create mode 100644 sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp create mode 100755 sensing/gnss_poser/launch/gnss_poser.launch.xml create mode 100644 sensing/gnss_poser/launch/ubloxfix2mgrs.launch.xml create mode 100644 sensing/gnss_poser/package.xml create mode 100644 sensing/gnss_poser/src/gnss_poser_core.cpp diff --git a/sensing/geo_pos_conv/CHANGELOG.rst b/sensing/geo_pos_conv/CHANGELOG.rst new file mode 100644 index 0000000000000..ead6db443a672 --- /dev/null +++ b/sensing/geo_pos_conv/CHANGELOG.rst @@ -0,0 +1,203 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package geo_pos_conv +^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.0.0 (2020-10-03) +------------------ +* Convert package to ROS2 + +1.11.0 (2019-03-21) +------------------- +* add constructor (`#1913 `_) +* Fix license notice in corresponding package.xml +* Contributors: YamatoAndo, amc-nu + +1.10.0 (2019-01-17) +------------------- +* Switch to Apache 2 license (develop branch) (`#1741 `_) + * Switch to Apache 2 + * Replace BSD-3 license header with Apache 2 and reassign copyright to the + Autoware Foundation. + * Update license on Python files + * Update copyright years + * Add #ifndef/define _POINTS_IMAGE_H\_ + * Updated license comment +* Contributors: Esteve Fernandez + +1.9.1 (2018-11-06) +------------------ + +1.9.0 (2018-10-31) +------------------ + +1.8.0 (2018-08-31) +------------------ + +1.7.0 (2018-05-18) +------------------ +* update Version from 1.6.3 to 1.7.0 in package.xml and CHANGELOG.rst +* [fix] Fixes for all packages and dependencies (`#1240 `_) + * Initial Cleanup + * fixed also for indigo + * kf cjeck + * Fix road wizard + * Added travis ci + * Trigger CI + * Fixes to cv_tracker and lidar_tracker cmake + * Fix kitti player dependencies + * Removed unnecessary dependencies + * messages fixing for can + * Update build script travis + * Travis Path + * Travis Paths fix + * Travis test + * Eigen checks + * removed unnecessary dependencies + * Eigen Detection + * Job number reduced + * Eigen3 more fixes + * More Eigen3 + * Even more Eigen + * find package cmake modules included + * More fixes to cmake modules + * Removed non ros dependency + * Enable industrial_ci for indidog and kinetic + * Wrong install command + * fix rviz_plugin install + * FastVirtualScan fix + * Fix Qt5 Fastvirtualscan + * Fixed qt5 system dependencies for rosdep + * NDT TKU Fix catkin not pacakged + * More in detail dependencies fixes for more packages + * GLEW library for ORB + * Ignore OrbLocalizer + * Ignore Version checker + * Fix for driveworks interface + * driveworks not catkinpackagedd + * Missing catkin for driveworks + * libdpm opencv not catkin packaged + * catkin lib gnss not included in obj_db + * Points2Polygon fix + * More missing dependencies + * image viewer not packaged + * Fixed SSH2 detection, added viewers for all distros + * Fix gnss localizer incorrect dependency config + * Fixes to multiple packages dependencies + * gnss plib and package + * More fixes to gnss + * gnss dependencies for gnss_loclaizer + * Missing gnss dependency for gnss on localizer + * More fixes for dependencies + Replaced gnss for autoware_gnss_library + * gnss more fixes + * fixes to more dependencies + * header dependency + * Debug message + * more debug messages changed back to gnss + * debud messages + * gnss test + * gnss install command + * Several fixes for OpenPlanner and its lbiraries + * Fixes to ROSInterface + * More fixes to robotsdk and rosinterface + * robotsdk calibration fix + * Fixes to rosinterface robotsdk libraries and its nodes + * Fixes to Qt5 missing dependencies in robotsdk + * glviewer missing dependencies + * Missing qt specific config cmake for robotsdk + * disable cv_tracker + * Fix to open planner un needed dependendecies + * Fixes for libraries indecision maker + * Fixes to libraries decision_maker installation + * Gazebo on Kinetic + * Added Missing library + * * Removed Gazebo and synchonization packages + * Renames vmap in lane_planner + * Added installation commands for missing pakcages + * Fixes to lane_planner + * Added NDT TKU Glut extra dependencies + * ndt localizer/lib fast pcl fixes + re enable cv_tracker + * Fix kf_lib + * Keep industrial_ci + * Fixes for dpm library + * Fusion lib fixed + * dpm and fusion header should match exported project name + * Fixes to dpm_ocv ndt_localizer and pcl_omp + * no fast_pcl anymore + * fixes to libdpm and its package + * CI test + * test with native travis ci + * missing update for apt + * Fixes to pcl_omp installation and headers + * Final fixes for tests, modified README + * * Fixes to README + * Enable industrial_ci + * re enable native travis tests +* Contributors: Abraham Monrroy, Kosuke Murakami + +1.6.3 (2018-03-06) +------------------ + +1.6.2 (2018-02-27) +------------------ +* Update CHANGELOG +* Contributors: Yusuke FUJII + +1.6.1 (2018-01-20) +------------------ +* update CHANGELOG +* Contributors: Yusuke FUJII + +1.6.0 (2017-12-11) +------------------ +* Prepare release for 1.6.0 +* support all plane(1-19) in geo_pos_conv +* Contributors: Yamato ANDO, yukikitsukawa + +1.5.1 (2017-09-25) +------------------ +* Release/1.5.1 (`#816 `_) + * fix a build error by gcc version + * fix build error for older indigo version + * update changelog for v1.5.1 + * 1.5.1 +* Contributors: Yusuke FUJII + +1.5.0 (2017-09-21) +------------------ +* Update changelog +* install target +* Contributors: Dejan Pangercic, Yusuke FUJII + +1.4.0 (2017-08-04) +------------------ +* version number must equal current release number so we can start releasing in the future +* added changelogs +* Contributors: Dejan Pangercic + +1.3.1 (2017-07-16) +------------------ + +1.3.0 (2017-07-14) +------------------ + +1.2.0 (2017-06-07) +------------------ + +1.1.2 (2017-02-27 23:10) +------------------------ + +1.1.1 (2017-02-27 22:25) +------------------------ + +1.1.0 (2017-02-24) +------------------ + +1.0.1 (2017-01-14) +------------------ + +1.0.0 (2016-12-22) +------------------ +* Initial commit for public release +* Contributors: Shinpei Kato diff --git a/sensing/geo_pos_conv/CMakeLists.txt b/sensing/geo_pos_conv/CMakeLists.txt new file mode 100644 index 0000000000000..501218f7f3e7a --- /dev/null +++ b/sensing/geo_pos_conv/CMakeLists.txt @@ -0,0 +1,28 @@ +cmake_minimum_required(VERSION 3.5) +project(geo_pos_conv) + +### Compile options +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +### Add dependencies +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +### Create library +ament_auto_add_library(geo_pos_conv SHARED + src/geo_pos_conv.cpp +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package() diff --git a/sensing/geo_pos_conv/README.md b/sensing/geo_pos_conv/README.md new file mode 100644 index 0000000000000..c75a642bdefb5 --- /dev/null +++ b/sensing/geo_pos_conv/README.md @@ -0,0 +1,21 @@ +# geo_pos_conv + +## Purpose + +The `geo_pos_conv` is a library to calculate the conversion between **x, y positions on the plane rectangular coordinate** and **latitude/longitude on the earth**. + +## Inner-workings / Algorithms + +## Inputs / Outputs + +## Parameters + +## Assumptions / Known limits + +## (Optional) Error detection and handling + +## (Optional) Performance characterization + +## (Optional) References/External links + +## (Optional) Future extensions / Unimplemented parts diff --git a/sensing/geo_pos_conv/include/geo_pos_conv/geo_pos_conv.hpp b/sensing/geo_pos_conv/include/geo_pos_conv/geo_pos_conv.hpp new file mode 100644 index 0000000000000..68c4b408efd2d --- /dev/null +++ b/sensing/geo_pos_conv/include/geo_pos_conv/geo_pos_conv.hpp @@ -0,0 +1,56 @@ +// Copyright 2015-2019 Autoware Foundation +// +// 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 GEO_POS_CONV__GEO_POS_CONV_HPP_ +#define GEO_POS_CONV__GEO_POS_CONV_HPP_ + +#include + +class geo_pos_conv +{ +private: + double m_x; // m + double m_y; // m + double m_z; // m + + double m_lat; // latitude + double m_lon; // longitude + double m_h; + + double m_PLato; // plane lat + double m_PLo; // plane lon + +public: + geo_pos_conv(); + double x() const; + double y() const; + double z() const; + + void set_plane(double lat, double lon); + void set_plane(int num); + void set_xyz(double cx, double cy, double cz); + + // set llh in radians + void set_llh(double lat, double lon, double h); + + // set llh in nmea degrees + void set_llh_nmea_degrees(double latd, double lond, double h); + + void llh_to_xyz(double lat, double lon, double ele); + + void conv_llh2xyz(void); + void conv_xyz2llh(void); +}; + +#endif // GEO_POS_CONV__GEO_POS_CONV_HPP_ diff --git a/sensing/geo_pos_conv/package.xml b/sensing/geo_pos_conv/package.xml new file mode 100644 index 0000000000000..ac0acbc0c6dc0 --- /dev/null +++ b/sensing/geo_pos_conv/package.xml @@ -0,0 +1,22 @@ + + + + geo_pos_conv + 2.0.0 + The ROS2 geo_pos_conv package + Ryo Watanabe + Tomomi HORII + + Apache License 2.0 + + ament_cmake_auto + + rclcpp + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/sensing/geo_pos_conv/src/geo_pos_conv.cpp b/sensing/geo_pos_conv/src/geo_pos_conv.cpp new file mode 100644 index 0000000000000..119424bf19db0 --- /dev/null +++ b/sensing/geo_pos_conv/src/geo_pos_conv.cpp @@ -0,0 +1,303 @@ +// Copyright 2020 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. +/* + * Copyright 2015-2019 Autoware Foundation. 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 "geo_pos_conv/geo_pos_conv.hpp" + +geo_pos_conv::geo_pos_conv() +: m_x(0), m_y(0), m_z(0), m_lat(0), m_lon(0), m_h(0), m_PLato(0), m_PLo(0) +{ +} + +double geo_pos_conv::x() const { return m_x; } + +double geo_pos_conv::y() const { return m_y; } + +double geo_pos_conv::z() const { return m_z; } + +void geo_pos_conv::set_plane(double lat, double lon) +{ + m_PLato = lat; + m_PLo = lon; +} + +void geo_pos_conv::set_plane(int num) +{ + int lon_deg, lon_min, lat_deg, + lat_min; // longitude and latitude of origin of each plane in Japan + if (num == 1) { + lon_deg = 33; + lon_min = 0; + lat_deg = 129; + lat_min = 30; + } else if (num == 2) { + lon_deg = 33; + lon_min = 0; + lat_deg = 131; + lat_min = 0; + } else if (num == 3) { + lon_deg = 36; + lon_min = 0; + lat_deg = 132; + lat_min = 10; + } else if (num == 4) { + lon_deg = 33; + lon_min = 0; + lat_deg = 133; + lat_min = 30; + } else if (num == 5) { + lon_deg = 36; + lon_min = 0; + lat_deg = 134; + lat_min = 20; + } else if (num == 6) { + lon_deg = 36; + lon_min = 0; + lat_deg = 136; + lat_min = 0; + } else if (num == 7) { + lon_deg = 36; + lon_min = 0; + lat_deg = 137; + lat_min = 10; + } else if (num == 8) { + lon_deg = 36; + lon_min = 0; + lat_deg = 138; + lat_min = 30; + } else if (num == 9) { + lon_deg = 36; + lon_min = 0; + lat_deg = 139; + lat_min = 50; + } else if (num == 10) { + lon_deg = 40; + lon_min = 0; + lat_deg = 140; + lat_min = 50; + } else if (num == 11) { + lon_deg = 44; + lon_min = 0; + lat_deg = 140; + lat_min = 15; + } else if (num == 12) { + lon_deg = 44; + lon_min = 0; + lat_deg = 142; + lat_min = 15; + } else if (num == 13) { + lon_deg = 44; + lon_min = 0; + lat_deg = 144; + lat_min = 15; + } else if (num == 14) { + lon_deg = 26; + lon_min = 0; + lat_deg = 142; + lat_min = 0; + } else if (num == 15) { + lon_deg = 26; + lon_min = 0; + lat_deg = 127; + lat_min = 30; + } else if (num == 16) { + lon_deg = 26; + lon_min = 0; + lat_deg = 124; + lat_min = 0; + } else if (num == 17) { + lon_deg = 26; + lon_min = 0; + lat_deg = 131; + lat_min = 0; + } else if (num == 18) { + lon_deg = 20; + lon_min = 0; + lat_deg = 136; + lat_min = 0; + } else if (num == 19) { + lon_deg = 26; + lon_min = 0; + lat_deg = 154; + lat_min = 0; + } else { + lon_deg = 0; + lon_min = 0; + lat_deg = 0; + lat_min = 0; + } + + // swap longitude and latitude + m_PLo = M_PI * (lat_deg + lat_min / 60.0) / 180.0; + m_PLato = M_PI * (lon_deg + lon_min / 60.0) / 180; +} + +void geo_pos_conv::set_xyz(double cx, double cy, double cz) +{ + m_x = cx; + m_y = cy; + m_z = cz; + conv_xyz2llh(); +} + +void geo_pos_conv::set_llh_nmea_degrees(double latd, double lond, double h) +{ + double lat, lad, lod, lon; + // 1234.56 -> 12'34.56 -> 12+ 34.56/60 + + lad = floor(latd / 100.); + lat = latd - lad * 100.; + lod = floor(lond / 100.); + lon = lond - lod * 100.; + + // Changing Longitude and Latitude to Radians + m_lat = (lad + lat / 60.0) * M_PI / 180; + m_lon = (lod + lon / 60.0) * M_PI / 180; + m_h = h; + + conv_llh2xyz(); +} + +void geo_pos_conv::llh_to_xyz(double lat, double lon, double ele) +{ + m_lat = lat * M_PI / 180; + m_lon = lon * M_PI / 180; + m_h = ele; + + conv_llh2xyz(); +} + +void geo_pos_conv::conv_llh2xyz(void) +{ + double PS; // + double PSo; // + double PDL; // + double Pt; // + double PN; // + double PW; // + + double PB1, PB2, PB3, PB4, PB5, PB6, PB7, PB8, PB9; + double PA, PB, PC, PD, PE, PF, PG, PH, PI; + double Pe; // + double Pet; // + double Pnn; // + double AW, FW, Pmo; + + Pmo = 0.9999; + + /*WGS84 Parameters*/ + AW = 6378137.0; // Semimajor Axis + FW = 1.0 / 298.257222101; // 298.257223563 //Geometrical flattening + + Pe = sqrt(2.0 * FW - pow(FW, 2)); + Pet = sqrt(pow(Pe, 2) / (1.0 - pow(Pe, 2))); + + PA = 1.0 + 3.0 / 4.0 * pow(Pe, 2) + 45.0 / 64.0 * pow(Pe, 4) + 175.0 / 256.0 * pow(Pe, 6) + + 11025.0 / 16384.0 * pow(Pe, 8) + 43659.0 / 65536.0 * pow(Pe, 10) + + 693693.0 / 1048576.0 * pow(Pe, 12) + 19324305.0 / 29360128.0 * pow(Pe, 14) + + 4927697775.0 / 7516192768.0 * pow(Pe, 16); + + PB = 3.0 / 4.0 * pow(Pe, 2) + 15.0 / 16.0 * pow(Pe, 4) + 525.0 / 512.0 * pow(Pe, 6) + + 2205.0 / 2048.0 * pow(Pe, 8) + 72765.0 / 65536.0 * pow(Pe, 10) + + 297297.0 / 262144.0 * pow(Pe, 12) + 135270135.0 / 117440512.0 * pow(Pe, 14) + + 547521975.0 / 469762048.0 * pow(Pe, 16); + + PC = 15.0 / 64.0 * pow(Pe, 4) + 105.0 / 256.0 * pow(Pe, 6) + 2205.0 / 4096.0 * pow(Pe, 8) + + 10395.0 / 16384.0 * pow(Pe, 10) + 1486485.0 / 2097152.0 * pow(Pe, 12) + + 45090045.0 / 58720256.0 * pow(Pe, 14) + 766530765.0 / 939524096.0 * pow(Pe, 16); + + PD = 35.0 / 512.0 * pow(Pe, 6) + 315.0 / 2048.0 * pow(Pe, 8) + 31185.0 / 131072.0 * pow(Pe, 10) + + 165165.0 / 524288.0 * pow(Pe, 12) + 45090045.0 / 117440512.0 * pow(Pe, 14) + + 209053845.0 / 469762048.0 * pow(Pe, 16); + + PE = 315.0 / 16384.0 * pow(Pe, 8) + 3465.0 / 65536.0 * pow(Pe, 10) + + 99099.0 / 1048576.0 * pow(Pe, 12) + 4099095.0 / 29360128.0 * pow(Pe, 14) + + 348423075.0 / 1879048192.0 * pow(Pe, 16); + + PF = 693.0 / 131072.0 * pow(Pe, 10) + 9009.0 / 524288.0 * pow(Pe, 12) + + 4099095.0 / 117440512.0 * pow(Pe, 14) + 26801775.0 / 469762048.0 * pow(Pe, 16); + + PG = 3003.0 / 2097152.0 * pow(Pe, 12) + 315315.0 / 58720256.0 * pow(Pe, 14) + + 11486475.0 / 939524096.0 * pow(Pe, 16); + + PH = 45045.0 / 117440512.0 * pow(Pe, 14) + 765765.0 / 469762048.0 * pow(Pe, 16); + + PI = 765765.0 / 7516192768.0 * pow(Pe, 16); + + PB1 = AW * (1.0 - pow(Pe, 2)) * PA; + PB2 = AW * (1.0 - pow(Pe, 2)) * PB / -2.0; + PB3 = AW * (1.0 - pow(Pe, 2)) * PC / 4.0; + PB4 = AW * (1.0 - pow(Pe, 2)) * PD / -6.0; + PB5 = AW * (1.0 - pow(Pe, 2)) * PE / 8.0; + PB6 = AW * (1.0 - pow(Pe, 2)) * PF / -10.0; + PB7 = AW * (1.0 - pow(Pe, 2)) * PG / 12.0; + PB8 = AW * (1.0 - pow(Pe, 2)) * PH / -14.0; + PB9 = AW * (1.0 - pow(Pe, 2)) * PI / 16.0; + + PS = PB1 * m_lat + PB2 * sin(2.0 * m_lat) + PB3 * sin(4.0 * m_lat) + PB4 * sin(6.0 * m_lat) + + PB5 * sin(8.0 * m_lat) + PB6 * sin(10.0 * m_lat) + PB7 * sin(12.0 * m_lat) + + PB8 * sin(14.0 * m_lat) + PB9 * sin(16.0 * m_lat); + + PSo = PB1 * m_PLato + PB2 * sin(2.0 * m_PLato) + PB3 * sin(4.0 * m_PLato) + + PB4 * sin(6.0 * m_PLato) + PB5 * sin(8.0 * m_PLato) + PB6 * sin(10.0 * m_PLato) + + PB7 * sin(12.0 * m_PLato) + PB8 * sin(14.0 * m_PLato) + PB9 * sin(16.0 * m_PLato); + + PDL = m_lon - m_PLo; + Pt = tan(m_lat); + PW = sqrt(1.0 - pow(Pe, 2) * pow(sin(m_lat), 2)); + PN = AW / PW; + Pnn = sqrt(pow(Pet, 2) * pow(cos(m_lat), 2)); + + m_x = ((PS - PSo) + (1.0 / 2.0) * PN * pow(cos(m_lat), 2.0) * Pt * pow(PDL, 2.0) + + (1.0 / 24.0) * PN * pow(cos(m_lat), 4) * Pt * + (5.0 - pow(Pt, 2) + 9.0 * pow(Pnn, 2) + 4.0 * pow(Pnn, 4)) * pow(PDL, 4) - + (1.0 / 720.0) * PN * pow(cos(m_lat), 6) * Pt * + (-61.0 + 58.0 * pow(Pt, 2) - pow(Pt, 4) - 270.0 * pow(Pnn, 2) + + 330.0 * pow(Pt, 2) * pow(Pnn, 2)) * + pow(PDL, 6) - + (1.0 / 40320.0) * PN * pow(cos(m_lat), 8) * Pt * + (-1385.0 + 3111 * pow(Pt, 2) - 543 * pow(Pt, 4) + pow(Pt, 6)) * pow(PDL, 8)) * + Pmo; + + m_y = (PN * cos(m_lat) * PDL - + 1.0 / 6.0 * PN * pow(cos(m_lat), 3) * (-1 + pow(Pt, 2) - pow(Pnn, 2)) * pow(PDL, 3) - + 1.0 / 120.0 * PN * pow(cos(m_lat), 5) * + (-5.0 + 18.0 * pow(Pt, 2) - pow(Pt, 4) - 14.0 * pow(Pnn, 2) + + 58.0 * pow(Pt, 2) * pow(Pnn, 2)) * + pow(PDL, 5) - + 1.0 / 5040.0 * PN * pow(cos(m_lat), 7) * + (-61.0 + 479.0 * pow(Pt, 2) - 179.0 * pow(Pt, 4) + pow(Pt, 6)) * pow(PDL, 7)) * + Pmo; + + m_z = m_h; +} + +void geo_pos_conv::conv_xyz2llh(void) +{ + // n/a +} diff --git a/sensing/gnss_poser/CMakeLists.txt b/sensing/gnss_poser/CMakeLists.txt new file mode 100644 index 0000000000000..ead7add7d6803 --- /dev/null +++ b/sensing/gnss_poser/CMakeLists.txt @@ -0,0 +1,55 @@ +cmake_minimum_required(VERSION 3.5) +project(gnss_poser) + +### Compile options +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror) +endif() + +## Find non-ROS library +find_package(PkgConfig) +find_path(GeographicLib_INCLUDE_DIR GeographicLib/Config.h + PATH_SUFFIXES GeographicLib +) + +set(GeographicLib_INCLUDE_DIRS ${GeographicLib_INCLUDE_DIR}) +find_library(GeographicLib_LIBRARIES + NAMES Geographic +) + +find_package(ament_cmake_auto) +ament_auto_find_build_dependencies() + +set(GNSS_POSER_HEADERS + include/gnss_poser/convert.hpp + include/gnss_poser/gnss_poser_core.hpp + include/gnss_poser/gnss_stat.hpp +) + +ament_auto_add_library(gnss_poser_node SHARED + src/gnss_poser_core.cpp + ${GNSS_POSER_HEADERS} +) + +target_link_libraries(gnss_poser_node + Geographic +) + +rclcpp_components_register_node(gnss_poser_node + PLUGIN "gnss_poser::GNSSPoser" + EXECUTABLE gnss_poser +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package(INSTALL_TO_SHARE + launch +) diff --git a/sensing/gnss_poser/README.md b/sensing/gnss_poser/README.md new file mode 100644 index 0000000000000..7180ef657b1cc --- /dev/null +++ b/sensing/gnss_poser/README.md @@ -0,0 +1,51 @@ +# gnss_poser + +## Purpose + +The `gnss_poser` is a node that subscribes gnss sensing messages and calculates vehicle pose with covariance. + +## Inner-workings / Algorithms + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ---------------- | ----------------------------- | ----------------------------------------------------------------------------------------------- | +| `~/input/fix` | `sensor_msgs::msg::NavSatFix` | gnss status message | +| `~/input/navpvt` | `ublox_msgs::msg::NavPVT` | position, velocity and time solution (You can see detail description in reference document [1]) | + +### Output + +| Name | Type | Description | +| ------------------------ | ----------------------------------------------- | -------------------------------------------------------------- | +| `~/output/pose` | `geometry_msgs::msg::PoseStamped` | vehicle pose calculated from gnss sensing data | +| `~/output/gnss_pose_cov` | `geometry_msgs::msg::PoseWithCovarianceStamped` | vehicle pose with covariance calculated from gnss sensing data | +| `~/output/gnss_fixed` | `autoware_debug_msgs::msg::BoolStamped` | gnss fix status | + +## Parameters + +### Core Parameters + +| Name | Type | Default Value | Description | +| -------------------- | ------ | ---------------- | ----------------------------------------------------------------------------------------------- | +| `base_frame` | string | "base_link" | frame d | +| `gnss_frame` | string | "gnss" | frame id | +| `gnss_base_frame` | string | "gnss_base_link" | frame id | +| `map_frame` | string | "map" | frame id | +| `use_ublox_receiver` | bool | false | flag to use ublox receiver | +| `plane_zone` | int | 9 | identification number of the plane rectangular coordinate systems (See, reference document [2]) | + +## Assumptions / Known limits + +## (Optional) Error detection and handling + +## (Optional) Performance characterization + +## (Optional) References/External links + +[1] + +[2] + +## (Optional) Future extensions / Unimplemented parts diff --git a/sensing/gnss_poser/include/gnss_poser/convert.hpp b/sensing/gnss_poser/include/gnss_poser/convert.hpp new file mode 100644 index 0000000000000..9f5dc2b789e2d --- /dev/null +++ b/sensing/gnss_poser/include/gnss_poser/convert.hpp @@ -0,0 +1,134 @@ +// Copyright 2020 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. +#ifndef GNSS_POSER__CONVERT_HPP_ +#define GNSS_POSER__CONVERT_HPP_ + +#include "gnss_poser/gnss_stat.hpp" + +#include +#include +#include +#include +#include + +#include + +#include + +namespace gnss_poser +{ +enum class MGRSPrecision { + _10_KIRO_METER = 1, + _1_KIRO_METER = 2, + _100_METER = 3, + _10_METER = 4, + _1_METER = 5, + _100_MIllI_METER = 6, + _10_MIllI_METER = 7, + _1_MIllI_METER = 8, + _100MICRO_METER = 9, +}; +// EllipsoidHeight:height above ellipsoid +// OrthometricHeight:height above geoid +double EllipsoidHeight2OrthometricHeight( + const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const rclcpp::Logger & logger) +{ + double OrthometricHeight{0.0}; + try { + GeographicLib::Geoid egm2008("egm2008-1"); + OrthometricHeight = egm2008.ConvertHeight( + nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, nav_sat_fix_msg.altitude, + GeographicLib::Geoid::ELLIPSOIDTOGEOID); + } catch (const GeographicLib::GeographicErr & err) { + RCLCPP_ERROR_STREAM( + logger, "Failed to convert Height from Ellipsoid to Orthometric" << err.what()); + } + return OrthometricHeight; +} + +GNSSStat NavSatFix2UTM( + const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const rclcpp::Logger & logger) +{ + GNSSStat utm; + utm.coordinate_system = CoordinateSystem::UTM; + + try { + GeographicLib::UTMUPS::Forward( + nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, utm.zone, utm.northup, utm.x, utm.y); + + utm.z = EllipsoidHeight2OrthometricHeight(nav_sat_fix_msg, logger); + + utm.latitude = nav_sat_fix_msg.latitude; + utm.longitude = nav_sat_fix_msg.longitude; + utm.altitude = nav_sat_fix_msg.altitude; + } catch (const GeographicLib::GeographicErr & err) { + RCLCPP_ERROR_STREAM(logger, "Failed to convert from LLH to UTM" << err.what()); + } + return utm; +} + +GNSSStat UTM2MGRS( + const GNSSStat & utm, const MGRSPrecision & precision, const rclcpp::Logger & logger) +{ + constexpr int GZD_ID_size = 5; // size of header like "53SPU" + + GNSSStat mgrs = utm; + mgrs.coordinate_system = CoordinateSystem::MGRS; + try { + std::string mgrs_code; + GeographicLib::MGRS::Forward( + utm.zone, utm.northup, utm.x, utm.y, utm.latitude, static_cast(precision), mgrs_code); + mgrs.zone = std::stod(mgrs_code.substr(0, GZD_ID_size)); + mgrs.x = std::stod(mgrs_code.substr(GZD_ID_size, static_cast(precision))) * + std::pow( + 10, static_cast(MGRSPrecision::_1_METER) - + static_cast(precision)); // set unit as [m] + mgrs.y = std::stod(mgrs_code.substr( + GZD_ID_size + static_cast(precision), static_cast(precision))) * + std::pow( + 10, static_cast(MGRSPrecision::_1_METER) - + static_cast(precision)); // set unit as [m] + mgrs.z = utm.z; // TODO(ryo.watanabe) + } catch (const GeographicLib::GeographicErr & err) { + RCLCPP_ERROR_STREAM(logger, "Failed to convert from UTM to MGRS" << err.what()); + } + return mgrs; +} + +GNSSStat NavSatFix2MGRS( + const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const MGRSPrecision & precision, + const rclcpp::Logger & logger) +{ + const auto utm = NavSatFix2UTM(nav_sat_fix_msg, logger); + const auto mgrs = UTM2MGRS(utm, precision, logger); + return mgrs; +} + +GNSSStat NavSatFix2PLANE( + const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, const int & plane_zone, + const rclcpp::Logger & logger) +{ + GNSSStat plane; + plane.coordinate_system = CoordinateSystem::PLANE; + geo_pos_conv geo; + geo.set_plane(plane_zone); + geo.llh_to_xyz(nav_sat_fix_msg.latitude, nav_sat_fix_msg.longitude, nav_sat_fix_msg.altitude); + plane.x = geo.y(); + plane.y = geo.x(); + plane.z = EllipsoidHeight2OrthometricHeight(nav_sat_fix_msg, logger); + return plane; +} +} // namespace gnss_poser + +#endif // GNSS_POSER__CONVERT_HPP_ diff --git a/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp new file mode 100644 index 0000000000000..7e3daedf8d8b7 --- /dev/null +++ b/sensing/gnss_poser/include/gnss_poser/gnss_poser_core.hpp @@ -0,0 +1,96 @@ +// Copyright 2020 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. +#ifndef GNSS_POSER__GNSS_POSER_CORE_HPP_ +#define GNSS_POSER__GNSS_POSER_CORE_HPP_ + +#include "gnss_poser/convert.hpp" +#include "gnss_poser/gnss_stat.hpp" + +#include + +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include + +#include + +namespace gnss_poser +{ +class GNSSPoser : public rclcpp::Node +{ +public: + explicit GNSSPoser(const rclcpp::NodeOptions & node_options); + +private: + void callbackNavSatFix(const sensor_msgs::msg::NavSatFix::ConstSharedPtr nav_sat_fix_msg_ptr); + void callbackNavPVT(const ublox_msgs::msg::NavPVT::ConstSharedPtr msg); + + bool isFixed(const sensor_msgs::msg::NavSatStatus & nav_sat_status_msg); + bool canGetCovariance(const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg); + GNSSStat convert( + const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, CoordinateSystem coordinate_system); + geometry_msgs::msg::Point getPosition(const GNSSStat & gnss_stat); + geometry_msgs::msg::Point getMedianPosition( + const boost::circular_buffer & position_buffer); + geometry_msgs::msg::Quaternion getQuaternionByHeading(const int heading); + geometry_msgs::msg::Quaternion getQuaternionByPositionDifference( + const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Point & prev_point); + + bool getTransform( + const std::string & target_frame, const std::string & source_frame, + const geometry_msgs::msg::TransformStamped::SharedPtr transform_stamped_ptr); + bool getStaticTransform( + const std::string & target_frame, const std::string & source_frame, + const geometry_msgs::msg::TransformStamped::SharedPtr transform_stamped_ptr, + const builtin_interfaces::msg::Time & stamp); + void publishTF( + const std::string & frame_id, const std::string & child_frame_id, + const geometry_msgs::msg::PoseStamped & pose_msg); + + tf2::BufferCore tf2_buffer_; + tf2_ros::TransformListener tf2_listener_; + tf2_ros::TransformBroadcaster tf2_broadcaster_; + + rclcpp::Subscription::SharedPtr nav_sat_fix_sub_; + rclcpp::Subscription::SharedPtr nav_pvt_sub_; + + rclcpp::Publisher::SharedPtr pose_pub_; + rclcpp::Publisher::SharedPtr pose_cov_pub_; + rclcpp::Publisher::SharedPtr fixed_pub_; + + CoordinateSystem coordinate_system_; + std::string base_frame_; + std::string gnss_frame_; + std::string gnss_base_frame_; + std::string map_frame_; + + bool use_ublox_receiver_; + + int plane_zone_; + + boost::circular_buffer position_buffer_; + ublox_msgs::msg::NavPVT::ConstSharedPtr nav_pvt_msg_ptr_; +}; +} // namespace gnss_poser + +#endif // GNSS_POSER__GNSS_POSER_CORE_HPP_ diff --git a/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp b/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp new file mode 100644 index 0000000000000..fa145b5b2098a --- /dev/null +++ b/sensing/gnss_poser/include/gnss_poser/gnss_stat.hpp @@ -0,0 +1,52 @@ +// Copyright 2020 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. +#ifndef GNSS_POSER__GNSS_STAT_HPP_ +#define GNSS_POSER__GNSS_STAT_HPP_ + +namespace gnss_poser +{ +enum class CoordinateSystem { + UTM = 0, + MGRS = 1, + PLANE = 2, +}; + +struct GNSSStat +{ + GNSSStat() + : coordinate_system(CoordinateSystem::MGRS), + northup(true), + zone(0), + x(0), + y(0), + z(0), + latitude(0), + longitude(0), + altitude(0) + { + } + + CoordinateSystem coordinate_system; + bool northup; + int zone; + double x; + double y; + double z; + double latitude; + double longitude; + double altitude; +}; +} // namespace gnss_poser + +#endif // GNSS_POSER__GNSS_STAT_HPP_ diff --git a/sensing/gnss_poser/launch/gnss_poser.launch.xml b/sensing/gnss_poser/launch/gnss_poser.launch.xml new file mode 100755 index 0000000000000..13fd07ab0e896 --- /dev/null +++ b/sensing/gnss_poser/launch/gnss_poser.launch.xml @@ -0,0 +1,43 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sensing/gnss_poser/launch/ubloxfix2mgrs.launch.xml b/sensing/gnss_poser/launch/ubloxfix2mgrs.launch.xml new file mode 100644 index 0000000000000..7267c3754cd31 --- /dev/null +++ b/sensing/gnss_poser/launch/ubloxfix2mgrs.launch.xml @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/sensing/gnss_poser/package.xml b/sensing/gnss_poser/package.xml new file mode 100644 index 0000000000000..396ec31cf95e3 --- /dev/null +++ b/sensing/gnss_poser/package.xml @@ -0,0 +1,31 @@ + + + + gnss_poser + 1.0.0 + The ROS2 gnss_poser package + Ryo Watanabe + Apache License 2.0 + + ament_cmake_auto + + autoware_debug_msgs + geo_pos_conv + geographiclib + geometry_msgs + sensor_msgs + tf2 + tf2_geometry_msgs + tf2_ros + ublox_msgs + + rclcpp + rclcpp_components + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/sensing/gnss_poser/src/gnss_poser_core.cpp b/sensing/gnss_poser/src/gnss_poser_core.cpp new file mode 100644 index 0000000000000..bc847bede40fd --- /dev/null +++ b/sensing/gnss_poser/src/gnss_poser_core.cpp @@ -0,0 +1,354 @@ +// Copyright 2020 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. + +#include "gnss_poser/gnss_poser_core.hpp" + +#include +#include +#include +#include + +namespace gnss_poser +{ +GNSSPoser::GNSSPoser(const rclcpp::NodeOptions & node_options) +: rclcpp::Node("gnss_poser", node_options), + tf2_listener_(tf2_buffer_), + tf2_broadcaster_(*this), + base_frame_(declare_parameter("base_frame", "base_link")), + gnss_frame_(declare_parameter("gnss_frame", "gnss")), + gnss_base_frame_(declare_parameter("gnss_base_frame", "gnss_base_link")), + map_frame_(declare_parameter("map_frame", "map")), + use_ublox_receiver_(declare_parameter("use_ublox_receiver", false)), + plane_zone_(declare_parameter("plane_zone", 9)) +{ + int coordinate_system = + declare_parameter("coordinate_system", static_cast(CoordinateSystem::MGRS)); + coordinate_system_ = static_cast(coordinate_system); + + int buff_epoch = declare_parameter("buff_epoch", 1); + position_buffer_.set_capacity(buff_epoch); + + nav_sat_fix_sub_ = create_subscription( + "fix", rclcpp::QoS{1}, std::bind(&GNSSPoser::callbackNavSatFix, this, std::placeholders::_1)); + nav_pvt_sub_ = create_subscription( + "navpvt", rclcpp::QoS{1}, std::bind(&GNSSPoser::callbackNavPVT, this, std::placeholders::_1)); + + pose_pub_ = create_publisher("gnss_pose", rclcpp::QoS{1}); + pose_cov_pub_ = create_publisher( + "gnss_pose_cov", rclcpp::QoS{1}); + fixed_pub_ = + create_publisher("gnss_fixed", rclcpp::QoS{1}); +} + +void GNSSPoser::callbackNavSatFix( + const sensor_msgs::msg::NavSatFix::ConstSharedPtr nav_sat_fix_msg_ptr) +{ + // check fixed topic + const bool is_fixed = isFixed(nav_sat_fix_msg_ptr->status); + + // publish is_fixed topic + autoware_debug_msgs::msg::BoolStamped is_fixed_msg; + is_fixed_msg.stamp = this->now(); + is_fixed_msg.data = is_fixed; + fixed_pub_->publish(is_fixed_msg); + + if (!is_fixed) { + RCLCPP_WARN_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), + "Not Fixed Topic. Skipping Calculate."); + return; + } + + // get position in coordinate_system + const auto gnss_stat = convert(*nav_sat_fix_msg_ptr, coordinate_system_); + const auto position = getPosition(gnss_stat); + + // calc median position + position_buffer_.push_front(position); + if (!position_buffer_.full()) { + RCLCPP_WARN_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), + "Buffering Position. Output Skipped."); + return; + } + const auto median_position = getMedianPosition(position_buffer_); + + // calc gnss antenna orientation + geometry_msgs::msg::Quaternion orientation; + if (use_ublox_receiver_ && nav_pvt_msg_ptr_ != nullptr) { + orientation = getQuaternionByHeading(nav_pvt_msg_ptr_->heading); + } else { + static auto prev_position = median_position; + orientation = getQuaternionByPositionDifference(median_position, prev_position); + prev_position = median_position; + } + + // generate gnss_antenna_pose + geometry_msgs::msg::Pose gnss_antenna_pose{}; + gnss_antenna_pose.position = median_position; + gnss_antenna_pose.orientation = orientation; + + // get TF from gnss_antenna to map + tf2::Transform tf_map2gnss_antenna{}; + tf2::fromMsg(gnss_antenna_pose, tf_map2gnss_antenna); + + // get TF from base_link to gnss_antenna + auto tf_gnss_antenna2base_link_msg_ptr = std::make_shared(); + getStaticTransform( + gnss_frame_, base_frame_, tf_gnss_antenna2base_link_msg_ptr, nav_sat_fix_msg_ptr->header.stamp); + tf2::Transform tf_gnss_antenna2base_link{}; + tf2::fromMsg(tf_gnss_antenna2base_link_msg_ptr->transform, tf_gnss_antenna2base_link); + + // transform pose from gnss_antenna(in map frame) to base_link(in map frame) + tf2::Transform tf_map2base_link{}; + tf_map2base_link = tf_map2gnss_antenna * tf_gnss_antenna2base_link; + + geometry_msgs::msg::PoseStamped gnss_base_pose_msg{}; + gnss_base_pose_msg.header.stamp = nav_sat_fix_msg_ptr->header.stamp; + gnss_base_pose_msg.header.frame_id = map_frame_; + tf2::toMsg(tf_map2base_link, gnss_base_pose_msg.pose); + + // publish gnss_base_link pose in map frame + pose_pub_->publish(gnss_base_pose_msg); + + // publish gnss_base_link pose_cov in map frame + geometry_msgs::msg::PoseWithCovarianceStamped gnss_base_pose_cov_msg; + gnss_base_pose_cov_msg.header = gnss_base_pose_msg.header; + gnss_base_pose_cov_msg.pose.pose = gnss_base_pose_msg.pose; + gnss_base_pose_cov_msg.pose.covariance[6 * 0 + 0] = + canGetCovariance(*nav_sat_fix_msg_ptr) ? nav_sat_fix_msg_ptr->position_covariance[0] : 10.0; + gnss_base_pose_cov_msg.pose.covariance[6 * 1 + 1] = + canGetCovariance(*nav_sat_fix_msg_ptr) ? nav_sat_fix_msg_ptr->position_covariance[4] : 10.0; + gnss_base_pose_cov_msg.pose.covariance[6 * 2 + 2] = + canGetCovariance(*nav_sat_fix_msg_ptr) ? nav_sat_fix_msg_ptr->position_covariance[8] : 10.0; + gnss_base_pose_cov_msg.pose.covariance[6 * 3 + 3] = 0.1; + gnss_base_pose_cov_msg.pose.covariance[6 * 4 + 4] = 0.1; + gnss_base_pose_cov_msg.pose.covariance[6 * 5 + 5] = 1.0; + pose_cov_pub_->publish(gnss_base_pose_cov_msg); + + // broadcast map to gnss_base_link + publishTF(map_frame_, gnss_base_frame_, gnss_base_pose_msg); +} + +void GNSSPoser::callbackNavPVT(const ublox_msgs::msg::NavPVT::ConstSharedPtr nav_pvt_msg_ptr) +{ + nav_pvt_msg_ptr_ = nav_pvt_msg_ptr; +} + +bool GNSSPoser::isFixed(const sensor_msgs::msg::NavSatStatus & nav_sat_status_msg) +{ + return nav_sat_status_msg.status >= sensor_msgs::msg::NavSatStatus::STATUS_FIX; +} + +bool GNSSPoser::canGetCovariance(const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg) +{ + return nav_sat_fix_msg.position_covariance_type > + sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN; +} + +GNSSStat GNSSPoser::convert( + const sensor_msgs::msg::NavSatFix & nav_sat_fix_msg, CoordinateSystem coordinate_system) +{ + GNSSStat gnss_stat; + if (coordinate_system == CoordinateSystem::UTM) { + gnss_stat = NavSatFix2UTM(nav_sat_fix_msg, this->get_logger()); + } else if (coordinate_system == CoordinateSystem::MGRS) { + gnss_stat = NavSatFix2MGRS(nav_sat_fix_msg, MGRSPrecision::_100MICRO_METER, this->get_logger()); + } else if (coordinate_system == CoordinateSystem::PLANE) { + gnss_stat = NavSatFix2PLANE(nav_sat_fix_msg, plane_zone_, this->get_logger()); + } else { + RCLCPP_ERROR_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), + "Unknown Coordinate System"); + } + return gnss_stat; +} + +geometry_msgs::msg::Point GNSSPoser::getPosition(const GNSSStat & gnss_stat) +{ + geometry_msgs::msg::Point point; + point.x = gnss_stat.x; + point.y = gnss_stat.y; + point.z = gnss_stat.z; + return point; +} + +geometry_msgs::msg::Point GNSSPoser::getMedianPosition( + const boost::circular_buffer & position_buffer) +{ + auto getMedian = [](std::vector array) { + std::sort(std::begin(array), std::end(array)); + const size_t median_index = array.size() / 2; + double median = (array.size() % 2) + ? (array.at(median_index)) + : ((array.at(median_index) + array.at(median_index - 1)) / 2); + return median; + }; + + std::vector array_x; + std::vector array_y; + std::vector array_z; + for (const auto & position : position_buffer) { + array_x.push_back(position.x); + array_y.push_back(position.y); + array_z.push_back(position.z); + } + + geometry_msgs::msg::Point median_point; + median_point.x = getMedian(array_x); + median_point.y = getMedian(array_y); + median_point.z = getMedian(array_z); + return median_point; +} + +geometry_msgs::msg::Quaternion GNSSPoser::getQuaternionByHeading(const int heading) +{ + int heading_conv = 0; + // convert heading[0(North)~360] to yaw[-M_PI(West)~M_PI] + if (heading >= 0 && heading <= 27000000) { + heading_conv = 9000000 - heading; + } else { + heading_conv = 45000000 - heading; + } + const double yaw = (heading_conv * 1e-5) * M_PI / 180.0; + + tf2::Quaternion quaternion; + quaternion.setRPY(0, 0, yaw); + + return tf2::toMsg(quaternion); +} + +geometry_msgs::msg::Quaternion GNSSPoser::getQuaternionByPositionDifference( + const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Point & prev_point) +{ + const double yaw = std::atan2(point.y - prev_point.y, point.x - prev_point.x); + tf2::Quaternion quaternion; + quaternion.setRPY(0, 0, yaw); + return tf2::toMsg(quaternion); +} + +bool GNSSPoser::getTransform( + const std::string & target_frame, const std::string & source_frame, + const geometry_msgs::msg::TransformStamped::SharedPtr transform_stamped_ptr) +{ + if (target_frame == source_frame) { + transform_stamped_ptr->header.stamp = this->now(); + transform_stamped_ptr->header.frame_id = target_frame; + transform_stamped_ptr->child_frame_id = source_frame; + transform_stamped_ptr->transform.translation.x = 0.0; + transform_stamped_ptr->transform.translation.y = 0.0; + transform_stamped_ptr->transform.translation.z = 0.0; + transform_stamped_ptr->transform.rotation.x = 0.0; + transform_stamped_ptr->transform.rotation.y = 0.0; + transform_stamped_ptr->transform.rotation.z = 0.0; + transform_stamped_ptr->transform.rotation.w = 1.0; + return true; + } + + try { + *transform_stamped_ptr = + tf2_buffer_.lookupTransform(target_frame, source_frame, tf2::TimePointZero); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), ex.what()); + RCLCPP_WARN_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), + "Please publish TF " << target_frame.c_str() << " to " << source_frame.c_str()); + + transform_stamped_ptr->header.stamp = this->now(); + transform_stamped_ptr->header.frame_id = target_frame; + transform_stamped_ptr->child_frame_id = source_frame; + transform_stamped_ptr->transform.translation.x = 0.0; + transform_stamped_ptr->transform.translation.y = 0.0; + transform_stamped_ptr->transform.translation.z = 0.0; + transform_stamped_ptr->transform.rotation.x = 0.0; + transform_stamped_ptr->transform.rotation.y = 0.0; + transform_stamped_ptr->transform.rotation.z = 0.0; + transform_stamped_ptr->transform.rotation.w = 1.0; + return false; + } + return true; +} + +bool GNSSPoser::getStaticTransform( + const std::string & target_frame, const std::string & source_frame, + const geometry_msgs::msg::TransformStamped::SharedPtr transform_stamped_ptr, + const builtin_interfaces::msg::Time & stamp) +{ + if (target_frame == source_frame) { + transform_stamped_ptr->header.stamp = stamp; + transform_stamped_ptr->header.frame_id = target_frame; + transform_stamped_ptr->child_frame_id = source_frame; + transform_stamped_ptr->transform.translation.x = 0.0; + transform_stamped_ptr->transform.translation.y = 0.0; + transform_stamped_ptr->transform.translation.z = 0.0; + transform_stamped_ptr->transform.rotation.x = 0.0; + transform_stamped_ptr->transform.rotation.y = 0.0; + transform_stamped_ptr->transform.rotation.z = 0.0; + transform_stamped_ptr->transform.rotation.w = 1.0; + return true; + } + + try { + *transform_stamped_ptr = tf2_buffer_.lookupTransform( + target_frame, source_frame, + tf2::TimePoint(std::chrono::seconds(stamp.sec) + std::chrono::nanoseconds(stamp.nanosec))); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), ex.what()); + RCLCPP_WARN_STREAM_THROTTLE( + this->get_logger(), *this->get_clock(), std::chrono::milliseconds(1000).count(), + "Please publish TF " << target_frame.c_str() << " to " << source_frame.c_str()); + + transform_stamped_ptr->header.stamp = stamp; + transform_stamped_ptr->header.frame_id = target_frame; + transform_stamped_ptr->child_frame_id = source_frame; + transform_stamped_ptr->transform.translation.x = 0.0; + transform_stamped_ptr->transform.translation.y = 0.0; + transform_stamped_ptr->transform.translation.z = 0.0; + transform_stamped_ptr->transform.rotation.x = 0.0; + transform_stamped_ptr->transform.rotation.y = 0.0; + transform_stamped_ptr->transform.rotation.z = 0.0; + transform_stamped_ptr->transform.rotation.w = 1.0; + return false; + } + return true; +} + +void GNSSPoser::publishTF( + const std::string & frame_id, const std::string & child_frame_id, + const geometry_msgs::msg::PoseStamped & pose_msg) +{ + geometry_msgs::msg::TransformStamped transform_stamped; + transform_stamped.header.frame_id = frame_id; + transform_stamped.child_frame_id = child_frame_id; + transform_stamped.header.stamp = pose_msg.header.stamp; + + transform_stamped.transform.translation.x = pose_msg.pose.position.x; + transform_stamped.transform.translation.y = pose_msg.pose.position.y; + transform_stamped.transform.translation.z = pose_msg.pose.position.z; + + tf2::Quaternion tf_quaternion; + tf2::fromMsg(pose_msg.pose.orientation, tf_quaternion); + transform_stamped.transform.rotation.x = tf_quaternion.x(); + transform_stamped.transform.rotation.y = tf_quaternion.y(); + transform_stamped.transform.rotation.z = tf_quaternion.z(); + transform_stamped.transform.rotation.w = tf_quaternion.w(); + + tf2_broadcaster_.sendTransform(transform_stamped); +} +} // namespace gnss_poser + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(gnss_poser::GNSSPoser)