move_base2/test/sensor_gateway_test.cpp

/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 * move_base2 — test đường vào cảm biến.
 *
 * Test này dựng `LayeredCostmap` THẬT và cắm vào đó các layer gián điệp. Lý do không dùng costmap
 * giả: thứ cần khoá lại ở đây chính là ba contract ẩn của `robot_costmap_2d`, và chúng chỉ tồn tại
 * trong lớp thật —
 *   1. `Layer::dataCallBack<T>` xoá kiểu về `void*` + `std::type_info`, sai kiểu KHÔNG gây lỗi biên
 *      dịch mà rơi im lặng;
 *   2. tham số `name` là khoá topic mà layer so lại, không phải nhãn tự do;
 *   3. bộ lọc chọn layer quyết định layer nào thấy dữ liệu.
 * Một costmap giả sẽ mô phỏng lại các contract đó theo cách tôi *nghĩ* chúng hoạt động — đúng loại
 * test không phát hiện được gì.
 *
 * Author: DuongTD
 *********************************************************************/
#include <gtest/gtest.h>

#include <stdexcept>
#include <string>
#include <typeinfo>
#include <vector>

#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>

#include <robot_costmap_2d/layer.h>
#include <robot_costmap_2d/layered_costmap.h>

#include <move_base2/io/sensor_gateway.h>

#include "spy_layer.h"

using move_base2::SensorGateway;
using move_base2::SensorGatewayConfig;
using move_base2::testing::attachSpy;
using move_base2::testing::SpyPtr;
using robot_costmap_2d::LayerType;

namespace
{

/**
 * @class Bench
 * @brief Một cặp costmap thật + cổng cảm biến đã gắn.
 */
class Bench
{
public:
  Bench()
    : global_("map", false, true)
    , local_("odom", true, false)
  {
    std::string error;
    EXPECT_TRUE(gateway_.configure(SensorGatewayConfig(), error)) << error;
  }

  /// @brief Cắm một layer gián điệp vào costmap global và trả con trỏ để kiểm tra sau.
  SpyPtr addGlobal(LayerType type, const std::string& name, bool enabled = true, bool explode = false)
  {
    return attachSpy(global_, type, name, enabled, explode);
  }

  SpyPtr addLocal(LayerType type, const std::string& name, bool enabled = true, bool explode = false)
  {
    return attachSpy(local_, type, name, enabled, explode);
  }

  void attach()
  {
    gateway_.attach(&global_, &local_);
  }

  SensorGateway& gateway()
  {
    return gateway_;
  }

private:
  robot_costmap_2d::LayeredCostmap global_;
  robot_costmap_2d::LayeredCostmap local_;
  SensorGateway gateway_;
};

robot_sensor_msgs::LaserScan makeScan(std::size_t rays = 40, float range = 1.0F)
{
  robot_sensor_msgs::LaserScan scan;
  scan.header.frame_id = "laser";
  scan.angle_min = -1.5F;                                             // [rad]
  scan.angle_max = 1.5F;                                              // [rad]
  scan.angle_increment = 3.0F / static_cast<float>(rays);             // [rad]
  scan.range_min = 0.05F;                                             // [m]
  scan.range_max = 10.0F;                                             // [m]
  scan.ranges.assign(rays, range);
  return scan;
}

robot_sensor_msgs::DepthCameraData::ConstPtr makeDepth()
{
  robot_sensor_msgs::DepthCameraData::Ptr data =
      boost::make_shared<robot_sensor_msgs::DepthCameraData>();
  data->header.frame_id = "camera_optical";
  return data;
}

}  // namespace

// ================================================================================================
// Contract ẩn #1 — kiểu phải tới nơi ĐÚNG NHƯ khi gửi đi
// ================================================================================================

TEST(SensorGateway, LaserScanArrivesWithLaserScanType)
{
  Bench bench;
  SpyPtr voxel = bench.addLocal(LayerType::VOXEL_LAYER, "obstacles");
  bench.attach();

  bench.gateway().pushLaserScan("/b_scan", makeScan());

  ASSERT_EQ(voxel->count(), 1U);
  EXPECT_TRUE(*voxel->records()[0].type == typeid(robot_sensor_msgs::LaserScan));
}

