Ground truth

igvc_gazebo/nodes/ground_truth/CMakeLists.txt

@@ -1,4 +1,4 @@
-add_executable(ground_truth_republisher main.cpp)
+add_executable(ground_truth_republisher ground_truth_republisher.cpp)
 add_dependencies(ground_truth_republisher ${catkin_EXPORTED_TARGETS})
 target_link_libraries(ground_truth_republisher ${catkin_LIBRARIES})
 

igvc_gazebo/nodes/ground_truth/main.cpp → igvc_gazebo/nodes/ground_truth/ground_truth_republisher.cpp

@@ -1,24 +1,47 @@
-#include <geometry_msgs/Vector3.h>
-#include <nav_msgs/Odometry.h>
+#include "ground_truth_republisher.h"
+#include <mutex>
 #include <parameter_assertions/assertions.h>
-#include <robot_localization/navsat_conversions.h>
 #include <ros/ros.h>
-#include <tf/transform_broadcaster.h>
-#include <tf/transform_datatypes.h>
-#include <tf/transform_listener.h>
-#include <mutex>
 
-ros::Publisher g_ground_truth_pub;
-// TODO make this a minimal object
-nav_msgs::Odometry g_og_pose;
-ros::Time g_last_estimate;
+GroundTruthRepublisher::GroundTruthRepublisher()
+{
+  ros::NodeHandle nh;
+  ros::NodeHandle pNh("~");
+
+  std::string ground_truth_topic, estimate_topic, pub_topic, diff_topic;
+
+  assertions::param(pNh, "ground_truth_sub_topic", ground_truth_topic, std::string("/ground_truth/state_raw"));
+  assertions::param(pNh, "ground_truth_pub_topic", pub_topic, std::string("/ground_truth"));
+
+  double longitude, latitude;
+  assertions::param(pNh, "longitude", longitude, -84.405001);
+  assertions::param(pNh, "latitude", latitude, 33.774497);
+
+  ros::Subscriber ground_truth =
+      nh.subscribe<nav_msgs::Odometry>(ground_truth_topic, 10, &GroundTruthRepublisher::groundTruthCallback, this);
+
+  ros::Subscriber estimate_sub =
+      nh.subscribe<nav_msgs::Odometry>("/odometry/filtered", 1, &GroundTruthRepublisher::odomCallback, this);
+  g_ground_truth_pub = nh.advertise<nav_msgs::Odometry>(pub_topic, 1);
+
+  double utm_x, utm_y;
+  RobotLocalization::NavsatConversions::UTM(latitude, longitude, &utm_x, &utm_y);
+
+  tf::Transform utm_to_odom;
+  utm_to_odom.setOrigin(
+      tf::Vector3(utm_x - g_og_pose.pose.pose.position.x, utm_y - g_og_pose.pose.pose.position.y, 0.0));
+  utm_to_odom.setRotation(tf::createQuaternionFromYaw(M_PI));
+
+  ros::Timer utm_timer = nh.createTimer(
+      ros::Duration(1.0), boost::bind(&GroundTruthRepublisher::utm_callback, this, _1, utm_to_odom.inverse()));
+}
 
-void odomCallback(const nav_msgs::Odometry::ConstPtr& msg)
+void GroundTruthRepublisher::odomCallback(const nav_msgs::Odometry::ConstPtr &msg)
 {
   g_last_estimate = msg->header.stamp;
 }
 
-void groundTruthCallback(const nav_msgs::Odometry::ConstPtr& msg)
+void GroundTruthRepublisher::groundTruthCallback(const nav_msgs::Odometry::ConstPtr &msg)
 {
   // get the starting location as the origin
   if (g_og_pose.header.stamp.toSec() == 0)
@@ -54,8 +77,9 @@ void groundTruthCallback(const nav_msgs::Odometry::ConstPtr& msg)
     // publish odom message
     g_ground_truth_pub.publish(result);
 
-    // publish transform for tf if there has not been a update from the localization node in the last second
-    // since it also publishes the same transform
+    // publish transform for tf if there has not been a update from the
+    // localization node in the last second since it also publishes the same
+    // transform
     if (std::abs(msg->header.stamp.toSec() - g_last_estimate.toSec()) > 1.0)
     {
       static tf::TransformBroadcaster br;
@@ -67,7 +91,7 @@ void groundTruthCallback(const nav_msgs::Odometry::ConstPtr& msg)
   }
 }
 
