117 lines
3.2 KiB
C++
117 lines
3.2 KiB
C++
/*********************************************************************
|
|
*
|
|
* Software License Agreement (BSD License)
|
|
*
|
|
* move_base2 — layer gián điệp dùng chung cho các test đường vào cảm biến.
|
|
*
|
|
* Author: DuongTD
|
|
*********************************************************************/
|
|
#ifndef MOVE_BASE2_TEST_SPY_LAYER_H_
|
|
#define MOVE_BASE2_TEST_SPY_LAYER_H_
|
|
|
|
#include <cstddef>
|
|
#include <functional>
|
|
#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>
|
|
|
|
namespace move_base2
|
|
{
|
|
namespace testing
|
|
{
|
|
|
|
/**
|
|
* @class SpyLayer
|
|
* @brief Layer chỉ ghi lại những gì nó nhận được.
|
|
*
|
|
* Dùng với `LayeredCostmap` **thật**: ba contract ẩn cần khoá lại (type erasure qua `void*` +
|
|
* `type_info`, `name` là khoá topic, bộ lọc chọn layer) đều nằm trong lớp thật, nên một costmap giả
|
|
* chỉ mô phỏng lại chúng theo cách người viết test *nghĩ* chúng hoạt động.
|
|
*
|
|
* @note `Layer::Layer()` đặt `enabled_ = false`; layer thật bật cờ này khi đọc config. Ở đây phải tự
|
|
* bật — và chính chỗ đó cho phép test nhánh "bỏ qua layer đang tắt".
|
|
*/
|
|
class SpyLayer : public robot_costmap_2d::Layer
|
|
{
|
|
public:
|
|
struct Record
|
|
{
|
|
const std::type_info* type = nullptr; ///< type_info có storage tĩnh nên giữ con trỏ là an toàn.
|
|
std::string topic;
|
|
};
|
|
|
|
/// @brief Hook để test tự sao chép phần dữ liệu nó quan tâm — con trỏ `data` treo sau khi trả về.
|
|
using Observer = std::function<void(const void*, const std::type_info&, const std::string&)>;
|
|
|
|
explicit SpyLayer(robot_costmap_2d::LayerType type, bool enabled = true, bool explode = false)
|
|
: type_(type), explode_(explode)
|
|
{
|
|
enabled_ = enabled;
|
|
}
|
|
|
|
robot_costmap_2d::LayerType getType() const override
|
|
{
|
|
return type_;
|
|
}
|
|
|
|
void setObserver(Observer observer)
|
|
{
|
|
observer_ = std::move(observer);
|
|
}
|
|
|
|
const std::vector<Record>& records() const
|
|
{
|
|
return records_;
|
|
}
|
|
|
|
std::size_t count() const
|
|
{
|
|
return records_.size();
|
|
}
|
|
|
|
protected:
|
|
void handleImpl(const void* data, const std::type_info& type, const std::string& topic) override
|
|
{
|
|
if (explode_)
|
|
{
|
|
throw std::runtime_error("SpyLayer was asked to throw an exception");
|
|
}
|
|
records_.push_back(Record{ &type, topic });
|
|
if (observer_)
|
|
{
|
|
observer_(data, type, topic);
|
|
}
|
|
}
|
|
|
|
private:
|
|
robot_costmap_2d::LayerType type_;
|
|
bool explode_;
|
|
Observer observer_;
|
|
std::vector<Record> records_;
|
|
};
|
|
|
|
using SpyPtr = boost::shared_ptr<SpyLayer>;
|
|
|
|
/// @brief Cắm một layer gián điệp vào @p costmap và trả con trỏ để kiểm tra sau.
|
|
inline SpyPtr attachSpy(robot_costmap_2d::LayeredCostmap& costmap,
|
|
robot_costmap_2d::LayerType type, const std::string& name,
|
|
bool enabled = true, bool explode = false)
|
|
{
|
|
SpyPtr spy = boost::make_shared<SpyLayer>(type, enabled, explode);
|
|
spy->initialize(&costmap, name, nullptr);
|
|
costmap.addPlugin(spy);
|
|
return spy;
|
|
}
|
|
|
|
} // namespace testing
|
|
} // namespace move_base2
|
|
|
|
#endif // MOVE_BASE2_TEST_SPY_LAYER_H_
|