From ef0ed0b583c2640f2bfb12a4053d23cf8e350ae9 Mon Sep 17 00:00:00 2001
From: patrykcieslak <patryk.cieslak@udg.edu>
Date: Wed, 25 Nov 2020 18:34:53 +0100
Subject: [PATCH] Modified trajectory iteration message.

---
 CMakeLists.txt            | 1 +
 msg/Int32Stamped.msg      | 2 ++
 src/ROSInterface.cpp      | 7 ++++---
 src/ROSScenarioParser.cpp | 4 ++--
 4 files changed, 9 insertions(+), 5 deletions(-)
 create mode 100644 msg/Int32Stamped.msg

diff --git a/CMakeLists.txt b/CMakeLists.txt
index f7ee480..be87912 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -20,6 +20,7 @@ pkg_check_modules(Stonefish REQUIRED stonefish>=1.2.0)
 
 add_message_files(
   FILES
+  Int32Stamped.msg
   ThrusterState.msg
 )
 
diff --git a/msg/Int32Stamped.msg b/msg/Int32Stamped.msg
new file mode 100644
index 0000000..8c5f757
--- /dev/null
+++ b/msg/Int32Stamped.msg
@@ -0,0 +1,2 @@
+Header header
+int32 data
\ No newline at end of file
diff --git a/src/ROSInterface.cpp b/src/ROSInterface.cpp
index 60ebd66..e500373 100644
--- a/src/ROSInterface.cpp
+++ b/src/ROSInterface.cpp
@@ -44,7 +44,6 @@
 #include <Stonefish/comms/USBL.h>
 #include <Stonefish/entities/AnimatedEntity.h>
 
-#include <std_msgs/UInt32.h>
 #include <sensor_msgs/FluidPressure.h>
 #include <sensor_msgs/Imu.h>
 #include <sensor_msgs/Range.h>
@@ -62,6 +61,7 @@
 #include <visualization_msgs/MarkerArray.h>
 #include <cola2_msgs/DVL.h>
 #include <cola2_msgs/Float32Stamped.h>
+#include <stonefish_ros/Int32Stamped.h>
 
 #include <Eigen/Core>
 #include <Eigen/Dense>
@@ -421,8 +421,9 @@ void ROSInterface::PublishTrajectoryState(ros::Publisher& odom, ros::Publisher&
     odom.publish(msg);
 
     //Iteration message
-    std_msgs::UInt32 msg2;
-    msg2.data = tr->getPlaybackIteration();
+    stonefish_ros::Int32Stamped msg2;
+    msg2.header = msg.header;
+    msg2.data = (int32_t)tr->getPlaybackIteration();
     iter.publish(msg2);
 }
 
diff --git a/src/ROSScenarioParser.cpp b/src/ROSScenarioParser.cpp
index c59c192..6319c22 100644
--- a/src/ROSScenarioParser.cpp
+++ b/src/ROSScenarioParser.cpp
@@ -40,7 +40,6 @@
 #include <Stonefish/sensors/vision/MSIS.h>
 #include <Stonefish/comms/Comm.h>
 #include <std_msgs/Float64.h>
-#include <std_msgs/UInt32.h>
 #include <sensor_msgs/FluidPressure.h>
 #include <sensor_msgs/Imu.h>
 #include <sensor_msgs/Range.h>
@@ -58,6 +57,7 @@
 #include <cola2_msgs/DVL.h>
 #include <cola2_msgs/Setpoints.h>
 #include <stonefish_ros/ThrusterState.h>
+#include <stonefish_ros/Int32Stamped.h>
 
 #include <ros/package.h>
 
@@ -305,7 +305,7 @@ bool ROSScenarioParser::ParseAnimated(XMLElement* element)
             return true;
         }
         pubs[nameStr + "/odometry"] = nh.advertise<nav_msgs::Odometry>(std::string(topic), 10);
-        pubs[nameStr + "/iteration"] = nh.advertise<std_msgs::UInt32>(std::string(topic) + "/iteration", 10);
+        pubs[nameStr + "/iteration"] = nh.advertise<stonefish_ros::Int32Stamped>(std::string(topic) + "/iteration", 10);
     }
     
     return true;