Merge branch 'develop'

MRPT · Aug 22, 2024 · dcb4ce7 · dcb4ce7
2 parents 35ecd6d + d8d7e8e
commit dcb4ce7
Show file tree

Hide file tree

Showing 3 changed files with 78 additions and 14 deletions.
diff --git a/apps/rosbag2rawlog/rosbag2rawlog_main.cpp b/apps/rosbag2rawlog/rosbag2rawlog_main.cpp
@@ -88,6 +88,9 @@ TCLAP::ValueArg<std::string> arg_base_link_frame(
     "base_link",
     cmd);
 
+std::optional<std::string> odom_from_tf_label;
+std::string odom_frame_id = "odom";
+
 using Obs = std::list<mrpt::serialization::CSerializable::Ptr>;
 
 using CallbackFunction = std::function<Obs(const rosbag::MessageInstance&)>;
@@ -534,29 +537,52 @@ Obs toRangeImage(
 template <bool isStatic>
 Obs toTf(tf2::BufferCore& tfBuf, const rosbag::MessageInstance& rosmsg)
 {
-  if (rosmsg.getDataType() == "tf2_msgs/TFMessage")
+  if (rosmsg.getDataType() != "tf2_msgs/TFMessage") return {};
+
+  Obs ret;
+  auto tfs = rosmsg.instantiate<tf2_msgs::TFMessage>();
+  for (auto& tf : tfs->transforms)
   {
-    auto tfs = rosmsg.instantiate<tf2_msgs::TFMessage>();
-    for (auto& tf : tfs->transforms)
+    try
     {
-      try
-      {
-        tfBuf.setTransform(tf, "bagfile", isStatic);
+      tfBuf.setTransform(tf, "bagfile", isStatic);
 
-        addTfFrameAsKnown(tf.child_frame_id);
-        addTfFrameAsKnown(tf.header.frame_id);
+      addTfFrameAsKnown(tf.child_frame_id);
+      addTfFrameAsKnown(tf.header.frame_id);
 #if 0
 				std::cout << "tf: " << tf.child_frame_id
 						  << " <= " << tf.header.frame_id << "\n";
 #endif
-      }
-      catch (const tf2::TransformException& ex)
+
+      // Process /tf -> odometry conversion, if enabled:
+      const auto baseLink = arg_base_link_frame.getValue();
+
+      if (odom_from_tf_label &&
+          (tf.child_frame_id == odom_frame_id || tf.header.frame_id == odom_frame_id) &&
+          (tf.child_frame_id == baseLink || tf.header.frame_id == baseLink))
       {
-        std::cerr << ex.what() << std::endl;
+        mrpt::poses::CPose3D p;
+        bool valid =
+            findOutSensorPose(p, odom_frame_id, arg_base_link_frame.getValue(), std::nullopt);
+        if (valid)
+        {
+          auto o = mrpt::obs::CObservationOdometry::Create();
+          o->sensorLabel = odom_from_tf_label.value();
+          o->timestamp = mrpt::ros1bridge::fromROS(tf.header.stamp);
+
+          // Convert data:
+          o->odometry = {p.x(), p.y(), p.yaw()};
+          o->hasVelocities = false;
+          ret.push_back(o);
+        }
       }
     }
+    catch (const tf2::TransformException& ex)
+    {
+      std::cerr << ex.what() << std::endl;
+    }
   }
-  return {};
+  return ret;
 }
 
 class Transcriber
@@ -566,12 +592,24 @@ class Transcriber
   {
     tfBuffer = std::make_shared<tf2::BufferCore>();
 
+    if (config.has("odom_from_tf"))
+    {
+      ASSERT_(config["odom_from_tf"].isMap());
+      ASSERTMSG_(
+          config["odom_from_tf"].has("sensor_label"),
+          "odom_from_tf YAML map must contain a sensor_label entry.");
+
+      const auto c = config["odom_from_tf"];
+      odom_from_tf_label = c["sensor_label"].as<std::string>();
+      if (c.has("odom_frame_id")) odom_frame_id = c["odom_frame_id"].as<std::string>();
+    }
+
     m_lookup["/tf"].emplace_back([=](const rosbag::MessageInstance& rosmsg)
                                  { return toTf<false>(*tfBuffer, rosmsg); });
     m_lookup["/tf_static"].emplace_back([=](const rosbag::MessageInstance& rosmsg)
                                         { return toTf<true>(*tfBuffer, rosmsg); });
 
-    for (auto& sensorNode : config["sensors"].asMap())
+    for (auto& sensorNode : config["sensors"].asMapRange())
     {
       auto sensorName = sensorNode.first.as<std::string>();
       auto& sensor = sensorNode.second.asMap();

diff --git a/doc/source/doxygen-docs/changelog.md b/doc/source/doxygen-docs/changelog.md
@@ -1,5 +1,11 @@
 \page changelog Change Log
 
+# Version 2.13.7: Released Aug 22nd, 2024
+- Changes in apps:
+  - rosbag2rawlog (ROS1): Implement generation of odometry from /tf messages.
+- BUG FIXES:
+  - Fix incorrect check in mrpt-io unit tests leading to potential false positives.
+
 # Version 2.13.6: Released Aug 14th, 2024
 - Build system:
   - This main MRPT repository is no longer directly built as a ROS package. Please, use the wrappers for better modularity:

diff --git a/libs/io/src/CFileGZStreams_unittest.cpp b/libs/io/src/CFileGZStreams_unittest.cpp
@@ -16,6 +16,23 @@
 #include <test_mrpt_common.h>
 
 #include <algorithm>  // std::equal
+#include <cstdint>
+#include <iomanip>
+#include <sstream>
+#include <string>
+
+namespace
+{
+std::string to_hex_string(const uint8_t* buf, size_t len)
+{
+  std::ostringstream oss;
+  for (size_t i = 0; i < len; ++i)
+  {
+    oss << std::hex << std::setw(2) << std::setfill('0') << static_cast<int>(buf[i]);
+  }
+  return oss.str();
+}
+}  // namespace
 
 const size_t tst_data_len = 1000U;
 
@@ -112,7 +129,10 @@ TEST(CFileGZStreams, readwriteTmpFileCompressed)
       const size_t rd_count = fil_in.Read(rd_buf, tst_data_len / 4);
       EXPECT_EQ(rd_count, tst_data_len / 4);
 
-      EXPECT_TRUE(std::equal(std::begin(tst_data), std::end(tst_data), std::begin(rd_buf)));
+      EXPECT_TRUE(std::equal(std::begin(rd_buf), std::end(rd_buf), std::begin(tst_data)))
+          << " compress_level: " << compress_level << "\n"
+          << " test_data: " << to_hex_string(tst_data.data(), tst_data.size()) << "\n"
+          << " read_data: " << to_hex_string(rd_buf, sizeof(rd_buf)) << "\n";
 
       // next I should find an EOF:
       const auto rdEof = fil_in.Read(rd_buf, 1);