ROS Driver for Fixposition Vision-RTK 2
-
fixposition_gnss_tf: Fixposition GNSS Transformation Lib, minimum version 2.0.0
This drvier operates as a ROS node, connecting to either a TCP or serial stream of Fixposition Vision-RTK output data, see Fixposition ASCII messages and the Integration Manual.
To install the node, extract / clone the code and fixposition_gnss_tf
to your catkin workspace's src
folder:
# The folder structure should look like this
fp_public_ws
└── src
├── fixposition_driver
└── gnsstransformationlib
and build it with:
catkin build fixposition_driver
Then source your development environment:
source devel/setup.bash
-
To launch the node in serial mode, run:
roslaunch fixposition_driver serial.launch
-
In TCP mode (Wi-Fi):
roslaunch fixposition_driver tcp.launch
-
In TCP mode (Ethernet):
roslaunch fixposition_driver 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.
The fp_ros_driver support inputing a Speed msg (msg/Speed.msg
) through the /fixposition/speed
topic.
The input velocity values should be in [mm/s] as integer 32bit. There are 2 Options:
- Option 1: only one vehicle speed, then only fill a single value as the vehicle speed
- Option 2: Fill in 4 Values of 4 wheels, in the order of FR, FL, RR, RL
The input values will be converted into a RAWDMI message and sent via the TCP interface to the Vision-RTK2, where it will be further processed and added into the positioning engine.
Note: Currently the wheelspeed input through the ROS doriver is only supported in the TCP mode
The output is published on the following:
-
From ODOMETRY, at the configured frequency
/fixposition/odometry (type: nav_msgs/Odometry) /fixposition/vrtk (type: fixposition_driver/VRTK) /fixposition/poiimu (type: sensor_msgs/Imu)
TFs:
ECEF-->FP_POI
andECEF-->FP_ENU
-
From LLH, at the configured frequency
/fixposition/navsatfix (type: sensor_msgs/NavSatFix)
-
From RAWIMU, at 200Hz
/fixposition/rawimu (type: sensor_msgs/Imu)
-
From CORRIMU, at 200Hz
/fixposition/corrimu (type: sensor_msgs/Imu)
-
From TF_POI_VRTK: static_tf
FP_POI-->FP_VRTK
-
From TF_VRTK_CAM: static_tf
FP_VRTK-->FP_CAM
-
ROS TF Tree:
graph TD; ECEF-->FP_POI-->FP_VRTK-->FP_CAM ECEF-->FP_ENU
Please note that the corresponding messages also has to be selected on the Fixposition V-RTK's configuration interface.
Frame ID | Explaination |
---|---|
ECEF | Earth-Center-Earth-Fixed frame |
FP_VRTK | The coordinate frame on the V-RTK's housing on the Fixposition-Logo "X" |
FP_POI | Point-Of-Interest, configured from V-RTK's web-interface with respect to the FP_VRTK frame. By default it is the same as FP_VRTK. |
FP_ENU | The local East-North-Up coordinate frame with the origin ar the same location as FP_POI |
FP_CAM | The camera coordinate frame of the V-RTK. |
Run doxygen Doxyfile
to generate Doxygen code documentation.
This is an exerpt from the Integration Manual
NMEA style framing is used. Frames (messages) are in this form:
$FP,msg_type,msg_version,field3,field4,...,fieldN*CC\r\n
Where:
- The NMEA style framing:
$
-- Start character ("$", ASCII 36)*CC
-- Checksum: "*" (ASCII 42) and two digit XOR value of all payload characters in captial hexadecimal notation, for example: "FPX" ='F' ^ 'P' ^ 'X' = 70 ^ 80 ^ 88 = 78 = 0x4e
= checksum4E
\r\n
-- Sentence termination characters (CR + LF, ASCII 13 + 10)
- A Fixposition identifier:
FP
-- Fixposition ASCII message identifier, "FP" (ASCII 70 + 80)
- Fixposition message type and version:
msg_type
(=field1
) -- Message type, all captial letters (ASCII 65--90)msg_version
(=field2
) -- Message version, decimal number (letters 0--9, ASCII 48--57), range 1--...
- Data fields (payload)
field3,field4,...,fieldN
-- The structure of the message data is defined by themsg_type
andversion
. Each field can contain all printable 7-bit ASCII characters (ASCII 32–126), excluding the reserved characters!
(ASCII 33),$
(ASCII 36),*
(ASCII 42),,
(ASCII 44),\
(ASCII 92),~
(ASCII 126).
- Field separators
- All fields (identifier, message type, message version, data fields) are separated by a comma (
,
, ASCII 44)
- All fields (identifier, message type, message version, data fields) are separated by a comma (
- Null fields
- Data fields can be null, meaning their value is absent to indicate that no data is
available. The data for null fields is the empty string. For example:
- Definition:
...,fieldi,fieldi+1,fieldi+2,...
- Values:
fieldi
= 123,fieldi+1
= null,fieldi+2
= 456 - Payload string:
...,123,,456,...
- Definition:
- Data fields can be null, meaning their value is absent to indicate that no data is
available. The data for null fields is the empty string. For example:
- Data field types:
- Numeric: Decimal integer number, one or more digits (0-9) and optional leading "-" sign
- Float (.x): Decimal floating point number, one or more digits (0-9) and optional leading "-" sign, with x digits fractional part separated by a dot (".")
- Float (x): Decimal floating point number with x significant digits, optional leading "-", optional fractional part separated by a dot (".")
- String: String of allowed payload characters (but not the
,
field separator) - ...
- ...
This message contains full fusion odometry output and additional status information. It is output at the configured rate.
Example message (wrapped on multiple lines for readability):
$FP,ODOMETRY,1,2197,126191.765,4278415.1169,636245.1942,4672227.8942,-0.921035,-0.001266,-0.365401,
-0.134863,0.6169,-0.0140,-0.0068,0.01857,-0.01427,-0.00746,-0.1185,-0.0795,9.7791,4,1,1,1,0.55214,
0.33578,0.50777,0.08625,-0.13062,-0.45209,0.00227,0.00020,0.00270,0.00027,0.00031,0.00232,
0.03314,0.03828,0.03199,-0.00290,0.00246,-0.00119,fp_release_vr2_2.36.1_67*47
Message fields:
# | Field | Format | Unit | Example | Description |
---|---|---|---|---|---|
1 | msg_type |
String | - | ODOMETRY |
Message type, always ODOMETRY for this message |
2 | msg_version |
Numeric | - | 1 |
Message version, always 1 for this version of the ODOMETRY message |
3 | gps_week |
Numeric | - | 2197 |
GPS week number, range 0--9999 |
4 | gps_tow |
Float (.6) | s | 126191.765 |
GPS time of week, range 0.000--604799.999 |
5 | pos_x |
Float (.4) | m | 4278415.1169 |
Position in ECEF, X component |
6 | pos_y |
Float (.4) | m | 636245.1942 |
Position in ECEF, Y component |
7 | pos_z |
Float (.4) | m | 4672227.8942 |
Position in ECEF, Z component |
8 | orientation_w |
Float (.6) | - | -0.921035 |
Quaternion with respect to ECEF, W component |
9 | orientation_x |
Float (.6) | - | -0.001266 |
Quaternion with respect to ECEF, X component |
10 | orientation_y |
Float (.6) | - | -0.365401 |
Quaternion with respect to ECEF, Y component |
11 | orientation_z |
Float (.6) | - | -0.134863 |
Quaternion with respect to ECEF, Z component |
12 | vel_x |
Float (.4) | m/s | 0.6169 |
Velocity in output frame, X component |
13 | vel_y |
Float (.4) | m/s | -0.0140 |
Velocity in output frame, Y component |
14 | vel_z |
Float (.4) | m/s | -0.0068 |
Velocity in output frame, Z component |
15 | rot_x |
Float (.5) | rad/s | 0.01857 |
Bias corrected angular velocity in output frame, X component |
16 | rot_y |
Float (.5) | rad/s | -0.01427 |
Bias corrected angular velocity in output frame, Y component |
17 | rot_z |
Float (.5) | rad/s | -0.00746 |
Bias corrected angular velocity in output frame, Z component |
18 | acc_x |
Float (.4) | m/s2 | -0.1185 |
Bias corrected acceleration in output frame, X component |
19 | acc_y |
Float (.4) | m/s2 | -0.0795 |
Bias corrected acceleration in output frame, Y component |
20 | acc_z |
Float (.4) | m/s2 | 9.7791 |
Bias corrected acceleration in output frame, Z component |
21 | fusion_status |
Numeric | - | 4 |
Fustion status, see below |
22 | imu_bias_status |
Numeric | - | 1 |
IMU bias status, see below |
23 | gnss_fix_type |
Numeric | - | 1 |
GNSS fix type, see below |
24 | wheelspeed_status |
Numeric | - | 1 |
Wheelspeed status, see below |
25 | pos_cov_xx |
Float (5) | m2 | 0.55214 |
Position covariance, element XX |
26 | pos_cov_yy |
Float (5) | m2 | 0.33578 |
Position covariance, element YY |
27 | pos_cov_zz |
Float (5) | m2 | 0.50777 |
Position covariance, element ZZ |
28 | pos_cov_xy |
Float (5) | m2 | 0.08625 |
Position covariance, element XY |
29 | pos_cov_yz |
Float (5) | m2 | -0.13062 |
Position covariance, element YZ |
30 | pos_cov_xz |
Float (5) | m2 | -0.45209 |
Position covariance, element XZ |
31 | orientation_cov_xx |
Float (5) | rad2 | 0.00227 |
Velocity covariance, element XX |
32 | orientation_cov_yy |
Float (5) | rad2 | 0.00020 |
Velocity covariance, element YY |
33 | orientation_cov_zz |
Float (5) | rad2 | 0.00270 |
Velocity covariance, element ZZ |
34 | orientation_cov_xy |
Float (5) | rad2 | 0.00027 |
Velocity covariance, element XY |
35 | orientation_cov_yz |
Float (5) | rad2 | 0.00031 |
Velocity covariance, element YZ |
36 | orientation_cov_xz |
Float (5) | rad2 | 0.00232 |
Velocity covariance, element XZ |
37 | vel_cov_xx |
Float (5) | m2/s2 | 0.03314 |
Velocity covariance, element XX |
38 | vel_cov_yy |
Float (5) | m2/s2 | 0.03828 |
Velocity covariance, element YY |
39 | vel_cov_zz |
Float (5) | m2/s2 | 0.03199 |
Velocity covariance, element ZZ |
40 | vel_cov_xy |
Float (5) | m2/s2 | -0.00290 |
Velocity covariance, element XY |
41 | vel_cov_yz |
Float (5) | m2/s2 | 0.00246 |
Velocity covariance, element YZ |
42 | vel_cov_xz |
Float (5) | m2/s2 | -0.00119 |
Velocity covariance, element XZ |
43 | sw_version |
String | - | fp_release_vr2_2.36.1_67 |
Software version |
Fusion status (fusion_status
):
Value | Description |
---|---|
0 |
Not started |
1 |
Vision only |
2 |
Visual inertial fusion |
3 |
Inertial-GNSS fusion |
4 |
Visual-inertial-GNSS fusion |
IMU bias status (imu_bias_status
):
Value | Description |
---|---|
0 |
Not converged |
1 |
IMU bias converged |
Wheelspeed status (wheelspeed_status
):
Value | Description |
---|---|
-1 |
No wheelspeed enabled |
0 |
At least one wheelspeed enabled, no wheelspeed converged |
1 |
At least one wheelspeed enabled and converged |
GNSS fix type (gnss_fix_type
):
Value | Description |
---|---|
0 |
Unknown |
1 |
No fix |
2 |
Dead-reckoning only |
3 |
Time-only fix |
4 |
Single 2D fix |
5 |
Single 3D fix |
6 |
Single 3D fix with dead-reckoning |
7 |
RTK float fix |
8 |
RTK fixed fix |
Remarks:
- The output frame is the frame configured on the web-interface.
This message contains time, geographic coordinates and the position covariance of the output frame in East-North-up (ENU). The coordinates are transformed from ECEF using the WGS-84 parameters (see also [[coordinates#vrtk-output-coordinate-system]]). It is output at the configured rate.
Example message (wrapped on multiple lines for readability):
$FP,LLH,1,2197,126191.765,47.398826818,8.458494107,457.518,0.31537,
1.0076,0.072696,-0.080012,0.0067274,-0.011602*4E\r\n
Message fields:
# | Field | Format | Unit | Example | Description |
---|---|---|---|---|---|
1 | msg_type |
String | - | LLH |
Message type, always LLH for this message |
2 | msg_version |
Numeric | - | 1 |
Message version, always 1 for this version of the LLH message |
3 | gps_week |
Numeric | - | 2197 |
GPS week number, range 0--9999 |
4 | gps_tow |
Float (.6) | s | 126191.765 |
GPS time of week, range 0.000--604799.999 |
5 | latitude |
Float (.9) | deg | 47.398826818 |
Latitude, range -90.000000000--90.000000000, > 0 for North, < 0 for South |
6 | longitude |
Float (.9) | deg | 8.458494107 |
Longitude, range -180.000000000--180.000000000, > 0 for East, < 0 for West |
7 | height |
Float (.4) | m | 457.518 |
Ellipsoidal height |
8 | pos_cov_ee |
Float (5) | m2 | 0.31537 |
Position covariance in ENU, element EE |
9 | pos_cov_nn |
Float (5) | m2 | 1.0076 |
Position covariance in ENU, element NN |
10 | pos_cov_uu |
Float (5) | m2 | 0.072696 |
Position covariance in ENU, element UU |
11 | pos_cov_en |
Float (5) | m2 | -0.080012 |
Position covariance in ENU, element EN |
12 | pos_cov_nu |
Float (5) | m2 | 0.0067274 |
Position covariance in ENU, element NU |
13 | pos_cov_eu |
Float (5) | m2 | -0.011602 |
Position covariance in ENU, element EU |
This message contains time, acceleration and angular velocity (raw value, no bias correction, only coordinate transformation applied) in the vrtk frame - the X on the sensor. See also [[coordinates#vrtk-output-coordinate-system]]. It is output at 200Hz IMU frequency regardless whether fusion is running or not.
Example message:
$FP,RAWIMU,1,2197,126191.777855,-0.199914,0.472851,9.917973,0.023436,0.007723,0.002131*34\r\n
# | Field | Format | Unit | Example | Description |
---|---|---|---|---|---|
1 | msg_type |
String | - | RAWIMU |
Message type, always RAWIMU for this message |
2 | msg_version |
Numeric | - | 1 |
Message version, always 1 for this version of the RAWIMU message |
3 | gps_week |
Numeric | - | 2197 |
GPS week number, range 0--9999 |
4 | gps_tow |
Float (.6) | s | 126191.777855 |
GPS time of week, range 0.000--604799.999 |
5 | acc_x |
Float (.6) | m/s2 | -0.199914 |
Raw acceleration in output frame, X component |
6 | acc_y |
Float (.6) | m/s2 | 0.472851 |
Raw acceleration in output frame, Y component |
7 | acc_z |
Float (.6) | m/s2 | 9.917973 |
Raw acceleration in output frame, Z component |
8 | rot_x |
Float (.6) | rad/s | 0.023436 |
Raw angular velocity in output frame, X component |
9 | rot_y |
Float (.6) | rad/s | 0.007723 |
Raw angular velocity in output frame, Y component |
10 | rot_z |
Float (.6) | rad/s | 0.002131 |
Raw angular velocity in output frame, Z component |
This message contains time, acceleration and angular velocity (coordinate transformation and bias correction applied) in the vrtk frame - the X on the sensor. See also [[coordinates#vrtk-output-coordinate-system]]. It is output at 200Hz IMU frequency, but only when fusion is initialized and IMU biases is converged.
Example message:
$FP,CORRIMU,1,2197,126191.777855,-0.195224,0.393969,9.869998,0.013342,-0.004620,-0.000728*7D\r\n
# | Field | Format | Unit | Example | Description |
---|---|---|---|---|---|
1 | msg_type |
String | - | CORRIMU |
Message type, always RAWIMU for this message |
2 | msg_version |
Numeric | - | 1 |
Message version, always 1 for this version of the RAWIMU message |
3 | gps_week |
Numeric | - | 2197 |
GPS week number, range 0--9999 |
4 | gps_tow |
Float (.6) | s | 126191.777855 |
GPS time of week, range 0.000--604799.999 |
5 | acc_x |
Float (.6) | m/s2 | -0.195224 |
Raw acceleration in output frame, X component |
6 | acc_y |
Float (.6) | m/s2 | 0.393969 |
Raw acceleration in output frame, Y component |
7 | acc_z |
Float (.6) | m/s2 | 9.869998 |
Raw acceleration in output frame, Z component |
8 | rot_x |
Float (.6) | rad/s | 0.013342 |
Raw angular velocity in output frame, X component |
9 | rot_y |
Float (.6) | rad/s | -0.004620 |
Raw angular velocity in output frame, Y component |
10 | rot_z |
Float (.6) | rad/s | -0.000728 |
Raw angular velocity in output frame, Z component |
Remarks:
- The output frame of the IMU messages is the X on VRTK sensor, NOT the frame configured from the webinterface (They are of course the same when on webinterface the configs are 0s).
This message contains information for static coordinate transformations.
Example message (wrapped on multiple lines for readability):
$FP,TF,1,VRTK,CAM,0.01795,0.00044,-0.01103,
0.485049,-0.508955,0.511098,-0.494440*4A
Message fields:
# | Field | Format | Unit | Example | Description |
---|---|---|---|---|---|
1 | msg_type |
String | - | TF |
Message type, always ODOMETRY for this message |
2 | msg_version |
Numeric | - | 1 |
Message version, always 1 for this version of the ODOMETRY message |
3 | from_frame |
Numeric | - | CAM |
GPS week number, range 0--9999 |
4 | to_frame |
Float (.6) | s | VRTK |
GPS time of week, range 0.000--604799.999 |
5 | translation_x |
Float (.5) | m | 0.01795 |
Position in ECEF, X component |
6 | translation_y |
Float (.5) | m | 0.00044 |
Position in ECEF, Y component |
7 | translation_z |
Float (.5) | m | -0.01103 |
Position in ECEF, Z component |
8 | orientation_w |
Float (.6) | - | 0.485049 |
Quaternion with respect to ECEF, W component |
9 | orientation_x |
Float (.6) | - | -0.508955 |
Quaternion with respect to ECEF, X component |
10 | orientation_y |
Float (.6) | - | 0.511098 |
Quaternion with respect to ECEF, Y component |
11 | orientation_z |
Float (.6) | - | -0.494440 |
Quaternion with respect to ECEF, Z component |
This project is licensed under the MIT License - see the LICENSE file for details