Commit e286a7d4 authored by Rousseau Vincent's avatar Rousseau Vincent
Browse files

Avoid NaN value in fix callback

parent 508a5c4c
......@@ -17,10 +17,10 @@ public:
is_enu_initialized_(false),
course_(0)
{
sub_fix_ = n_.subscribe("navsat/fix", 10, &NavsatToOdom::fixCallback, this);
sub_vel_ = n_.subscribe("navsat/vel", 10, &NavsatToOdom::velCallback, this);
sub_vel_simul_ = n_.subscribe("navsat/vel_simul", 10, &NavsatToOdom::velSimulCallback, this);
pub_odom_ = n_.advertise<nav_msgs::Odometry>("navsat/odom", 1);
sub_fix_ = n_.subscribe("fix", 10, &NavsatToOdom::fixCallback, this);
sub_vel_ = n_.subscribe("vel", 10, &NavsatToOdom::velCallback, this);
sub_vel_simul_ = n_.subscribe("vel_simul", 10, &NavsatToOdom::velSimulCallback, this);
pub_odom_ = n_.advertise<nav_msgs::Odometry>("odom", 1);
transform_stamped_.header.frame_id = "odom";
transform_stamped_.child_frame_id = "base_footprint";
......@@ -28,7 +28,8 @@ public:
void fixCallback(const sensor_msgs::NavSatFix::ConstPtr & fix)
{
if(fix->status.status != sensor_msgs::NavSatStatus::STATUS_NO_FIX)
if(fix->status.status != sensor_msgs::NavSatStatus::STATUS_NO_FIX &&
fix->latitude == fix->latitude && fix->longitude == fix->longitude) // avoid NaN value
{
if(is_enu_initialized_ == false)
{
......@@ -52,7 +53,6 @@ public:
transform_stamped_.transform.translation.y = north;
transform_stamped_.transform.rotation = tf::createQuaternionMsgFromYaw(course_);
br_.sendTransform(transform_stamped_);
}
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment