minor updates to fix bagplayback

apl-ocean-engineering · Sep 6, 2024 · 12ca27c · 12ca27c
1 parent 7e674f6
commit 12ca27c
Show file tree

Hide file tree

Showing 4 changed files with 48 additions and 36 deletions.
diff --git a/src/enhanced_player.cpp b/src/enhanced_player.cpp
@@ -31,6 +31,8 @@ bool EnhancedPlayer::hasTopics(const std::vector<std::string> & topics)
     if (pubs.find(topic) == pubs.end()) {
       LOG_ERROR("topic " << topic << " is not in bag!");
       all_there = false;
+    } else {
+      LOG_INFO("topic " << topic << " is in bag!");
     }
   }
   return (all_there);

diff --git a/src/python/camerainfo_to_kalibr.py b/src/python/camerainfo_to_kalibr.py
@@ -18,49 +18,52 @@
 import argparse
 import os
 
-
 import yaml
 
 
 def write_cameras_file(c, f):
-    K = c['camera_matrix']['data']
-    D = c['distortion_coefficients']['data']
-    f.write('cam0:\n')
-    f.write('  camera_model: pinhole\n')
-    f.write('  intrinsics: [%.5f, %.5f, %.5f, %.5f]\n' % (K[0], K[4], K[2], K[5]))
-    f.write('  distortion_model: %s\n' % c['distortion_model'])
-    f.write('  distortion_coeffs: [%.5f, %.5f, %.5f, %.5f, %.5f]\n' % tuple(D))
-    f.write('  resolution: [%d, %d]\n' % (c['image_width'], c['image_height']))
-    f.write('  image_topic: /%s/tags\n' % (c['camera_name']))
-    f.write('  tag_topic: /%s/tags\n' % (c['camera_name']))
-    f.write('  rig_body: XXX_NAME_OF_RIG_BODY\n')
+    K = c["camera_matrix"]["data"]
+    D = c["distortion_coefficients"]["data"]
+    f.write("cam0:\n")
+    f.write("  camera_model: pinhole\n")
+    f.write("  intrinsics: [%.5f, %.5f, %.5f, %.5f]\n" % (K[0], K[4], K[2], K[5]))
+    f.write("  distortion_model: %s\n" % c["distortion_model"])
+    f.write("  distortion_coeffs: [%.5f, %.5f, %.5f, %.5f, %.5f]\n" % tuple(D))
+    f.write("  resolution: [%d, %d]\n" % (c["image_width"], c["image_height"]))
+    f.write("  image_topic: /%s/tags\n" % (c["camera_name"]))
+    f.write("  tag_topic: /%s/tags\n" % (c["camera_name"]))
+    f.write("  rig_body: XXX_NAME_OF_RIG_BODY\n")
 
 
 def read_yaml(filename):
-    with open(filename, 'r') as y:
+    with open(filename, "r") as y:
         try:
-            return yaml.load(y)
+            return yaml.safe_load(y)
         except yaml.YAMLError as e:
             print(e)
 
 
-if __name__ == '__main__':
-    parser = argparse.ArgumentParser(description='convert ROS style format to calibr.')
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser(description="convert ROS style format to calibr.")
     parser.add_argument(
-        '--in_file', '-i', action='store', required=True, help='ROS calibration input yaml file'
+        "--in_file",
+        "-i",
+        action="store",
+        required=True,
+        help="ROS calibration input yaml file",
     )
     parser.add_argument(
-        '--out_file',
-        '-o',
-        action='store',
+        "--out_file",
+        "-o",
+        action="store",
         default=None,
         required=False,
-        help='TagSLAM output file cameras.yaml',
+        help="TagSLAM output file cameras.yaml",
     )
     args = parser.parse_args()
     ros_calib = read_yaml(args.in_file)
     if args.out_file is None:
         file_and_ext = os.path.splitext(args.in_file)
-        args.out_file = file_and_ext[0] + '_kalibr' + file_and_ext[1]
-    with open(args.out_file, 'w') as f:
+        args.out_file = file_and_ext[0] + "_kalibr" + file_and_ext[1]
+    with open(args.out_file, "w") as f:
         write_cameras_file(ros_calib, f)
