Skip to content

Commit

Permalink
Improved time handling and patch GPGGA message
Browse files Browse the repository at this point in the history
  • Loading branch information
Facundo Garcia committed Jan 25, 2024
1 parent 420ca7b commit 2add8c2
Show file tree
Hide file tree
Showing 10 changed files with 117 additions and 21 deletions.
9 changes: 6 additions & 3 deletions fixposition_driver_lib/include/fixposition_driver_lib/msg_data.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -140,15 +140,17 @@ struct GpggaData {
double altitude;
Eigen::Matrix<double, 3, 3> cov;
int position_covariance_type;
GpggaData() : latitude(0.0), longitude(0.0), altitude(0.0), position_covariance_type(0) { cov.setZero(); }
bool valid;
GpggaData() : latitude(0.0), longitude(0.0), altitude(0.0), position_covariance_type(0), valid(false) { cov.setZero(); }
};

struct GpzdaData {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
std::string time;
std::string date;
times::GpsTime stamp;
GpzdaData() : time(""), date("") {}
bool valid;
GpzdaData() : time(""), date(""), valid(false) {}
};

struct GprmcData {
Expand All @@ -159,7 +161,8 @@ struct GprmcData {
double longitude;
double speed;
double course;
GprmcData() : latitude(0.0), longitude(0.0), speed(0.0), course(0.0) {}
bool valid;
GprmcData() : latitude(0.0), longitude(0.0), speed(0.0), course(0.0), valid(false) {}
};

} // namespace fixposition
Expand Down
11 changes: 11 additions & 0 deletions fixposition_driver_lib/src/gpgga.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,14 @@ void GpggaConverter::ConvertTokens(const std::vector<std::string>& tokens) {
return;
}

// Check that critical message fields are populated
for (int i = 1; i < 11; i++) {
if (tokens.at(i).empty()) {
msg_ = GpggaData();
return;
}
}

// Header stamps
msg_.time = tokens.at(time_idx);