TEST(SensorGateway, DepthCameraDataArrivesAsConstPtrNotAsValue)
{
  // Đây là test đắt nhất của file. `ObstacleLayer::handleImpl` so
  // `typeid(DepthCameraData::ConstPtr)`; gửi đi dạng giá trị sẽ khớp `typeid(DepthCameraData)` và
  // rơi qua MỌI nhánh if mà không có lỗi biên dịch, không có log — depth camera lặng lẽ ngừng hoạt
  // động. Không có cách nào bắt được lỗi đó ngoài việc kiểm đúng type_info tới nơi.
  Bench bench;
  SpyPtr voxel = bench.addLocal(LayerType::VOXEL_LAYER, "obstacles");
  bench.attach();

  bench.gateway().pushDepthCameraData("/camera/depth/data", makeDepth());

  ASSERT_EQ(voxel->count(), 1U);
  EXPECT_TRUE(*voxel->records()[0].type == typeid(robot_sensor_msgs::DepthCameraData::ConstPtr));
  EXPECT_FALSE(*voxel->records()[0].type == typeid(robot_sensor_msgs::DepthCameraData));
}

TEST(SensorGateway, StaticMapArrivesWithOccupancyGridType)
{
  Bench bench;
  SpyPtr layer = bench.addGlobal(LayerType::STATIC_LAYER, "navigation_map");
  bench.attach();

  bench.gateway().pushStaticMap("/map", robot_nav_msgs::OccupancyGrid());

  ASSERT_EQ(layer->count(), 1U);
  EXPECT_TRUE(*layer->records()[0].type == typeid(robot_nav_msgs::OccupancyGrid));
}

TEST(SensorGateway, PointCloudAndPointCloud2AreDistinctTypes)
{
  Bench bench;
  SpyPtr voxel = bench.addLocal(LayerType::VOXEL_LAYER, "obstacles");
  bench.attach();

  bench.gateway().pushPointCloud("/pc", robot_sensor_msgs::PointCloud());
  bench.gateway().pushPointCloud2("/pc2", robot_sensor_msgs::PointCloud2());

  ASSERT_EQ(voxel->count(), 2U);
  EXPECT_TRUE(*voxel->records()[0].type == typeid(robot_sensor_msgs::PointCloud));
  EXPECT_TRUE(*voxel->records()[1].type == typeid(robot_sensor_msgs::PointCloud2));
}

// ================================================================================================
// Contract ẩn #2 — `name` là khoá topic, phải tới nguyên văn
// ================================================================================================

TEST(SensorGateway, TopicNameReachesTheLayerVerbatim)
{
  // Layer so chuỗi này với `map_topic` / `observation_sources[i].topic` trong YAML. Sửa nó dù chỉ
  // một ký tự — kể cả thêm/bớt dấu '/' — là mất hẳn một cảm biến, không cảnh báo.
  Bench bench;
  SpyPtr voxel = bench.addLocal(LayerType::VOXEL_LAYER, "obstacles");
  bench.attach();

  bench.gateway().pushLaserScan("/b_scan", makeScan());
  bench.gateway().pushDepthCameraData("/camera_right/depth/data", makeDepth());

  ASSERT_EQ(voxel->count(), 2U);
  EXPECT_EQ(voxel->records()[0].topic, "/b_scan");
  EXPECT_EQ(voxel->records()[1].topic, "/camera_right/depth/data");
}

// ================================================================================================
// Contract ẩn #3 — bộ lọc chọn layer (C5)
// ================================================================================================

TEST(SensorGateway, StaticMapGoesOnlyToStaticLayers)
{
  Bench bench;
  SpyPtr static_layer = bench.addGlobal(LayerType::STATIC_LAYER, "navigation_map");
  SpyPtr voxel = bench.addGlobal(LayerType::VOXEL_LAYER, "obstacles");
  SpyPtr inflation = bench.addGlobal(LayerType::INFLATION_LAYER, "inflation");
  bench.attach();

  bench.gateway().pushStaticMap("/map", robot_nav_msgs::OccupancyGrid());

  EXPECT_EQ(static_layer->count(), 1U);
  EXPECT_EQ(voxel->count(), 0U);
  EXPECT_EQ(inflation->count(), 0U);
}

