Modified trajectory iteration message.

patrykcieslak · Nov 25, 2020 · ef0ed0b · ef0ed0b
1 parent 9a0c128
commit ef0ed0b
Show file tree

Hide file tree

Showing 4 changed files with 9 additions and 5 deletions.
diff --git 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
@@ -0,0 +1,2 @@
+Header header
+int32 data
diff --git 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
@@ -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;
-Original file line number
+Diff line change
@@ Expand Up / @@ -20,6 +20,7 @@ pkg_check_modules(Stonefish REQUIRED stonefish>=1.2.0) @@
     add_message_files(
       FILES
+      Int32Stamped.msg
       ThrusterState.msg
     )
@@ Expand Down @@