Expand Down Expand Up @@ -73,6 +81,9 @@ void GpggaConverter::ConvertTokens(const std::vector<std::string>& tokens) {
msg_.cov(1, 2) = msg_.cov(2, 1) = 0.0;
msg_.position_covariance_type = 1; // COVARIANCE_TYPE_APPROXIMATED

// Set message as valid
msg_.valid = true;

// Process all observers
for (auto& ob : obs_) {
ob(msg_);
Expand Down
11 changes: 11 additions & 0 deletions fixposition_driver_lib/src/gprmc.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,14 @@ void GprmcConverter::ConvertTokens(const std::vector<std::string>& tokens) {
return;
}

// Check that critical message fields are populated
for (int i = 1; i < 9; i++) {
if (tokens.at(i).empty()) {
msg_ = GprmcData();
return;
}
}

// Header stamps
msg_.time = tokens.at(time_idx);

Expand All @@ -67,6 +75,9 @@ void GprmcConverter::ConvertTokens(const std::vector<std::string>& tokens) {
// Get GPS status
msg_.mode = tokens.at(mode_idx);

// Set message as valid
msg_.valid = true;

// Process all observers
for (auto& ob : obs_) {
ob(msg_);
Expand Down
13 changes: 12 additions & 1 deletion fixposition_driver_lib/src/gpzda.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,7 +69,7 @@ void convertToGPSTime(const std::string& utcTimeString, std::string& gpsWeek, st
double gpsTime = std::fmod(timeDifference, secondsInWeek);

// Add milliseconds to GPS time
gpsTime += (18 + ms);
gpsTime += (fixposition::times::Constants::gps_leap_time_s + ms);

// Convert results to strings
std::ostringstream ossWeek, ossTime;
Expand All @@ -89,6 +89,14 @@ void GpzdaConverter::ConvertTokens(const std::vector<std::string>& tokens) {
return;
}

// Check that critical message fields are populated
for (int i = 1; i < 6; i++) {
if (tokens.at(i).empty()) {
msg_ = GpzdaData();
return;
}
}

// Populate time fields
msg_.time = tokens.at(time_idx);
msg_.date = tokens.at(day_idx) + '/' + tokens.at(month_idx) + '/' + tokens.at(year_idx);
Expand All @@ -99,6 +107,9 @@ void GpzdaConverter::ConvertTokens(const std::vector<std::string>& tokens) {
convertToGPSTime(utcTimeString, gps_week, gps_tow);
msg_.stamp = ConvertGpsTime(gps_week, gps_tow);

// Set message as valid
msg_.valid = true;

// Process all observers
for (auto& ob : obs_) {
ob(msg_);
Expand Down
8 changes: 6 additions & 2 deletions fixposition_driver_ros1/include/fixposition_driver_ros1/fixposition_driver_node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -61,8 +61,12 @@ class FixpositionDriverNode : public FixpositionDriver {
* @brief Construct a new Fixposition Driver Node object
*/
bool checkEpoch() {
if ((gpgga.time.compare(gpzda.time) == 0) && (gpgga.time.compare(gprmc.time) == 0)) {
return true;
if (gpgga.valid && gpgga.valid && gpgga.valid) {
if ((gpgga.time.compare(gpzda.time) == 0) && (gpgga.time.compare(gprmc.time) == 0)) {
return true;
} else {
return false;
}
} else {
return false;
}
Expand Down
28 changes: 24 additions & 4 deletions fixposition_driver_ros1/src/data_to_ros1.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,12 @@

namespace fixposition {
void ImuDataToMsg(const ImuData& data, sensor_msgs::Imu& msg) {
msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.stamp));
if (data.stamp.tow == 0.0 && data.stamp.wno == 0) {
msg.header.stamp = ros::Time::now();
} else {
msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.stamp));
}

msg.header.frame_id = data.frame_id;
tf::vectorEigenToMsg(data.linear_acceleration, msg.linear_acceleration);
tf::vectorEigenToMsg(data.angular_velocity, msg.angular_velocity);
Expand All @@ -29,7 +34,12 @@ void NavSatStatusDataToMsg(const NavSatStatusData& data, sensor_msgs::NavSatStat
}

void NavSatFixDataToMsg(const NavSatFixData& data, sensor_msgs::NavSatFix& msg) {
msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.stamp));
if (data.stamp.tow == 0.0 && data.stamp.wno == 0) {
msg.header.stamp = ros::Time::now();
} else {
msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.stamp));
}

msg.header.frame_id = data.frame_id;
NavSatStatusDataToMsg(data.status, msg.status);
msg.latitude = data.latitude;
Expand Down Expand Up @@ -60,7 +70,12 @@ void TwistWithCovDataToMsg(const fixposition::TwistWithCovData& data, geometry_m
}

void OdometryDataToMsg(const fixposition::OdometryData& data, nav_msgs::Odometry& msg) {
msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.stamp));
if (data.stamp.tow == 0.0 && data.stamp.wno == 0) {
msg.header.stamp = ros::Time::now();
} else {
msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.stamp));
}

msg.header.frame_id = data.frame_id;
msg.child_frame_id = data.child_frame_id;

Expand All @@ -69,7 +84,12 @@ void OdometryDataToMsg(const fixposition::OdometryData& data, nav_msgs::Odometry
}

void VrtkDataToMsg(const VrtkData& data, fixposition_driver_ros1::VRTK& msg) {
msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.stamp));
if (data.stamp.tow == 0.0 && data.stamp.wno == 0) {
msg.header.stamp = ros::Time::now();
} else {
msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.stamp));
}

msg.header.frame_id = data.frame_id;
msg.pose_frame = data.pose_frame;
msg.kin_frame = data.kin_frame;
Expand Down
12 changes: 10 additions & 2 deletions fixposition_driver_ros1/src/fixposition_driver_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -94,7 +94,11 @@ void FixpositionDriverNode::RegisterObservers() {
}
if (eul_pub_.getNumSubscribers() > 0) {
geometry_msgs::Vector3Stamped ypr;
ypr.header.stamp = ros::Time::fromBoost(fixposition::times::GpsTimeToPtime(data.odometry.stamp));
if (data.odometry.stamp.tow == 0.0 && data.odometry.stamp.wno == 0) {
ypr.header.stamp = ros::Time::now();
} else {
ypr.header.stamp = ros::Time::fromBoost(fixposition::times::GpsTimeToPtime(data.odometry.stamp));
}
ypr.header.frame_id = "FP_POI";
tf::vectorEigenToMsg(data.eul, ypr.vector);
eul_pub_.publish(ypr);
Expand Down Expand Up @@ -197,7 +201,11 @@ void FixpositionDriverNode::PublishNmea(NmeaMessage data) {
fixposition_driver_ros1::NMEA msg;

// ROS Header
msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.gpzda.stamp));
if (data.gpzda.stamp.tow == 0.0 && data.gpzda.stamp.wno == 0) {
msg.header.stamp = ros::Time::now();
} else {
msg.header.stamp = ros::Time::fromBoost(GpsTimeToPtime(data.gpzda.stamp));
}
msg.header.frame_id = "LLH";