TEST(SensorGateway, ObstacleDataGoesOnlyToVoxelLayers)
{
  Bench bench;
  SpyPtr static_layer = bench.addLocal(LayerType::STATIC_LAYER, "navigation_map");
  SpyPtr voxel = bench.addLocal(LayerType::VOXEL_LAYER, "obstacles");
  SpyPtr inflation = bench.addLocal(LayerType::INFLATION_LAYER, "inflation");
  bench.attach();

  bench.gateway().pushLaserScan("/b_scan", makeScan());

  EXPECT_EQ(voxel->count(), 1U);
  EXPECT_EQ(static_layer->count(), 0U);
  EXPECT_EQ(inflation->count(), 0U);
}

TEST(SensorGateway, LayerNamedAfterATopicDoesNotReceiveTheSample)
{
  // Hồi quy cho C5. Bản `move_base` cũ lọc bằng
  //     getType() == layer_type || getName() == name
  // Vế thứ hai là bẫy: đặt tên một layer trùng tên topic thì nó nhận dữ liệu nó không hiểu. Với
  // InflationLayer — có handleImpl chỉ biết log error — hậu quả là spam log ở đúng tần số cảm biến.
  Bench bench;
  SpyPtr trap = bench.addLocal(LayerType::INFLATION_LAYER, "/b_scan");
  SpyPtr voxel = bench.addLocal(LayerType::VOXEL_LAYER, "obstacles");
  bench.attach();

  bench.gateway().pushLaserScan("/b_scan", makeScan());

  EXPECT_EQ(trap->count(), 0U) << "layer trùng TÊN topic nhưng sai KIỂU vẫn nhận được dữ liệu";
  EXPECT_EQ(voxel->count(), 1U);
}

TEST(SensorGateway, SampleReachesBothGlobalAndLocalCostmaps)
{
  Bench bench;
  SpyPtr global_voxel = bench.addGlobal(LayerType::VOXEL_LAYER, "obstacles");
  SpyPtr local_voxel = bench.addLocal(LayerType::VOXEL_LAYER, "obstacles");
  bench.attach();

  bench.gateway().pushLaserScan("/b_scan", makeScan());

  EXPECT_EQ(global_voxel->count(), 1U);
  EXPECT_EQ(local_voxel->count(), 1U);
  EXPECT_EQ(bench.gateway().stats().delivered, 2U);
}

// ================================================================================================
// Layer tắt (C8) và các nhánh bỏ mẫu — phải ĐẾM được, không im lặng
// ================================================================================================

TEST(SensorGateway, DisabledLayerIsSkippedAndCounted)
{
  Bench bench;
  SpyPtr disabled = bench.addGlobal(LayerType::VOXEL_LAYER, "obstacles", /*enabled=*/false);
  bench.attach();

  bench.gateway().pushLaserScan("/b_scan", makeScan());

  EXPECT_EQ(disabled->count(), 0U);
  EXPECT_EQ(bench.gateway().stats().skipped_disabled, 1U);
  EXPECT_EQ(bench.gateway().stats().delivered, 0U);
}

TEST(SensorGateway, SamplesBeforeAnyCostmapIsAttachedAreCounted)
{
  // Bản cũ mở đầu bằng `if (!costmap) return;` không log. Mọi mẫu tới trước khi costmap tồn tại
  // biến mất không dấu vết, và đó là trạng thái BÌNH THƯỜNG lúc khởi động.
  SensorGateway gateway;
  std::string error;
  ASSERT_TRUE(gateway.configure(SensorGatewayConfig(), error)) << error;
  ASSERT_FALSE(gateway.attached());

  gateway.pushStaticMap("/map", robot_nav_msgs::OccupancyGrid());
  gateway.pushLaserScan("/b_scan", makeScan());
  gateway.pushDepthCameraData("/camera/depth/data", makeDepth());

  EXPECT_EQ(gateway.stats().dropped_no_costmap, 3U);
  EXPECT_EQ(gateway.stats().delivered, 0U);
}

TEST(SensorGateway, NullDepthPointerIsIgnoredWithoutCountingAsADrop)
{
  Bench bench;
  bench.addLocal(LayerType::VOXEL_LAYER, "obstacles");
  bench.attach();

  bench.gateway().pushDepthCameraData("/camera/depth/data",
                                      robot_sensor_msgs::DepthCameraData::ConstPtr());

  EXPECT_EQ(bench.gateway().stats().delivered, 0U);
  EXPECT_EQ(bench.gateway().stats().dropped_no_costmap, 0U);
}

