From fa905c625c71bee840b55e1f0bd72ae6cabb4ae7 Mon Sep 17 00:00:00 2001
From: vagrant <diaz.cpu1@gmail.com>
Date: Sat, 12 Dec 2020 19:34:57 +0000
Subject: [PATCH] Split magnetometer node into C++ class and header

---
 igvc_gazebo/nodes/magnetometer/CMakeLists.txt |  2 +-
 .../nodes/magnetometer/Magnetometer.cpp       | 36 +++++++++++++++++++
 igvc_gazebo/nodes/magnetometer/Magnetometer.h | 18 ++++++++++
 igvc_gazebo/nodes/magnetometer/main.cpp       | 32 -----------------
 4 files changed, 55 insertions(+), 33 deletions(-)
 create mode 100644 igvc_gazebo/nodes/magnetometer/Magnetometer.cpp
 create mode 100644 igvc_gazebo/nodes/magnetometer/Magnetometer.h
 delete mode 100644 igvc_gazebo/nodes/magnetometer/main.cpp

diff --git a/igvc_gazebo/nodes/magnetometer/CMakeLists.txt b/igvc_gazebo/nodes/magnetometer/CMakeLists.txt
index 790bfd43d..32c504055 100644
--- a/igvc_gazebo/nodes/magnetometer/CMakeLists.txt
+++ b/igvc_gazebo/nodes/magnetometer/CMakeLists.txt
@@ -1,4 +1,4 @@
-add_executable(mag_republisher main.cpp)
+add_executable(mag_republisher Magnetometer.cpp)
 add_dependencies(mag_republisher ${catkin_EXPORTED_TARGETS})
 target_link_libraries(mag_republisher ${catkin_LIBRARIES})
 
diff --git a/igvc_gazebo/nodes/magnetometer/Magnetometer.cpp b/igvc_gazebo/nodes/magnetometer/Magnetometer.cpp
new file mode 100644
index 000000000..94c5c9178
--- /dev/null
+++ b/igvc_gazebo/nodes/magnetometer/Magnetometer.cpp
@@ -0,0 +1,36 @@
+#include "Magnetometer.h"
+
+#include <ros/ros.h>
+#include <geometry_msgs/Vector3Stamped.h>
+#include <sensor_msgs/MagneticField.h>
+
+Magnetometer::Magnetometer()
+{
+	ros::NodeHandle nh;
+    ros::NodeHandle pnh("~");
+    std::string sub_topic = pnh.param("mag_sub_topic", std::string("/magnetometer/vector"));
+    std::string pub_topic = pnh.param("mag_pub_topic", std::string("/magnetometer_mag"));
+    g_mag_field_covar = pnh.param("mag_field_variance", 1e-6);
+    g_mag_field_pub = nh.advertise<sensor_msgs::MagneticField>(pub_topic, 10);
+    ros::Subscriber scan_sub = nh.subscribe(sub_topic, 1, magCallback);
+}
+
+void magCallback(const geometry_msgs::Vector3Stamped& msg)
+{
+    sensor_msgs::MagneticField magnet_msg;
+    magnet_msg.header.seq = msg.header.seq;
+    magnet_msg.header.stamp = msg.header.stamp;
+    magnet_msg.header.frame_id = msg.header.frame_id;
+    magnet_msg.magnetic_field.x = msg.vector.x;
+    magnet_msg.magnetic_field.y = msg.vector.y;
+    magnet_msg.magnetic_field.z = msg.vector.z;
+    magnet_msg.magnetic_field_covariance = { g_mag_field_covar, 0, 0, 0, g_mag_field_covar, 0, 0, 0, g_mag_field_covar };
+    g_mag_field_pub.publish(magnet_msg);
+}
+
+int main(int argc, char** argv)
+{
+	ros::init(argc, argv, "mag_republisher");
+	Magnetometer m;
+	ros::spin();
+}
diff --git a/igvc_gazebo/nodes/magnetometer/Magnetometer.h b/igvc_gazebo/nodes/magnetometer/Magnetometer.h
new file mode 100644
index 000000000..bac7d6547
--- /dev/null
+++ b/igvc_gazebo/nodes/magnetometer/Magnetometer.h
@@ -0,0 +1,18 @@
+// Magnetometer.h
+#ifndef Magnetometer_H
+#define Magnetometer_H
+
+#include <ros/ros.h>
+#include <sensor_msgs/MagneticField.h>
+
+class Magnetometer 
+{
+
+public:
+	Magnetometer::Magnetometer();
+private:
+	ros::Publisher mag_field_pub;
+	static double mag_field_covar;
+	void magCallback(const geometry_msgs::Vector3Stamped& msg);
+};
+#endif
\ No newline at end of file
diff --git a/igvc_gazebo/nodes/magnetometer/main.cpp b/igvc_gazebo/nodes/magnetometer/main.cpp
deleted file mode 100644
index 7d8391316..000000000
--- a/igvc_gazebo/nodes/magnetometer/main.cpp
+++ /dev/null
@@ -1,32 +0,0 @@
-#include <ros/ros.h>
-#include <geometry_msgs/Vector3Stamped.h>
-#include <sensor_msgs/MagneticField.h>
-
-ros::Publisher g_mag_field_pub;
-static double g_mag_field_covar;
-
-void magCallback(const geometry_msgs::Vector3Stamped& msg)
-{
-  sensor_msgs::MagneticField magnet_msg;
-  magnet_msg.header.seq = msg.header.seq;
-  magnet_msg.header.stamp = msg.header.stamp;
-  magnet_msg.header.frame_id = msg.header.frame_id;
-  magnet_msg.magnetic_field.x = msg.vector.x;
-  magnet_msg.magnetic_field.y = msg.vector.y;
-  magnet_msg.magnetic_field.z = msg.vector.z;
-  magnet_msg.magnetic_field_covariance = { g_mag_field_covar, 0, 0, 0, g_mag_field_covar, 0, 0, 0, g_mag_field_covar };
-  g_mag_field_pub.publish(magnet_msg);
-}
-
-int main(int argc, char** argv)
-{
-  ros::init(argc, argv, "mag_republisher");
-  ros::NodeHandle nh;
-  ros::NodeHandle pnh("~");
-  std::string sub_topic = pnh.param("mag_sub_topic", std::string("/magnetometer/vector"));
-  std::string pub_topic = pnh.param("mag_pub_topic", std::string("/magnetometer_mag"));
-  g_mag_field_covar = pnh.param("mag_field_variance", 1e-6);
-  g_mag_field_pub = nh.advertise<sensor_msgs::MagneticField>(pub_topic, 10);
-  ros::Subscriber scan_sub = nh.subscribe(sub_topic, 1, magCallback);
-  ros::spin();
-}
\ No newline at end of file