diff --git a/.github/workflows/build_test_ros.yml b/.github/workflows/build_test_ros.yml index 66a7845..026253b 100644 --- a/.github/workflows/build_test_ros.yml +++ b/.github/workflows/build_test_ros.yml @@ -62,5 +62,5 @@ jobs: - name: Build and Test run: | source /opt/ros/$ROS_DISTRO/setup.bash - catkin build fixposition_driver_lib fixposition_driver_ros1 fixposition_odometry_converter --force-cmake -DBUILD_TESTING=ON + catkin build fixposition_driver_lib fixposition_driver_ros1 fixposition_odometry_converter_ros1 --force-cmake -DBUILD_TESTING=ON catkin run_tests diff --git a/.github/workflows/build_test_ros2.yml b/.github/workflows/build_test_ros2.yml index 8cb591c..0111191 100644 --- a/.github/workflows/build_test_ros2.yml +++ b/.github/workflows/build_test_ros2.yml @@ -48,7 +48,7 @@ jobs: - name: Ignore ROS1 node run: | touch src/fixposition_driver/fixposition_driver_ros1/COLCON_IGNORE - touch src/fixposition_driver/fixposition_odometry_converter/COLCON_IGNORE + touch src/fixposition_driver/fixposition_odometry_converter_ros1/COLCON_IGNORE - name: Build and Test run: | source /opt/ros/$ROS_DISTRO/setup.bash diff --git a/README.md b/README.md index f9dce85..36edfca 100644 --- a/README.md +++ b/README.md @@ -239,48 +239,9 @@ _Please note that the corresponding messages also has to be selected on the Fixp ## Input Wheelspeed through the driver -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], respectively [mrad/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: One vehicle speed and the rotation around the vehicle's rotation center -- Option 3: Fill in 4 Values of 4 wheels, in the order of FR, FL, RR, RL - -The input values will be converted into a NOV_B-RAWDMI message and sent via the TCP interface to the Vision-RTK2, where it will be further processed and added into the positioning engine. The following protocol is used when filling the DMI messages, as per the documentation: - -| # | Offset | Field | Type | Unit | Example | Description | -| ---: | -----: | ------------- | -------------- | ---- | ------- | ------------------------------------------------------- | -| - | 0 | `sync1` | uint8_t | - | `0xaa` | Sync byte 1 (always `0xaa`) | -| - | 1 | `sync2` | uint8_t | - | `0x44` | Sync byte 2 (always `0x44`) | -| - | 2 | `sync3` | uint8_t | - | `0x13` | Sync byte 3 (always `0x13`) | -| - | 3 | `payload_len` | uint8_t | - | `20` | Payload length (always `20` for this message) | -| - | 4 | `msg_id` | uint16_t | - | `2269` | Message ID (always `2269` for this message) | -| 1 | 6 | `gps_wno` | uint16_t | - | `0` | Week number, set to `0`, not supported by VRTK2 | -| 2 | 8 | `gps_tow` | int32_t | ms | `0` | Time of week [ms], set to `0`, no supported by VRTK2 | -| 3 | 12 | `dmi1` | int32_t | - | | Measurement value 1, for RC or FR wheel | -| 4 | 16 | `dmi2` | int32_t | - | | Measurement value 2, for FL wheel or YW sensor | -| 5 | 20 | `dmi3` | int32_t | - | | Measurement value 3, for RR wheel | -| 6 | 24 | `dmi4` | int32_t | - | | Measurement value 4, for RL wheel | -| - | 28 | `mask` | uint32_t | - | | _Bitfield:_ | -| 7 | | `dmi1_valid` | - _bit 0_ | - | | Validity flag for _dmi1_ value (0 = invalid, 1 = valid) | -| 8 | | `dmi2_valid` | - _bit 1_ | - | | Validity flag for _dmi2_ value (0 = invalid, 1 = valid) | -| 9 | | `dmi3_valid` | - _bit 2_ | - | | Validity flag for _dmi3_ value (0 = invalid, 1 = valid) | -| 10 | | `dmi4_valid` | - _bit 3_ | - | | Validity flag for _dmi4_ value (0 = invalid, 1 = valid) | -| 11 | | `dmi1_type` | - _bits 10…4_ | - | | Type of measurement present in _dmi1_ value (see below) | -| 12 | | `dmi2_type` | - _bits 17…11_ | - | | Type of measurement present in _dmi2_ value (see below) | -| 13 | | `dmi3_type` | - _bits 24…18_ | - | | Type of measurement present in _dmi3_ value (see below) | -| 14 | | `dmi4_type` | - _bits 31…25_ | - | | Type of measurement present in _dmi3_ value (see below) | -| - | 32 | `checksum` | uint32_t | - | | CRC32 checksum (see VRTK2 documentation) | - -Measurement types (`dmi1_type`, `dmi2_type`, `dmi3_type` and `dmi4_type`): - -| Value | Description | -| :---: | ----------------------- | -| `0` | Linear velocity (speed) | -| `1` | Angular velocity | - -Note: _Currently the wheelspeed input through the ROS driver is only supported in the TCP mode_ +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. ## Code Documentation @@ -560,7 +521,7 @@ Message fields: # 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](fixposition_odometry_converter/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. +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. # License diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/fixposition_driver.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/fixposition_driver.hpp index 51d2a3c..b14107c 100644 --- a/fixposition_driver_lib/include/fixposition_driver_lib/fixposition_driver.hpp +++ b/fixposition_driver_lib/include/fixposition_driver_lib/fixposition_driver.hpp @@ -23,8 +23,10 @@ /* EXTERNAL */ #include +#include +#include +#include #include -#include namespace fixposition { @@ -52,9 +54,28 @@ class FixpositionDriver { /** * @brief * - * @param[in] msg + * @param[in] sensors_meas map wheelspeed of sensors, each sensor containing speed values and their validity flag */ - virtual void WsCallback(const std::vector& speeds); + virtual void WsCallback(const std::unordered_map>>& sensors_meas); + + /** + * @brief + * + * @param[in] meas_vec measurements from one specific wheelspeed sensor, with their validity flag + * @param[in] meas_loc location from the specific wheelspeed sensor + * @param[out] meas_fpb fpb measurement to be filled from the vector + * @return true if the measurement was successfully filled, false otherwise + */ + virtual bool FillWsSensorMeas(const std::vector>& meas_vec, + const FpbMeasurementsMeasLoc meas_loc, FpbMeasurementsMeas& meas_fpb); + + /** + * @brief Converts the measurement location from string to the enum values + * + * @param[in] meas_loc user input location in string format + * @return FpbMeasurementsMeasLoc converted measurement location + */ + virtual FpbMeasurementsMeasLoc WsMeasStringToLoc(const std::string& meas_loc); /** * @brief Convert the Nmea like string using correct converter @@ -113,8 +134,6 @@ class FixpositionDriver { FixpositionDriverParams params_; - RAWDMI rawdmi_; //!< RAWDMI msg struct - std::unordered_map> a_converters_; //!< ascii converters corresponding to the input formats diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/fpb.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/fpb.hpp new file mode 100644 index 0000000..cafb3b4 --- /dev/null +++ b/fixposition_driver_lib/include/fixposition_driver_lib/fpb.hpp @@ -0,0 +1,91 @@ +/** + * @file + * @brief Declaration of Fpb message framework + * + * \verbatim + * ___ ___ + * \ \ / / + * \ \/ / Fixposition AG + * / /\ \ All right reserved. + * /__/ \__\ + * \endverbatim + * + */ + +#ifndef __FIXPOSITION_DRIVER_LIB_FPB__ +#define __FIXPOSITION_DRIVER_LIB_FPB__ + +#include + +namespace fixposition { + +static uint32_t const k_crc32_fpb[] = { + 0x00000000, 0x32c00699, 0x65800d32, 0x57400bab, 0xcb001a64, 0xf9c01cfd, + 0xae801756, 0x9c4011cf, 0xa4c03251, 0x960034c8, 0xc1403f63, 0xf38039fa, + 0x6fc02835, 0x5d002eac, 0x0a402507, 0x3880239e, 0x7b40623b, 0x498064a2, + 0x1ec06f09, 0x2c006990, 0xb040785f, 0x82807ec6, 0xd5c0756d, 0xe70073f4, + 0xdf80506a, 0xed4056f3, 0xba005d58, 0x88c05bc1, 0x14804a0e, 0x26404c97, + 0x7100473c, 0x43c041a5, 0xf680c476, 0xc440c2ef, 0x9300c944, 0xa1c0cfdd, + 0x3d80de12, 0x0f40d88b, 0x5800d320, 0x6ac0d5b9, 0x5240f627, 0x6080f0be, + 0x37c0fb15, 0x0500fd8c, 0x9940ec43, 0xab80eada, 0xfcc0e171, 0xce00e7e8, + 0x8dc0a64d, 0xbf00a0d4, 0xe840ab7f, 0xda80ade6, 0x46c0bc29, 0x7400bab0, + 0x2340b11b, 0x1180b782, 0x2900941c, 0x1bc09285, 0x4c80992e, 0x7e409fb7, + 0xe2008e78, 0xd0c088e1, 0x8780834a, 0xb54085d3, 0xdfc18e75, 0xed0188ec, + 0xba418347, 0x888185de, 0x14c19411, 0x26019288, 0x71419923, 0x43819fba, + 0x7b01bc24, 0x49c1babd, 0x1e81b116, 0x2c41b78f, 0xb001a640, 0x82c1a0d9, + 0xd581ab72, 0xe741adeb, 0xa481ec4e, 0x9641ead7, 0xc101e17c, 0xf3c1e7e5, + 0x6f81f62a, 0x5d41f0b3, 0x0a01fb18, 0x38c1fd81, 0x0041de1f, 0x3281d886, + 0x65c1d32d, 0x5701d5b4, 0xcb41c47b, 0xf981c2e2, 0xaec1c949, 0x9c01cfd0, + 0x29414a03, 0x1b814c9a, 0x4cc14731, 0x7e0141a8, 0xe2415067, 0xd08156fe, + 0x87c15d55, 0xb5015bcc, 0x8d817852, 0xbf417ecb, 0xe8017560, 0xdac173f9, + 0x46816236, 0x744164af, 0x23016f04, 0x11c1699d, 0x52012838, 0x60c12ea1, + 0x3781250a, 0x05412393, 0x9901325c, 0xabc134c5, 0xfc813f6e, 0xce4139f7, + 0xf6c11a69, 0xc4011cf0, 0x9341175b, 0xa18111c2, 0x3dc1000d, 0x0f010694, + 0x58410d3f, 0x6a810ba6, 0x8d431a73, 0xbf831cea, 0xe8c31741, 0xda0311d8, + 0x46430017, 0x7483068e, 0x23c30d25, 0x11030bbc, 0x29832822, 0x1b432ebb, + 0x4c032510, 0x7ec32389, 0xe2833246, 0xd04334df, 0x87033f74, 0xb5c339ed, + 0xf6037848, 0xc4c37ed1, 0x9383757a, 0xa14373e3, 0x3d03622c, 0x0fc364b5, + 0x58836f1e, 0x6a436987, 0x52c34a19, 0x60034c80, 0x3743472b, 0x058341b2, + 0x99c3507d, 0xab0356e4, 0xfc435d4f, 0xce835bd6, 0x7bc3de05, 0x4903d89c, + 0x1e43d337, 0x2c83d5ae, 0xb0c3c461, 0x8203c2f8, 0xd543c953, 0xe783cfca, + 0xdf03ec54, 0xedc3eacd, 0xba83e166, 0x8843e7ff, 0x1403f630, 0x26c3f0a9, + 0x7183fb02, 0x4343fd9b, 0x0083bc3e, 0x3243baa7, 0x6503b10c, 0x57c3b795, + 0xcb83a65a, 0xf943a0c3, 0xae03ab68, 0x9cc3adf1, 0xa4438e6f, 0x968388f6, + 0xc1c3835d, 0xf30385c4, 0x6f43940b, 0x5d839292, 0x0ac39939, 0x38039fa0, + 0x52829406, 0x6042929f, 0x37029934, 0x05c29fad, 0x99828e62, 0xab4288fb, + 0xfc028350, 0xcec285c9, 0xf642a657, 0xc482a0ce, 0x93c2ab65, 0xa102adfc, + 0x3d42bc33, 0x0f82baaa, 0x58c2b101, 0x6a02b798, 0x29c2f63d, 0x1b02f0a4, + 0x4c42fb0f, 0x7e82fd96, 0xe2c2ec59, 0xd002eac0, 0x8742e16b, 0xb582e7f2, + 0x8d02c46c, 0xbfc2c2f5, 0xe882c95e, 0xda42cfc7, 0x4602de08, 0x74c2d891, + 0x2382d33a, 0x1142d5a3, 0xa4025070, 0x96c256e9, 0xc1825d42, 0xf3425bdb, + 0x6f024a14, 0x5dc24c8d, 0x0a824726, 0x384241bf, 0x00c26221, 0x320264b8, + 0x65426f13, 0x5782698a, 0xcbc27845, 0xf9027edc, 0xae427577, 0x9c8273ee, + 0xdf42324b, 0xed8234d2, 0xbac23f79, 0x880239e0, 0x1442282f, 0x26822eb6, + 0x71c2251d, 0x43022384, 0x7b82001a, 0x49420683, 0x1e020d28, 0x2cc20bb1, + 0xb0821a7e, 0x82421ce7, 0xd502174c, 0xe7c211d5 +}; + +uint32_t Crc32fpb(const uint8_t* data, const int size) { + uint32_t crc = 0; + if (data != nullptr) { + for (int ix = 0; ix < size; ix++) { + crc = (crc << 8) ^ k_crc32_fpb[((crc >> 24) ^ data[ix]) & 0xff]; + } + } + return crc; +} + +struct FpbHeader { + uint8_t sync1; //!< FP_B frame sync char 1 (0x66) + uint8_t sync2; //!< FP_B frame sync char 2 (0x21) + uint16_t msg_id; //!< ID of the FP_B message (2001 for measurements message) + uint16_t payload_size; //!< Size of the payload + uint16_t time; //!< Time of the message. Unused, set to 0. +}; +static_assert(sizeof(FpbHeader) == 8, ""); +const int FP_B_HEAD_SIZE = 8; //!< Size of FP_B frame header +const int FP_B_CRC_SIZE = 4; //!< Size of FP_B crc + +} // namespace fixposition + +#endif // __FIXPOSITION_DRIVER_LIB_FPB__ diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/fpb_measurements.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/fpb_measurements.hpp new file mode 100644 index 0000000..06ac1e6 --- /dev/null +++ b/fixposition_driver_lib/include/fixposition_driver_lib/fpb_measurements.hpp @@ -0,0 +1,73 @@ +/** + * @file + * @brief Declaration of FpbMeasurementsMeas message struct + * + * \verbatim + * ___ ___ + * \ \ / / + * \ \/ / Fixposition AG + * / /\ \ All right reserved. + * /__/ \__\ + * \endverbatim + * + */ + +#ifndef __FIXPOSITION_DRIVER_LIB_FPBMEASUREMENTS__ +#define __FIXPOSITION_DRIVER_LIB_FPBMEASUREMENTS__ + +/* EXTERNAL */ +#include + +/* PACKAGE */ +#include + +namespace fixposition { + +struct FpbMeasurementsHeader { + uint8_t version; //!< Message version (= FpbMeasurementsVersion for the current version of this message) + uint8_t num_meas; //!< Number of measurements in the body (1..FP_B_MEASUREMENTS_MAX_NUM_MEAS) + uint8_t reserved0[6]; //!< Reserved for future use. Set to 0. +}; +static_assert(sizeof(FpbMeasurementsHeader) == 8, ""); +const int FP_B_MEASUREMENTS_HEAD_SIZE = 8; + +enum FpbMeasurementsMeasType { + MEASTYPE_UNSPECIFIED = 0, //!< Message type unspecified, measurement will not be processed + MEASTYPE_VELOCITY = 1, //!< Velocity message, measurement will be interpreted as a wheel speed +}; + +enum FpbMeasurementsMeasLoc { + MEASLOC_UNSPECIFIED = 0, //!< Location of the sensor unspecified, measurement will not be processed + MEASLOC_RC = 1, //!< Measurement coming from the RC sensor + MEASLOC_FR = 2, //!< Measurement coming from the FR sensor + MEASLOC_FL = 3, //!< Measurement coming from the FL sensor + MEASLOC_RR = 4, //!< Measurement coming from the RR sensor + MEASLOC_RL = 5, //!< Measurement coming from the RL sensor +}; + +enum FpbMeasurementsTimestampType { + TIME_UNSPECIFIED = 0, //!< Timestamp type unspecified, measurement will not be processed + TIME_TOA = 1, //!< Use time of arrival of the measurement +}; + +struct FpbMeasurementsMeas { + int32_t meas_x; //!< Measurement x + int32_t meas_y; //!< Measurement y + int32_t meas_z; //!< Measurement z + uint8_t meas_x_valid; //!< Validity of measurement x (1 = meas_x contains valid data, 0 = data invalid or n/a) + uint8_t meas_y_valid; //!< Validity of measurement y (1 = meas_x contains valid data, 0 = data invalid or n/a) + uint8_t meas_z_valid; //!< Validity of measurement z (1 = meas_x contains valid data, 0 = data invalid or n/a) + uint8_t meas_type; //!< See FpbMeasurementsMeasType + uint8_t meas_loc; //!< See FpbMeasurementsMeasLoc + uint8_t reserved1[4]; //!< Reserved for future use. Set to 0. + uint8_t timestamp_type; //!< See FpbMeasurementsTimestampType + uint16_t gps_wno; //!< GPS week number [-] + uint32_t gps_tow; //!< GPS time of week [ms] or monotonic time [-] +}; + +static_assert(sizeof(FpbMeasurementsMeas) == 28, ""); +const int FP_B_MEASUREMENTS_BODY_SIZE = 28; + +} // namespace fixposition + +#endif // __FIXPOSITION_DRIVER_LIB_FPBMEASUREMENTS__ diff --git a/fixposition_driver_lib/include/fixposition_driver_lib/rawdmi.hpp b/fixposition_driver_lib/include/fixposition_driver_lib/rawdmi.hpp deleted file mode 100644 index dc39318..0000000 --- a/fixposition_driver_lib/include/fixposition_driver_lib/rawdmi.hpp +++ /dev/null @@ -1,45 +0,0 @@ -/** - * @file - * @brief Declaration of RAWDMI message struct - * - * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Fixposition AG - * / /\ \ All right reserved. - * /__/ \__\ - * \endverbatim - * - */ - -#ifndef __FIXPOSITION_DRIVER_LIB_RAWDMI__ -#define __FIXPOSITION_DRIVER_LIB_RAWDMI__ - -#include -#include - -namespace fixposition { - - -/** - * @brief RAWDMI message struct - * - */ -struct RAWDMI { - uint8_t head1; - uint8_t head2; - uint8_t head3; - uint8_t payloadLen; - uint16_t msgId; - uint16_t wno; - int32_t tow; - int32_t dmi1; - int32_t dmi2; - int32_t dmi3; - int32_t dmi4; - int32_t mask; -}; - -} // namespace fixposition - -#endif // __FIXPOSITION_DRIVER_LIB_RAWDMI__ diff --git a/fixposition_driver_lib/src/fixposition_driver.cpp b/fixposition_driver_lib/src/fixposition_driver.cpp index 2d3c188..50f0155 100644 --- a/fixposition_driver_lib/src/fixposition_driver.cpp +++ b/fixposition_driver_lib/src/fixposition_driver.cpp @@ -16,16 +16,17 @@ #include #include #include + #include /* PACKAGE */ +#include +#include +#include #include #include #include #include -#include -#include -#include #include #include #include @@ -56,21 +57,6 @@ FixpositionDriver::FixpositionDriver(const FixpositionDriverParams& params) : pa } } - // static headers - rawdmi_.head1 = 0xaa; - rawdmi_.head2 = 0x44; - rawdmi_.head3 = 0x13; - rawdmi_.payloadLen = 20; - rawdmi_.msgId = 2269; - // these to be filled by each rosmsg - rawdmi_.wno = 0; - rawdmi_.tow = 0; - rawdmi_.dmi1 = 0; - rawdmi_.dmi2 = 0; - rawdmi_.dmi3 = 0; - rawdmi_.dmi4 = 0; - rawdmi_.mask = 0; - // initialize converters if (!InitializeConverters()) { throw std::runtime_error("Could not initialize output converter!"); @@ -100,38 +86,60 @@ bool FixpositionDriver::Connect() { } } -void FixpositionDriver::WsCallback(const std::vector& speeds) { - if (speeds.size() == 1) { - rawdmi_.dmi1 = speeds[0]; - rawdmi_.mask = (1 << 0) | (0 << 1) | (0 << 2) | (0 << 3); - } else if (speeds.size() == 2) { - rawdmi_.dmi1 = speeds[0]; - rawdmi_.dmi2 = speeds[1]; - rawdmi_.mask = (1 << 0) | (1 << 1) | (0 << 2) | (0 << 3) | (1 << 11); - } else if (speeds.size() == 4) { - rawdmi_.dmi1 = speeds[0]; - rawdmi_.dmi2 = speeds[1]; - rawdmi_.dmi3 = speeds[2]; - rawdmi_.dmi4 = speeds[3]; - rawdmi_.mask = (1 << 0) | (1 << 1) | (1 << 2) | (1 << 3); - } else { - return; +void FixpositionDriver::WsCallback( + const std::unordered_map>>& sensors_meas) { + std::vector sensor_measurements; + for (const auto& sensor : sensors_meas) { + const FpbMeasurementsMeasLoc location = WsMeasStringToLoc(sensor.first); + if (location == MEASLOC_UNSPECIFIED) { + std::cerr << "Unknown measurement type will not be processed!\n"; + continue; + } + FpbMeasurementsMeas fpb_meas; + if (FillWsSensorMeas(sensor.second, location, fpb_meas)) { + sensor_measurements.push_back(fpb_meas); + } } - // Calculate CRC - const uint32_t checksum = nov_crc32((const uint8_t*)&rawdmi_, sizeof(rawdmi_)); + const size_t num_meas = sensor_measurements.size(); + if (num_meas == 0 || num_meas > 10) { + std::cerr << "Number of wheel speed sensors is invalid.\n"; + return; + } - // Compose entire message - uint8_t message[sizeof(rawdmi_) + sizeof(checksum)]; - memcpy(&message[0], &rawdmi_, sizeof(rawdmi_)); - memcpy(&message[sizeof(rawdmi_)], &checksum, sizeof(checksum)); + FpbHeader header; + header.sync1 = 0x66; + header.sync2 = 0x21; + header.msg_id = 2001; + header.payload_size = FP_B_MEASUREMENTS_HEAD_SIZE + (FP_B_MEASUREMENTS_BODY_SIZE * num_meas); + header.time = 0; + + FpbMeasurementsHeader meas_header; + meas_header.version = 1; + meas_header.num_meas = num_meas; + std::fill(&meas_header.reserved0[0], &meas_header.reserved0[6], 0); + + const int msg_sz = + FP_B_HEAD_SIZE + FP_B_MEASUREMENTS_HEAD_SIZE + (FP_B_MEASUREMENTS_BODY_SIZE * num_meas) + FP_B_CRC_SIZE; + std::vector message(msg_sz); + + memcpy(&message[0], (uint8_t*)&header, sizeof(header)); + memcpy(&message[FP_B_HEAD_SIZE], (uint8_t*)&meas_header, sizeof(meas_header)); + for (size_t i = 0; i < num_meas; i++) { + memcpy(&message[FP_B_HEAD_SIZE + FP_B_MEASUREMENTS_HEAD_SIZE + (FP_B_MEASUREMENTS_BODY_SIZE * i)], + (uint8_t*)&sensor_measurements[i], sizeof(sensor_measurements[i])); + } + const uint32_t crc = Crc32fpb( + message.data(), FP_B_HEAD_SIZE + FP_B_MEASUREMENTS_HEAD_SIZE + (FP_B_MEASUREMENTS_BODY_SIZE * num_meas)); + memcpy(&message[FP_B_HEAD_SIZE + FP_B_MEASUREMENTS_HEAD_SIZE + (FP_B_MEASUREMENTS_BODY_SIZE * num_meas)], &crc, + sizeof(crc)); switch (params_.fp_output.type) { case INPUT_TYPE::TCP: - send(this->client_fd_, &message[0], sizeof(message), MSG_DONTWAIT); + send(this->client_fd_, &message[0], message.size(), MSG_DONTWAIT); break; case INPUT_TYPE::SERIAL: - (void)!write(this->client_fd_, &message[0], sizeof(message)); + (void)!write(this->client_fd_, &message[0], message.size()); // Suppress warning - https://stackoverflow.com/a/64407070/7944565 break; default: @@ -140,6 +148,38 @@ void FixpositionDriver::WsCallback(const std::vector& speeds) { } } +bool FixpositionDriver::FillWsSensorMeas(const std::vector>& meas_vec, + const FpbMeasurementsMeasLoc meas_loc, FpbMeasurementsMeas& meas_fpb) { + const size_t num_axis = meas_vec.size(); + if (num_axis != 3) { + std::cerr << "Wheelspeed sensor has an invalid number of measurements.\n"; + return false; + } + meas_fpb.meas_type = MEASTYPE_VELOCITY; + meas_fpb.meas_loc = meas_loc; + std::fill(&meas_fpb.reserved1[0], &meas_fpb.reserved1[4], 0); + // In the current setup, the sensor will handle the timestamping as time of arrival. + meas_fpb.timestamp_type = TIME_TOA; + meas_fpb.gps_wno = 0; + meas_fpb.gps_tow = 0; + meas_fpb.meas_x_valid = meas_vec[0].first; + meas_fpb.meas_x = meas_vec[0].second; + meas_fpb.meas_y_valid = meas_vec[1].first; + meas_fpb.meas_y = meas_vec[1].second; + meas_fpb.meas_z_valid = meas_vec[2].first; + meas_fpb.meas_z = meas_vec[2].second; + return true; +} + +FpbMeasurementsMeasLoc FixpositionDriver::WsMeasStringToLoc(const std::string& meas_loc) { + if (meas_loc == "RC") return MEASLOC_RC; + if (meas_loc == "FR") return MEASLOC_FR; + if (meas_loc == "FL") return MEASLOC_FL; + if (meas_loc == "RR") return MEASLOC_RR; + if (meas_loc == "RL") return MEASLOC_RL; + return MEASLOC_UNSPECIFIED; +} + bool FixpositionDriver::InitializeConverters() { for (const auto& format : params_.fp_output.formats) { if (format == "ODOMETRY") { @@ -288,7 +328,7 @@ void FixpositionDriver::NovConvertAndPublish(const uint8_t* msg, int size) { bool FixpositionDriver::CreateTCPSocket() { if (client_fd_ != -1) { - std::cerr << "TCP connection already exists" << "\n"; + std::cerr << "TCP connection already exists.\n"; return true; } @@ -318,7 +358,7 @@ bool FixpositionDriver::CreateTCPSocket() { bool FixpositionDriver::CreateSerialConnection() { if (client_fd_ != -1) { - std::cerr << "Serial connection already exists" << "\n"; + std::cerr << "Serial connection already exists.\n"; return true; } diff --git a/fixposition_driver_ros1/CMakeLists.txt b/fixposition_driver_ros1/CMakeLists.txt index 74ce039..408b0d8 100644 --- a/fixposition_driver_ros1/CMakeLists.txt +++ b/fixposition_driver_ros1/CMakeLists.txt @@ -26,6 +26,7 @@ add_message_files( FILES VRTK.msg Speed.msg + WheelSensor.msg NMEA.msg ) diff --git a/fixposition_driver_ros1/launch/driver_with_odom_converter.launch b/fixposition_driver_ros1/launch/driver_with_odom_converter.launch index da86e64..bf789cf 100644 --- a/fixposition_driver_ros1/launch/driver_with_odom_converter.launch +++ b/fixposition_driver_ros1/launch/driver_with_odom_converter.launch @@ -1,6 +1,6 @@ - + - + diff --git a/fixposition_driver_ros1/launch/tcp.yaml b/fixposition_driver_ros1/launch/tcp.yaml index 7bf49c4..bb8add1 100644 --- a/fixposition_driver_ros1/launch/tcp.yaml +++ b/fixposition_driver_ros1/launch/tcp.yaml @@ -2,7 +2,7 @@ fp_output: formats: ["ODOMETRY", "LLH", "RAWIMU", "CORRIMU", "GPGGA", "GPZDA", "GPRMC", "TF"] type: tcp port: "21000" - ip: "10.0.2.1" # change to VRTK2's IP address in the network + ip: "172.22.1.46" # 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/msg/Speed.msg b/fixposition_driver_ros1/msg/Speed.msg index ed43cd0..d8efeab 100644 --- a/fixposition_driver_ros1/msg/Speed.msg +++ b/fixposition_driver_ros1/msg/Speed.msg @@ -13,9 +13,5 @@ # #################################################################################################### -# Velocity values in [mm/s] (or [mrad/s]) as integer 32bit -# 2 Options: -# Option 1: One vehicle speed, of sensor RC -# Option 2: One vehicle speed, of sensor RC and the yaw rate of the vehicle -# Option 3: Fill in 4 Values of 4 wheels, in the order of FR, FL, RR, RL -int32[] speeds +# Velocity values for one or several WheelSensors +fixposition_driver_ros1/WheelSensor[] sensors diff --git a/fixposition_driver_ros1/msg/WheelSensor.msg b/fixposition_driver_ros1/msg/WheelSensor.msg new file mode 100644 index 0000000..33c7552 --- /dev/null +++ b/fixposition_driver_ros1/msg/WheelSensor.msg @@ -0,0 +1,29 @@ +#################################################################################################### +# +# Copyright (c) 2023 ___ ___ +# \\ \\ / / +# \\ \\/ / +# / /\\ \\ +# /__/ \\__\\ Fixposition AG +# +#################################################################################################### +# +# Individual wheelspeed measurement +# +# +#################################################################################################### +# Standard metadata +std_msgs/Header header + +# Location of the wheelspeed measurement (one of: RC, FR, FL, RR, RL) +string location + +# Velocity values in [mm/s] as integer 32bit +int32 vx +int32 vy +int32 vz + +# Velocity validity +bool vx_valid +bool vy_valid +bool vz_valid diff --git a/fixposition_driver_ros1/src/fixposition_driver_node.cpp b/fixposition_driver_ros1/src/fixposition_driver_node.cpp index b0f8fa4..0609519 100644 --- a/fixposition_driver_ros1/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros1/src/fixposition_driver_node.cpp @@ -256,7 +256,13 @@ void FixpositionDriverNode::Run() { } void FixpositionDriverNode::WsCallback(const fixposition_driver_ros1::SpeedConstPtr& msg) { - FixpositionDriver::WsCallback(msg->speeds); + std::unordered_map>> measurements; + for (const auto &sensor : msg->sensors) { + measurements[sensor.location].push_back({sensor.vx_valid, sensor.vx}); + measurements[sensor.location].push_back({sensor.vy_valid, sensor.vy}); + measurements[sensor.location].push_back({sensor.vz_valid, sensor.vz}); + } + FixpositionDriver::WsCallback(measurements); } void FixpositionDriverNode::BestGnssPosToPublishNavSatFix(const Oem7MessageHeaderMem* header, diff --git a/fixposition_driver_ros2/CMakeLists.txt b/fixposition_driver_ros2/CMakeLists.txt index 7353ac4..f3c0f09 100644 --- a/fixposition_driver_ros2/CMakeLists.txt +++ b/fixposition_driver_ros2/CMakeLists.txt @@ -27,6 +27,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} msg/VRTK.msg msg/Speed.msg msg/NMEA.msg + msg/WheelSensor.msg DEPENDENCIES std_msgs nav_msgs diff --git a/fixposition_driver_ros2/msg/Speed.msg b/fixposition_driver_ros2/msg/Speed.msg index c671bbe..9a5e295 100644 --- a/fixposition_driver_ros2/msg/Speed.msg +++ b/fixposition_driver_ros2/msg/Speed.msg @@ -10,9 +10,5 @@ # #################################################################################################### -# Velocity values in [mm/s] (or [mrad/s]) as integer 32bit -# 2 Options: -# Option 1: One vehicle speed, of sensor RC -# Option 2: One vehicle speed, of sensor RC and the yaw rate of the vehicle -# Option 3: Fill in 4 Values of 4 wheels, in the order of FR, FL, RR, RL -int32[] speeds +# Velocity values for one or several WheelSensors +fixposition_driver_ros2/WheelSensor[] sensors diff --git a/fixposition_driver_ros2/msg/WheelSensor.msg b/fixposition_driver_ros2/msg/WheelSensor.msg new file mode 100644 index 0000000..c59889b --- /dev/null +++ b/fixposition_driver_ros2/msg/WheelSensor.msg @@ -0,0 +1,26 @@ +#################################################################################################### +# +# Copyright (c) 2023 +# Fixposition AG +# +#################################################################################################### +# +# Fixposition WheelSensor Message +# +# +#################################################################################################### +# Standard metadata +std_msgs/Header header + +# Location of the wheelspeed measurement (one of: RC, FR, FL, RR, RL) +string location + +# Velocity values in [mm/s] as integer 32bit +int32 vx +int32 vy +int32 vz + +# Velocity validity +bool vx_valid +bool vy_valid +bool vz_valid diff --git a/fixposition_driver_ros2/src/fixposition_driver_node.cpp b/fixposition_driver_ros2/src/fixposition_driver_node.cpp index 938ed17..c8b7469 100644 --- a/fixposition_driver_ros2/src/fixposition_driver_node.cpp +++ b/fixposition_driver_ros2/src/fixposition_driver_node.cpp @@ -263,7 +263,13 @@ void FixpositionDriverNode::PublishNmea(NmeaMessage data) { } void FixpositionDriverNode::WsCallback(const fixposition_driver_ros2::msg::Speed::ConstSharedPtr msg) { - FixpositionDriver::WsCallback(msg->speeds); + std::unordered_map>> measurements; + for (const auto &sensor : msg->sensors) { + measurements[sensor.location].push_back({sensor.vx_valid, sensor.vx}); + measurements[sensor.location].push_back({sensor.vy_valid, sensor.vy}); + measurements[sensor.location].push_back({sensor.vz_valid, sensor.vz}); + } + FixpositionDriver::WsCallback(measurements); } void FixpositionDriverNode::BestGnssPosToPublishNavSatFix(const Oem7MessageHeaderMem* header, diff --git a/fixposition_odometry_converter/launch/odom_converter.launch b/fixposition_odometry_converter/launch/odom_converter.launch deleted file mode 100644 index 6b1e8dc..0000000 --- a/fixposition_odometry_converter/launch/odom_converter.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - diff --git a/fixposition_odometry_converter/CMakeLists.txt b/fixposition_odometry_converter_ros1/CMakeLists.txt similarity index 96% rename from fixposition_odometry_converter/CMakeLists.txt rename to fixposition_odometry_converter_ros1/CMakeLists.txt index 52d5a4d..9de66b7 100644 --- a/fixposition_odometry_converter/CMakeLists.txt +++ b/fixposition_odometry_converter_ros1/CMakeLists.txt @@ -1,6 +1,6 @@ # GENERAL ============================================================================================================== cmake_minimum_required(VERSION 3.5) -project(fixposition_odometry_converter LANGUAGES CXX) +project(fixposition_odometry_converter_ros1 LANGUAGES CXX) set(CMAKE_CXX_STANDARD 14) set(CMAKE_BUILD_TYPE "Release") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -fdiagnostics-color=always -Wall -Wextra -Wpedantic -Wno-unused-parameter") diff --git a/fixposition_odometry_converter/README.md b/fixposition_odometry_converter_ros1/README.md similarity index 55% rename from fixposition_odometry_converter/README.md rename to fixposition_odometry_converter_ros1/README.md index 6095c03..83e2acb 100644 --- a/fixposition_odometry_converter/README.md +++ b/fixposition_odometry_converter_ros1/README.md @@ -3,9 +3,10 @@ ## Description -An extra node is provided to help with the integration of the wheel odometry on your vehicle. This node is intended to be used as a middleware if you already have a topic with the wheel odometry values running on your system. At the moment, messages of the type `geometry_msgs/Twist`, `geometry_msgs/TwistWithCov` and `nav_msgs/Odometry` are accepted. To select one of these, modify the `topic_type` in the `odom_converter.yaml` file. The x component of the velocity in the input messages is then extracted, converted, and republished to the `/fixposition/speed` topic, where they will be consumed by the VRTK2. If the param `use_angular` is selected, the z component of the angular velocity (yaw rate) of the input messages is also extracted and placed in the speed vector. +An extra node is provided to help with the integration of the wheel odometry on your vehicle. This node is intended to be used as a middleware if you already have a topic with the wheel odometry values running on your system. At the moment, messages of the type `geometry_msgs/Twist`, `geometry_msgs/TwistWithCov` and `nav_msgs/Odometry` are accepted. To select one of these, modify the `topic_type` in the `odom_converter.yaml` file. You can choose to send just the x component of this velocity, both the x and y components, or the entire velocity. If sending more than one component (and intending for it to be used in the fusion engine), make sure that the wheelspeed sensor's configuration has the dimensions field correctly configured. +The speed values from the selected topic are extracted, converted, and republished to the `/fixposition/speed` topic, where they will be consumed by the main ROS driver node, and sent to the VRTK2. -_Please note that currently, the odometry converter only works for situations where the desired input odometry has just one or two values, i.e. the total vehicle speed or the total vehicle speed and yaw rate. It also assumes that the x axis of the odometry output and the VRTK2 axis are aligned. For situations where all 4 inputs are desired, a custom converter is necessary._ +_Please note that currently, the odometry converter only works for situations where the desired input odometry has just one value, the total vehicle speed or the total vehicle speed, configured to be RC. For situations where more sensor inputs are desired, a custom converter is necessary._ ## Input Parameters @@ -15,7 +16,7 @@ The `odom_converter.yaml` file exposes the necessary parameters for the correct After the configuration is set, to launch the node simply run: - `roslaunch fixposition_odometry_converter odom_converter.launch` + `roslaunch fixposition_odometry_converter_ros1 odom_converter.launch` # License diff --git a/fixposition_odometry_converter/include/fixposition_odometry_converter/odom_converter.hpp b/fixposition_odometry_converter_ros1/include/fixposition_odometry_converter_ros1/odom_converter.hpp similarity index 87% rename from fixposition_odometry_converter/include/fixposition_odometry_converter/odom_converter.hpp rename to fixposition_odometry_converter_ros1/include/fixposition_odometry_converter_ros1/odom_converter.hpp index ba933f8..750cba7 100644 --- a/fixposition_odometry_converter/include/fixposition_odometry_converter/odom_converter.hpp +++ b/fixposition_odometry_converter_ros1/include/fixposition_odometry_converter_ros1/odom_converter.hpp @@ -30,7 +30,7 @@ /* PACKAGE */ #include -#include +#include namespace fixposition { @@ -56,11 +56,11 @@ class OdomConverter { void Subscribe(); /** - * @brief Converts and publishes the speed to an integer value in [mm/s] + * @brief Converts the speed(s) to an integer value in [mm/s] and publishes it * - * @param speed + * @param speeds vector containing x, y, and z speeds and their validity flag */ - void ConvertAndPublish(const double speed, const double angular, bool use_angular = false); + void ConvertAndPublish(const std::vector> speeds); private: /** diff --git a/fixposition_odometry_converter/include/fixposition_odometry_converter/params.hpp b/fixposition_odometry_converter_ros1/include/fixposition_odometry_converter_ros1/params.hpp similarity index 94% rename from fixposition_odometry_converter/include/fixposition_odometry_converter/params.hpp rename to fixposition_odometry_converter_ros1/include/fixposition_odometry_converter_ros1/params.hpp index e06f4b2..620508c 100644 --- a/fixposition_odometry_converter/include/fixposition_odometry_converter/params.hpp +++ b/fixposition_odometry_converter_ros1/include/fixposition_odometry_converter_ros1/params.hpp @@ -31,7 +31,9 @@ struct OdomInputParams { std::string input_topic; std::string fixposition_speed_topic; int multiplicative_factor; - bool use_angular; + bool use_x; + bool use_y; + bool use_z; /** * @brief Load all parameters from ROS parameter server * diff --git a/fixposition_odometry_converter_ros1/launch/odom_converter.launch b/fixposition_odometry_converter_ros1/launch/odom_converter.launch new file mode 100644 index 0000000..1594048 --- /dev/null +++ b/fixposition_odometry_converter_ros1/launch/odom_converter.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/fixposition_odometry_converter/launch/odom_converter.yaml b/fixposition_odometry_converter_ros1/launch/odom_converter.yaml similarity index 64% rename from fixposition_odometry_converter/launch/odom_converter.yaml rename to fixposition_odometry_converter_ros1/launch/odom_converter.yaml index 5c2c8db..224a2bb 100644 --- a/fixposition_odometry_converter/launch/odom_converter.yaml +++ b/fixposition_odometry_converter_ros1/launch/odom_converter.yaml @@ -2,4 +2,6 @@ topic_type: "Twist" # Twist, TwistWithCov or Odometry input_topic: "/robot/odom" # Original topic name fixposition_speed_topic: "/fixposition/speed" # Fixposition topic name, to be subscribed by the fixposition_driver multiplicative_factor: 1000 # To convert from the original unit of measurement to mm/s -use_angular: false # If the angular velocity of the speed message is filled +use_x: true # Transmit the x axis of the input velocity +use_y: true # Transmit the y axis of the input velocity +use_z: true # Transmit the z axis of the input velocity diff --git a/fixposition_odometry_converter/launch/rosconsole.conf b/fixposition_odometry_converter_ros1/launch/rosconsole.conf similarity index 100% rename from fixposition_odometry_converter/launch/rosconsole.conf rename to fixposition_odometry_converter_ros1/launch/rosconsole.conf diff --git a/fixposition_odometry_converter/package.xml b/fixposition_odometry_converter_ros1/package.xml similarity index 89% rename from fixposition_odometry_converter/package.xml rename to fixposition_odometry_converter_ros1/package.xml index ce1696a..f127506 100644 --- a/fixposition_odometry_converter/package.xml +++ b/fixposition_odometry_converter_ros1/package.xml @@ -1,6 +1,6 @@ - fixposition_odometry_converter + fixposition_odometry_converter_ros1 1.0.0 Conversion from Odometry messages to Fixposition's Speed input diff --git a/fixposition_odometry_converter/src/odom_converter.cpp b/fixposition_odometry_converter_ros1/src/odom_converter.cpp similarity index 51% rename from fixposition_odometry_converter/src/odom_converter.cpp rename to fixposition_odometry_converter_ros1/src/odom_converter.cpp index b0e35b3..d50b7f1 100644 --- a/fixposition_odometry_converter/src/odom_converter.cpp +++ b/fixposition_odometry_converter_ros1/src/odom_converter.cpp @@ -13,12 +13,12 @@ */ /* PACKAGE */ -#include +#include namespace fixposition { OdomConverter::OdomConverter(ros::NodeHandle* nh) : nh_(*nh) { // read parameters - if (!params_.LoadFromRos("/fixposition_odometry_converter")) { + if (!params_.LoadFromRos("/fixposition_odometry_converter_ros1")) { ROS_ERROR_STREAM("Parameter Loading failed, shutting down..."); ros::shutdown(); } @@ -45,29 +45,48 @@ void OdomConverter::Subscribe() { } } -void OdomConverter::ConvertAndPublish(const double speed, const double angular, bool use_angular) { +void OdomConverter::ConvertAndPublish(const std::vector> speeds) { if (ws_pub_.getNumSubscribers() > 0) { - const int int_speed = round(speed * params_.multiplicative_factor); - const int angular_speed = round(angular * params_.multiplicative_factor); - fixposition_driver_ros1::Speed msg; - msg.speeds.push_back(int_speed); - if (params_.use_angular) { - msg.speeds.push_back(angular_speed); + if (speeds.size() != 3) { + ROS_ERROR("Speed vector has an invalid size!"); + return; } + fixposition_driver_ros1::Speed msg; + fixposition_driver_ros1::WheelSensor sensor; + sensor.location = "RC"; + sensor.vx_valid = speeds[0].first; + sensor.vx = round(speeds[0].second * params_.multiplicative_factor); + sensor.vy_valid = speeds[1].first; + sensor.vy = round(speeds[1].second * params_.multiplicative_factor); + sensor.vz_valid = speeds[2].first; + sensor.vz = round(speeds[2].second * params_.multiplicative_factor); + msg.sensors.push_back(sensor); ws_pub_.publish(msg); } } void OdomConverter::TwistWithCovCallback(const geometry_msgs::TwistWithCovarianceConstPtr& msg) { - ConvertAndPublish(msg->twist.linear.x, msg->twist.angular.z, params_.use_angular); + std::vector> velocities; + velocities.push_back({params_.use_x, msg->twist.linear.x}); + velocities.push_back({params_.use_y, msg->twist.linear.y}); + velocities.push_back({params_.use_z, msg->twist.linear.z}); + ConvertAndPublish(velocities); } void OdomConverter::OdometryCallback(const nav_msgs::OdometryConstPtr& msg) { - ConvertAndPublish(msg->twist.twist.linear.x, msg->twist.twist.angular.z, params_.use_angular); + std::vector> velocities; + velocities.push_back({params_.use_x, msg->twist.twist.linear.x}); + velocities.push_back({params_.use_y, msg->twist.twist.linear.y}); + velocities.push_back({params_.use_z, msg->twist.twist.linear.z}); + ConvertAndPublish(velocities); } void OdomConverter::TwistCallback(const geometry_msgs::TwistConstPtr& msg) { - ConvertAndPublish(msg->linear.x, msg->angular.z, params_.use_angular); + std::vector> velocities; + velocities.push_back({params_.use_x, msg->linear.x}); + velocities.push_back({params_.use_y, msg->linear.y}); + velocities.push_back({params_.use_z, msg->linear.z}); + ConvertAndPublish(velocities); } } // namespace fixposition diff --git a/fixposition_odometry_converter/src/odom_converter_node.cpp b/fixposition_odometry_converter_ros1/src/odom_converter_node.cpp similarity index 74% rename from fixposition_odometry_converter/src/odom_converter_node.cpp rename to fixposition_odometry_converter_ros1/src/odom_converter_node.cpp index 42f2a49..411a467 100644 --- a/fixposition_odometry_converter/src/odom_converter_node.cpp +++ b/fixposition_odometry_converter_ros1/src/odom_converter_node.cpp @@ -17,14 +17,14 @@ #include /* PACKAGE */ -#include +#include int main(int argc, char** argv) { - ros::init(argc, argv, "fixposition_odometry_converter"); + ros::init(argc, argv, "fixposition_odometry_converter_ros1"); ros::NodeHandle node_handle; ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info); fixposition::OdomConverter odom_converter(&node_handle); - ROS_DEBUG("Starting fixposition_odometry_converter node..."); + ROS_DEBUG("Starting fixposition_odometry_converter_ros1 node..."); ros::spin(); ROS_DEBUG("Exiting."); return 0; diff --git a/fixposition_odometry_converter/src/params.cpp b/fixposition_odometry_converter_ros1/src/params.cpp similarity index 85% rename from fixposition_odometry_converter/src/params.cpp rename to fixposition_odometry_converter_ros1/src/params.cpp index cc043d0..23fbb17 100644 --- a/fixposition_odometry_converter/src/params.cpp +++ b/fixposition_odometry_converter_ros1/src/params.cpp @@ -17,7 +17,7 @@ #include /* PACKAGE */ -#include +#include namespace fixposition { @@ -25,7 +25,9 @@ bool OdomInputParams::LoadFromRos(const std::string& ns) { if (!ros::param::get(ns + "/fixposition_speed_topic", fixposition_speed_topic)) fixposition_speed_topic = "/fixposition/speed"; if (!ros::param::get(ns + "/multiplicative_factor", multiplicative_factor)) multiplicative_factor = 1000; - if (!ros::param::get(ns + "/use_angular", use_angular)) use_angular = false; + if (!ros::param::get(ns + "/use_x", use_x)) use_x = 1; + if (!ros::param::get(ns + "/use_y", use_y)) use_y = 0; + if (!ros::param::get(ns + "/use_z", use_z)) use_z = 0; if (!ros::param::get(ns + "/input_topic", input_topic)) { ROS_ERROR("Couldn't read the input topic name."); return false; diff --git a/fixposition_odometry_converter_ros2/README.md b/fixposition_odometry_converter_ros2/README.md index 697834b..f836182 100644 --- a/fixposition_odometry_converter_ros2/README.md +++ b/fixposition_odometry_converter_ros2/README.md @@ -3,9 +3,10 @@ ## Description -An extra node is provided to help with the integration of the wheel odometry on your vehicle. This node is intended to be used as a middleware if you already have a topic with the wheel odometry values running on your system. At the moment, messages of the type `geometry_msgs/Twist`, `geometry_msgs/TwistWithCov` and `nav_msgs/Odometry` are accepted. To select one of these, modify the `topic_type` in the `odom_converter.yaml` file. The x component of the velocity in the input messages is then extracted, converted, and republished to the `/fixposition/speed` topic, where they will be consumed by the VRTK2. If the param `use_angular` is selected, the z component of the angular velocity (yaw rate) of the input messages is also extracted and placed in the speed vector. +An extra node is provided to help with the integration of the wheel odometry on your vehicle. This node is intended to be used as a middleware if you already have a topic with the wheel odometry values running on your system. At the moment, messages of the type `geometry_msgs/Twist`, `geometry_msgs/TwistWithCov` and `nav_msgs/Odometry` are accepted. To select one of these, modify the `topic_type` in the `odom_converter.yaml` file. You can choose to send just the x component of this velocity, both the x and y components, or the entire velocity. If sending more than one component (and intending for it to be used in the fusion engine), make sure that the wheelspeed sensor's configuration has the `Dimensions` field correctly configured. +The speed values from the selected topic are extracted, converted, and republished to the `/fixposition/speed` topic, where they will be consumed by the main ROS driver node, and sent to the VRTK2. -_Please note that currently, the odometry converter only works for situations where the desired input odometry has just one or two values, i.e. the total vehicle speed or the total vehicle speed and yaw rate. It also assumes that the x axis of the odometry output and the VRTK2 axis are aligned. For situations where all 4 inputs are desired, a custom converter is necessary._ +_Please note that currently, the odometry converter only works for situations where the desired input odometry has just one value, the total vehicle speed or the total vehicle speed, configured to be RC. For situations where more sensor inputs are desired, a custom converter is necessary._ ## Input Parameters diff --git a/fixposition_odometry_converter_ros2/include/fixposition_odometry_converter_ros2/odom_converter_node.hpp b/fixposition_odometry_converter_ros2/include/fixposition_odometry_converter_ros2/odom_converter_node.hpp index 382ed3e..fd9161c 100644 --- a/fixposition_odometry_converter_ros2/include/fixposition_odometry_converter_ros2/odom_converter_node.hpp +++ b/fixposition_odometry_converter_ros2/include/fixposition_odometry_converter_ros2/odom_converter_node.hpp @@ -51,13 +51,11 @@ class OdomConverterNode : public rclcpp::Node { void Subscribe(); /** - * @brief Converts and publishes the speed to an integer value in [mm/s] + * @brief Converts the speed(s) to an integer value in [mm/s] and publishes it * - * @param speed - * @param angular - * @param use_angular default false + * @param speeds vector containing x, y, and z speeds and their validity flag */ - void ConvertAndPublish(const double speed, const double angular, bool use_angular = false); + void ConvertAndPublish(const std::vector> speeds); private: /** 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 4513ae2..71257e8 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 @@ -32,7 +32,9 @@ struct OdomInputParams { std::string input_topic; std::string fixposition_speed_topic; int multiplicative_factor; - bool use_angular; + bool use_x; + bool use_y; + bool use_z; /** * @brief Load all parameters from ROS 2 * diff --git a/fixposition_odometry_converter_ros2/launch/odom_converter.yaml b/fixposition_odometry_converter_ros2/launch/odom_converter.yaml index a7fb525..05ea555 100644 --- a/fixposition_odometry_converter_ros2/launch/odom_converter.yaml +++ b/fixposition_odometry_converter_ros2/launch/odom_converter.yaml @@ -1,7 +1,9 @@ /**: ros__parameters: - topic_type: "Odometry" # Twist, TwistWithCov or Odometry + topic_type: "Twist" # Twist, TwistWithCov or Odometry input_topic: "/robot/odom" # Original topic name fixposition_speed_topic: "/fixposition/speed" # Fixposition topic name, to be subscribed by the fixposition_driver multiplicative_factor: 1000 # To convert from the original unit of measurement to mm/s - use_angular: false # If the angular velocity of the speed message is filled + use_x: true # Transmit the x axis of the input velocity + use_y: true # Transmit the y axis of the input velocity + use_z: true # Transmit the z axis of the input velocity diff --git a/fixposition_odometry_converter_ros2/src/odom_converter_node.cpp b/fixposition_odometry_converter_ros2/src/odom_converter_node.cpp index 68ad991..21214a3 100644 --- a/fixposition_odometry_converter_ros2/src/odom_converter_node.cpp +++ b/fixposition_odometry_converter_ros2/src/odom_converter_node.cpp @@ -51,29 +51,48 @@ void OdomConverterNode::Subscribe() { } } -void OdomConverterNode::ConvertAndPublish(const double speed, const double angular, bool use_angular) { +void OdomConverterNode::ConvertAndPublish(const std::vector> speeds) { if (ws_pub_->get_subscription_count() > 0) { - const int int_speed = round(speed * params_.multiplicative_factor); - const int angular_speed = round(angular * params_.multiplicative_factor); - fixposition_driver_ros2::msg::Speed msg; - msg.speeds.push_back(int_speed); - if (params_.use_angular) { - msg.speeds.push_back(angular_speed); + if (speeds.size() != 3) { + RCLCPP_ERROR(this->get_logger(), "Speed vector has an invalid size"); + return; } + fixposition_driver_ros2::msg::Speed msg; + fixposition_driver_ros2::msg::WheelSensor sensor; + sensor.location = "RC"; + sensor.vx_valid = speeds[0].first; + sensor.vx = round(speeds[0].second * params_.multiplicative_factor); + sensor.vy_valid = speeds[1].first; + sensor.vy = round(speeds[1].second * params_.multiplicative_factor); + sensor.vz_valid = speeds[2].first; + sensor.vz = round(speeds[2].second * params_.multiplicative_factor); + msg.sensors.push_back(sensor); ws_pub_->publish(msg); } } void OdomConverterNode::TwistWithCovCallback(const geometry_msgs::msg::TwistWithCovariance::SharedPtr msg) { - ConvertAndPublish(msg->twist.linear.x, msg->twist.angular.z, params_.use_angular); + std::vector> velocities; + velocities.push_back({params_.use_x, msg->twist.linear.x}); + velocities.push_back({params_.use_y, msg->twist.linear.y}); + velocities.push_back({params_.use_z, msg->twist.linear.z}); + ConvertAndPublish(velocities); } void OdomConverterNode::OdometryCallback(const nav_msgs::msg::Odometry::SharedPtr msg) { - ConvertAndPublish(msg->twist.twist.linear.x, msg->twist.twist.angular.z, params_.use_angular); + std::vector> velocities; + velocities.push_back({params_.use_x, msg->twist.twist.linear.x}); + velocities.push_back({params_.use_y, msg->twist.twist.linear.y}); + velocities.push_back({params_.use_z, msg->twist.twist.linear.z}); + ConvertAndPublish(velocities); } void OdomConverterNode::TwistCallback(const geometry_msgs::msg::Twist::SharedPtr msg) { - ConvertAndPublish(msg->linear.x, msg->angular.z, params_.use_angular); + std::vector> velocities; + velocities.push_back({params_.use_x, msg->linear.x}); + velocities.push_back({params_.use_y, msg->linear.y}); + velocities.push_back({params_.use_z, msg->linear.z}); + ConvertAndPublish(velocities); } } // namespace fixposition diff --git a/fixposition_odometry_converter_ros2/src/params.cpp b/fixposition_odometry_converter_ros2/src/params.cpp index e4abab2..709e98b 100644 --- a/fixposition_odometry_converter_ros2/src/params.cpp +++ b/fixposition_odometry_converter_ros2/src/params.cpp @@ -27,8 +27,14 @@ bool OdomInputParams::LoadFromRos(const rclcpp::node_interfaces::NodeParametersI param_itf->declare_parameter("multiplicative_factor", rclcpp::ParameterValue(1000)); multiplicative_factor = param_itf->get_parameter("multiplicative_factor").as_int(); - param_itf->declare_parameter("use_angular", rclcpp::ParameterValue(false)); - use_angular = param_itf->get_parameter("use_angular").as_bool(); + param_itf->declare_parameter("use_x", rclcpp::ParameterValue(true)); + use_x = param_itf->get_parameter("use_x").as_bool(); + + param_itf->declare_parameter("use_y", rclcpp::ParameterValue(false)); + use_y = param_itf->get_parameter("use_y").as_bool(); + + param_itf->declare_parameter("use_z", rclcpp::ParameterValue(false)); + use_z = param_itf->get_parameter("use_z").as_bool(); param_itf->declare_parameter("input_topic", rclcpp::PARAMETER_STRING); try {