From 2d42dd117e9fcf490021194e1aa0103d4ef37404 Mon Sep 17 00:00:00 2001 From: Zhe Feng Date: Fri, 5 Apr 2024 15:30:42 +0200 Subject: [PATCH 1/5] add height above the geoid output --- fixposition_driver_ros1/CMakeLists.txt | 7 +++++ .../fixposition_driver_node.hpp | 1 + fixposition_driver_ros1/launch/tcp.yaml | 4 +-- fixposition_driver_ros1/src/README | 5 ++++ .../src/fixposition_driver_node.cpp | 26 +++++++++++++++++++ 5 files changed, 41 insertions(+), 2 deletions(-) create mode 100644 fixposition_driver_ros1/src/README diff --git a/fixposition_driver_ros1/CMakeLists.txt b/fixposition_driver_ros1/CMakeLists.txt index 408b0d8..54c0d2f 100644 --- a/fixposition_driver_ros1/CMakeLists.txt +++ b/fixposition_driver_ros1/CMakeLists.txt @@ -9,6 +9,11 @@ set(CMAKE_EXPORT_COMPILE_COMMANDS ON) # DEPENDENCIES ========================================================================================================= find_package(Boost 1.65.0 REQUIRED) + +# GEOID APPLICATION +list(APPEND CMAKE_MODULE_PATH "/usr/share/cmake/geographiclib") +find_package(GeographicLib REQUIRED) + find_package(Eigen3 REQUIRED) find_package(fixposition_gnss_tf REQUIRED) find_package(fixposition_driver_lib REQUIRED) @@ -52,6 +57,7 @@ include_directories( ${fixposition_driver_lib_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIR} + ${GeographicLib_INCLUDE_DIRS} ) # BUILD EXECUTABLE ===================================================================================================== @@ -68,6 +74,7 @@ target_link_libraries( ${fixposition_gnss_tf_LIBRARIES} ${fixposition_driver_lib_LIBRARIES} ${Boost_LIBRARIES} + ${GeographicLib_LIBRARIES} pthread ) diff --git a/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp b/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp index f7e6ace..1472ffe 100644 --- a/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp +++ b/fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp @@ -97,6 +97,7 @@ class FixpositionDriverNode : public FixpositionDriver { ros::Publisher navsatfix_pub_; ros::Publisher navsatfix_gnss1_pub_; ros::Publisher navsatfix_gnss2_pub_; + ros::Publisher navsatfix_mining_pub_; ros::Publisher nmea_pub_; ros::Publisher odometry_pub_; //!< ECEF Odometry ros::Publisher poiimu_pub_; //!< Bias corrected IMU diff --git a/fixposition_driver_ros1/launch/tcp.yaml b/fixposition_driver_ros1/launch/tcp.yaml index bb8add1..b20cb93 100644 --- a/fixposition_driver_ros1/launch/tcp.yaml +++ b/fixposition_driver_ros1/launch/tcp.yaml @@ -1,8 +1,8 @@ fp_output: - formats: ["ODOMETRY", "LLH", "RAWIMU", "CORRIMU", "GPGGA", "GPZDA", "GPRMC", "TF"] + formats: ["ODOMETRY", "LLH", "LLH_MINING", "RAWIMU", "CORRIMU", "GPGGA", "GPZDA", "GPRMC", "TF"] type: tcp port: "21000" - ip: "172.22.1.46" # change to VRTK2's IP address in the network + ip: "172.22.1.13" # change to VRTK2's IP address in the network rate: 200 reconnect_delay: 5.0 # wait time in [s] until retry connection diff --git a/fixposition_driver_ros1/src/README b/fixposition_driver_ros1/src/README new file mode 100644 index 0000000..eaf009c --- /dev/null +++ b/fixposition_driver_ros1/src/README @@ -0,0 +1,5 @@ +sudo apt install geographiclib-tools +sudo geographiclib-get-geoids egm2008-1 +#takes very long time... +ls -l /usr/share/GeographicLib/geoids/ +#verify if they are there... diff --git a/fixposition_driver_ros1/src/fixposition_driver_node.cpp b/fixposition_driver_ros1/src/fixposition_driver_node.cpp index 0609519..9a888ec 100644 --- a/fixposition_driver_ros1/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros1/src/fixposition_driver_node.cpp @@ -34,6 +34,9 @@ #include #include +/* Geoid */ +#include + namespace fixposition { FixpositionDriverNode::FixpositionDriverNode(const FixpositionDriverParams& params) @@ -45,6 +48,10 @@ FixpositionDriverNode::FixpositionDriverNode(const FixpositionDriverParams& para navsatfix_gnss1_pub_(nh_.advertise("/fixposition/gnss1", 100)), navsatfix_gnss2_pub_(nh_.advertise("/fixposition/gnss2", 100)), nmea_pub_(nh_.advertise("/fixposition/nmea", 100)), + + //Special LLH publish for mining industry + navsatfix_mining_pub_(nh_.advertise("/fixposition/navsatfix_mining", 100)), + // ODOMETRY odometry_pub_(nh_.advertise("/fixposition/odometry", 100)), poiimu_pub_(nh_.advertise("/fixposition/poiimu", 100)), @@ -130,6 +137,25 @@ void FixpositionDriverNode::RegisterObservers() { NavSatFixDataToMsg(data, msg); navsatfix_pub_.publish(msg); }); + } else if (format == "LLH_MINING" && a_converters_["LLH"]) { + dynamic_cast(a_converters_["LLH"].get())->AddObserver([this](const NavSatFixData& data) { + //LLH Observer Lambda + sensor_msgs::NavSatFix msg; + + GeographicLib::Geoid geoid("egm2008-1"); + //new function for generating new llh + + NavSatFixDataToMsg(data, msg); + double geodetic_height = geoid(msg.latitude,msg.longitude); + msg.altitude = msg.altitude - geodetic_height; + + //The geoid height, N, can be used to convert a height above the ellipsoid, h, to the corresponding height above the geoid (roughly the height above mean sea level), H, using the relations + //h = N + H; H = −N + h. + //ROS_INFO("Geodetic Height: %f meters", geodetic_height); + //ROS_INFO("Height Above the Geoid: %f meters", msg.altitude); + + navsatfix_mining_pub_.publish(msg); + }); } else if (format == "RAWIMU") { dynamic_cast(a_converters_["RAWIMU"].get())->AddObserver([this](const ImuData& data) { // RAWIMU Observer Lambda From 6f9e3f873f6f7ae2209deafa1c44d3dfbe3ffe9a Mon Sep 17 00:00:00 2001 From: Zhe Feng Date: Fri, 5 Apr 2024 17:01:33 +0200 Subject: [PATCH 2/5] first draft version can be used --- .../src/fixposition_driver.cpp | 2 +- .../GEODETIC_HEIGHT.markdown | 182 ++++++++++++++++++ fixposition_driver_ros1/src/README | 5 - 3 files changed, 183 insertions(+), 6 deletions(-) create mode 100644 fixposition_driver_ros1/GEODETIC_HEIGHT.markdown delete mode 100644 fixposition_driver_ros1/src/README diff --git a/fixposition_driver_lib/src/fixposition_driver.cpp b/fixposition_driver_lib/src/fixposition_driver.cpp index 50f0155..a5f9175 100644 --- a/fixposition_driver_lib/src/fixposition_driver.cpp +++ b/fixposition_driver_lib/src/fixposition_driver.cpp @@ -185,7 +185,7 @@ bool FixpositionDriver::InitializeConverters() { if (format == "ODOMETRY") { a_converters_["ODOMETRY"] = std::unique_ptr(new OdometryConverter()); a_converters_["TF"] = std::unique_ptr(new TfConverter()); - } else if (format == "LLH") { + } else if (format == "LLH" || format == "LLH_MINING") { a_converters_["LLH"] = std::unique_ptr(new LlhConverter()); } else if (format == "RAWIMU") { a_converters_["RAWIMU"] = std::unique_ptr(new ImuConverter(false)); diff --git a/fixposition_driver_ros1/GEODETIC_HEIGHT.markdown b/fixposition_driver_ros1/GEODETIC_HEIGHT.markdown new file mode 100644 index 0000000..3aa73e2 --- /dev/null +++ b/fixposition_driver_ros1/GEODETIC_HEIGHT.markdown @@ -0,0 +1,182 @@ +## Height Calculation + +To calculate the height above the geoid (normal altitude used in the mining industry), use the following equation: + +```bash +Height above the Geoid = Height above the Ellipsoid - Geodetic Height +``` + +Where: +- **Height above the Geoid** is the normal altitude used in the mining industry. +- **Height above the Ellipsoid** is the normal height in Fixposition's output. +- **Geodetic Height** is the height above the geoid model. + +To achieve this, leveraging GeographicLib is a good option. Below are the steps to consider for enabling this in the Fixposition driver ROS1 (Enable ROS2 is in the todo list): + +## Git Clone the Codes from the Specific Branch + +Clone the Repository: Use the git clone command with the -b option to specify the branch `feature/mining` branch from the fixposition_driver GitHub repository. + +``` +git clone -b feature/mining https://github.com/fixposition/fixposition_driver.git +``` +Then we are going to integrate GeographicLib with Fixposition driver ros 1 + +## Prerequisites + +Before proceeding with the installation of GeographicLib, ensure your system meets the following requirements: +- Ubuntu 18.04 or newer +- ROS1 +- C++ Compiler with C++11 support + +## Dependencies + +- [Eigen3](https://eigen.tuxfamily.org/index.php?title=Main_Page), tested with version [3.3.7](https://gitlab.com/libeigen/eigen/-/releases/3.3.7) +- [Boost](https://www.boost.org/), tested with version [1.65.0](https://www.boost.org/users/history/version_1_65_0.html) +- [CMake](https://cmake.org/) +- [tf](http://wiki.ros.org/tf) ROS1 library +- [eigen_conversions](https://wiki.ros.org/eigen_conversions) ROS1 library +- [Catkin](http://wiki.ros.org/catkin) for ROS1 + +- **[fixposition_gnss_tf](https://github.com/fixposition/fixposition_gnss_tf)**: Fixposition GNSS Transformation Lib + + +This driver operates as a ROS node, connecting to either a TCP or serial stream of Fixposition Vision-RTK output data, see [Fixposition ASCII messages](#fixposition-ascii-messages) and the **Integration Manual**. + +## Installing dependencies on Ubuntu system + +``` + sudo apt update + sudo apt install -y build-essential cmake + sudo apt install -y libeigen3-dev + sudo apt install -y ros-{ROS_DISTRO}-tf ros-{ROS_DISTRO}-eigen-conversions +``` + +## Installation Instructions + +### GeographicLib Installation + +Install GeographicLib and its development files: + +```sh +sudo apt-get update +sudo apt-get install libgeographic-dev +``` + +### GeographicLib Tools + +Install the `geographiclib-tools` package, which includes utilities for managing geoid model data: + +```sh +sudo apt install geographiclib-tools +``` + +### Downloading Geoid Model Data + +Download the EGM2008 geoid model data. Note that this might take some time due to the size of the data files: + +```sh +sudo geographiclib-get-geoids egm2008-1 +``` + +**Note**: It's recommended to perform this step on your host PC rather than embedding it directly into VRTK2's firmware, due to the substantial size of the geoid model data files. + +### Verification + +Confirm that the geoid models have been correctly installed: + +```sh +ls -l /usr/share/GeographicLib/geoids/ +``` + +## Configuration in CMakeLists.txt + +Incorporate GeographicLib into your ROS1 package by adjusting your `CMakeLists.txt`: + +```cmake +list(APPEND CMAKE_MODULE_PATH "/usr/share/cmake/geographiclib") +find_package(GeographicLib REQUIRED) +``` +This modification ensures CMake locates the GeographicLib configuration module, enabling its functionalities within your project. + +After installing `libgeographic-dev`, the `FindGeographicLib.cmake` file is typically located in a directory where CMake can find it. However, the exact path (e.g. `/usr/share/cmake/geographiclib`) can vary based on the system configuration and how GeographicLib was installed. + +you can use `dpkg-query` to list the files installed by the `libgeographic-dev` package: + +``` +dpkg-query -L libgeographic-dev +``` + +Look for paths that contain cmake or geographiclib in the output. These paths are likely where CMake configuration files are located. Then you can use this path to modify the `CMakeLists.txt`. + +## Installation + +To install the node, arrange the folders like this: (don't forget to git clone the `fixposition_gnss_tf` from branch `main`) + +```bash +# The folder structure should look like this +fp_public_ws +├── src +│ ├── fixposition_driver +│ │ ├── fixposition_driver_lib +│ │ ├── fixposition_driver_ros1 +│ │ ├── fixposition_driver_ros2 # will be ignore by catkin +│ ├── fixposition_gnss_tf +``` +make sure you have sourced the setup.bash from ros: + +`/opt/ros/{ROS_DISTRO}/setup.bash`, for example + +``` +source /opt/ros/{ROS_DISTRO}/setup.bash +``` + +and build it with: + +`catkin build fixposition_driver_ros1` + +This will build the ROS1 driver node and all its dependencies. + +Then source your development environment: + +`source devel/setup.bash` + +## Launch the Driver + +- Enable the output for "LLH_MINING" in the yaml file to obtain specialized height-above-geoid information, This setting will be utilized in future operations to access specific geodetic data: + + ` formats: ["ODOMETRY", "LLH", "LLH_MINING", "RAWIMU", "CORRIMU", "GPGGA", "GPZDA", "GPRMC", "TF"] ` + +- To launch the node in serial mode, run: + + `roslaunch fixposition_driver_ros1 serial.launch` + +- In TCP mode (Wi-Fi): + + `roslaunch fixposition_driver_ros1 tcp.launch` + +- In TCP mode (Ethernet): + + `roslaunch fixposition_driver_ros1 tcp.launch` + +To change the settings of TCP (IP, Port) or Serial (Baudrate, Port) connections, check the `launch/tcp.yaml` and `launch/serial.yaml` files. + +## Check the messages +- Check rostopics: + + `rostopic list` + + + there should be topics under the `/fixposition` namespace, for example: + ```bash + /fixposition/navsatfix_mining + ``` + +- Check published message in the topic, for example the VRTK message: + `rostopic echo /fixposition/mining` + +Now the height inside the topic is the height above the geoid (normal altitude used in the mining industry) + + + + diff --git a/fixposition_driver_ros1/src/README b/fixposition_driver_ros1/src/README deleted file mode 100644 index eaf009c..0000000 --- a/fixposition_driver_ros1/src/README +++ /dev/null @@ -1,5 +0,0 @@ -sudo apt install geographiclib-tools -sudo geographiclib-get-geoids egm2008-1 -#takes very long time... -ls -l /usr/share/GeographicLib/geoids/ -#verify if they are there... From dbb7a777443f429ffa5f19b9ef1171983053a437 Mon Sep 17 00:00:00 2001 From: Zhe Feng Date: Fri, 5 Apr 2024 17:10:05 +0200 Subject: [PATCH 3/5] first draft version can be used --- fixposition_driver_ros1/GEODETIC_HEIGHT.markdown | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/fixposition_driver_ros1/GEODETIC_HEIGHT.markdown b/fixposition_driver_ros1/GEODETIC_HEIGHT.markdown index 3aa73e2..793b017 100644 --- a/fixposition_driver_ros1/GEODETIC_HEIGHT.markdown +++ b/fixposition_driver_ros1/GEODETIC_HEIGHT.markdown @@ -1,4 +1,15 @@ -## Height Calculation +## Calculation Altitude in Mining Industry + +Reference +``` +https://docs.ros.org/en/kinetic/api/gtsam/html/classNETGeographicLib_1_1Geoid.html + +``` +Online tool to calculate the geodetic height (it is different from height above the geoid) + +``` +https://geographiclib.sourceforge.io/cgi-bin/GeodSolve +``` To calculate the height above the geoid (normal altitude used in the mining industry), use the following equation: From 306db3ec7f3cf55d6ec7876b8ee8616c80d700b0 Mon Sep 17 00:00:00 2001 From: Zhe Feng Date: Fri, 5 Apr 2024 17:23:26 +0200 Subject: [PATCH 4/5] first draft version can be used --- fixposition_driver_ros1/GEODETIC_HEIGHT.markdown | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/fixposition_driver_ros1/GEODETIC_HEIGHT.markdown b/fixposition_driver_ros1/GEODETIC_HEIGHT.markdown index 793b017..7a3bada 100644 --- a/fixposition_driver_ros1/GEODETIC_HEIGHT.markdown +++ b/fixposition_driver_ros1/GEODETIC_HEIGHT.markdown @@ -1,6 +1,6 @@ ## Calculation Altitude in Mining Industry -Reference +Reference: ``` https://docs.ros.org/en/kinetic/api/gtsam/html/classNETGeographicLib_1_1Geoid.html @@ -26,10 +26,10 @@ To achieve this, leveraging GeographicLib is a good option. Below are the steps ## Git Clone the Codes from the Specific Branch -Clone the Repository: Use the git clone command with the -b option to specify the branch `feature/mining` branch from the fixposition_driver GitHub repository. +Clone the Repository: Use the git clone command with the -b option to specify the branch `geodetic_height/egm2008` branch from the Jelvon GitHub repository. ``` -git clone -b feature/mining https://github.com/fixposition/fixposition_driver.git +git clone -b geodetic_height/egm2008 https://github.com/Jelvon/fixposition_driver.git ``` Then we are going to integrate GeographicLib with Fixposition driver ros 1 From 11409bf7bcddaf65258529391edc25aa548f8891 Mon Sep 17 00:00:00 2001 From: Zhe Feng Date: Fri, 5 Apr 2024 17:30:33 +0200 Subject: [PATCH 5/5] first draft version can be used --- fixposition_driver_ros1/GEODETIC_HEIGHT.markdown | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/fixposition_driver_ros1/GEODETIC_HEIGHT.markdown b/fixposition_driver_ros1/GEODETIC_HEIGHT.markdown index 7a3bada..37640c6 100644 --- a/fixposition_driver_ros1/GEODETIC_HEIGHT.markdown +++ b/fixposition_driver_ros1/GEODETIC_HEIGHT.markdown @@ -9,6 +9,8 @@ Online tool to calculate the geodetic height (it is different from height above ``` https://geographiclib.sourceforge.io/cgi-bin/GeodSolve + +https://support.virtual-surveyor.com/support/solutions/articles/1000261349-the-difference-between-ellipsoidal-geoid-and-orthometric-elevations- ``` To calculate the height above the geoid (normal altitude used in the mining industry), use the following equation: @@ -18,8 +20,8 @@ Height above the Geoid = Height above the Ellipsoid - Geodetic Height ``` Where: -- **Height above the Geoid** is the normal altitude used in the mining industry. -- **Height above the Ellipsoid** is the normal height in Fixposition's output. +- **Height above the Geoid** is the normal altitude used in the mining industry. Also called orthometric height +- **Height above the Ellipsoid** is the normal height in Fixposition's output. Also called ellipsoidal height - **Geodetic Height** is the height above the geoid model. To achieve this, leveraging GeographicLib is a good option. Below are the steps to consider for enabling this in the Fixposition driver ROS1 (Enable ROS2 is in the todo list):