diff --git a/src/tagslam.cpp b/src/tagslam.cpp
@@ -188,7 +188,7 @@ TagPtr TagSLAM::addTag(int tagId, const std::shared_ptr<Body> & body) const
 
 void TagSLAM::readParams()
 {
-  outBagName_ = declare_parameter("outbag", "out.bag");
+  outBagName_ = declare_parameter("outbag", "output");
   playbackRate_ = declare_parameter("playback_rate", 5.0);
   outDir_ = declare_parameter("output_directory", ".");
   fixedFrame_ = declare_parameter<string>("fixed_frame_id", "map");
@@ -351,12 +351,6 @@ void TagSLAM::doDump(bool optimize)
   publishTransforms(
     times_.empty() ? 0ULL : (*(times_.rbegin()) + 1000ULL), true);
   tagCornerFile_.flush();
-  openOutputBag(outBagName_);
-  writeToBag_ = true;
-  doReplay(0);  //  0 = playback at full speed
-  writeToBag_ = false;
-  outputBag_->close();
-  outputBag_.reset();
 
   writeCameraPoses(outDir_ + "/camera_poses.yaml");
   writeFullCalibration(outDir_ + "/calibration.yaml");
@@ -376,6 +370,14 @@ void TagSLAM::doDump(bool optimize)
   for (auto & m : measurements_) {
     m->writeDiagnostics(graph_);
   }
+
+  openOutputBag(outBagName_);
+  writeToBag_ = true;
+  doReplay(0);  //  0 = playback at full speed
+  writeToBag_ = false;
+  outputBag_->close();
+  outputBag_.reset();
+
   profiler_.record("writeMeasurementDiagnostics");
   std::cout << profiler_ << std::endl;
   std::cout.flush();

diff --git a/src/tagslam_from_bag.cpp b/src/tagslam_from_bag.cpp
@@ -60,7 +60,8 @@ int main(int argc, char ** argv)
   exec.add_node(tagslam_node);
 
   const auto recorded_topics = tagslam_node->getPublishedTopics();
-  printTopics("recorded topic", recorded_topics);
+  printTopics("recorded topics", recorded_topics);
+
 
   const std::string in_uri =
     tagslam_node->declare_parameter<std::string>("in_bag", "");
@@ -72,13 +73,14 @@ int main(int argc, char ** argv)
   if (!std::filesystem::exists(in_uri)) {
     LOG_ERROR("cannot find input bag: " << in_uri);
   }
-  rclcpp::NodeOptions player_options;
-  using Parameter = rclcpp::Parameter;
+
   const auto images = tagslam_node->getImageTopics();
   const auto tags = tagslam_node->getTagTopics();
   const auto odoms = tagslam_node->getOdomTopics();
-  // const auto in_topics = merge({images, tags, odoms});
-  // printTopics("taglslam input topics", in_topics);
+  printTopics("input tags", tags);
+
+  rclcpp::NodeOptions player_options;
+  using Parameter = rclcpp::Parameter;
 
   player_options.parameter_overrides(
     {Parameter("storage.uri", in_uri),  // Parameter("play.topics", in_topics),
@@ -87,7 +89,7 @@ int main(int argc, char ** argv)
      Parameter("play.disable_keyboard_controls", true)});
   auto player_node =
     std::make_shared<tagslam::EnhancedPlayer>("rosbag_player", player_options);
-  player_node->get_logger().set_level(rclcpp::Logger::Level::Warn);
+  // player_node->get_logger().set_level(rclcpp::Logger::Level::Warn);
   exec.add_node(player_node);
 
   std::shared_ptr<tagslam::SyncAndDetect> sync_node;
@@ -103,6 +105,7 @@ int main(int argc, char ** argv)
       exec.add_node(sync_node);
     }
   }
+
   if (!odoms.empty() && !player_node->hasTopics(odoms)) {
     LOG_ERROR("odom topics not in bag, tagslam may hang!");
   }
@@ -111,6 +114,7 @@ int main(int argc, char ** argv)
     tagslam_node->declare_parameter<std::string>("out_bag", "");
   size_t num_frames =
     tagslam_node->get_parameter_or<int>("max_number_of_frames", 0);
+    LOG_INFO("Max Frames: " << num_frames);
 
   std::shared_ptr<rosbag2_transport::Recorder> recorder_node;
   if (!out_uri.empty()) {
@@ -131,6 +135,7 @@ int main(int argc, char ** argv)
   while (player_node->play_next() && rclcpp::ok()) {
     exec.spin_some();
     if (tagslam_node->getNumberOfFrames() >= num_frames) {
+      LOG_INFO("Quitting!");
       break;
     }
   }