From 43be1d0c3a44b79e47b748d2a09b5834b4fbd668 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu>
Date: Thu, 5 Mar 2020 15:29:02 +0100
Subject: [PATCH] latlon in radiants
---
src/wolf_ros_subscriber_gnss_fix.cpp | 13 ++++++++-----
1 file changed, 8 insertions(+), 5 deletions(-)
diff --git a/src/wolf_ros_subscriber_gnss_fix.cpp b/src/wolf_ros_subscriber_gnss_fix.cpp
index 2c8ea95..ab17ef1 100644
--- a/src/wolf_ros_subscriber_gnss_fix.cpp
+++ b/src/wolf_ros_subscriber_gnss_fix.cpp
@@ -16,15 +16,18 @@ void SubscriberWrapperGnssFix::initSubscriber(ros::NodeHandle& nh, const std::st
void SubscriberWrapperGnssFix::callback(const sensor_msgs::NavSatFix::ConstPtr& msg)
{
- ROS_INFO("callback!");
-
Eigen::Matrix3d cov = Eigen::Map<const Eigen::Matrix3d>(msg->position_covariance.data());
- CaptureGnssFixPtr cap_gnss_ptr = std::make_shared<CaptureGnssFix>(TimeStamp(0),
+ std::cout << "New fix: " << Eigen::Vector3d(msg->latitude,msg->longitude,msg->altitude).transpose() << std::endl;
+
+ Eigen::Vector3d latlonalt_rads(msg->latitude,msg->longitude,msg->altitude);
+ latlonalt_rads.head<2>() *= M_PI/180.0;
+
+ CaptureGnssFixPtr cap_gnss_ptr = std::make_shared<CaptureGnssFix>(TimeStamp(msg->header.stamp.sec, msg->header.stamp.nsec),
sensor_ptr_,
- Eigen::Vector3d(msg->latitude,msg->longitude,msg->altitude),
+ latlonalt_rads,
cov,
- false); //false = {LatLonAlt fix and ENU cov}
+ false); // false = {LatLonAlt fix and ENU cov}
cap_gnss_ptr->process();
}
std::shared_ptr<SubscriberWrapper> SubscriberWrapperGnssFix::create(const std::string& _unique_name, const ParamsServer& _params, const SensorBasePtr _sensor_ptr)
--
GitLab