92 lines
2.8 KiB
C++
92 lines
2.8 KiB
C++
#include "goal_source_adapter.h"
|
|
|
|
#include <cmath>
|
|
|
|
#include <boost/dll/alias.hpp>
|
|
|
|
namespace mission_plugins
|
|
{
|
|
|
|
mission_adapters::MissionSourceAdapter::Ptr GoalSourceAdapter::create()
|
|
{
|
|
return std::make_shared<GoalSourceAdapter>();
|
|
}
|
|
|
|
bool GoalSourceAdapter::configure(const std::string& name, robot::NodeHandle& nh)
|
|
{
|
|
(void)nh; // nguồn này chưa có param riêng
|
|
name_ = name;
|
|
return true;
|
|
}
|
|
|
|
std::string GoalSourceAdapter::schema() const
|
|
{
|
|
return mission_adapters::schema::kPoseStamped;
|
|
}
|
|
|
|
bool GoalSourceAdapter::validate(const mission_adapters::MissionRequest& request,
|
|
std::string& reason) const
|
|
{
|
|
if (!request.pose)
|
|
{
|
|
reason = "request is missing the pose payload";
|
|
return false;
|
|
}
|
|
|
|
const auto& position = request.pose->pose.position;
|
|
const auto& orientation = request.pose->pose.orientation;
|
|
|
|
// NaN/Inf từ host phải bị chặn ngay tại biên: lọt xuống dưới thì mọi phép so khoảng cách tới
|
|
// goal đều trả false và robot chạy tới khi có người bấm dừng.
|
|
if (!std::isfinite(position.x) || !std::isfinite(position.y) || !std::isfinite(position.z))
|
|
{
|
|
reason = "goal contains NaN/Inf in position";
|
|
return false;
|
|
}
|
|
|
|
if (!std::isfinite(orientation.x) || !std::isfinite(orientation.y) ||
|
|
!std::isfinite(orientation.z) || !std::isfinite(orientation.w))
|
|
{
|
|
reason = "goal contains NaN/Inf in orientation";
|
|
return false;
|
|
}
|
|
|
|
// Quaternion toàn 0 là lỗi hay gặp khi host quên set orientation — nó không phải "hướng bất kỳ",
|
|
// nó là dữ liệu hỏng.
|
|
const double norm_squared = orientation.x * orientation.x +
|
|
orientation.y * orientation.y +
|
|
orientation.z * orientation.z +
|
|
orientation.w * orientation.w;
|
|
|
|
if (std::sqrt(norm_squared) < kMinQuaternionNorm)
|
|
{
|
|
reason = "goal has a quaternion that cannot be normalized (norm ~ 0)";
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
mission_adapters::ConversionResult
|
|
GoalSourceAdapter::convert(const mission_adapters::MissionRequest& request)
|
|
{
|
|
mission_adapters::ConversionResult result;
|
|
|
|
if (!request.pose)
|
|
return result;
|
|
|
|
auto mission = std::make_shared<mission_adapters::Mission>();
|
|
mission->type = mission_adapters::MissionType::SIMPLE_GOAL;
|
|
mission->goal = *request.pose;
|
|
mission->motion_hint = "position";
|
|
|
|
// Goal đơn lẻ luôn thay việc đang chạy: người dùng bấm một đích mới nghĩa là bỏ đích cũ.
|
|
result.mode = mission_adapters::SubmitMode::kReplace;
|
|
result.missions.push_back(mission);
|
|
return result;
|
|
}
|
|
|
|
} // namespace mission_plugins
|
|
|
|
BOOST_DLL_ALIAS(mission_plugins::GoalSourceAdapter::create, GoalSourceAdapter)
|