play around with fixing astro input relative pose

Author: Schmluk

hydra_ros/include/hydra_ros/places/astro_traversability_estimator.h

@@ -51,7 +51,7 @@ class AstroTraversabilityEstimator : public TraversabilityEstimator {
   struct Config {
     //! @brief The height above the robot body to consider for traversability in meters.
     std::string input_topic = "/local_cost_map";
-    unsigned int queue_size = 1;
+    unsigned int queue_size = 2;
   } const config;
 
   using State = spark_dsg::TraversabilityState;

hydra_ros/src/frontend/traversability_visualizer.cpp

@@ -173,7 +173,7 @@ Color TraversabilityVisualizer::debugColor(float value) const {
   if (value < 0.0f) {
     return Color::black();
   }
-  return spark_dsg::colormaps::rainbowId(static_cast<int>(value), 5);
+  return spark_dsg::colormaps::rainbowId(static_cast<int>(value), 10);
 }
 
 }  // namespace hydra

hydra_ros/src/places/astro_traversability_estimator.cpp

@@ -108,7 +108,7 @@ void AstroTraversabilityEstimator::callback(
       const State state = occupancyToTraversability(msg->data[idx]);
       // Compute the voxel coordinates.
       const Eigen::Vector3f position =
-          Eigen::Vector3f(x + 0.5f, y + 0.5f, 0.0f) * voxel_size + origin;
+          Eigen::Vector3f(x, y, 0.0f) * voxel_size + origin;
       const auto index = traversability_layer_->globalIndexFromPoint(position);
       // TODO(lschmid): Fix the allocate block interfaces for the traversability layer.
       auto& block = traversability_layer_->allocateBlock(