Merge pull request #136 from AutoModality/AM-677/culvert_alt

fix: added log of currentENU

Author: danhennage

src/am_super/am_super.cpp

@@ -3,6 +3,7 @@
 
 #include <ros/ros.h>
 #include <diagnostic_msgs/DiagnosticArray.h>
+#include <nav_msgs/Odometry.h>
 #include <sensor_msgs/Joy.h>
 #include <sensor_msgs/PointCloud2.h>
 #include <std_msgs/Int16.h>
@@ -69,6 +70,7 @@ class AMSuper : AMLifeCycle
   ros::Subscriber operator_command_sub_;
   ros::Subscriber controller_state_sub;
   ros::Subscriber diagnostics_sub;
+  ros::Subscriber current_enu_sub;
   ros::Timer heartbeat_timer_;
 
   ros::Subscriber log_control_sub_;
@@ -223,6 +225,8 @@ class AMSuper : AMLifeCycle
 
     diagnostics_sub = nh_.subscribe("/diagnostics", 100, &AMSuper::diagnosticsCB, this);
 
+    current_enu_sub = nh_.subscribe(am_topics::CTRL_VX_VEHICLE_CURRENTENU, 100, &AMSuper::currentENUCB, this);
+
     heartbeat_timer_ = nh_.createTimer(ros::Duration(1.0), &AMSuper::heartbeatCB, this);
   }
 
@@ -788,6 +792,11 @@ class AMSuper : AMLifeCycle
       LOG_MSG("/diagnostics", msg, SU_LOG_LEVEL);
   }
 
+  void currentENUCB(const nav_msgs::Odometry::ConstPtr &msg)
+  {
+      LOG_MSG(am_topics::CTRL_VX_VEHICLE_CURRENTENU, msg, SU_LOG_LEVEL);
+  }
+
   BagLogger::BagLoggerLevel intToLoggerLevel(int level)
   {
     switch (level)