diff --git a/.github/workflows/build_test_ros.yml b/.github/workflows/build_test_ros.yml index 53b0a31..1445c7d 100644 --- a/.github/workflows/build_test_ros.yml +++ b/.github/workflows/build_test_ros.yml @@ -29,11 +29,6 @@ jobs: - uses: actions/checkout@v4 with: path: src/fixposition_driver - - name: Checkout Deps - uses: actions/checkout@v4 - with: - repository: fixposition/fixposition_gnss_tf - path: src/fixposition_gnss_tf - name: Ignore ROS2 node run: | touch src/fixposition_driver/fixposition_driver_ros2/CATKIN_IGNORE diff --git a/.github/workflows/build_test_ros2.yml b/.github/workflows/build_test_ros2.yml index e593dee..9d810be 100644 --- a/.github/workflows/build_test_ros2.yml +++ b/.github/workflows/build_test_ros2.yml @@ -32,11 +32,6 @@ jobs: - uses: actions/checkout@v4 with: path: src/fixposition_driver - - name: Checkout Deps - uses: actions/checkout@v4 - with: - repository: fixposition/fixposition_gnss_tf - path: src/fixposition_gnss_tf - name: Ignore ROS1 node run: | touch src/fixposition_driver/fixposition_driver_ros1/COLCON_IGNORE @@ -46,4 +41,3 @@ jobs: source /opt/ros/$ROS_DISTRO/setup.bash colcon build --packages-up-to fixposition_driver_ros2 --cmake-args -DBUILD_TESTING=ON colcon test --packages-up-to fixposition_driver_ros2 - colcon test-result --test-result-base build/fixposition_gnss_tf/ diff --git a/.gitignore b/.gitignore index 92a8124..392ef2b 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,4 @@ +build/* doc_gen/* +*/doc_gen/* *.vscode* diff --git a/README.md b/README.md index 14c10f8..87bd689 100644 --- a/README.md +++ b/README.md @@ -5,281 +5,18 @@ [ROS](https://www.ros.org/) (both ROS1 and ROS2) Driver for [Fixposition Vision-RTK 2](https://www.fixposition.com/product). -The driver is designed to listen on a TCP or Serial port for the [_Fixposition ASCII Messages_](#fixposition-ascii-messages), and then publish them as corresponding ROS messages. At the same time, the driver can also subscribe to a speed input message, which will be sent back to the Vision-RTK 2 sensor and provide an external speed input. +## Driver documentation -- For the output ROS messages, see [Output of the driver](#output-of-the-driver) -- For the input ROS messages for speed input, see [Input Wheelspeed through the driver](#input-wheelspeed-through-the-driver) - -## How to use it - -The code is split in the following 3 parts: - -- `fixposition_driver_lib`: common CMake library to parse [_Fixposition ASCII Messages_](#fixposition-ascii-messages). For more details and build instructions, see [here](fixposition_driver_lib/README.md). -- `fixposition_driver_ros1`: ROS1 driver node to subscribe and publish in the ROS1 framework. For more details and build instructions, see [here](fixposition_driver_ros1/README.md). -- `fixposition_driver_ros2`: ROS2 driver node to subscribe and publish in the ROS2 framework. For more details and build instructions, see [here](fixposition_driver_ros2/README.md). - -### ROS 1 - -For more details and build instructions, see [here](fixposition_driver_ros1/README.md). - -#### Installation - -To install the node, extract / clone the code and `fixposition_gnss_tf` to your catkin workspace's `src` folder: - -```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 -``` - -**Add a file named 'CATKIN_IGNORE' to `fixposition_driver_ros2` folder.** - -make sure you have sourced the setup.bash from ros: - -`/opt/ros/{ROS_DISTRO}/setup.bash`, for example - -``` -source /opt/ros/melodic/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 - -- To launch the node in serial mode, run: - - `roslaunch fixposition_driver_ros1 serial.launch` - -- In TCP mode (Ethernet or Wi-Fi): - - `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. - -### ROS 2 - -For more details and build instructions, see [here](fixposition_driver_ros2/README.md). - -#### Installation - -To install the node, extract / clone the code and `fixposition_gnss_tf` to your catkin workspace's `src` folder: - -```bash -# The folder structure should look like this -fp_public_ws -├── src -│ ├── fixposition_driver -│ │ ├── fixposition_driver_lib -│ │ ├── fixposition_driver_ros1 # will be ignore by colcon when building for ROS2 -│ │ ├── fixposition_driver_ros2 -│ ├── fixposition_gnss_tf -``` - -**Add a file named 'COLCON_IGNORE' to `fixposition_driver_ros1` folder.** - -make sure you have sourced the setup.bash from ros: - -`/opt/ros/{ROS_DISTRO}/setup.bash`, for example - -``` -source /opt/ros/foxy/setup.bash -``` - -and build it with: - -`colcon build --packages-up-to fixposition_driver_ros2` - -This will build the ROS2 driver node and all its dependencies. - -Then source your environment after the build: - -`source install/setup.bash` - -#### Launch the Driver - -- To launch the node in serial mode, run: - - `ros2 launch fixposition_driver_ros2 serial.launch` - -- In TCP mode (Ethernet or Wi-Fi): - - `ros2 launch fixposition_driver_ros2 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 and read this note below.** - -> [!NOTE] -> ROS2, unlike ROS1, by default uses a `install` directory in the workspace. So when you do `ros2 launch xxx`, the configuration and launch files are taken from the `install` and not directly from the `src` directory. -> -> If you want to modify the parameters in the YAML files. You can: -> - Modify the YAML file in the `src` directory and then re-run `colcon build --packages-up-to fixposition_driver_ros2` to update them into the `install` directory. -> -> or -> - Modify the YAML file in `install`. However, the next time you do `colcon build` they will be overriden by the files in `src`. - - -## Output of the driver - -### Messages and TF tree - -The output is published on the following: - -#### Vision-RTK2 I/O messages - -- From **FP_A-**, at the configured frequency (default 10Hz, output generator -> Fusion frequency): - - | Topic | Message Type | I/O message | Frequency | Description | - | --------------------------- | ----------------------------- | ------------- | ------------------------------ | --------------------------------------------- | - | `/fixposition/fpa/odometry` | `fixposition_msgs/odometry` | FP_A-ODOMETRY | as configured on web-interface | https://docs.fixposition.com/fd/fp_a-odometry | - | `/fixposition/fpa/llh` | `fixposition_msgs/llh` | FP_A-LLH | as configured on web-interface | https://docs.fixposition.com/fd/fp_a-llh | - | `/fixposition/fpa/odomenu` | `fixposition_msgs/odomenu` | FP_A-ODOMENU | as configured on web-interface | https://docs.fixposition.com/fd/fp_a-odomenu | - | `/fixposition/fpa/odomsh` | `fixposition_msgs/odomsh` | FP_A-ODOMSH | as configured on web-interface | https://docs.fixposition.com/fd/fp_a-odomsh | - | `/fixposition/fpa/gnssant` | `fixposition_msgs/gnssant` | FP_A-GNSSANT | as configured on web-interface | https://docs.fixposition.com/fd/fp_a-gnssant | - | `/fixposition/fpa/gnsscorr` | `fixposition_msgs/gnsscorr` | FP_A-GNSSCORR | as configured on web-interface | https://docs.fixposition.com/fd/fp_a-gnsscorr | - | `/fixposition/fpa/text` | `fixposition_msgs/text` | FP_A-TEXT | as configured on web-interface | https://docs.fixposition.com/fd/fp_a-text | - -- From **NMEA-**, at the configured frequency (default 5Hz): - - | Topic | Message Type | I/O message | Frequency | Description | - | ------------------------- | ------------------------ | ----------- | ------------------------------ | ------------------------------------------- | - | `/fixposition/nmea/gpgga` | `fixposition_msgs/gpgga` | NMEA-GP-GGA | as configured on web-interface | https://docs.fixposition.com/fd/nmea-gp-gga | - | `/fixposition/nmea/gpgll` | `fixposition_msgs/gpgll` | NMEA-GP-GLL | as configured on web-interface | https://docs.fixposition.com/fd/nmea-gp-gll | - | `/fixposition/nmea/gngsa` | `fixposition_msgs/gngsa` | NMEA-GN-GSA | as configured on web-interface | https://docs.fixposition.com/fd/nmea-gp-gsa | - | `/fixposition/nmea/gpgst` | `fixposition_msgs/gpgst` | NMEA-GP-GST | as configured on web-interface | https://docs.fixposition.com/fd/nmea-gp-gst | - | `/fixposition/nmea/gxgsv` | `fixposition_msgs/gxgsv` | NMEA-GX-GSV | as configured on web-interface | https://docs.fixposition.com/fd/nmea-gp-gsv | - | `/fixposition/nmea/gphdt` | `fixposition_msgs/gphdt` | NMEA-GP-HDT | as configured on web-interface | https://docs.fixposition.com/fd/nmea-gp-hdt | - | `/fixposition/nmea/gprmc` | `fixposition_msgs/gprmc` | NMEA-GP-RMC | as configured on web-interface | https://docs.fixposition.com/fd/nmea-gp-rmc | - | `/fixposition/nmea/gpvtg` | `fixposition_msgs/gpvtg` | NMEA-GP-VTG | as configured on web-interface | https://docs.fixposition.com/fd/nmea-gp-vtg | - | `/fixposition/nmea/gpzda` | `fixposition_msgs/gpzda` | NMEA-GP-ZDA | as configured on web-interface | https://docs.fixposition.com/fd/nmea-gp-zda | - -- In addition, from **NMEA-**, at the configured frequency (default 5Hz): - - | Topic | Message Type | Frequency | Description | - | ------------------- | ------------------------ | ------------------------------ | --------------------------------------------------------------------------------- | - | `/fixposition/nmea` | `fixposition_msgs/nmea` | as configured on web-interface | Collects and combines all NMEA messages and outputs at the end of the GNSS epoch. | - -#### Vision-RTK2 Fusion - -- From **FP_A-ODOMETRY**, at the configured frequency (default 10Hz, output generator -> Fusion frequency): - - - Messages - - | Topic | Message Type | Frequency | Description | - | --------------------------- | ------------------------- | ------------------------------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------- | - | `/fixposition/odom_ecef` | `nav_msgs/Odometry` | as configured on web-interface | Position, Orientation from ECEF to FP_POI, Velocity and Angular Velocity in FP_POI. | - | `/fixposition/odom_llh` | `sensor_msgs/NavSatFix` | as configured on web-interface | Position from LLH (Latitude, Longitude and Height) to FP_POI. Based on WGS-84 ellipsoid. | - | `/fixposition/odom_enu` | `nav_msgs/Odometry` | as configured on web-interface | Position, Orientation from ENU0 to FP_POI, Velocity and Angular Velocity in FP_POI. | - | `/fixposition/odom_smooth` | `nav_msgs/Odometry` | as configured on web-interface | Position, Orientation from ECEF to FP_POISH, Velocity and Angular Velocity in FP_POI. Based on smooth odometry output. | - | `/fixposition/poiimu` | `sensor_msgs/Imu` | as configured on web-interface | Bias Corrected acceleration and rotation rate in FP_POI. | - | `/fixposition/ypr` | `geometry_msgs/Vector3` | as configured on web-interface | x = Yaw, y = Pitch, z = Roll in radian. Euler angles representation of rotation between ENU and FP_POI. Only available after fusion initialization. | - -#### Vision-RTK2 GNSS Antenna Positions - -**If GNSS Antenna positions are needed, please enable this on the sensor's configuration interface.** - -- From **NOV_B-BESTGNSSPOS_GNSS[1,2]**, at the configured frequency, GNSS1 and GNSS2 raw antenna positions (default 5Hz): - - | Topic | Message Type | Frequency | Description | - | -------------------- | ----------------------- | ------------------------------ | ------------------------------ | - | `/fixposition/gnss1` | `sensor_msgs/NavSatFix` | as configured on web-interface | Latitude, Longitude and Height | - | `/fixposition/gnss2` | `sensor_msgs/NavSatFix` | as configured on web-interface | Latitude, Longitude and Height | - -- From **NMEA-GP-GGA_GNSS**, **NMEA-GP-RMC_GNSS**, and **NMEA-GP-ZDA_GNSS**, at the configured frequency (default 5Hz): The Vision-RTK2 can also output an average GNSS-based LLH position (i.e., **only GNSS, not Fusion**) and heading estimate based on speed over ground (**SOG**) and course over ground (**COG**) - (i.e., **the platform must be moving for it to be accurate**) by utilizing several NMEA messages, which serves as an auxiliary output until Fusion is fully initialized. This message's output frequency equals the lowest frequency of any of these three message types. **Note: This output should only be used until Fusion is fully initialized.** Warning: This topic will only be populated when the sampling rate of all required NMEA messages is 1 (i.e., the default output frequency of the messages). - - | Topic | Message Type | Frequency | Description | - | -------------------- | ----------------------- | ------------------------------ | ------------------------------ | - | `/fixposition/nmea` | `fixposition_driver/NMEA` | as configured on web-interface | Latitude, Longitude and Height | - -#### Vision-RTK2 IMU data - -- From **FP_A-RAWIMU**, at 200Hz: - - | Topic | Message Type | Frequency | Description | - | --------------------- | ----------------- | --------- | ----------------------------------------------------------------------------------------- | - | `/fixposition/rawimu` | `sensor_msgs/Imu` | 200Hz | Raw (without bias correction) IMU acceleration and angular velocity data in FP_VRTK frame | - -- From **FP_A-CORRIMU**, at 200Hz: - - | Topic | Message Type | Frequency | Description | - | ---------------------- | ----------------- | --------- | -------------------------------------------------------------------------- | - | `/fixposition/corrimu` | `sensor_msgs/Imu` | 200Hz | Bias Corrected IMU acceleration and angular velocity data in FP_VRTK frame | - -- From **FP_A-TF_POIIMUH**, at 200Hz: - - | Topic | Message Type | Frequency | Description | - | ------------------------ | ----------------------- | ------------------------------ | ------------------------------ | - | `/fixposition/imu_ypr` | `geometry_msgs/Vector3` | 200Hz | x = 0.0, y = Pitch, z = Roll in radian. Euler angles representation of rotation between a local horizontal frame and P_POI. Rough estimation using IMU alone. | - -#### Transforms - -- TFs: - | Frames | Topic | Message needed to be selected on the web-interface | Frequency | - | ------------------- | ------------ | -------------------------------------------------- | ---------------------------------- | - | `FP_ECEF-->FP_POI` | `/tf` | `ODOMETRY` | as configured on the web-interface | - | `FP_ECEF-->FP_MAP` | `/tf_static` | `ODOMETRY` | 1Hz | - | `FP_ECEF-->FP_ENU` | `/tf` | `ODOMENU` | as configured on the web-interface | - | `FP_ECEF-->FP_ENU0` | `/tf_static` | `ODOMENU` | 1Hz | - | `FP_POI-->FP_POISH` | `/tf` | `ODOMSH` | as configured on the web-interface | - | `FP_POI-->FP_IMUH` | `/tf` | `TF_POIIMUH` | 200Hz | - | `FP_POI-->FP_VRTK` | `/tf_static` | `TF_POI_VRTK` | 1Hz | - | `FP_VRTK-->FP_CAM` | `/tf_static` | `TF_VRTK_CAM` | 1Hz | - - -- ROS TF Tree: - - ```mermaid - graph TD; - FP_ECEF-->FP_POI - FP_POI-->FP_VRTK-->FP_CAM - FP_POI-->FP_IMUH - FP_POI-->FP_POISH - FP_ECEF-->FP_ENU0 - ``` - -_Please note that the corresponding messages also has to be selected on the Fixposition Vision-RTK2's configuration interface._ - -### Explaination of frame ids - -| Frame ID | Explaination | -| ------------ | -------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| **FP_ECEF** | Earth-Center-Earth-Fixed frame. | -| **FP_POI** | Point-Of-Interest, configured from Vision-RTK2's web-interface with respect to the FP_VRTK frame. By default it is the same as FP_VRTK. | -| **FP_VRTK** | The coordinate frame on the Vision-RTK2's housing on the Fixposition-Logo "X". | -| **FP_CAM** | The camera coordinate frame of the Vision-RTK2. | -| **FP_POISH** | Point-Of-Interest of the Smooth Odometry frame, configured from Vision-RTK2's web-interface with respect to the FP_VRTK frame. | -| **FP_IMUH** | A local horizontal frame with the origin at the same location as FP_POI. This frame is a rough estimate determined by the IMU alone. | -| **FP_ENU0** | The **global fixed** East-North-Up coordinate frame with the origin at the configured ENU0 position on the web-interface. Also used as the MAP frame for Rviz. | - -# Input Wheelspeed through the driver - -The fp_ros_driver supports inputting a Speed msg (`msg/Speed.msg`) through the `/fixposition/speed` topic. The Speed msg is defined as a vector of WheelSensor msgs (`msg/WheelSensor.msg`). This message in turn, is intended to be a simplified version of the FP_B-MEASUREMENTS, containing three integers (vx, vy, and vz), three booleans (validity of the three velocity integers), and a string indicating the sensor from which the measurement originates. The integer velocity values should be in [mm/s], to have enough precision when being converted into an integer format. - -Internally, upon arriving to the ros driver, wheelspeed measurements are converted into a full FP_B-MEASUREMENTS message, and sent via the TCP or serial interface to the Vision-RTK2, where they will be further processed. For more details regarding the definition FP_B-MEASUREMENTS message, please refer to [the following page](https://docs.fixposition.com/fd/fp_b-measurements), or to the VRTK2 integration manual. - -# Fixposition Odometry Converter - -This is an extra node is provided to help with the integration of the wheel odometry on your vehicle. For details, see the subfolder [fixposition_odometry_converter_ros1](fixposition_odometry_converter_ros1/README.md) (ROS 1) and [fixposition_odometry_converter_ros2](fixposition_odometry_converter_ros2/README.md) (ROS 2). When building the ROS 1 version add a file named 'CATKIN_IGNORE' to the `fixposition_odometry_converter_ros2` folder. +For installation, usage, and more information, please refer to [Fixposition Docs: ROS Driver](https://docs.fixposition.com/fd/fixposition-ros-driver). ## Fixposition ASCII messages -For more information about the ASCII messages parsed from the Vision-RTK 2, please refer to [Fixposition Docs](https://docs.fixposition.com/fd/i-o-messages). +For more information about the ASCII messages parsed from the Vision-RTK 2, please refer to [Fixposition Docs: I/O messages](https://docs.fixposition.com/fd/i-o-messages). ## Code Documentation Run `doxygen Doxyfile` to generate Doxygen code documentation. -# License +## License This project is licensed under the MIT License - see the [LICENSE](LICENSE) file for details diff --git a/fixposition_driver_lib/CMakeLists.txt b/fixposition_driver_lib/CMakeLists.txt index 5acdadc..9fe0c2d 100644 --- a/fixposition_driver_lib/CMakeLists.txt +++ b/fixposition_driver_lib/CMakeLists.txt @@ -12,10 +12,7 @@ set(CMAKE_EXPORT_COMPILE_COMMANDS ON) find_package(Boost 1.65.0 REQUIRED) find_package(Eigen3 REQUIRED) -find_package(fixposition_gnss_tf REQUIRED) - include_directories(include - ${fixposition_gnss_tf_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIR} ) @@ -46,9 +43,10 @@ add_library( src/fixposition_driver.cpp src/helper.cpp src/parser.cpp + src/gnss_tf.cpp ) -target_link_libraries(${PROJECT_NAME} ${fixposition_gnss_tf_LIBRARIES} ${Boost_LIBRARIES} pthread) +target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES} pthread) # INSTALL ============================================================================================================== list(APPEND PACKAGE_LIBRARIES ${PROJECT_NAME}) diff --git a/fixposition_driver_lib/Doxyfile b/fixposition_driver_lib/Doxyfile index f540bc9..87bffb6 100644 --- a/fixposition_driver_lib/Doxyfile +++ b/fixposition_driver_lib/Doxyfile @@ -1471,7 +1471,7 @@ FORMULA_TRANSPARENT = YES # The default value is: NO. # This tag requires that the tag GENERATE_HTML is set to YES. -USE_MATHJAX = NO +USE_MATHJAX = YES # When MathJax is enabled you can set the default output format to be used for # the MathJax output. See the MathJax site (see: diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/gnss_tf.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/gnss_tf.hpp new file mode 100644 index 0000000..4a7fe13 --- /dev/null +++ b/fixposition_driver_lib/include/fixposition_driver_lib/gnss_tf.hpp @@ -0,0 +1,164 @@ +/** + * @file + * @brief Helper functions + * + * \verbatim + * ___ ___ + * \ \ / / + * \ \/ / Fixposition AG + * / /\ \ All right reserved. + * /__/ \__\ + * \endverbatim + * + */ + +#ifndef __FIXPOSITION_DRIVER_LIB_GNSS_TF__ +#define __FIXPOSITION_DRIVER_LIB_GNSS_TF__ + +/* PACKAGE */ +#include + +namespace fixposition { + +/** + * @brief Calculate the rotation matrix from ECEF to + * ENU for a given reference latitude/longitude. + * + * @param[in] lat Reference latitude [rad] + * @param[in] lon Reference longitude [rad] + * @return Eigen::Matrix3d Rotation matrix from ECEF -> ENU + */ +Eigen::Matrix3d RotEnuEcef(double lat, double lon); + +/** + * @brief Calculate the rotation matrix from ECEF to + * ENU for a given reference origin. + * + * @param[in] in_pos Reference position in ECEF [m] + * @return Eigen::Matrix3d Rotation matrix ECEF -> ENU + */ +Eigen::Matrix3d RotEnuEcef(const Eigen::Vector3d &ecef); + +/** + * @brief Returns rotation matrix between NED and ENU + * @details | 0, 1, 0 | + * | 1, 0, 0 | + * | 0, 0,-1 | + * + * @return Eigen::Matrix3d + */ +Eigen::Matrix3d RotNedEnu(); + +/** + * @brief Calculate the rotation matrix from ECEF to + * NED for a given reference latitude/longitude. + * + * @param[in] lat Reference latitude [rad] + * @param[in] lon Reference longitude [rad] + * @return Eigen::Matrix3d Rotation matrix from ECEF -> NED + */ +Eigen::Matrix3d RotNedEcef(double lat, double lon); + +/** + * @brief Calculate the rotation matrix from ECEF to + * NED for a given reference origin. + * + * @param[in] in_pos Reference position in ECEF [m] + * @return Eigen::Matrix3d Rotation matrix ECEF -> NED + */ +Eigen::Matrix3d RotNedEcef(const Eigen::Vector3d &ecef); + +/** + * @brief Transform ECEF coordinate to ENU with specified ENU-origin + * + * @param[in] xyz ECEF position to be transsformed [m] + * @param[in] wgs84llh_ref ENU-origin in Geodetic coordinates (Lat[rad], Lon[rad], Height[m]) + * @return Eigen::Vector3d Position in ENU coordinates + */ +Eigen::Vector3d TfEnuEcef(const Eigen::Vector3d &ecef, const Eigen::Vector3d &wgs84llh_ref); + +/** + * @brief Transform ENU coordinate to ECEF with specified ENU-origin + * + * @param[in] enu ENU position to be transsformed [m] + * @param[in] wgs84llh_ref ENU-origin in Geodetic coordinates (Lat[rad], Lon[rad], Height[m]) + * @return Eigen::Vector3d + */ +Eigen::Vector3d TfEcefEnu(const Eigen::Vector3d &enu, const Eigen::Vector3d &wgs84llh_ref); + +Eigen::Vector3d TfNedEcef(const Eigen::Vector3d &ecef, const Eigen::Vector3d &wgs84llh_ref); +Eigen::Vector3d TfEcefNed(const Eigen::Vector3d &ned, const Eigen::Vector3d &wgs84llh_ref); + +/** + * @brief Convert Geodetic coordinates (latitude, longitude, height) to + * ECEF (x, y, z). + * + * @param[in] wgs84llh Geodetic coordinates (Lat[rad], Lon[rad], Height[m]) + * @return Eigen::Vector3d ECEF coordinates [m] + */ +Eigen::Vector3d TfEcefWgs84Llh(const Eigen::Vector3d &wgs84llh); + +/** + * @brief Convert ECEF (x, y, z) coordinates to Lat Lon Height coordinates + * (latitude, longitude, altitude). + * + * @param[in] ecef ECEF coordinates [m] + * @return Eigen::Vector3d Geodetic coordinates (Lat[rad], Lon[rad], Height[m]) + */ +Eigen::Vector3d TfWgs84LlhEcef(const Eigen::Vector3d &ecef); + +/** + * @brief Given Pose in ECEF, calculate Yaw-Pitch-Roll in ENU + * @details Yaw will be -Pi/2 when X is pointing North, because ENU starts with East + * + * @param ecef_p 3D Position Vector in ECEF + * @param ecef_r 3x3 Rotation matrix representing rotation Body --> ECEF + * @return Eigen::Vector3d Yaw-Pitch-Roll in ENU + * (Yaw will be -Pi/2 when X is pointing North, because ENU starts with + + */ +Eigen::Vector3d EcefPoseToEnuEul(const Eigen::Vector3d &ecef_p, const Eigen::Matrix3d &ecef_r); + +/** + * @brief + * + * @param ecef_p 3D Position Vector in ECEF + * @param ecef_r 3x3 Rotation matrix representing rotation Body --> ECEF + * @return Eigen::Vector3d Yaw-Pitch-Roll in NED + */ +Eigen::Vector3d EcefPoseToNedEul(const Eigen::Vector3d &ecef_p, const Eigen::Matrix3d &ecef_r); + +/** + * @brief Vector4 quaternion to intrinsic Euler Angles in ZYX (yaw,pitch,roll) + * + * @param[in] quat Eigen::Quaterniond in w,i,j,k + * @return Eigen::Vector3d intrinsic euler angle in ZYX (Yaw/Pitch/Roll) order + */ +inline Eigen::Vector3d QuatToEul(const Eigen::Quaterniond &q) { + auto qw = q.w(); + auto qx = q.x(); + auto qy = q.y(); + auto qz = q.z(); + auto eul0 = atan2(2 * (qx * qy + qw * qz), qw * qw + qx * qx - qy * qy - qz * qz); + auto eul1 = asin(-2.0 * (qx * qz - qw * qy)); + auto eul2 = atan2(2 * (qy * qz + qw * qx), qw * qw - qx * qx - qy * qy + qz * qz); + + Eigen::Vector3d eul; + eul << eul0, eul1, eul2; + + return eul; +} + +/** + * @brief Rotation Matrix to intrinsic Euler Angles in ZYX (Yaw-Pitch-Roll) order in radian + * + * @param rot Eigen::Matrix3d + * @return Eigen::Vector3d intrinsic euler angle in ZYX (Yaw-Pitch-Roll) order in radian + */ +inline Eigen::Vector3d RotToEul(const Eigen::Matrix3d &rot) { + const Eigen::Quaterniond quat(rot); + return QuatToEul(quat); +} + +} // namespace fixposition +#endif // __FIXPOSITION_DRIVER_LIB_GNSS_TF__ diff --git a/fixposition_driver_lib/package.xml b/fixposition_driver_lib/package.xml index 62ec541..09ca91a 100644 --- a/fixposition_driver_lib/package.xml +++ b/fixposition_driver_lib/package.xml @@ -20,7 +20,6 @@ geometry_msgs message_generation message_runtime - fixposition_gnss_tf cmake diff --git a/fixposition_driver_lib/src/gnss_tf.cpp b/fixposition_driver_lib/src/gnss_tf.cpp new file mode 100644 index 0000000..309d32a --- /dev/null +++ b/fixposition_driver_lib/src/gnss_tf.cpp @@ -0,0 +1,197 @@ +/** + * @file + * @brief Helper functions + * + * \verbatim + * ___ ___ + * \ \ / / + * \ \/ / Fixposition AG + * / /\ \ All right reserved. + * /__/ \__\ + * \endverbatim + * + */ + +/* PACKAGE */ +#include + +namespace fixposition { + +namespace Constants { +// static constexpr values representing the earth WGS84 +// https://en.wikipedia.org/wiki/World_Geodetic_System#Defining_Parameters +static constexpr double wgs84_a = 6378137.0; //!< earth radius major axis [m] +static constexpr double wgs84_b = 6356752.314245; //!< earth radius minor axis [m] +static constexpr double wgs84_inv_f = 298.257223563; //!< 1/f inverse of flattening parameter +static constexpr double wgs84_e_2 = 6.69437999014e-3; //!< first eccentricity Squared + +static constexpr double wgs84_a_2 = wgs84_a * wgs84_a; //!< a_^2 +static constexpr double wgs84_b_2 = wgs84_b * wgs84_b; //!< a_^2 +static constexpr double wgs84_e_prime_2 = wgs84_a_2 / wgs84_b_2 - 1; //!< e'^2 second eccentricity squared +} // namespace Constants + +/** + * @details + * + * \f[ + * + * \text{Rotate a vector from ECEF to ENU:} \\ + * V_{ENU} = M \cdot V_{ECEF} \\ + * \text{Rotate a covariance matrix from ECEF to ENU:} \\ + * Cov_{ENU} = M \cdot Cov_{ECEF} \cdot M^{T} \\ + * + * \f] + * + * Reference https://gssc.esa.int/navipedia/index.php/Transformations_between_ECEF_and_ENU_coordinates + * + */ +Eigen::Matrix3d RotEnuEcef(double lat, double lon) { + const double s_lon = sin(lon); + const double c_lon = cos(lon); + const double s_lat = sin(lat); + const double c_lat = cos(lat); + const double m00 = -s_lon; + const double m01 = c_lon; + const double m02 = 0; + const double m10 = -c_lon * s_lat; + const double m11 = -s_lon * s_lat; + const double m12 = c_lat; + const double m20 = c_lon * c_lat; + const double m21 = s_lon * c_lat; + const double m22 = s_lat; + + Eigen::Matrix3d res; + // This initializes the matrix row by row + res << m00, m01, m02, m10, m11, m12, m20, m21, m22; + + return res; +} + +/** + * @details + * + * \f[ + * + * \text{Rotate a vector from ECEF to ENU:} \\ + * V_{ENU} = M \cdot V_{ECEF} \\ + * \text{Rotate a covariance matrix from ECEF to ENU:} \\ + * Cov_{ENU} = M \cdot Cov_{ECEF} \cdot M^{T} \\ + * + * \f] + * + */ +Eigen::Matrix3d RotEnuEcef(const Eigen::Vector3d &ecef) { + const Eigen::Vector3d wgs84llh = TfWgs84LlhEcef(ecef); + return RotEnuEcef(wgs84llh.x(), wgs84llh.y()); +} + +Eigen::Matrix3d RotNedEnu() { + /** + * | 0, 1, 0 | + * | 1, 0, 0 | + * | 0, 0,-1 | + * + */ + + Eigen::Matrix3d rot_ned_enu; + rot_ned_enu << 0, 1, 0, 1, 0, 0, 0, 0, -1; + return rot_ned_enu; +} + +Eigen::Matrix3d RotNedEcef(double lat, double lon) { return RotNedEnu() * RotEnuEcef(lat, lon); } + +Eigen::Matrix3d RotNedEcef(const Eigen::Vector3d &ecef) { + const Eigen::Vector3d wgs84llh = TfWgs84LlhEcef(ecef); + return RotNedEcef(wgs84llh.x(), wgs84llh.y()); +} + +Eigen::Vector3d TfEnuEcef(const Eigen::Vector3d &ecef, const Eigen::Vector3d &wgs84llh_ref) { + const Eigen::Vector3d ecef_ref = TfEcefWgs84Llh(wgs84llh_ref); + return RotEnuEcef(wgs84llh_ref.x(), wgs84llh_ref.y()) * (ecef - ecef_ref); +} + +Eigen::Vector3d TfEcefEnu(const Eigen::Vector3d &enu, const Eigen::Vector3d &wgs84llh_ref) { + const Eigen::Vector3d ecef_ref = TfEcefWgs84Llh(wgs84llh_ref); + return ecef_ref + RotEnuEcef(wgs84llh_ref.x(), wgs84llh_ref.y()).transpose() * enu; +} + +Eigen::Vector3d TfNedEcef(const Eigen::Vector3d &ecef, const Eigen::Vector3d &wgs84llh_ref) { + const Eigen::Vector3d ecef_ref = TfEcefWgs84Llh(wgs84llh_ref); + return RotNedEcef(wgs84llh_ref.x(), wgs84llh_ref.y()) * (ecef - ecef_ref); +} + +Eigen::Vector3d TfEcefNed(const Eigen::Vector3d &ned, const Eigen::Vector3d &wgs84llh_ref) { + const Eigen::Vector3d ecef_ref = TfEcefWgs84Llh(wgs84llh_ref); + return ecef_ref + RotNedEcef(wgs84llh_ref.x(), wgs84llh_ref.y()).transpose() * ned; +} +/** + * @details Implementation based on paper + * https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#The_application_of_Ferrari's_solution + * + */ +Eigen::Vector3d TfWgs84LlhEcef(const Eigen::Vector3d &ecef) { + const double x = ecef.x(); + const double y = ecef.y(); + const double z = ecef.z(); + + const double x_2 = x * x; //!< x^2 + const double y_2 = y * y; //!< y^2 + const double z_2 = z * z; //!< z^2 + + const double r_2 = (x_2 + y_2); //!< r^2 + const double r = sqrt(r_2); //!< r + + const double F = 54.0 * Constants::wgs84_b_2 * z_2; + const double G = + r_2 + (1 - Constants::wgs84_e_2) * z_2 - Constants::wgs84_e_2 * (Constants::wgs84_a_2 - Constants::wgs84_b_2); + const double c = Constants::wgs84_e_2 * Constants::wgs84_e_2 * F * r_2 / (G * G * G); + const double s = cbrt(1 + c + sqrt(c * c + 2 * c)); + const double P = F / (3.0 * (s + 1.0 + 1.0 / s) * (s + 1.0 + 1.0 / s) * G * G); + const double Q = sqrt(1 + 2 * Constants::wgs84_e_2 * Constants::wgs84_e_2 * P); + const double r0 = -P * Constants::wgs84_e_2 * r / (1 + Q) + + sqrt(0.5 * Constants::wgs84_a_2 * (1.0 + 1.0 / Q) - + (P * (1 - Constants::wgs84_e_2) * z_2 / (Q + Q * Q)) - 0.5 * P * r_2); + const double t1 = (r - Constants::wgs84_e_2 * r0); + const double t1_2 = t1 * t1; + const double U = sqrt(t1_2 + z_2); + const double V = sqrt(t1_2 + (1 - Constants::wgs84_e_2) * z_2); + const double a_V = Constants::wgs84_a * V; + const double z0 = Constants::wgs84_b_2 * z / a_V; + + const double h = U * (1 - Constants::wgs84_b_2 / a_V); + const double lat = atan((z + Constants::wgs84_e_prime_2 * z0) / r); + const double lon = atan2(y, x); + + return Eigen::Vector3d(lat, lon, h); +} + +Eigen::Vector3d TfEcefWgs84Llh(const Eigen::Vector3d &wgs84llh) { + const double lat = wgs84llh.x(); + const double lon = wgs84llh.y(); + const double height = wgs84llh.z(); + const double s_lat = sin(lat); + const double c_lat = cos(lat); + const double s_lon = sin(lon); + const double c_lon = cos(lon); + // N is in meters + const double n = Constants::wgs84_a / sqrt(1.0 - Constants::wgs84_e_2 * s_lat * s_lat); + const double n_plus_height = n + height; + + Eigen::Vector3d ecef; + ecef.x() = n_plus_height * c_lat * c_lon; + ecef.y() = n_plus_height * c_lat * s_lon; + ecef.z() = (n * (1 - Constants::wgs84_e_2) + height) * s_lat; + + return ecef; +} + +Eigen::Vector3d EcefPoseToEnuEul(const Eigen::Vector3d &ecef_p, const Eigen::Matrix3d &ecef_r) { + //! Rotation Matrix to convert to ENU frame + const Eigen::Matrix3d rot_enu_ecef = RotEnuEcef(ecef_p); + //! Convert the Pose into ENU frame + const Eigen::Matrix3d rot_enu_body = rot_enu_ecef * ecef_r; + //! Convert Rotation Matrix into Yaw-Pitch-Roll + return RotToEul(rot_enu_body); +} + +} // namespace fixposition diff --git a/fixposition_driver_ros1/CMakeLists.txt b/fixposition_driver_ros1/CMakeLists.txt index 1cec5e9..09f643f 100644 --- a/fixposition_driver_ros1/CMakeLists.txt +++ b/fixposition_driver_ros1/CMakeLists.txt @@ -11,7 +11,6 @@ set(CMAKE_EXPORT_COMPILE_COMMANDS ON) # DEPENDENCIES ========================================================================================================= find_package(Boost 1.65.0 REQUIRED) find_package(Eigen3 REQUIRED) -find_package(fixposition_gnss_tf REQUIRED) find_package(fixposition_driver_lib REQUIRED) find_package(catkin REQUIRED COMPONENTS roscpp @@ -65,7 +64,6 @@ catkin_package( include_directories( include ${catkin_INCLUDE_DIRS} - ${fixposition_gnss_tf_INCLUDE_DIRS} ${fixposition_driver_lib_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIR} @@ -82,7 +80,6 @@ add_executable( target_link_libraries( ${PROJECT_NAME} ${catkin_LIBRARIES} - ${fixposition_gnss_tf_LIBRARIES} ${fixposition_driver_lib_LIBRARIES} ${Boost_LIBRARIES} pthread diff --git a/fixposition_driver_ros1/README.md b/fixposition_driver_ros1/README.md index e6ea9e4..726d789 100644 --- a/fixposition_driver_ros1/README.md +++ b/fixposition_driver_ros1/README.md @@ -1,248 +1,21 @@ -# Fixposition Driver ROS1 +# Fixposition ROS1 Driver -[ROS](https://www.ros.org/) (both ROS1 and ROS2) Driver for [Fixposition Vision-RTK 2](https://www.fixposition.com/product) +- [ROS1 Noetic ![](./../../actions/workflows/build_test_ros.yml/badge.svg)](./../../actions/workflows/build_test_ros.yml) -## Dependencies +[ROS](https://www.ros.org/) Driver for [Fixposition Vision-RTK 2](https://www.fixposition.com/product). -- [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 +## Driver documentation -- **[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 - -To install the node, extract / clone the code and `fixposition_gnss_tf` to your catkin workspace's `src` folder: - -```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/melodic/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 - -- To launch the node in serial mode, run: - - `roslaunch fixposition_driver_ros1 serial.launch` - -- In TCP mode (Ethernet or Wi-Fi): - - `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/odom_ecef - /fixposition/odom_llh - /fixposition/odom_enu - /fixposition/odom_smooth - /fixposition/vrtk - /fixposition/poiimu - /fixposition/rawimu - /fixposition/corrimu - ``` - -- Check published message in the topic, for example the VRTK message: - `rostopic echo /fixposition/vrtk` - - -## Output of the driver - -### Messages and TF tree - -The output is published on the following: - -#### Vision-RTK2 I/O messages - -- From **FP_A-**, at the configured frequency (default 10Hz, output generator -> Fusion frequency): - - | Topic | Message Type | I/O message | Frequency | Description | - | --------------------------- | ----------------------------- | ------------- | ------------------------------ | --------------------------------------------- | - | `/fixposition/fpa/odometry` | `fixposition_msgs/odometry` | FP_A-ODOMETRY | as configured on web-interface | https://docs.fixposition.com/fd/fp_a-odometry | - | `/fixposition/fpa/llh` | `fixposition_msgs/llh` | FP_A-LLH | as configured on web-interface | https://docs.fixposition.com/fd/fp_a-llh | - | `/fixposition/fpa/odomenu` | `fixposition_msgs/odomenu` | FP_A-ODOMENU | as configured on web-interface | https://docs.fixposition.com/fd/fp_a-odomenu | - | `/fixposition/fpa/odomsh` | `fixposition_msgs/odomsh` | FP_A-ODOMSH | as configured on web-interface | https://docs.fixposition.com/fd/fp_a-odomsh | - | `/fixposition/fpa/gnssant` | `fixposition_msgs/gnssant` | FP_A-GNSSANT | as configured on web-interface | https://docs.fixposition.com/fd/fp_a-gnssant | - | `/fixposition/fpa/gnsscorr` | `fixposition_msgs/gnsscorr` | FP_A-GNSSCORR | as configured on web-interface | https://docs.fixposition.com/fd/fp_a-gnsscorr | - | `/fixposition/fpa/text` | `fixposition_msgs/text` | FP_A-TEXT | as configured on web-interface | https://docs.fixposition.com/fd/fp_a-text | - -- From **NMEA-**, at the configured frequency (default 5Hz): - - | Topic | Message Type | I/O message | Frequency | Description | - | ------------------------- | ------------------------ | ----------- | ------------------------------ | ------------------------------------------- | - | `/fixposition/nmea/gpgga` | `fixposition_msgs/gpgga` | NMEA-GP-GGA | as configured on web-interface | https://docs.fixposition.com/fd/nmea-gp-gga | - | `/fixposition/nmea/gpgll` | `fixposition_msgs/gpgll` | NMEA-GP-GLL | as configured on web-interface | https://docs.fixposition.com/fd/nmea-gp-gll | - | `/fixposition/nmea/gngsa` | `fixposition_msgs/gngsa` | NMEA-GN-GSA | as configured on web-interface | https://docs.fixposition.com/fd/nmea-gp-gsa | - | `/fixposition/nmea/gpgst` | `fixposition_msgs/gpgst` | NMEA-GP-GST | as configured on web-interface | https://docs.fixposition.com/fd/nmea-gp-gst | - | `/fixposition/nmea/gxgsv` | `fixposition_msgs/gxgsv` | NMEA-GX-GSV | as configured on web-interface | https://docs.fixposition.com/fd/nmea-gp-gsv | - | `/fixposition/nmea/gphdt` | `fixposition_msgs/gphdt` | NMEA-GP-HDT | as configured on web-interface | https://docs.fixposition.com/fd/nmea-gp-hdt | - | `/fixposition/nmea/gprmc` | `fixposition_msgs/gprmc` | NMEA-GP-RMC | as configured on web-interface | https://docs.fixposition.com/fd/nmea-gp-rmc | - | `/fixposition/nmea/gpvtg` | `fixposition_msgs/gpvtg` | NMEA-GP-VTG | as configured on web-interface | https://docs.fixposition.com/fd/nmea-gp-vtg | - | `/fixposition/nmea/gpzda` | `fixposition_msgs/gpzda` | NMEA-GP-ZDA | as configured on web-interface | https://docs.fixposition.com/fd/nmea-gp-zda | - -- In addition, from **NMEA-**, at the configured frequency (default 5Hz): - - | Topic | Message Type | Frequency | Description | - | ------------------- | ------------------------ | ------------------------------ | --------------------------------------------------------------------------------- | - | `/fixposition/nmea` | `fixposition_msgs/nmea` | as configured on web-interface | Collects and combines all NMEA messages and outputs at the end of the GNSS epoch. | - -#### Vision-RTK2 Fusion - -- From **FP_A-ODOMETRY**, at the configured frequency (default 10Hz, output generator -> Fusion frequency): - - - Messages - - | Topic | Message Type | Frequency | Description | - | --------------------------- | ------------------------- | ------------------------------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------- | - | `/fixposition/odom_ecef` | `nav_msgs/Odometry` | as configured on web-interface | Position, Orientation from ECEF to FP_POI, Velocity and Angular Velocity in FP_POI | - | `/fixposition/odom_llh` | `sensor_msgs/NavSatFix` | as configured on web-interface | Position from LLH (Latitude, Longitude and Height) to FP_POI. Based on WGS-84 ellipsoid. | - | `/fixposition/odom_enu` | `nav_msgs/Odometry` | as configured on web-interface | Position, Orientation from ENU0 to FP_POI, Velocity and Angular Velocity in FP_POI | - | `/fixposition/odom_smooth` | `nav_msgs/Odometry` | as configured on web-interface | Position, Orientation from ECEF to FP_POISH, Velocity and Angular Velocity in FP_POI. Based on smooth odometry output. | - | `/fixposition/vrtk` | `fixposition_driver/VRTK` | as configured on web-interface | Custom Message containing same Odometry information as well as status flags | - | `/fixposition/poiimu` | `sensor_msgs/Imu` | as configured on web-interface | Bias Corrected acceleration and rotation rate in FP_POI | - | `/fixposition/ypr` | `geometry_msgs/Vector3` | as configured on web-interface | x = Yaw, y = Pitch, z = Roll in radian. Euler angles representation of rotation between ENU and FP_POI. Only available after fusion initialization. | - -#### Vision-RTK2 GNSS Antenna Positions - -**If GNSS Antenna positions are needed, please enable this on the sensor's configuration interface.** - -- From **NOV_B-BESTGNSSPOS_GNSS[1,2]**, at the configured frequency, GNSS1 and GNSS2 raw antenna positions (default 5Hz): - - | Topic | Message Type | Frequency | Description | - | -------------------- | ----------------------- | ------------------------------ | ------------------------------ | - | `/fixposition/gnss1` | `sensor_msgs/NavSatFix` | as configured on web-interface | Latitude, Longitude and Height | - | `/fixposition/gnss2` | `sensor_msgs/NavSatFix` | as configured on web-interface | Latitude, Longitude and Height | - -- From **NMEA-GP-GGA_GNSS**, **NMEA-GP-RMC_GNSS**, and **NMEA-GP-ZDA_GNSS**, at the configured frequency (default 5Hz): The Vision-RTK2 can also output an average GNSS-based LLH position (i.e., **only GNSS, not Fusion**) and heading estimate based on speed over ground (**SOG**) and course over ground (**COG**) - (i.e., **the platform must be moving for it to be accurate**) by utilizing several NMEA messages, which serves as an auxiliary output until Fusion is fully initialized. This message's output frequency equals the lowest frequency of any of these three message types. **Note: This output should only be used until Fusion is fully initialized.** Warning: This topic will only be populated when the sampling rate of all required NMEA messages is 1 (i.e., the default output frequency of the messages). - - | Topic | Message Type | Frequency | Description | - | -------------------- | ----------------------- | ------------------------------ | ------------------------------ | - | `/fixposition/nmea` | `fixposition_driver/NMEA` | as configured on web-interface | Latitude, Longitude and Height | - -#### Vision-RTK2 IMU data - -- From **FP_A-RAWIMU**, at 200Hz: - - | Topic | Message Type | Frequency | Description | - | --------------------- | ----------------- | --------- | ----------------------------------------------------------------------------------------- | - | `/fixposition/rawimu` | `sensor_msgs/Imu` | 200Hz | Raw (without bias correction) IMU acceleration and angular velocity data in FP_VRTK frame | - -- From **FP_A-CORRIMU**, at 200Hz: - - | Topic | Message Type | Frequency | Description | - | ---------------------- | ----------------- | --------- | -------------------------------------------------------------------------- | - | `/fixposition/corrimu` | `sensor_msgs/Imu` | 200Hz | Bias Corrected IMU acceleration and angular velocity data in FP_VRTK frame | - -- From **FP_A-TF_POIIMUH**, at 200Hz: - - | Topic | Message Type | Frequency | Description | - | ------------------------ | ----------------------- | ------------------------------ | ------------------------------ | - | `/fixposition/imu_ypr` | `geometry_msgs/Vector3` | 200Hz | x = 0.0, y = Pitch, z = Roll in radian. Euler angles representation of rotation between a local horizontal frame and P_POI. Rough estimation using IMU alone. | - -#### Transforms - -- TFs: - | Frames | Topic | Message needed to be selected on the web-interface | Frequency | - | ------------------- | ------------ | -------------------------------------------------- | ---------------------------------- | - | `FP_ECEF-->FP_POI` | `/tf` | `ODOMETRY` | as configured on the web-interface | - | `FP_ECEF-->FP_MAP` | `/tf_static` | `ODOMETRY` | 1Hz | - | `FP_ECEF-->FP_ENU` | `/tf` | `ODOMENU` | as configured on the web-interface | - | `FP_ECEF-->FP_ENU0` | `/tf_static` | `ODOMENU` | 1Hz | - | `FP_POI-->FP_POISH` | `/tf` | `ODOMSH` | as configured on the web-interface | - | `FP_POI-->FP_IMUH` | `/tf` | `TF_POIIMUH` | 200Hz | - | `FP_POI-->FP_VRTK` | `/tf_static` | `TF_POI_VRTK` | 1Hz | - | `FP_VRTK-->FP_CAM` | `/tf_static` | `TF_VRTK_CAM` | 1Hz | - - -- ROS TF Tree: - - ```mermaid - graph TD; - FP_ECEF-->FP_POI - FP_POI-->FP_VRTK-->FP_CAM - FP_POI-->FP_IMUH - FP_POI-->FP_POISH - FP_ECEF-->FP_ENU0 - ``` - -_Please note that the corresponding messages also has to be selected on the Fixposition Vision-RTK2's configuration interface._ - -### Explaination of frame ids - -| Frame ID | Explaination | -| ------------ | -------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| **FP_ECEF** | Earth-Center-Earth-Fixed frame. | -| **FP_POI** | Point-Of-Interest, configured from Vision-RTK2's web-interface with respect to the FP_VRTK frame. By default it is the same as FP_VRTK. | -| **FP_VRTK** | The coordinate frame on the Vision-RTK2's housing on the Fixposition-Logo "X". | -| **FP_CAM** | The camera coordinate frame of the Vision-RTK2. | -| **FP_POISH** | Point-Of-Interest of the Smooth Odometry frame, configured from Vision-RTK2's web-interface with respect to the FP_VRTK frame. | -| **FP_IMUH** | A local horizontal frame with the origin at the same location as FP_POI. This frame is a rough estimate determined by the IMU alone. | -| **FP_ENU0** | The **global fixed** East-North-Up coordinate frame with the origin at the configured ENU0 position on the web-interface. Also used as the MAP frame for Rviz. | - -# Input Wheelspeed through the driver - -The fp_ros_driver supports inputting a Speed msg (`msg/Speed.msg`) through the `/fixposition/speed` topic. The Speed msg is defined as a vector of WheelSensor msgs (`msg/WheelSensor.msg`). This message in turn, is intended to be a simplified version of the FP_B-MEASUREMENTS, containing three integers (vx, vy, and vz), three booleans (validity of the three velocity integers), and a string indicating the sensor from which the measurement originates. The integer velocity values should be in [mm/s], to have enough precision when being converted into an integer format. - -Internally, upon arriving to the ros driver, wheelspeed measurements are converted into a full FP_B-MEASUREMENTS message, and sent via the TCP or serial interface to the Vision-RTK2, where they will be further processed. For more details regarding the definition FP_B-MEASUREMENTS message, please refer to [the following page](https://docs.fixposition.com/fd/fp_b-measurements), or to the VRTK2 integration manual. - -# Fixposition Odometry Converter - -This is an extra node is provided to help with the integration of the wheel odometry on your vehicle. For details, see the subfolder [fixposition_odometry_converter_ros1](fixposition_odometry_converter_ros1/README.md) (ROS 1). When building the ROS 1 version add a file named 'CATKIN_IGNORE' to the `fixposition_odometry_converter_ros2` folder. +For installation, usage, and more information, please refer to [Fixposition Docs: ROS Driver](https://docs.fixposition.com/fd/fixposition-ros-driver). ## Fixposition ASCII messages -For more information about the ASCII messages parsed from the Vision-RTK 2, please refer to [Fixposition Docs](https://docs.fixposition.com/fd/i-o-messages). +For more information about the ASCII messages parsed from the Vision-RTK 2, please refer to [Fixposition Docs: I/O messages](https://docs.fixposition.com/fd/i-o-messages). ## Code Documentation Run `doxygen Doxyfile` to generate Doxygen code documentation. -# License +## License -This project is licensed under the MIT License - see the [LICENSE](LICENSE) file for details +This project is licensed under the MIT License - see the [LICENSE](LICENSE) file for details \ No newline at end of file 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 e9d5fe2..1264956 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 @@ -24,7 +24,7 @@ /* FIXPOSITION */ #include -#include +#include /* PACKAGE */ #include diff --git a/fixposition_driver_ros1/package.xml b/fixposition_driver_ros1/package.xml index e586fff..ee29d50 100644 --- a/fixposition_driver_ros1/package.xml +++ b/fixposition_driver_ros1/package.xml @@ -17,6 +17,5 @@ geometry_msgs message_generation message_runtime - fixposition_gnss_tf fixposition_driver_lib diff --git a/fixposition_driver_ros1/src/data_to_ros1.cpp b/fixposition_driver_ros1/src/data_to_ros1.cpp index f58879c..49ee507 100644 --- a/fixposition_driver_ros1/src/data_to_ros1.cpp +++ b/fixposition_driver_ros1/src/data_to_ros1.cpp @@ -527,17 +527,17 @@ void OdomToNavSatFix(const FP_ODOMETRY& data, ros::Publisher& pub) { } else { msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.odom.stamp)); } - msg.header.frame_id = data.odom.frame_id; + msg.header.frame_id = data.odom.child_frame_id; // Populate LLH position - const Eigen::Vector3d llh_pos = gnss_tf::TfWgs84LlhEcef(data.odom.pose.position); + const Eigen::Vector3d llh_pos = TfWgs84LlhEcef(data.odom.pose.position); msg.latitude = RadToDeg(llh_pos(0)); msg.longitude = RadToDeg(llh_pos(1)); msg.altitude = llh_pos(2); // Populate LLH covariance const Eigen::Matrix3d p_cov_e = data.odom.pose.cov.topLeftCorner(3, 3); - const Eigen::Matrix3d C_l_e = gnss_tf::RotEnuEcef(data.odom.pose.position); + const Eigen::Matrix3d C_l_e = RotEnuEcef(data.odom.pose.position); const Eigen::Matrix3d p_cov_l = C_l_e * p_cov_e * C_l_e.transpose(); Eigen::Map> cov_map = @@ -605,7 +605,7 @@ void OdomToYprMsg(const OdometryData& data, ros::Publisher& pub) { msg.header.frame_id = "FP_ENU"; // Euler angle wrt. ENU frame in the order of Yaw Pitch Roll - Eigen::Vector3d enu_euler = gnss_tf::RotToEul(data.pose.orientation.toRotationMatrix()); + Eigen::Vector3d enu_euler = RotToEul(data.pose.orientation.toRotationMatrix()); tf::vectorEigenToMsg(enu_euler, msg.vector); // Publish message diff --git a/fixposition_driver_ros1/src/fixposition_driver_node.cpp b/fixposition_driver_ros1/src/fixposition_driver_node.cpp index 1279dac..b2bd06a 100644 --- a/fixposition_driver_ros1/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros1/src/fixposition_driver_node.cpp @@ -146,7 +146,7 @@ void FixpositionDriverNode::RegisterObservers() { br_.sendTransform(tf); // Publish Pitch Roll based on IMU only - Eigen::Vector3d imu_ypr_eigen = gnss_tf::QuatToEul(data.tf.rotation); + Eigen::Vector3d imu_ypr_eigen = QuatToEul(data.tf.rotation); imu_ypr_eigen.x() = 0.0; // the yaw value is not observable using IMU alone geometry_msgs::Vector3Stamped imu_ypr; imu_ypr.header.stamp = tf.header.stamp; diff --git a/fixposition_driver_ros2/CMakeLists.txt b/fixposition_driver_ros2/CMakeLists.txt index e45fa3e..021b01f 100644 --- a/fixposition_driver_ros2/CMakeLists.txt +++ b/fixposition_driver_ros2/CMakeLists.txt @@ -21,7 +21,6 @@ find_package(rclcpp REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) find_package(tf2_eigen REQUIRED) -find_package(fixposition_gnss_tf REQUIRED) find_package(fixposition_driver_lib REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} @@ -56,7 +55,6 @@ ament_export_dependencies(rosidl_default_runtime) include_directories( include ${sensor_msgs_INCLUDE_DIR} - ${fixposition_gnss_tf_INCLUDE_DIR} ${fixposition_driver_lib_INCLUDE_DIR} ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIR} @@ -80,7 +78,6 @@ endif() target_link_libraries( ${PROJECT_NAME}_exec - ${fixposition_gnss_tf_LIBRARIES} ${fixposition_driver_lib_LIBRARIES} ${Boost_LIBRARIES} ${EIGEN3_LIBRARIES} @@ -103,7 +100,7 @@ install(DIRECTORY "launch" DESTINATION share/${PROJECT_NAME}/ ) -ament_target_dependencies(${PROJECT_NAME}_exec rclcpp std_msgs nav_msgs geometry_msgs sensor_msgs tf2_ros tf2_eigen fixposition_gnss_tf fixposition_driver_lib) +ament_target_dependencies(${PROJECT_NAME}_exec rclcpp std_msgs nav_msgs geometry_msgs sensor_msgs tf2_ros tf2_eigen fixposition_driver_lib) # define ament package for this project ament_package() diff --git a/fixposition_driver_ros2/README.md b/fixposition_driver_ros2/README.md index 486d00b..eaf1734 100644 --- a/fixposition_driver_ros2/README.md +++ b/fixposition_driver_ros2/README.md @@ -1,248 +1,21 @@ -# Fixposition Driver ROS2 +# Fixposition ROS2 Driver -[ROS](https://www.ros.org/) (both ROS1 and ROS2) Driver for [Fixposition Vision-RTK 2](https://www.fixposition.com/product) +- [ROS2 Humble / Jazzy ![](./../../actions/workflows/build_test_ros2.yml/badge.svg)](./../../actions/workflows/build_test_ros2.yml) -## Dependencies +[ROS](https://www.ros.org/) Driver for [Fixposition Vision-RTK 2](https://www.fixposition.com/product). -- [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/) -- [Transforms](https://wiki.ros.org/tf) -- [Colcon](https://colcon.readthedocs.io/en/released/) +## Driver documentation -- **[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 libboost-date-time-dev -``` - - -## Installation - -To install the node, extract / clone the code and `fixposition_gnss_tf` to your catkin workspace's `src` folder: - -```bash -# The folder structure should look like this -fp_public_ws -├── src -│ ├── fixposition_driver -│ │ ├── fixposition_driver_lib -│ │ ├── fixposition_driver_ros1 # will be ignored by colcon when building for ROS2 -│ │ ├── fixposition_driver_ros2 -│ ├── fixposition_gnss_tf -``` -make sure you have sourced the setup.bash from ros: - -`/opt/ros/{ROS_DISTRO}/setup.bash`, for example - -``` -source /opt/ros/foxy/setup.bash -``` - -and build it with: - -`colcon build --packages-up-to fixposition_driver_ros2` - -This will build the ROS2 driver node and all its dependencies. - - -Then source your environment after the build: - -`source install/setup.bash` - -## Launch the Driver - -- To launch the node in serial mode, run: - - `ros2 launch fixposition_driver_ros2 serial.launch` - -- In TCP mode (Ethernet or Wi-Fi): - - `ros2 launch fixposition_driver_ros2 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: - - `ros2 topic list` - - - there should be topics under the `/fixposition` namespace, for example: - ```bash - /fixposition/odom_ecef - /fixposition/odom_llh - /fixposition/odom_enu - /fixposition/odom_smooth - /fixposition/vrtk - /fixposition/poiimu - /fixposition/rawimu - /fixposition/corrimu - ``` - -- Check published message in the topic, for example the VRTK message: - `ros2 topic echo /fixposition/vrtk` - - -## Output of the driver - -### Messages and TF tree - -The output is published on the following: - -#### Vision-RTK2 I/O messages - -- From **FP_A-**, at the configured frequency (default 10Hz, output generator -> Fusion frequency): - - | Topic | Message Type | I/O message | Frequency | Description | - | --------------------------- | ----------------------------- | ------------- | ------------------------------ | --------------------------------------------- | - | `/fixposition/fpa/odometry` | `fixposition_msgs/odometry` | FP_A-ODOMETRY | as configured on web-interface | https://docs.fixposition.com/fd/fp_a-odometry | - | `/fixposition/fpa/llh` | `fixposition_msgs/llh` | FP_A-LLH | as configured on web-interface | https://docs.fixposition.com/fd/fp_a-llh | - | `/fixposition/fpa/odomenu` | `fixposition_msgs/odomenu` | FP_A-ODOMENU | as configured on web-interface | https://docs.fixposition.com/fd/fp_a-odomenu | - | `/fixposition/fpa/odomsh` | `fixposition_msgs/odomsh` | FP_A-ODOMSH | as configured on web-interface | https://docs.fixposition.com/fd/fp_a-odomsh | - | `/fixposition/fpa/gnssant` | `fixposition_msgs/gnssant` | FP_A-GNSSANT | as configured on web-interface | https://docs.fixposition.com/fd/fp_a-gnssant | - | `/fixposition/fpa/gnsscorr` | `fixposition_msgs/gnsscorr` | FP_A-GNSSCORR | as configured on web-interface | https://docs.fixposition.com/fd/fp_a-gnsscorr | - | `/fixposition/fpa/text` | `fixposition_msgs/text` | FP_A-TEXT | as configured on web-interface | https://docs.fixposition.com/fd/fp_a-text | - -- From **NMEA-**, at the configured frequency (default 5Hz): - - | Topic | Message Type | I/O message | Frequency | Description | - | ------------------------- | ------------------------ | ----------- | ------------------------------ | ------------------------------------------- | - | `/fixposition/nmea/gpgga` | `fixposition_msgs/gpgga` | NMEA-GP-GGA | as configured on web-interface | https://docs.fixposition.com/fd/nmea-gp-gga | - | `/fixposition/nmea/gpgll` | `fixposition_msgs/gpgll` | NMEA-GP-GLL | as configured on web-interface | https://docs.fixposition.com/fd/nmea-gp-gll | - | `/fixposition/nmea/gngsa` | `fixposition_msgs/gngsa` | NMEA-GN-GSA | as configured on web-interface | https://docs.fixposition.com/fd/nmea-gp-gsa | - | `/fixposition/nmea/gpgst` | `fixposition_msgs/gpgst` | NMEA-GP-GST | as configured on web-interface | https://docs.fixposition.com/fd/nmea-gp-gst | - | `/fixposition/nmea/gxgsv` | `fixposition_msgs/gxgsv` | NMEA-GX-GSV | as configured on web-interface | https://docs.fixposition.com/fd/nmea-gp-gsv | - | `/fixposition/nmea/gphdt` | `fixposition_msgs/gphdt` | NMEA-GP-HDT | as configured on web-interface | https://docs.fixposition.com/fd/nmea-gp-hdt | - | `/fixposition/nmea/gprmc` | `fixposition_msgs/gprmc` | NMEA-GP-RMC | as configured on web-interface | https://docs.fixposition.com/fd/nmea-gp-rmc | - | `/fixposition/nmea/gpvtg` | `fixposition_msgs/gpvtg` | NMEA-GP-VTG | as configured on web-interface | https://docs.fixposition.com/fd/nmea-gp-vtg | - | `/fixposition/nmea/gpzda` | `fixposition_msgs/gpzda` | NMEA-GP-ZDA | as configured on web-interface | https://docs.fixposition.com/fd/nmea-gp-zda | - -- In addition, from **NMEA-**, at the configured frequency (default 5Hz): - - | Topic | Message Type | Frequency | Description | - | ------------------- | ------------------------ | ------------------------------ | --------------------------------------------------------------------------------- | - | `/fixposition/nmea` | `fixposition_msgs/nmea` | as configured on web-interface | Collects and combines all NMEA messages and outputs at the end of the GNSS epoch. | - -#### Vision-RTK2 Fusion - -- From **FP_A-ODOMETRY**, at the configured frequency (default 10Hz, output generator -> Fusion frequency): - - - Messages - - | Topic | Message Type | Frequency | Description | - | --------------------------- | ------------------------- | ------------------------------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------- | - | `/fixposition/odom_ecef` | `nav_msgs/Odometry` | as configured on web-interface | Position, Orientation from ECEF to FP_POI, Velocity and Angular Velocity in FP_POI | - | `/fixposition/odom_llh` | `sensor_msgs/NavSatFix` | as configured on web-interface | Position from LLH (Latitude, Longitude and Height) to FP_POI. Based on WGS-84 ellipsoid | - | `/fixposition/odom_enu` | `nav_msgs/Odometry` | as configured on web-interface | Position, Orientation from ENU0 to FP_POI, Velocity and Angular Velocity in FP_POI | - | `/fixposition/odom_smooth` | `nav_msgs/Odometry` | as configured on web-interface | Position, Orientation from ECEF to FP_POISH, Velocity and Angular Velocity in FP_POI. Based on smooth odometry output | - | `/fixposition/vrtk` | `fixposition_driver/VRTK` | as configured on web-interface | Custom Message containing same Odometry information as well as status flags | - | `/fixposition/poiimu` | `sensor_msgs/Imu` | as configured on web-interface | Bias Corrected acceleration and rotation rate in FP_POI | - | `/fixposition/ypr` | `geometry_msgs/Vector3` | as configured on web-interface | x = Yaw, y = Pitch, z = Roll in radian. Euler angles representation of rotation between ENU and FP_POI. Only available after fusion initialization | - -#### Vision-RTK2 GNSS Antenna Positions - -**If GNSS Antenna positions are needed, please enable this on the sensor's configuration interface.** - -- From **NOV_B-BESTGNSSPOS_GNSS[1,2]**, at the configured frequency, GNSS1 and GNSS2 raw antenna positions (default 5Hz): - - | Topic | Message Type | Frequency | Description | - | -------------------- | ----------------------- | ------------------------------ | ------------------------------ | - | `/fixposition/gnss1` | `sensor_msgs/NavSatFix` | as configured on web-interface | Latitude, Longitude and Height | - | `/fixposition/gnss2` | `sensor_msgs/NavSatFix` | as configured on web-interface | Latitude, Longitude and Height | - -- From **NMEA-GP-GGA_GNSS**, **NMEA-GP-RMC_GNSS**, and **NMEA-GP-ZDA_GNSS**, at the configured frequency (default 5Hz): The Vision-RTK2 can also output an average GNSS-based LLH position (i.e., **only GNSS, not Fusion**) and heading estimate based on speed over ground (**SOG**) and course over ground (**COG**) - (i.e., **the platform must be moving for it to be accurate**) by utilizing several NMEA messages, which serves as an auxiliary output until Fusion is fully initialized. This message's output frequency equals the lowest frequency of any of these three message types. **Note: This output should only be used until Fusion is fully initialized.** Warning: This topic will only be populated when the sampling rate of all required NMEA messages is 1 (i.e., the default output frequency of the messages). - - | Topic | Message Type | Frequency | Description | - | -------------------- | ------------------------- | ------------------------------ | ------------------------------- | - | `/fixposition/nmea` | `fixposition_driver/NMEA` | as configured on web-interface | Latitude, Longitude and Height. | - -#### Vision-RTK2 IMU data - -- From **FP_A-RAWIMU**, at 200Hz: - - | Topic | Message Type | Frequency | Description | - | --------------------- | ----------------- | --------- | ------------------------------------------------------------------------------------------ | - | `/fixposition/rawimu` | `sensor_msgs/Imu` | 200Hz | Raw (without bias correction) IMU acceleration and angular velocity data in FP_VRTK frame. | - -- From **FP_A-CORRIMU**, at 200Hz: - - | Topic | Message Type | Frequency | Description | - | ---------------------- | ----------------- | --------- | --------------------------------------------------------------------------- | - | `/fixposition/corrimu` | `sensor_msgs/Imu` | 200Hz | Bias Corrected IMU acceleration and angular velocity data in FP_VRTK frame. | - -- From **FP_A-TF_POIIMUH**, at 200Hz: - - | Topic | Message Type | Frequency | Description | - | ------------------------ | ----------------------- | ------------------------------ | ------------------------------ | - | `/fixposition/imu_ypr` | `geometry_msgs/Vector3` | 200Hz | x = 0.0, y = Pitch, z = Roll in radian. Euler angles representation of rotation between a local horizontal frame and P_POI. Rough estimation using IMU alone. | - -#### Transforms - -- TFs: - | Frames | Topic | Message needed to be selected on the web-interface | Frequency | - | ------------------- | ------------ | -------------------------------------------------- | ---------------------------------- | - | `FP_ECEF-->FP_POI` | `/tf` | `ODOMETRY` | as configured on the web-interface | - | `FP_ECEF-->FP_MAP` | `/tf_static` | `ODOMETRY` | 1Hz | - | `FP_ECEF-->FP_ENU` | `/tf` | `ODOMENU` | as configured on the web-interface | - | `FP_ECEF-->FP_ENU0` | `/tf_static` | `ODOMENU` | 1Hz | - | `FP_POI-->FP_POISH` | `/tf` | `ODOMSH` | as configured on the web-interface | - | `FP_POI-->FP_IMUH` | `/tf` | `TF_POIIMUH` | 200Hz | - | `FP_POI-->FP_VRTK` | `/tf_static` | `TF_POI_VRTK` | 1Hz | - | `FP_VRTK-->FP_CAM` | `/tf_static` | `TF_VRTK_CAM` | 1Hz | - - -- ROS TF Tree: - - ```mermaid - graph TD; - FP_ECEF-->FP_POI - FP_POI-->FP_VRTK-->FP_CAM - FP_POI-->FP_IMUH - FP_POI-->FP_POISH - FP_ECEF-->FP_ENU0 - ``` - -_Please note that the corresponding messages also has to be selected on the Fixposition Vision-RTK2's configuration interface._ - -### Explaination of frame ids - -| Frame ID | Explaination | -| ------------ | -------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| **FP_ECEF** | Earth-Center-Earth-Fixed frame. | -| **FP_POI** | Point-Of-Interest, configured from Vision-RTK2's web-interface with respect to the FP_VRTK frame. By default it is the same as FP_VRTK. | -| **FP_VRTK** | The coordinate frame on the Vision-RTK2's housing on the Fixposition-Logo "X". | -| **FP_CAM** | The camera coordinate frame of the Vision-RTK2. | -| **FP_POISH** | Point-Of-Interest of the Smooth Odometry frame, configured from Vision-RTK2's web-interface with respect to the FP_VRTK frame. | -| **FP_IMUH** | A local horizontal frame with the origin at the same location as FP_POI. This frame is a rough estimate determined by the IMU alone. | -| **FP_ENU0** | The **global fixed** East-North-Up coordinate frame with the origin at the configured ENU0 position on the web-interface. Also used as the MAP frame for Rviz. | - -# Input Wheelspeed through the driver - -The fp_ros_driver supports inputting a Speed msg (`msg/Speed.msg`) through the `/fixposition/speed` topic. The Speed msg is defined as a vector of WheelSensor msgs (`msg/WheelSensor.msg`). This message in turn, is intended to be a simplified version of the FP_B-MEASUREMENTS, containing three integers (vx, vy, and vz), three booleans (validity of the three velocity integers), and a string indicating the sensor from which the measurement originates. The integer velocity values should be in [mm/s], to have enough precision when being converted into an integer format. - -Internally, upon arriving to the ros driver, wheelspeed measurements are converted into a full FP_B-MEASUREMENTS message, and sent via the TCP or serial interface to the Vision-RTK2, where they will be further processed. For more details regarding the definition FP_B-MEASUREMENTS message, please refer to [the following page](https://docs.fixposition.com/fd/fp_b-measurements), or to the VRTK2 integration manual. - -# Fixposition Odometry Converter - -This is an extra node is provided to help with the integration of the wheel odometry on your vehicle. For details, see the subfolder [fixposition_odometry_converter_ros2](fixposition_odometry_converter_ros2/README.md) (ROS 2). +For installation, usage, and more information, please refer to [Fixposition Docs: ROS Driver](https://docs.fixposition.com/fd/fixposition-ros-driver). ## Fixposition ASCII messages -For more information about the ASCII messages parsed from the Vision-RTK 2, please refer to [Fixposition Docs](https://docs.fixposition.com/fd/i-o-messages). +For more information about the ASCII messages parsed from the Vision-RTK 2, please refer to [Fixposition Docs: I/O messages](https://docs.fixposition.com/fd/i-o-messages). ## Code Documentation Run `doxygen Doxyfile` to generate Doxygen code documentation. -# License +## License This project is licensed under the MIT License - see the [LICENSE](LICENSE) file for details diff --git a/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp b/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp index 5fcb8cc..cf47bee 100644 --- a/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp +++ b/fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp @@ -24,7 +24,7 @@ /* FIXPOSITION */ #include -#include +#include /* PACKAGE */ #include diff --git a/fixposition_driver_ros2/package.xml b/fixposition_driver_ros2/package.xml index 620e9e6..00c8a89 100644 --- a/fixposition_driver_ros2/package.xml +++ b/fixposition_driver_ros2/package.xml @@ -16,7 +16,6 @@ builtin_interfaces fixposition_driver_lib - fixposition_gnss_tf geometry_msgs nav_msgs sensor_msgs diff --git a/fixposition_driver_ros2/src/data_to_ros2.cpp b/fixposition_driver_ros2/src/data_to_ros2.cpp index 6df905d..d60f80e 100644 --- a/fixposition_driver_ros2/src/data_to_ros2.cpp +++ b/fixposition_driver_ros2/src/data_to_ros2.cpp @@ -525,17 +525,17 @@ void OdomToNavSatFix(const fixposition::FP_ODOMETRY& data, rclcpp::Publisher> cov_map = @@ -603,7 +603,7 @@ void OdomToYprMsg(const OdometryData& data, rclcpp::PublishersendTransform(tf); // Publish Pitch Roll based on IMU only - Eigen::Vector3d imu_ypr_eigen = gnss_tf::QuatToEul(data.tf.rotation); + Eigen::Vector3d imu_ypr_eigen = QuatToEul(data.tf.rotation); imu_ypr_eigen.x() = 0.0; // the yaw value is not observable using IMU alone geometry_msgs::msg::Vector3Stamped imu_ypr; imu_ypr.header.stamp = tf.header.stamp; diff --git a/fixposition_odometry_converter_ros2/include/fixposition_odometry_converter_ros2/params.hpp b/fixposition_odometry_converter_ros2/include/fixposition_odometry_converter_ros2/params.hpp index b1de5ff..181571d 100644 --- a/fixposition_odometry_converter_ros2/include/fixposition_odometry_converter_ros2/params.hpp +++ b/fixposition_odometry_converter_ros2/include/fixposition_odometry_converter_ros2/params.hpp @@ -21,7 +21,7 @@ #include /* PACKAGE */ -#include +#include namespace fixposition {