cras_topic_tools: Added support for ros::message_traits operations on…

… cras::ShapeShifter on Melodic.
ctu-vras · Jan 6, 2025 · 3cbe47c · 3cbe47c
1 parent 1792942
commit 3cbe47c
Show file tree

Hide file tree

Showing 3 changed files with 101 additions and 2 deletions.
diff --git a/cras_topic_tools/include/cras_topic_tools/shape_shifter.h b/cras_topic_tools/include/cras_topic_tools/shape_shifter.h
@@ -9,6 +9,7 @@
  * \author Martin Pecka
  */
 
+#include <string>
 #include <type_traits>
 
 #include <ros/message_traits.h>
@@ -131,4 +132,74 @@ class ShapeShifter : public ::topic_tools::ShapeShifter
 
 }
 
+#if !ROS_VERSION_MINIMUM(1, 15, 0)
+namespace ros {
+namespace message_traits {
+
+template <> struct IsMessage<cras::ShapeShifter> : TrueType { };
+template <> struct IsMessage<const cras::ShapeShifter> : TrueType { };
+
+template<>
+struct MD5Sum<cras::ShapeShifter>
+{
+  static const char* value(const cras::ShapeShifter& m) { return m.getMD5Sum().c_str(); }
+  static const char* value() { return "*"; }
+};
+
+template<>
+struct DataType<cras::ShapeShifter>
+{
+  static const char* value(const cras::ShapeShifter& m) { return m.getDataType().c_str(); }
+  static const char* value() { return "*"; }
+};
+
+template<>
+struct Definition<cras::ShapeShifter>
+{
+  static const char* value(const cras::ShapeShifter& m) { return m.getMessageDefinition().c_str(); }
+};
+
+}
+
+namespace serialization
+{
+
+template<>
+struct Serializer<cras::ShapeShifter>
+{
+  template<typename Stream>
+  inline static void write(Stream& stream, const cras::ShapeShifter& m) {
+    m.write(stream);
+  }
+
+  template<typename Stream>
+  inline static void read(Stream& stream, cras::ShapeShifter& m)
+  {
+    m.read(stream);
+  }
+
+  inline static uint32_t serializedLength(const cras::ShapeShifter& m) {
+    return m.size();
+  }
+};
+
+
+template<>
+struct PreDeserialize<cras::ShapeShifter>
+{
+  static void notify(const PreDeserializeParams<cras::ShapeShifter>& params)
+  {
+    std::string md5      = (*params.connection_header)["md5sum"];
+    std::string datatype = (*params.connection_header)["type"];
+    std::string msg_def  = (*params.connection_header)["message_definition"];
+    std::string latching  = (*params.connection_header)["latching"];
+
+    params.message->morph(md5, datatype, msg_def, latching);
+  }
+};
+
+}
+}
+#endif
+
 #include "impl/shape_shifter.hpp"
diff --git a/cras_topic_tools/src/shape_shifter.cpp b/cras_topic_tools/src/shape_shifter.cpp
@@ -200,8 +200,8 @@ bool setHeader(topic_tools::ShapeShifter& msg, std_msgs::Header& header)
 #if ROS_VERSION_MINIMUM(1, 15, 0)
 #else
 
-ShapeShifter::ShapeShifter() = default;
-ShapeShifter::~ShapeShifter() = default;
+ShapeShifter::ShapeShifter() {}
+ShapeShifter::~ShapeShifter() {}
 
 ShapeShifter::ShapeShifter(const topic_tools::ShapeShifter& other) :
   ShapeShifter(reinterpret_cast<const ShapeShifter&>(other))

diff --git a/cras_topic_tools/test/test_shape_shifter.cpp b/cras_topic_tools/test/test_shape_shifter.cpp
@@ -340,6 +340,34 @@ TEST(ShapeShifter, CopyCrasShapeShifter)  // NOLINT
   EXPECT_NE(cras::getBuffer(shifter), cras::getBuffer(shifter3));
 }
 
+TEST(ShapeShifter, MessageTraits)  // NOLINT
+{
+  // Create a message
+  geometry_msgs::PointStamped msg;
+  msg.header.stamp.sec = 1;
+  msg.header.stamp.nsec = 2;
+  msg.header.frame_id = "test";
+  msg.point.x = 1;
+  msg.point.y = 2;
+  msg.point.z = 3;
+
+  // Load the message into the shape shifter object
+  topic_tools::ShapeShifter shifter;
+  cras::msgToShapeShifter(msg, shifter);
+
+  namespace mt = ros::message_traits;
+
+  EXPECT_STREQ(mt::datatype(msg), mt::datatype(shifter));
+  EXPECT_STREQ(mt::definition(msg), mt::definition(shifter));
+  EXPECT_STREQ(mt::md5sum(msg), mt::md5sum(shifter));
+  // We need to use the + trick so that we don't get undefined reference errors
+  // https://stackoverflow.com/a/46296720/1076564
+  EXPECT_EQ(true, +mt::IsMessage<cras::ShapeShifter>::value);
+  EXPECT_EQ(false, +mt::IsSimple<cras::ShapeShifter>::value);
+  EXPECT_EQ(false, +mt::IsFixedSize<cras::ShapeShifter>::value);
+  EXPECT_EQ(false, +mt::HasHeader<cras::ShapeShifter>::value);
+}
+
 int main(int argc, char **argv)
 {
   testing::InitGoogleTest(&argc, argv);