129 lines
4.0 KiB
C++
129 lines
4.0 KiB
C++
/*********************************************************************
|
|
*
|
|
* Software License Agreement (BSD License)
|
|
*
|
|
* move_base2 — plugin global planner CHỈ dùng cho test.
|
|
*
|
|
* Bốn alias được export từ cùng một thư viện, mỗi alias là một hành vi mà `PlannerRunner` phải xử lý
|
|
* đúng. Chọn cách này thay vì một plugin đọc config: nó làm test không phụ thuộc vào cây config, và
|
|
* mỗi kịch bản gọi tên đúng thứ nó kiểm.
|
|
*
|
|
* Không alias nào chạm vào con trỏ costmap — đó là điều kiện để test truyền vào một con trỏ giả
|
|
* thay vì phải dựng `Costmap2DROBOT` thật (cần tf3::BufferCore và cây config đầy đủ).
|
|
*
|
|
* Author: DuongTD
|
|
*********************************************************************/
|
|
#include <stdexcept>
|
|
#include <string>
|
|
#include <vector>
|
|
|
|
#include <boost/config.hpp>
|
|
#include <boost/dll/alias.hpp>
|
|
#include <boost/make_shared.hpp>
|
|
|
|
#include <robot_nav_core/base_global_planner.h>
|
|
|
|
namespace move_base2
|
|
{
|
|
namespace testing
|
|
{
|
|
|
|
/// @brief Số pose trong plan mà `TestPlannerOk` sinh ra.
|
|
constexpr std::size_t kPlanLength = 3;
|
|
|
|
/**
|
|
* @class TestGlobalPlanner
|
|
* @brief Planner giả, hành vi cố định theo tham số dựng.
|
|
*/
|
|
class TestGlobalPlanner : public robot_nav_core::BaseGlobalPlanner
|
|
{
|
|
public:
|
|
enum class Behavior
|
|
{
|
|
kOk, ///< Trả plan hợp lệ.
|
|
kEmptyPlan, ///< Trả true kèm plan RỖNG — bẫy mà PlannerRunner phải quy về false.
|
|
kThrow, ///< Ném exception giữa lúc lập plan.
|
|
kInitFails ///< initialize() trả false.
|
|
};
|
|
|
|
explicit TestGlobalPlanner(Behavior behavior) : behavior_(behavior)
|
|
{
|
|
}
|
|
|
|
bool initialize(std::string name, robot_costmap_2d::Costmap2DROBOT* /*costmap_robot*/) override
|
|
{
|
|
// Cố ý KHÔNG chạm costmap_robot — xem chú thích đầu file.
|
|
name_ = std::move(name);
|
|
return behavior_ != Behavior::kInitFails;
|
|
}
|
|
|
|
bool makePlan(const robot_geometry_msgs::PoseStamped& start,
|
|
const robot_geometry_msgs::PoseStamped& goal,
|
|
std::vector<robot_geometry_msgs::PoseStamped>& plan) override
|
|
{
|
|
++call_count_;
|
|
|
|
if (behavior_ == Behavior::kThrow)
|
|
{
|
|
throw std::runtime_error("TestGlobalPlanner was asked to throw an exception");
|
|
}
|
|
|
|
plan.clear();
|
|
if (behavior_ == Behavior::kEmptyPlan)
|
|
{
|
|
return true; // true + rỗng: đúng thứ contract PlannerPort cấm lọt qua.
|
|
}
|
|
|
|
plan.push_back(start);
|
|
for (std::size_t i = plan.size(); i + 1 < kPlanLength; ++i)
|
|
{
|
|
plan.push_back(start);
|
|
}
|
|
plan.push_back(goal);
|
|
return true;
|
|
}
|
|
|
|
bool makePlan(const robot_protocol_msgs::Order& /*msg*/,
|
|
const robot_geometry_msgs::PoseStamped& start,
|
|
const robot_geometry_msgs::PoseStamped& goal,
|
|
std::vector<robot_geometry_msgs::PoseStamped>& plan) override
|
|
{
|
|
saw_order_ = true;
|
|
return makePlan(start, goal, plan);
|
|
}
|
|
|
|
private:
|
|
Behavior behavior_;
|
|
std::string name_;
|
|
std::size_t call_count_ = 0;
|
|
bool saw_order_ = false;
|
|
};
|
|
|
|
robot_nav_core::BaseGlobalPlanner::Ptr createOk()
|
|
{
|
|
return std::make_shared<TestGlobalPlanner>(TestGlobalPlanner::Behavior::kOk);
|
|
}
|
|
|
|
robot_nav_core::BaseGlobalPlanner::Ptr createEmptyPlan()
|
|
{
|
|
return std::make_shared<TestGlobalPlanner>(TestGlobalPlanner::Behavior::kEmptyPlan);
|
|
}
|
|
|
|
robot_nav_core::BaseGlobalPlanner::Ptr createThrowing()
|
|
{
|
|
return std::make_shared<TestGlobalPlanner>(TestGlobalPlanner::Behavior::kThrow);
|
|
}
|
|
|
|
robot_nav_core::BaseGlobalPlanner::Ptr createInitFailing()
|
|
{
|
|
return std::make_shared<TestGlobalPlanner>(TestGlobalPlanner::Behavior::kInitFails);
|
|
}
|
|
|
|
} // namespace testing
|
|
} // namespace move_base2
|
|
|
|
BOOST_DLL_ALIAS(move_base2::testing::createOk, TestPlannerOk)
|
|
BOOST_DLL_ALIAS(move_base2::testing::createEmptyPlan, TestPlannerEmptyPlan)
|
|
BOOST_DLL_ALIAS(move_base2::testing::createThrowing, TestPlannerThrowing)
|
|
BOOST_DLL_ALIAS(move_base2::testing::createInitFailing, TestPlannerInitFails)
|