-void utm_callback(const ros::TimerEvent& event, const tf::Transform& odom_to_utm)
+void GroundTruthRepublisher::utm_callback(const ros::TimerEvent &event, const tf::Transform &odom_to_utm)
 {
   static tf::TransformBroadcaster br;
   static tf::TransformListener tf_listener;
@@ -81,51 +105,26 @@ void utm_callback(const ros::TimerEvent& event, const tf::Transform& odom_to_utm
     {
       tf_listener.lookupTransform("odom", "utm", ros::Time(0), transform);
     }
-    catch (const tf::TransformException& ex)
+    catch (const tf::TransformException &ex)
     {
       found = false;
     }
 
     if (found && transform.getRotation() != odom_to_utm.getRotation() &&
         transform.getOrigin() != odom_to_utm.getOrigin())
     {
-      ROS_WARN_STREAM("Anther odom -> utm tf broadcast detected. Disabling ground_truth odom -> utm tf broadcast.");
+      ROS_WARN_STREAM("Anther odom -> utm tf broadcast detected. Disabling "
+                      "ground_truth odom -> utm tf broadcast.");
       enabled = false;
       return;
     }
     br.sendTransform(tf::StampedTransform(odom_to_utm, event.current_real, "odom", "utm"));
   }
 }
 
-int main(int argc, char** argv)
+int main(int argc, char **argv)
 {
   ros::init(argc, argv, "ground_truth_republisher");
-
-  ros::NodeHandle nh;
-  ros::NodeHandle pNh("~");
-
-  std::string ground_truth_topic, estimate_topic, pub_topic, diff_topic;
-
-  assertions::param(pNh, "ground_truth_sub_topic", ground_truth_topic, std::string("/ground_truth/state_raw"));
-  assertions::param(pNh, "ground_truth_pub_topic", pub_topic, std::string("/ground_truth"));
-
-  double longitude, latitude;
-  assertions::param(pNh, "longitude", longitude, -84.405001);
-  assertions::param(pNh, "latitude", latitude, 33.774497);
-
-  ros::Subscriber ground_truth = nh.subscribe<nav_msgs::Odometry>(ground_truth_topic, 10, groundTruthCallback);
-
-  ros::Subscriber estimate_sub = nh.subscribe<nav_msgs::Odometry>("/odometry/filtered", 1, odomCallback);
-  g_ground_truth_pub = nh.advertise<nav_msgs::Odometry>(pub_topic, 1);
-
-  double utm_x, utm_y;
-  RobotLocalization::NavsatConversions::UTM(latitude, longitude, &utm_x, &utm_y);
-
-  tf::Transform utm_to_odom;
-  utm_to_odom.setOrigin(
-      tf::Vector3(utm_x - g_og_pose.pose.pose.position.x, utm_y - g_og_pose.pose.pose.position.y, 0.0));
-  utm_to_odom.setRotation(tf::createQuaternionFromYaw(M_PI));
-
-  ros::Timer utm_timer = nh.createTimer(ros::Duration(1.0), boost::bind(utm_callback, _1, utm_to_odom.inverse()));
+  GroundTruthRepublisher ground_truth_republisher = GroundTruthRepublisher();
   ros::spin();
-}
+}

igvc_gazebo/nodes/ground_truth/ground_truth_republisher.h

@@ -0,0 +1,26 @@
+#ifndef GROUND_TRUTH_REPUBLISHER_H
+#define GROUND_TRUTH_REPUBLISHER_H
+
+#include <geometry_msgs/Vector3.h>
+#include <nav_msgs/Odometry.h>
+#include <robot_localization/navsat_conversions.h>
+#include <tf/transform_broadcaster.h>
+#include <tf/transform_datatypes.h>
+#include <tf/transform_listener.h>
+
+class GroundTruthRepublisher
+{
+public:
+  GroundTruthRepublisher();
+
+private:
+  ros::Publisher g_ground_truth_pub;
+  nav_msgs::Odometry g_og_pose;
+  ros::Time g_last_estimate;
+
+  void odomCallback(const nav_msgs::Odometry::ConstPtr& msg);
+  void groundTruthCallback(const nav_msgs::Odometry::ConstPtr& msg);
+  void utm_callback(const ros::TimerEvent& event, const tf::Transform& odom_to_utm);
+};
+
+#endif