latlon in radiants

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)