// Latitude [degrees]. Positive is north of equator; negative is south
Expand Down
10 changes: 7 additions & 3 deletions fixposition_driver_ros2/include/fixposition_driver_ros2/fixposition_driver_node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,12 +64,16 @@ class FixpositionDriverNode : public FixpositionDriver {
* @brief Check if GNSS epoch is complete
*/
bool checkEpoch() {
if ((gpgga.time.compare(gpzda.time) == 0) && (gpgga.time.compare(gprmc.time) == 0)) {
return true;
if (gpgga.valid && gpgga.valid && gpgga.valid) {
if ((gpgga.time.compare(gpzda.time) == 0) && (gpgga.time.compare(gprmc.time) == 0)) {
return true;
} else {
return false;
}
} else {
return false;
}
}
}
};

private:
Expand Down
24 changes: 20 additions & 4 deletions fixposition_driver_ros2/src/data_to_ros2.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,11 @@

namespace fixposition {
void ImuDataToMsg(const ImuData& data, sensor_msgs::msg::Imu& msg) {
msg.header.stamp = GpsTimeToMsgTime(data.stamp);
if (data.stamp.tow == 0.0 && data.stamp.wno == 0) {
msg.header.stamp = rclcpp::Clock().now();
} else {
msg.header.stamp = GpsTimeToMsgTime(data.stamp);
}
msg.header.frame_id = data.frame_id;

tf2::toMsg(data.linear_acceleration, msg.linear_acceleration);
Expand All @@ -38,7 +42,11 @@ void NavSatStatusDataToMsg(const NavSatStatusData& data, sensor_msgs::msg::NavSa
}

void NavSatFixDataToMsg(const NavSatFixData& data, sensor_msgs::msg::NavSatFix& msg) {
msg.header.stamp = GpsTimeToMsgTime(data.stamp);
if (data.stamp.tow == 0.0 && data.stamp.wno == 0) {
msg.header.stamp = rclcpp::Clock().now();
} else {
msg.header.stamp = GpsTimeToMsgTime(data.stamp);
}
msg.header.frame_id = data.frame_id;
NavSatStatusDataToMsg(data.status, msg.status);
msg.latitude = data.latitude;
Expand Down Expand Up @@ -69,7 +77,11 @@ void TwistWithCovDataToMsg(const fixposition::TwistWithCovData& data, geometry_m
}

void OdometryDataToMsg(const fixposition::OdometryData& data, nav_msgs::msg::Odometry& msg) {
msg.header.stamp = GpsTimeToMsgTime(data.stamp);
if (data.stamp.tow == 0.0 && data.stamp.wno == 0) {
msg.header.stamp = rclcpp::Clock().now();
} else {
msg.header.stamp = GpsTimeToMsgTime(data.stamp);
}
msg.header.frame_id = data.frame_id;
msg.child_frame_id = data.child_frame_id;

Expand All @@ -78,7 +90,11 @@ void OdometryDataToMsg(const fixposition::OdometryData& data, nav_msgs::msg::Odo
}

void VrtkDataToMsg(const VrtkData& data, fixposition_driver_ros2::msg::VRTK& msg) {
msg.header.stamp = GpsTimeToMsgTime(data.stamp);
if (data.stamp.tow == 0.0 && data.stamp.wno == 0) {
msg.header.stamp = rclcpp::Clock().now();
} else {
msg.header.stamp = GpsTimeToMsgTime(data.stamp);
}
msg.header.frame_id = data.frame_id;
msg.pose_frame = data.pose_frame;
msg.kin_frame = data.kin_frame;
Expand Down
12 changes: 10 additions & 2 deletions fixposition_driver_ros2/src/fixposition_driver_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -115,7 +115,11 @@ void FixpositionDriverNode::RegisterObservers() {
}
if (eul_pub_->get_subscription_count() > 0) {
geometry_msgs::msg::Vector3Stamped ypr;
ypr.header.stamp = GpsTimeToMsgTime(data.odometry.stamp);
if (data.odometry.stamp.tow == 0.0 && data.odometry.stamp.wno == 0) {
ypr.header.stamp = rclcpp::Clock().now();
} else {
ypr.header.stamp = GpsTimeToMsgTime(data.odometry.stamp);
}
ypr.header.frame_id = "FP_POI";
ypr.vector.set__x(data.eul.x());
ypr.vector.set__y(data.eul.y());
Expand Down Expand Up @@ -222,7 +226,11 @@ void FixpositionDriverNode::PublishNmea(NmeaMessage data) {
fixposition_driver_ros2::msg::NMEA msg;

// ROS Header
msg.header.stamp = GpsTimeToMsgTime(data.gpzda.stamp);
if (data.gpzda.stamp.tow == 0.0 && data.gpzda.stamp.wno == 0) {
msg.header.stamp = rclcpp::Clock().now();
} else {
msg.header.stamp = GpsTimeToMsgTime(data.gpzda.stamp);
}
msg.header.frame_id = "LLH";

// Latitude [degrees]. Positive is north of equator; negative is south
Expand Down

0 comments on commit 2add8c2

Please sign in to comment.