Skip to content

Commit

Permalink
Merge pull request #42 from Greenzie/IntelligentJackal/NavPVT-localiz…
Browse files Browse the repository at this point in the history 
…ation-fixes

Cleanup and adding in angular rates
  • Loading branch information
@IntelligentJackal
IntelligentJackal authored Nov 10, 2023
2 parents c46d588 + 38d4126 commit 9bbc047
Show file tree
Hide file tree
Showing 2 changed files with 72 additions and 1 deletion.
18 changes: 18 additions & 0 deletions include/robot_localization/navsat_transform.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,7 @@
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/NavSatFix.h>
#include <ublox_msgs/NavPVT.h>
#include <ublox_msgs/EsfINS.h>

extern "C" {
#include "mkgmtime.h"
Expand Down Expand Up @@ -141,6 +142,11 @@ class NavSatTransform
//!
void gpsNavPVTCallback(const ublox_msgs::NavPVTConstPtr& msg);

//! @brief Callback for the GPS esfINS data
//! @param[in] msg The esfINS message to process
//!
void gpsEsfINSCallback(const ublox_msgs::EsfINSConstPtr& msg);

//! @brief Converts the odometry data back to GPS and broadcasts it
//! @param[out] filtered_gps The NavSatFix message to prepare
//!
Expand Down Expand Up @@ -283,6 +289,10 @@ class NavSatTransform
//!
double magnetic_declination_;

//! @brief INS timeout when using NavPVT + esfINS
//!
uint32_t ins_timeout_ms_;

//! @brief UTM's meridian convergence
//!
//! Angle between projected meridian (True North) and UTM's grid Y-axis.
Expand Down Expand Up @@ -399,6 +409,10 @@ class NavSatTransform
//!
ros::Subscriber gps_nav_pvt_sub_;

//! @brief GPS esfINS subscriber
//!
ros::Subscriber gps_esf_ins_sub_;

//! @brief Subscribes to imu topic
//!
ros::Subscriber imu_sub_;
Expand Down Expand Up @@ -462,6 +476,10 @@ class NavSatTransform
//! @brief odom to base_link
//!
geometry_msgs::TransformStamped transform_stamped_odom_base_footprint_;

//! @brief latest esfINS message
//!
ublox_msgs::EsfINS gps_esf_ins_;
};

} // namespace RobotLocalization
Expand Down
55 changes: 54 additions & 1 deletion src/navsat_transform.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,6 +62,7 @@ namespace RobotLocalization
current_good_gps_count_(0),
current_delayed_gps_count_(0),
magnetic_declination_(0.0),
ins_timeout_ms_(static_cast<uint32_t>(2.5 * 1000.0 / 30.0)), // Set for 30Hz and 1 missed message
yaw_offset_(0.0),
base_link_frame_id_("base_link"),
gps_frame_id_(""),
Expand Down Expand Up @@ -183,7 +184,7 @@ namespace RobotLocalization

if(use_nav_pvt_)
{
gps_nav_pvt_sub_ = nh.subscribe("gps/navpvt", 1, &NavSatTransform::gpsNavPVTCallback, this);
gps_nav_pvt_sub_ = nh.subscribe("ublox/ubx_nav_pvt", 1, &NavSatTransform::gpsNavPVTCallback, this);
nh_priv.param("gps_frame", gps_frame_id_, std::string("")); // Default to none if not set
nh_priv.param("world_frame", world_frame_id_, std::string("")); // Default to none if not set
nh_priv.param("base_link_frame", base_link_frame_id_, std::string("")); // Default to none if not set
Expand All @@ -193,6 +194,11 @@ namespace RobotLocalization
double offsetTmp;
nh_priv.param("transform_time_offset", offsetTmp, 0.0);
tf_time_offset_.fromSec(offsetTmp);
// Set up the second subscription for INS data
gps_esf_ins_sub_ = nh.subscribe("ublox/ubx_esf_ins", 1, &NavSatTransform::gpsEsfINSCallback, this);
int ins_timeout_ms_int = static_cast<int>(ins_timeout_ms_);
nh_priv.param("ins_timeout_ms", ins_timeout_ms_int, ins_timeout_ms_int);
ins_timeout_ms_ = static_cast<uint32_t>(ins_timeout_ms_int);
}
else
{
Expand Down Expand Up @@ -750,6 +756,11 @@ namespace RobotLocalization
}
}

void NavSatTransform::gpsEsfINSCallback(const ublox_msgs::EsfINSConstPtr& msg)
{
gps_esf_ins_ = *msg;
}

void NavSatTransform::gpsNavPVTCallback(const ublox_msgs::NavPVTConstPtr& msg)
{
// gps_frame_id_ set manually if using NavPVT
Expand Down Expand Up @@ -979,6 +990,48 @@ namespace RobotLocalization
{
gps_odom.twist.twist.linear.x = msg->gSpeed * 1e-3; // Ground speed in m/s
}
// If the efsINS message is up to date with valid angular rates, use those.
// Unsigned int32s, so not using abs to avoid losing precision or causing a negative issue
// If one has rolled over, then a single point will be zeroed, then go back to normal.
uint32_t itow_diff = (msg->iTOW > gps_esf_ins_.iTOW) ?
msg->iTOW - gps_esf_ins_.iTOW :
gps_esf_ins_.iTOW - msg->iTOW;
if(itow_diff < ins_timeout_ms_)
{
// X
if((gps_esf_ins_.bitfield0 & ublox_msgs::EsfINS::BITFIELD0_X_ANG_RATE_VALID) > 0)
{
gps_odom.twist.twist.angular.x = static_cast<float>(gps_esf_ins_.xAngRate) / 1e3;
// TODO: Guess for now
gps_odom.twist.covariance[21] = pow(0.1, 2); // (0.1 rad/s)^2 - don't know rotation rate
}
else
{
gps_odom.twist.covariance[21] = pow(TAU, 2); // (2 rad/s)^2 - don't know rotation rate
}
// Y
if((gps_esf_ins_.bitfield0 & ublox_msgs::EsfINS::BITFIELD0_Y_ANG_RATE_VALID) > 0)
{
gps_odom.twist.twist.angular.y = static_cast<float>(gps_esf_ins_.yAngRate) / 1e3;
// TODO: Guess for now
gps_odom.twist.covariance[28] = pow(0.1, 2); // (0.1 rad/s)^2 - don't know rotation rate
}
else
{
gps_odom.twist.covariance[28] = pow(TAU, 2); // (2 rad/s)^2 - don't know rotation rate
}
// Z
if((gps_esf_ins_.bitfield0 & ublox_msgs::EsfINS::BITFIELD0_Z_ANG_RATE_VALID) > 0)
{
gps_odom.twist.twist.angular.z = static_cast<float>(gps_esf_ins_.zAngRate) / 1e3;
// TODO: Guess for now
gps_odom.twist.covariance[35] = pow(0.1, 2); // (0.1 rad/s)^2 - don't know rotation rate
}
else
{
gps_odom.twist.covariance[35] = pow(TAU, 2); // (2 rad/s)^2 - don't know rotation rate
}
}
gps_odom.twist.covariance[0] = pow(msg->sAcc * 1e-3, 2); // Speed accuracy in (m/s)^2
gps_odom.twist.covariance[21] = pow(TAU, 2); // (2 rad/s)^2 - don't know rotation rate
gps_odom.twist.covariance[28] = pow(TAU, 2); // (2 rad/s)^2 - don't know rotation rate
Expand Down

0 comments on commit 9bbc047

Please sign in to comment.