Skip to content

Commit

Permalink
Support new fixposition ascii message specs
Browse files Browse the repository at this point in the history
Kailin committed Feb 28, 2023
1 parent d5798a5 commit 59defcd
Showing 17 changed files with 266 additions and 189 deletions.
245 changes: 126 additions & 119 deletions README.md

Large diffs are not rendered by default.

Original file line number Diff line number Diff line change
@@ -36,15 +36,6 @@ class BaseConverter {
*/
virtual void ConvertTokens(const std::vector<std::string>& tokens) = 0;

/**
* @brief Check the header and version of the corresponding message to make sure it is correct
*
* @param[in] msg_header expected header
* @param[in] msg_version expected version
* @return true correct
* @return false incorrect
*/
virtual bool CheckHeaderAndVersion(const std::string msg_header, const std::string msg_version) = 0;
};

//===================================================
Original file line number Diff line number Diff line change
@@ -91,10 +91,17 @@ struct VrtkData {
Eigen::Vector3d acceleration;
int fusion_status;
int imu_bias_status;
int gnss_status;
int gnss1_status;
int gnss2_status;
int wheelspeed_status;
std::string version;
VrtkData() : fusion_status(-1), imu_bias_status(-1), gnss_status(-1), wheelspeed_status(-1), version("") {
VrtkData()
: fusion_status(-1),
imu_bias_status(-1),
gnss1_status(-1),
gnss2_status(-1),
wheelspeed_status(-1),
version("") {
acceleration.setZero();
}
};
Original file line number Diff line number Diff line change
@@ -57,10 +57,6 @@ class OdometryConverter : public BaseConverter {

~OdometryConverter() = default;

bool CheckHeaderAndVersion(const std::string msg_header, const std::string msg_version) final {
return msg_header == header_ && std::stoi(msg_version) == kVersion_;
}

/**
* @brief Comma Delimited FP,ODOMETRY message, convert to Data structs and if available,
* call observers
@@ -84,7 +80,8 @@ class OdometryConverter : public BaseConverter {

private:
const std::string header_ = "ODOMETRY";
static constexpr const int kVersion_ = 1;
static constexpr const int kVersion_ = 2;
static constexpr const int kSize_ = 45;

//! transform between ECEF and ENU0
bool tf_ecef_enu0_set_; //!< flag to indicate if the tf is already set
Original file line number Diff line number Diff line change
@@ -33,9 +33,6 @@ class TfConverter : public BaseConverter {

~TfConverter() = default;

bool CheckHeaderAndVersion(const std::string msg_header, const std::string msg_version) final {
return msg_header == header_ && std::stoi(msg_version) == kVersion_;
}
/**
* @brief ake comma-delimited tokens of FP,TF message, convert to Data structs and if available,
* call observers
@@ -58,7 +55,8 @@ class TfConverter : public BaseConverter {

const std::string header_ = "TF";

static constexpr const int kVersion_ = 1;
static constexpr const int kVersion_ = 2;
static constexpr const int kSize_ = 14;
};
} // namespace fixposition
#endif // __FIXPOSITION_DRIVER_LIB_CONVERTER_TF__
20 changes: 18 additions & 2 deletions fixposition_driver_lib/src/imu.cpp
Original file line number Diff line number Diff line change
@@ -31,8 +31,24 @@ static constexpr const int rot_y_idx = 9;
static constexpr const int rot_z_idx = 10;

void ImuConverter::ConvertTokens(const std::vector<std::string>& tokens) {
if (tokens.size() != 11) {
std::cout << "Error in parsing IMU string with" << tokens.size() << "fields! IMU message will be empty.\n";
bool ok = tokens.size() == kSize_;
if (!ok) {
// Size is wrong
std::cout << "Error in parsing IMU string with " << tokens.size() << " fields! IMU message will be empty.\n";

} else {
// If size is ok, check version
const int version = std::stoi(tokens.at(msg_version_idx));

ok = version == kVersion_;
if (!ok) {
// Version is wrong
std::cout << "Error in parsing IMU string with verion " << version << " ! IMU message will be empty.\n";
}
}

if (!ok) {
// Reset message and return
msg_ = ImuData();
return;
}
26 changes: 22 additions & 4 deletions fixposition_driver_lib/src/llh.cpp
Original file line number Diff line number Diff line change
@@ -17,7 +17,7 @@ namespace fixposition {

/// msg field indices
static constexpr const int msg_type_idx = 1;
static constexpr const int msg_versio_idx = 2;
static constexpr const int msg_version_idx = 2;
static constexpr const int gps_week_idx = 3;
static constexpr const int gps_tow_idx = 4;
static constexpr const int latitude_idx = 5;
@@ -31,12 +31,30 @@ static constexpr const int pos_cov_nu_idx = 12;
static constexpr const int pos_cov_eu_idx = 13;

void LlhConverter::ConvertTokens(const std::vector<std::string>& tokens) {
if (tokens.size() != 14) {
std::cout << "Error in parsing LLH string with" << tokens.size()
<< "fields! NavSatFix message will be empty.\n";
bool ok = tokens.size() == kSize_;
if (!ok) {
// Size is wrong
std::cout << "Error in parsing LLH string with " << tokens.size()
<< " fields! NavSatFix message will be empty.\n";

} else {
// If size is ok, check version
const int version = std::stoi(tokens.at(msg_version_idx));

ok = version == kVersion_;
if (!ok) {
// Version is wrong
std::cout << "Error in parsing LLH string with verion " << version
<< " ! NavSatFix message will be empty.\n";
}
}

if (!ok) {
// Reset message and return
msg_ = NavSatFixData();
return;
}

// header stamps
msg_.stamp = ConvertGpsTime(tokens.at(gps_week_idx), tokens.at(gps_tow_idx));

67 changes: 43 additions & 24 deletions fixposition_driver_lib/src/odometry.cpp
Original file line number Diff line number Diff line change
@@ -46,27 +46,28 @@ static constexpr const int acc_y_idx = 19;
static constexpr const int acc_z_idx = 20;
static constexpr const int fusion_status_idx = 21;
static constexpr const int imu_bias_status_idx = 22;
static constexpr const int gnss_fix_type_idx = 23;
static constexpr const int wheelspeed_status_idx = 24;
static constexpr const int pos_cov_xx_idx = 25;
static constexpr const int pos_cov_yy_idx = 26;
static constexpr const int pos_cov_zz_idx = 27;
static constexpr const int pos_cov_xy_idx = 28;
static constexpr const int pos_cov_yz_idx = 29;
static constexpr const int pos_cov_xz_idx = 30;
static constexpr const int orientation_cov_xx_idx = 31;
static constexpr const int orientation_cov_yy_idx = 32;
static constexpr const int orientation_cov_zz_idx = 33;
static constexpr const int orientation_cov_xy_idx = 34;
static constexpr const int orientation_cov_yz_idx = 35;
static constexpr const int orientation_cov_xz_idx = 36;
static constexpr const int vel_cov_xx_idx = 37;
static constexpr const int vel_cov_yy_idx = 38;
static constexpr const int vel_cov_zz_idx = 39;
static constexpr const int vel_cov_xy_idx = 40;
static constexpr const int vel_cov_yz_idx = 41;
static constexpr const int vel_cov_xz_idx = 42;
static constexpr const int sw_version_idx = 43;
static constexpr const int gnss1_fix_type_idx = 23;
static constexpr const int gnss2_fix_type_idx = 24;
static constexpr const int wheelspeed_status_idx = 25;
static constexpr const int pos_cov_xx_idx = 26;
static constexpr const int pos_cov_yy_idx = 27;
static constexpr const int pos_cov_zz_idx = 28;
static constexpr const int pos_cov_xy_idx = 29;
static constexpr const int pos_cov_yz_idx = 30;
static constexpr const int pos_cov_xz_idx = 31;
static constexpr const int orientation_cov_xx_idx = 32;
static constexpr const int orientation_cov_yy_idx = 33;
static constexpr const int orientation_cov_zz_idx = 34;
static constexpr const int orientation_cov_xy_idx = 35;
static constexpr const int orientation_cov_yz_idx = 36;
static constexpr const int orientation_cov_xz_idx = 37;
static constexpr const int vel_cov_xx_idx = 38;
static constexpr const int vel_cov_yy_idx = 39;
static constexpr const int vel_cov_zz_idx = 40;
static constexpr const int vel_cov_xy_idx = 41;
static constexpr const int vel_cov_yz_idx = 42;
static constexpr const int vel_cov_xz_idx = 43;
static constexpr const int sw_version_idx = 44;

/**
* @brief Parse status flag field
@@ -84,13 +85,30 @@ int ParseStatusFlag(const std::vector<std::string>& tokens, const int idx) {
}

void OdometryConverter::ConvertTokens(const std::vector<std::string>& tokens) {
if (tokens.size() != 44) {
bool ok = tokens.size() == kSize_;
if (!ok) {
// Size is wrong
std::cout << "Error in parsing Odometry string with " << tokens.size()
<< " fields! odometry and status messages will be empty.\n";
<< " fields! Odometry and status messages will be empty.\n";

} else {
// If size is ok, check version
const int version = std::stoi(tokens.at(msg_version_idx));

ok = version == kVersion_;
if (!ok) {
// Version is wrong
std::cout << "Error in parsing Odometry string with verion " << version
<< " ! Odometry and status messages will be empty.\n";
}
}

if (!ok) {
// Reset message and return
msgs_ = Msgs();
return;
}

const int fusion_status = ParseStatusFlag(tokens, fusion_status_idx);

const bool fusion_init = fusion_status >= 3;
@@ -203,7 +221,8 @@ void OdometryConverter::ConvertTokens(const std::vector<std::string>& tokens) {
// Status, regardless of fusion_init
msgs_.vrtk.fusion_status = fusion_status;
msgs_.vrtk.imu_bias_status = ParseStatusFlag(tokens, imu_bias_status_idx);
msgs_.vrtk.gnss_status = ParseStatusFlag(tokens, gnss_fix_type_idx);
msgs_.vrtk.gnss1_status = ParseStatusFlag(tokens, gnss1_fix_type_idx);
msgs_.vrtk.gnss2_status = ParseStatusFlag(tokens, gnss2_fix_type_idx);
msgs_.vrtk.wheelspeed_status = ParseStatusFlag(tokens, wheelspeed_status_idx);
msgs_.vrtk.version = tokens.at(sw_version_idx).empty() ? "UNKNOWN" : tokens.at(sw_version_idx);

42 changes: 31 additions & 11 deletions fixposition_driver_lib/src/tf.cpp
Original file line number Diff line number Diff line change
@@ -21,23 +21,43 @@ namespace fixposition {
/// msg field indices
static constexpr const int msg_type_idx = 1;
static constexpr const int msg_version_idx = 2;
static constexpr const int from_frame_idx = 3;
static constexpr const int to_frame_idx = 4;
static constexpr const int translation_x_idx = 5;
static constexpr const int translation_y_idx = 6;
static constexpr const int translation_z_idx = 7;
static constexpr const int orientation_w_idx = 8;
static constexpr const int orientation_x_idx = 9;
static constexpr const int orientation_y_idx = 10;
static constexpr const int orientation_z_idx = 11;
static constexpr const int gps_week_idx = 3;
static constexpr const int gps_tow_idx = 4;
static constexpr const int from_frame_idx = 5;
static constexpr const int to_frame_idx = 6;
static constexpr const int translation_x_idx = 7;
static constexpr const int translation_y_idx = 8;
static constexpr const int translation_z_idx = 9;
static constexpr const int orientation_w_idx = 10;
static constexpr const int orientation_x_idx = 11;
static constexpr const int orientation_y_idx = 12;
static constexpr const int orientation_z_idx = 13;

void TfConverter::ConvertTokens(const std::vector<std::string>& tokens) {
if (tokens.size() != 12) {
std::cout << "Error in parsing TF string with " << tokens.size() << " fields! TFs will be empty.\n";
bool ok = tokens.size() == kSize_;
if (!ok) {
// Size is wrong
std::cout << "Error in parsing TF string with " << tokens.size() << " fields! TF will be empty.\n";

} else {
// If size is ok, check version
const int version = std::stoi(tokens.at(msg_version_idx));

ok = version == kVersion_;
if (!ok) {
// Version is wrong
std::cout << "Error in parsing TF string with verion " << version << " ! TF will be empty.\n";
}
}

if (!ok) {
// Reset message and return
msg_ = TfData();
return;
}

// header stamps
msg_.stamp = ConvertGpsTime(tokens.at(gps_week_idx), tokens.at(gps_tow_idx));
msg_.frame_id = "FP_" + tokens.at(from_frame_idx);
msg_.child_frame_id = "FP_" + tokens.at(to_frame_idx);

3 changes: 2 additions & 1 deletion fixposition_driver_ros1/msg/VRTK.msg
Original file line number Diff line number Diff line change
@@ -22,6 +22,7 @@ geometry_msgs/Vector3 acceleration # linear acceleration

int16 fusion_status # field for the fusion status
int16 imu_bias_status # field for the IMU bias status
int16 gnss_status # field for the gnss status
int16 gnss1_status # field for the gnss1 status
int16 gnss2_status # field for the gnss2 status
int16 wheelspeed_status # field for the wheelspeed status
string version # Fixposition software version
3 changes: 2 additions & 1 deletion fixposition_driver_ros1/src/data_to_ros1.cpp
Original file line number Diff line number Diff line change
@@ -72,7 +72,8 @@ void VrtkDataToMsg(const VrtkData& data, fixposition_driver_ros1::VRTK& msg) {
tf::vectorEigenToMsg(data.acceleration, msg.acceleration);
msg.fusion_status = data.fusion_status;
msg.imu_bias_status = data.imu_bias_status;
msg.gnss_status = data.gnss_status;
msg.gnss2_status = data.gnss1_status;
msg.gnss2_status = data.gnss2_status;
msg.wheelspeed_status = data.wheelspeed_status;
msg.version = data.version;
}
3 changes: 2 additions & 1 deletion fixposition_driver_ros2/msg/VRTK.msg
Original file line number Diff line number Diff line change
@@ -19,6 +19,7 @@ geometry_msgs/Vector3 acceleration # linear acceleration

int16 fusion_status # field for the fusion status
int16 imu_bias_status # field for the IMU bias status
int16 gnss_status # field for the gnss status
int16 gnss1_status # field for the gnss1 status
int16 gnss2_status # field for the gnss2 status
int16 wheelspeed_status # field for the wheelspeed status
string version # Fixposition software version
3 changes: 2 additions & 1 deletion fixposition_driver_ros2/src/data_to_ros2.cpp
Original file line number Diff line number Diff line change
@@ -76,7 +76,8 @@ void VrtkDataToMsg(const VrtkData& data, fixposition_driver_ros2::msg::VRTK& msg
tf2::toMsg(data.acceleration, msg.acceleration);
msg.fusion_status = data.fusion_status;
msg.imu_bias_status = data.imu_bias_status;
msg.gnss_status = data.gnss_status;
msg.gnss1_status = data.gnss1_status;
msg.gnss2_status = data.gnss2_status;
msg.wheelspeed_status = data.wheelspeed_status;
msg.version = data.version;
}
2 changes: 1 addition & 1 deletion fixposition_odometry_converter/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -14,7 +14,7 @@ find_package(catkin REQUIRED COMPONENTS
roscpp
nav_msgs
geometry_msgs
fixposition_driver
fixposition_driver_ros1
)

catkin_package(
Original file line number Diff line number Diff line change
@@ -26,7 +26,7 @@
#include <ros/ros.h>

/* PACKAGE */
#include <fixposition_driver/Speed.h>
#include <fixposition_driver_ros1/Speed.h>

#include <fixposition_odometry_converter/params.hpp>

2 changes: 1 addition & 1 deletion fixposition_odometry_converter/package.xml
Original file line number Diff line number Diff line change
@@ -12,5 +12,5 @@
<depend>roscpp</depend>
<depend>geometry_msgs</depend>
<depend>nav_msgs</depend>
<depend>fixposition_driver</depend>
<depend>fixposition_driver_ros1</depend>
</package>
4 changes: 2 additions & 2 deletions fixposition_odometry_converter/src/odom_converter.cpp
Original file line number Diff line number Diff line change
@@ -25,7 +25,7 @@ OdomConverter::OdomConverter(ros::NodeHandle* nh) : nh_(*nh) {
Subscribe();

// initialize the publisher
ws_pub_ = nh_.advertise<fixposition_driver::Speed>(params_.fixposition_speed_topic, 1);
ws_pub_ = nh_.advertise<fixposition_driver_ros1::Speed>(params_.fixposition_speed_topic, 1);
}

void OdomConverter::Subscribe() {
@@ -47,7 +47,7 @@ void OdomConverter::ConvertAndPublish(const double speed, const double angular,
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::Speed msg;
fixposition_driver_ros1::Speed msg;
msg.speeds.push_back(int_speed);
if (params_.use_angular) {
msg.speeds.push_back(angular_speed);

0 comments on commit 59defcd

Please sign in to comment.