TEST(SensorGateway, ExceptionFromOneLayerDoesNotStarveTheNextOnes)
{
  // Bản cũ bọc try/catch quanh CẢ vòng lặp rồi `return`, nên một layer ném exception làm mọi layer
  // đứng sau nó mất luôn mẫu đó — và với một layer hỏng cố định thì mất vĩnh viễn.
  Bench bench;
  SpyPtr exploding = bench.addLocal(LayerType::VOXEL_LAYER, "broken", true, /*explode=*/true);
  SpyPtr healthy = bench.addLocal(LayerType::VOXEL_LAYER, "obstacles");
  bench.attach();

  bench.gateway().pushLaserScan("/b_scan", makeScan());

  EXPECT_EQ(exploding->count(), 0U);
  EXPECT_EQ(healthy->count(), 1U) << "layer lành bị bỏ qua vì layer trước nó ném exception";
  EXPECT_EQ(bench.gateway().stats().layer_exceptions, 1U);
  EXPECT_EQ(bench.gateway().stats().delivered, 1U);
}

// ================================================================================================
// Lọc laser (C7)
// ================================================================================================

TEST(SensorGatewayConfigTest, LaserFilterIsOffByDefaultAndLeavesTheScanUntouched)
{
  SensorGateway gateway;
  std::string error;
  ASSERT_TRUE(gateway.configure(SensorGatewayConfig(), error)) << error;

  const robot_sensor_msgs::LaserScan scan = makeScan();
  const robot_sensor_msgs::LaserScan prepared = gateway.prepareLaserScan(scan);

  EXPECT_EQ(prepared.ranges, scan.ranges);
  EXPECT_EQ(prepared.angle_increment, scan.angle_increment);
}

TEST(SensorGatewayConfigTest, EnabledLaserFilterKeepsScanStructure)
{
  // Bộ lọc giữ nguyên cấu trúc scan (outlier thành NaN) — chỉ số tia phải khớp một-một với góc, nếu
  // không thì mọi phép chiếu tia sang điểm sau đó đều lệch.
  SensorGatewayConfig config;
  config.laser_sor_enabled = true;
  config.laser_sor_mean_k = 5;
  config.laser_sor_stddev_mul = 1.0;

  SensorGateway gateway;
  std::string error;
  ASSERT_TRUE(gateway.configure(config, error)) << error;

  const robot_sensor_msgs::LaserScan scan = makeScan();
  const robot_sensor_msgs::LaserScan prepared = gateway.prepareLaserScan(scan);

  EXPECT_EQ(prepared.ranges.size(), scan.ranges.size());
  EXPECT_EQ(prepared.angle_increment, scan.angle_increment);
  EXPECT_EQ(prepared.header.frame_id, scan.header.frame_id);
}

TEST(SensorGatewayConfigTest, RejectsOutOfRangeFilterParametersOnlyWhenFilterIsOn)
{
  std::string error;

  SensorGatewayConfig off;
  off.laser_sor_enabled = false;
  off.laser_sor_mean_k = 0;         // vô nghĩa, nhưng tính năng đang tắt
  off.laser_sor_stddev_mul = -1.0;  // vô nghĩa, nhưng tính năng đang tắt
  EXPECT_TRUE(off.validate(error)) << error;

  SensorGatewayConfig bad_k;
  bad_k.laser_sor_enabled = true;
  bad_k.laser_sor_mean_k = 1;
  EXPECT_FALSE(bad_k.validate(error));

  SensorGatewayConfig bad_mul;
  bad_mul.laser_sor_enabled = true;
  bad_mul.laser_sor_stddev_mul = 0.0;
  EXPECT_FALSE(bad_mul.validate(error));
}

TEST(SensorGatewayConfigTest, ConfigureFailsAndReportsWhyOnInvalidParameters)
{
  SensorGatewayConfig config;
  config.laser_sor_enabled = true;
  config.laser_sor_mean_k = 0;

  SensorGateway gateway;
  std::string error;
  EXPECT_FALSE(gateway.configure(config, error));
  EXPECT_FALSE(error.empty());
}

int main(int argc, char** argv)
{
  ::testing::InitGoogleTest(&argc, argv);
  return RUN_ALL_TESTS();
}