fix: restore SRC1100 free navigation protocol
This commit is contained in:
parent
f092e2539d
commit
b5c6d50022
@ -85,10 +85,14 @@ private:
|
||||
bool connected_() const;
|
||||
|
||||
AgvResult acquireControl_() const;
|
||||
AgvResult confirmPoseNavigationStarted_(
|
||||
const std::string& task_id,
|
||||
std::uint64_t navigation_generation) const;
|
||||
AgvResult sendControlledCommand_(int sock,
|
||||
std::uint16_t command,
|
||||
const Json::Value& payload,
|
||||
Json::Value* response) const;
|
||||
Json::Value* response,
|
||||
std::uint64_t* accepted_navigation_generation = nullptr) const;
|
||||
AgvResult sendCommand_(int sock,
|
||||
std::uint16_t command,
|
||||
const Json::Value& payload,
|
||||
@ -160,13 +164,16 @@ private:
|
||||
std::size_t map_update_history_size_{8};
|
||||
|
||||
mutable std::mutex mutex_;
|
||||
mutable std::mutex status_io_mutex_;
|
||||
mutable std::mutex control_sequence_mutex_;
|
||||
int sock_status_{-1};
|
||||
int sock_control_{-1};
|
||||
int sock_navigation_{-1};
|
||||
int sock_config_{-1};
|
||||
int sock_other_{-1};
|
||||
int sock_push_{-1};
|
||||
mutable std::atomic<std::uint64_t> navigation_generation_{0};
|
||||
mutable std::atomic<std::uint64_t> pose_task_sequence_{0};
|
||||
mutable int sock_status_{-1};
|
||||
mutable int sock_control_{-1};
|
||||
mutable int sock_navigation_{-1};
|
||||
mutable int sock_config_{-1};
|
||||
mutable int sock_other_{-1};
|
||||
mutable int sock_push_{-1};
|
||||
std::string last_error_;
|
||||
|
||||
std::atomic<bool> push_running_{false};
|
||||
|
||||
@ -30,6 +30,7 @@ namespace {
|
||||
constexpr std::uint16_t kRobotStatusLoc = 1004;
|
||||
constexpr std::uint16_t kRobotStatusBattery = 1007;
|
||||
constexpr std::uint16_t kRobotStatusTask = 1020;
|
||||
constexpr std::uint16_t kRobotStatusTaskPackage = 1110;
|
||||
constexpr std::uint16_t kRobotStatusMap = 1300;
|
||||
constexpr std::uint16_t kRobotStatusStation = 1301;
|
||||
constexpr std::uint16_t kRobotStatusMappingFileList = 1780;
|
||||
@ -40,7 +41,6 @@ constexpr std::uint16_t kRobotControlLoadMap = 2022;
|
||||
constexpr std::uint16_t kRobotTaskPause = 3001;
|
||||
constexpr std::uint16_t kRobotTaskResume = 3002;
|
||||
constexpr std::uint16_t kRobotTaskCancel = 3003;
|
||||
constexpr std::uint16_t kRobotTaskGoPoint = 3050;
|
||||
constexpr std::uint16_t kRobotTaskGoTarget = 3051;
|
||||
constexpr std::uint16_t kRobotTaskGoTargetList = 3066;
|
||||
constexpr std::uint16_t kRobotConfigLock = 4005;
|
||||
@ -52,6 +52,8 @@ constexpr std::uint16_t kRobotPushConfigReq = 9300;
|
||||
constexpr std::uint16_t kRobotPushConfigRes = 19300;
|
||||
constexpr std::uint16_t kRobotPush = 19301;
|
||||
constexpr std::uint32_t kMaxFramePayloadBytes = 512U * 1024U * 1024U;
|
||||
constexpr auto kPoseNavigationStartTimeout = std::chrono::milliseconds(1500);
|
||||
constexpr auto kPoseNavigationPollInterval = std::chrono::milliseconds(50);
|
||||
constexpr int kDefaultMapUpdateIntervalMs = 1000;
|
||||
constexpr std::size_t kDefaultMapUpdateHistorySize = 8;
|
||||
constexpr std::uint64_t kMapSnapshotSequenceStart = 1;
|
||||
@ -171,6 +173,16 @@ bool jsonHas(const Json::Value& value, const char* key)
|
||||
return jsonFind(value, key) != nullptr;
|
||||
}
|
||||
|
||||
bool hasNumericControllerRetCode(const Json::Value& response)
|
||||
{
|
||||
const auto* ret_code = jsonFind(response, "ret_code");
|
||||
return ret_code
|
||||
&& (ret_code->isInt()
|
||||
|| ret_code->isUInt()
|
||||
|| ret_code->isInt64()
|
||||
|| ret_code->isUInt64());
|
||||
}
|
||||
|
||||
bool hasFaultArray(const Json::Value& value, const char* key)
|
||||
{
|
||||
const auto* found = jsonFind(value, key);
|
||||
@ -191,6 +203,28 @@ std::string jsonValueToString(const Json::Value& value)
|
||||
return Json::writeString(builder, value);
|
||||
}
|
||||
|
||||
AgvResult withUnknownControllerOutcome(AgvResult result)
|
||||
{
|
||||
const auto code = result.ok() ? AgvErrorCode::CommandFailed : result.code;
|
||||
std::string detail = result.message.empty() ? "unknown transport or protocol error" : result.message;
|
||||
detail +=
|
||||
"; SRC1100 controller outcome is unknown after the command attempt; "
|
||||
"the command may already have taken effect; do not issue another motion "
|
||||
"command automatically; query status and cancel or stop first";
|
||||
return AgvResult::failure(code, detail);
|
||||
}
|
||||
|
||||
std::string makePoseTaskId(
|
||||
const std::string& device_id,
|
||||
const std::uint64_t task_sequence)
|
||||
{
|
||||
const auto timestamp = std::chrono::duration_cast<std::chrono::microseconds>(
|
||||
std::chrono::system_clock::now().time_since_epoch()).count();
|
||||
const std::string prefix = device_id.empty() ? "cmvr-es" : device_id;
|
||||
return prefix + "_pose_" + std::to_string(timestamp)
|
||||
+ "_" + std::to_string(task_sequence);
|
||||
}
|
||||
|
||||
void putPropertyIfPresent(
|
||||
std::unordered_map<std::string, std::string>& properties,
|
||||
const Json::Value& value,
|
||||
@ -461,6 +495,12 @@ AgvNavigationStatus Src1100Agv::navigationStatus() const
|
||||
status.message = result.message;
|
||||
return status;
|
||||
}
|
||||
const auto controller_result = resultFromResponse_(response);
|
||||
if (!controller_result.ok()) {
|
||||
status.state = AgvTaskState::Failed;
|
||||
status.message = controller_result.message;
|
||||
return status;
|
||||
}
|
||||
|
||||
status.state = toTaskState(jsonGet(response, "task_status", 0).asInt());
|
||||
status.type = toTaskType(jsonGet(response, "task_type", 0).asInt());
|
||||
@ -477,6 +517,10 @@ AgvResult Src1100Agv::connect_()
|
||||
stopMapUpdateThread_();
|
||||
|
||||
{
|
||||
// Status requests may wait for a controller receive timeout without
|
||||
// holding mutex_. Serialize lifecycle changes with that channel before
|
||||
// replacing or closing its descriptor.
|
||||
std::lock_guard<std::mutex> status_io_lock(status_io_mutex_);
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
closeSocket_(sock_status_);
|
||||
closeSocket_(sock_control_);
|
||||
@ -550,6 +594,7 @@ AgvResult Src1100Agv::disconnect_()
|
||||
{
|
||||
stopMapUpdateThread_();
|
||||
stopPushThread_();
|
||||
std::lock_guard<std::mutex> status_io_lock(status_io_mutex_);
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
closeSocket_(sock_status_);
|
||||
closeSocket_(sock_control_);
|
||||
@ -575,6 +620,10 @@ AgvResult Src1100Agv::emergencyStop()
|
||||
"SRC1100 acquire control authority failed: " + detail);
|
||||
}
|
||||
|
||||
struct StopOutcome {
|
||||
AgvResult result;
|
||||
bool controller_outcome_unknown{false};
|
||||
};
|
||||
const auto send_stop = [this](const int sock, const std::uint16_t command) {
|
||||
Json::Value response;
|
||||
auto result = sendCommand_(
|
||||
@ -582,32 +631,60 @@ AgvResult Src1100Agv::emergencyStop()
|
||||
command,
|
||||
Json::Value(Json::objectValue),
|
||||
&response);
|
||||
return result.ok() ? resultFromResponse_(response) : result;
|
||||
if (!result.ok()) {
|
||||
return StopOutcome{
|
||||
withUnknownControllerOutcome(std::move(result)),
|
||||
true};
|
||||
}
|
||||
if (!hasNumericControllerRetCode(response)) {
|
||||
return StopOutcome{
|
||||
withUnknownControllerOutcome(resultFromResponse_(response)),
|
||||
true};
|
||||
}
|
||||
return StopOutcome{resultFromResponse_(response), false};
|
||||
};
|
||||
|
||||
bool generation_advanced = false;
|
||||
const auto advance_generation_if_needed = [this, &generation_advanced](
|
||||
const StopOutcome& outcome) {
|
||||
if (!generation_advanced
|
||||
&& (outcome.result.ok() || outcome.controller_outcome_unknown)) {
|
||||
// Publish immediately after the first accepted or indeterminate stop
|
||||
// outcome. Waiting for the second stop response would leave a window
|
||||
// in which pose-start confirmation could incorrectly return success.
|
||||
navigation_generation_.fetch_add(1, std::memory_order_relaxed);
|
||||
generation_advanced = true;
|
||||
}
|
||||
};
|
||||
|
||||
const auto motion_stop = send_stop(sock_control_, kRobotControlStop);
|
||||
advance_generation_if_needed(motion_stop);
|
||||
const auto navigation_cancel = send_stop(sock_navigation_, kRobotTaskCancel);
|
||||
if (!motion_stop.ok()) {
|
||||
const std::string detail = motion_stop.message.empty() ? "unknown error" : motion_stop.message;
|
||||
if (!navigation_cancel.ok()) {
|
||||
const std::string cancel_detail = navigation_cancel.message.empty()
|
||||
advance_generation_if_needed(navigation_cancel);
|
||||
|
||||
if (!motion_stop.result.ok()) {
|
||||
const std::string detail = motion_stop.result.message.empty()
|
||||
? "unknown error"
|
||||
: navigation_cancel.message;
|
||||
: motion_stop.result.message;
|
||||
if (!navigation_cancel.result.ok()) {
|
||||
const std::string cancel_detail = navigation_cancel.result.message.empty()
|
||||
? "unknown error"
|
||||
: navigation_cancel.result.message;
|
||||
return AgvResult::failure(
|
||||
motion_stop.code,
|
||||
motion_stop.result.code,
|
||||
"SRC1100 software stop failed: control stop: " + detail
|
||||
+ "; cancel navigation: " + cancel_detail);
|
||||
}
|
||||
return AgvResult::failure(
|
||||
motion_stop.code,
|
||||
motion_stop.result.code,
|
||||
"SRC1100 software stop failed: control stop: " + detail);
|
||||
}
|
||||
if (!navigation_cancel.ok()) {
|
||||
const std::string detail = navigation_cancel.message.empty()
|
||||
if (!navigation_cancel.result.ok()) {
|
||||
const std::string detail = navigation_cancel.result.message.empty()
|
||||
? "unknown error"
|
||||
: navigation_cancel.message;
|
||||
: navigation_cancel.result.message;
|
||||
return AgvResult::failure(
|
||||
navigation_cancel.code,
|
||||
navigation_cancel.result.code,
|
||||
"SRC1100 software stop failed: cancel navigation: " + detail);
|
||||
}
|
||||
return AgvResult::success();
|
||||
@ -625,20 +702,73 @@ AgvResult Src1100Agv::navigateToPose(
|
||||
const AgvMotionOptions& options,
|
||||
const AgvAdapterParams& adapter_params)
|
||||
{
|
||||
(void)adapter_params;
|
||||
// This SRC1100 firmware exposes arbitrary-pose navigation through the
|
||||
// vendor-specific freeGo extension of API 3051. Keep the required station
|
||||
// identifiers non-empty even though the controller ignores id when freeGo
|
||||
// is present.
|
||||
std::string source_id = adapter_params.getString("source_id").value_or("SELF_POSITION");
|
||||
if (source_id.empty()) {
|
||||
source_id = "SELF_POSITION";
|
||||
}
|
||||
if (source_id != "SELF_POSITION") {
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::InvalidArgument,
|
||||
"SRC1100 free navigation source_id must be SELF_POSITION");
|
||||
}
|
||||
std::string target_id = adapter_params.getString("target_id").value_or("SELF_POSITION");
|
||||
if (target_id.empty()) {
|
||||
target_id = "SELF_POSITION";
|
||||
}
|
||||
if (target_id != "SELF_POSITION") {
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::InvalidArgument,
|
||||
"SRC1100 free navigation target_id must be SELF_POSITION so a "
|
||||
"malformed freeGo request cannot fall back to station navigation");
|
||||
}
|
||||
|
||||
const auto skill_name = adapter_params.getString("skill_name");
|
||||
if (skill_name && !skill_name->empty() && *skill_name != "GotoSpecifiedPose") {
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::InvalidArgument,
|
||||
"SRC1100 free navigation skill_name must be GotoSpecifiedPose");
|
||||
}
|
||||
|
||||
const auto task_sequence =
|
||||
pose_task_sequence_.fetch_add(1, std::memory_order_relaxed) + 1;
|
||||
const std::string task_id_prefix =
|
||||
adapter_params.getString("task_id").value_or(id_);
|
||||
const std::string task_id = makePoseTaskId(task_id_prefix, task_sequence);
|
||||
|
||||
// API 3050 is the controller's arbitrary world-coordinate navigation
|
||||
// command. Do not encode a map pose as API 3051/freeGo: that extension is
|
||||
// only defined for differential-drive chassis, and a multi-steer chassis
|
||||
// may accept the command before the navigation task fails.
|
||||
Json::Value payload(Json::objectValue);
|
||||
jsonMember(payload, "x") = pose.x;
|
||||
jsonMember(payload, "y") = pose.y;
|
||||
jsonMember(payload, "angle") = pose.theta;
|
||||
applyMotionOptions_(payload, options, false);
|
||||
jsonMember(payload, "source_id") = source_id;
|
||||
jsonMember(payload, "id") = target_id;
|
||||
jsonMember(payload, "task_id") = task_id;
|
||||
if (skill_name && !skill_name->empty()) {
|
||||
jsonMember(payload, "skill_name") = *skill_name;
|
||||
}
|
||||
|
||||
auto& free_go = jsonMember(payload, "freeGo");
|
||||
jsonMember(free_go, "x") = pose.x;
|
||||
jsonMember(free_go, "y") = pose.y;
|
||||
jsonMember(free_go, "theta") = pose.theta;
|
||||
|
||||
// Only strongly typed motion fields and the string whitelist above are
|
||||
// accepted here. Generic adapter passthrough could inject unrelated 3051
|
||||
// operations such as lift, fork, script, or digital-I/O actions.
|
||||
applyMotionOptions_(payload, options);
|
||||
|
||||
Json::Value response;
|
||||
auto result = sendControlledCommand_(sock_navigation_, kRobotTaskGoPoint, payload, &response);
|
||||
return result.ok() ? resultFromResponse_(response) : result;
|
||||
std::uint64_t navigation_generation = 0;
|
||||
auto result = sendControlledCommand_(
|
||||
sock_navigation_,
|
||||
kRobotTaskGoTarget,
|
||||
payload,
|
||||
&response,
|
||||
&navigation_generation);
|
||||
if (!result.ok()) {
|
||||
return result;
|
||||
}
|
||||
return confirmPoseNavigationStarted_(task_id, navigation_generation);
|
||||
}
|
||||
|
||||
AgvResult Src1100Agv::navigateToStation(
|
||||
@ -654,8 +784,13 @@ AgvResult Src1100Agv::navigateToStation(
|
||||
// extensions so the SRC controller receives JSON numbers.
|
||||
applyMotionOptions_(payload, options);
|
||||
Json::Value response;
|
||||
auto result = sendControlledCommand_(sock_navigation_, kRobotTaskGoTarget, payload, &response);
|
||||
return result.ok() ? resultFromResponse_(response) : result;
|
||||
std::uint64_t accepted_generation = 0;
|
||||
return sendControlledCommand_(
|
||||
sock_navigation_,
|
||||
kRobotTaskGoTarget,
|
||||
payload,
|
||||
&response,
|
||||
&accepted_generation);
|
||||
}
|
||||
|
||||
AgvResult Src1100Agv::followPath(const std::vector<AgvPathSegment>& path)
|
||||
@ -672,41 +807,49 @@ AgvResult Src1100Agv::followPath(const std::vector<AgvPathSegment>& path)
|
||||
}
|
||||
jsonMember(payload, "move_task_list") = tasks;
|
||||
Json::Value response;
|
||||
auto result = sendControlledCommand_(sock_navigation_, kRobotTaskGoTargetList, payload, &response);
|
||||
return result.ok() ? resultFromResponse_(response) : result;
|
||||
std::uint64_t accepted_generation = 0;
|
||||
return sendControlledCommand_(
|
||||
sock_navigation_,
|
||||
kRobotTaskGoTargetList,
|
||||
payload,
|
||||
&response,
|
||||
&accepted_generation);
|
||||
}
|
||||
|
||||
AgvResult Src1100Agv::pauseNavigation()
|
||||
{
|
||||
Json::Value response;
|
||||
auto result = sendControlledCommand_(
|
||||
std::uint64_t accepted_generation = 0;
|
||||
return sendControlledCommand_(
|
||||
sock_navigation_,
|
||||
kRobotTaskPause,
|
||||
Json::Value(Json::objectValue),
|
||||
&response);
|
||||
return result.ok() ? resultFromResponse_(response) : result;
|
||||
&response,
|
||||
&accepted_generation);
|
||||
}
|
||||
|
||||
AgvResult Src1100Agv::resumeNavigation()
|
||||
{
|
||||
Json::Value response;
|
||||
auto result = sendControlledCommand_(
|
||||
std::uint64_t accepted_generation = 0;
|
||||
return sendControlledCommand_(
|
||||
sock_navigation_,
|
||||
kRobotTaskResume,
|
||||
Json::Value(Json::objectValue),
|
||||
&response);
|
||||
return result.ok() ? resultFromResponse_(response) : result;
|
||||
&response,
|
||||
&accepted_generation);
|
||||
}
|
||||
|
||||
AgvResult Src1100Agv::cancelNavigation()
|
||||
{
|
||||
Json::Value response;
|
||||
auto result = sendControlledCommand_(
|
||||
std::uint64_t accepted_generation = 0;
|
||||
return sendControlledCommand_(
|
||||
sock_navigation_,
|
||||
kRobotTaskCancel,
|
||||
Json::Value(Json::objectValue),
|
||||
&response);
|
||||
return result.ok() ? resultFromResponse_(response) : result;
|
||||
&response,
|
||||
&accepted_generation);
|
||||
}
|
||||
|
||||
AgvResult Src1100Agv::setVelocity(const AgvVelocity& velocity)
|
||||
@ -716,8 +859,13 @@ AgvResult Src1100Agv::setVelocity(const AgvVelocity& velocity)
|
||||
jsonMember(payload, "vy") = velocity.vy;
|
||||
jsonMember(payload, "w") = velocity.wz;
|
||||
Json::Value response;
|
||||
auto result = sendControlledCommand_(sock_control_, kRobotControlMotion, payload, &response);
|
||||
return result.ok() ? resultFromResponse_(response) : result;
|
||||
std::uint64_t accepted_generation = 0;
|
||||
return sendControlledCommand_(
|
||||
sock_control_,
|
||||
kRobotControlMotion,
|
||||
payload,
|
||||
&response,
|
||||
&accepted_generation);
|
||||
}
|
||||
|
||||
AgvResult Src1100Agv::listMaps(std::vector<std::string>& maps) const
|
||||
@ -1580,15 +1728,156 @@ AgvResult Src1100Agv::acquireControl_() const
|
||||
return result.ok() ? resultFromResponse_(response) : result;
|
||||
}
|
||||
|
||||
AgvResult Src1100Agv::confirmPoseNavigationStarted_(
|
||||
const std::string& task_id,
|
||||
const std::uint64_t navigation_generation) const
|
||||
{
|
||||
const auto deadline = std::chrono::steady_clock::now() + kPoseNavigationStartTimeout;
|
||||
std::string last_status = "no task status received";
|
||||
|
||||
const auto cached_fault_detail = [this]() {
|
||||
std::lock_guard<std::mutex> lock(runtime_state_mutex_);
|
||||
if (!cached_runtime_state_valid_ || !cached_runtime_state_.fault) {
|
||||
return std::string{};
|
||||
}
|
||||
return cached_runtime_state_.last_error;
|
||||
};
|
||||
|
||||
while (true) {
|
||||
if (navigation_generation_.load(std::memory_order_relaxed) != navigation_generation) {
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::CommandFailed,
|
||||
"SRC1100 free-navigation start confirmation was superseded by "
|
||||
"another accepted navigation, velocity, pause, or stop command; "
|
||||
"the controller task state is unknown, so do not retry automatically "
|
||||
"before querying or canceling navigation");
|
||||
}
|
||||
|
||||
Json::Value payload(Json::objectValue);
|
||||
Json::Value task_ids(Json::arrayValue);
|
||||
task_ids.append(task_id);
|
||||
jsonMember(payload, "task_ids") = std::move(task_ids);
|
||||
|
||||
Json::Value response;
|
||||
const auto query_result = sendCommand_(
|
||||
sock_status_,
|
||||
kRobotStatusTaskPackage,
|
||||
payload,
|
||||
&response);
|
||||
if (!query_result.ok()) {
|
||||
const std::string detail = query_result.message.empty()
|
||||
? "unknown error"
|
||||
: query_result.message;
|
||||
return AgvResult::failure(
|
||||
query_result.code,
|
||||
"SRC1100 accepted the free-navigation command, but task start "
|
||||
"could not be verified: " + detail
|
||||
+ "; do not retry automatically before checking or canceling navigation");
|
||||
}
|
||||
|
||||
const auto controller_result = resultFromResponse_(response);
|
||||
if (!controller_result.ok()) {
|
||||
return AgvResult::failure(
|
||||
controller_result.code,
|
||||
"SRC1100 accepted the free-navigation command, but task status "
|
||||
"query failed: " + controller_result.message);
|
||||
}
|
||||
|
||||
if (navigation_generation_.load(std::memory_order_relaxed) != navigation_generation) {
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::CommandFailed,
|
||||
"SRC1100 free-navigation start confirmation was superseded by "
|
||||
"another accepted navigation, velocity, pause, or stop command; "
|
||||
"the controller task state is unknown, so do not retry automatically "
|
||||
"before querying or canceling navigation");
|
||||
}
|
||||
|
||||
const auto* package = jsonFind(response, "task_status_package");
|
||||
const auto* status_list = package ? jsonFind(*package, "task_status_list") : nullptr;
|
||||
bool matching_task_found = false;
|
||||
if (status_list && status_list->isArray()) {
|
||||
for (const auto& item : *status_list) {
|
||||
if (jsonGet(item, "task_id", "").asString() != task_id) {
|
||||
continue;
|
||||
}
|
||||
matching_task_found = true;
|
||||
const int task_state = jsonGet(item, "status", 0).asInt();
|
||||
const int task_type = jsonGet(item, "type", 0).asInt();
|
||||
last_status = "task_id=" + task_id
|
||||
+ ", task_status=" + std::to_string(task_state)
|
||||
+ ", task_type=" + std::to_string(task_type);
|
||||
if (package) {
|
||||
const std::string info = jsonGet(*package, "info", "").asString();
|
||||
if (!info.empty()) {
|
||||
last_status += ", info=" + info;
|
||||
}
|
||||
}
|
||||
|
||||
if (task_type != 1) {
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::TaskRejected,
|
||||
"SRC1100 created an unexpected task type for free navigation: "
|
||||
+ last_status);
|
||||
}
|
||||
if (task_state == 1 || task_state == 2 || task_state == 4) {
|
||||
return AgvResult::success();
|
||||
}
|
||||
if (task_state == 3) {
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::TaskRejected,
|
||||
"SRC1100 free-navigation task was established but is paused: "
|
||||
+ last_status
|
||||
+ "; do not retry automatically before querying or canceling it");
|
||||
}
|
||||
if (task_state == 5 || task_state == 6) {
|
||||
std::string detail = last_status;
|
||||
const std::string fault = cached_fault_detail();
|
||||
if (!fault.empty()) {
|
||||
detail += ", " + fault;
|
||||
}
|
||||
return AgvResult::failure(
|
||||
task_state == 5 ? AgvErrorCode::TaskFailed : AgvErrorCode::TaskCanceled,
|
||||
task_state == 5
|
||||
? "SRC1100 free-navigation task failed: " + detail
|
||||
: "SRC1100 free-navigation task was canceled: " + detail);
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!matching_task_found) {
|
||||
last_status = "task_id=" + task_id + " not present in task_status_package";
|
||||
}
|
||||
|
||||
if (std::chrono::steady_clock::now() >= deadline) {
|
||||
break;
|
||||
}
|
||||
std::this_thread::sleep_for(kPoseNavigationPollInterval);
|
||||
}
|
||||
|
||||
std::string detail = last_status;
|
||||
const std::string fault = cached_fault_detail();
|
||||
if (!fault.empty()) {
|
||||
detail += ", " + fault;
|
||||
}
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::TaskRejected,
|
||||
"SRC1100 accepted the free-navigation command, but no matching pose "
|
||||
"task was established within "
|
||||
+ std::to_string(kPoseNavigationStartTimeout.count())
|
||||
+ " ms; last " + detail
|
||||
+ "; do not retry automatically before checking or canceling navigation");
|
||||
}
|
||||
|
||||
AgvResult Src1100Agv::sendControlledCommand_(
|
||||
const int sock,
|
||||
const std::uint16_t command,
|
||||
const Json::Value& payload,
|
||||
Json::Value* response) const
|
||||
Json::Value* response,
|
||||
std::uint64_t* accepted_navigation_generation) const
|
||||
{
|
||||
// Keep the permission acquisition and the following write ordered with
|
||||
// respect to other control RPCs in this process. sendCommand_ has its own
|
||||
// socket mutex, so this must remain a distinct lock.
|
||||
// respect to other control RPCs in this process. Channel I/O serialization
|
||||
// is separate, so this must remain a distinct lock.
|
||||
std::lock_guard<std::mutex> sequence_lock(control_sequence_mutex_);
|
||||
const auto authority = acquireControl_();
|
||||
if (!authority.ok()) {
|
||||
@ -1597,7 +1886,45 @@ AgvResult Src1100Agv::sendControlledCommand_(
|
||||
authority.code,
|
||||
"SRC1100 acquire control authority failed: " + detail);
|
||||
}
|
||||
return sendCommand_(sock, command, payload, response);
|
||||
auto result = sendCommand_(sock, command, payload, response);
|
||||
if (!result.ok()) {
|
||||
if (accepted_navigation_generation) {
|
||||
// Once the control write has been attempted, a timeout, disconnect,
|
||||
// wrong response opcode, or malformed JSON cannot prove rejection:
|
||||
// the controller may already have executed the command.
|
||||
*accepted_navigation_generation =
|
||||
navigation_generation_.fetch_add(1, std::memory_order_relaxed) + 1;
|
||||
return withUnknownControllerOutcome(std::move(result));
|
||||
}
|
||||
return result;
|
||||
}
|
||||
if (!accepted_navigation_generation) {
|
||||
return result;
|
||||
}
|
||||
if (!response) {
|
||||
*accepted_navigation_generation =
|
||||
navigation_generation_.fetch_add(1, std::memory_order_relaxed) + 1;
|
||||
return withUnknownControllerOutcome(AgvResult::failure(
|
||||
AgvErrorCode::CommandFailed,
|
||||
"SRC1100 cannot confirm navigation command without a response"));
|
||||
}
|
||||
if (!hasNumericControllerRetCode(*response)) {
|
||||
*accepted_navigation_generation =
|
||||
navigation_generation_.fetch_add(1, std::memory_order_relaxed) + 1;
|
||||
return withUnknownControllerOutcome(resultFromResponse_(*response));
|
||||
}
|
||||
result = resultFromResponse_(*response);
|
||||
if (!result.ok()) {
|
||||
return result;
|
||||
}
|
||||
|
||||
// Advance only after the controller accepted the command, and do it before
|
||||
// releasing control_sequence_mutex_. This prevents a failed cancel/pause or
|
||||
// failed authority acquisition from falsely reporting a pose task canceled,
|
||||
// while preserving the controller's actual command order under concurrency.
|
||||
*accepted_navigation_generation =
|
||||
navigation_generation_.fetch_add(1, std::memory_order_relaxed) + 1;
|
||||
return result;
|
||||
}
|
||||
|
||||
AgvResult Src1100Agv::sendCommand_(
|
||||
@ -1634,15 +1961,14 @@ AgvResult Src1100Agv::sendCommandRaw_(
|
||||
const Json::Value& payload,
|
||||
std::string* response_payload) const
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
if (sock < 0) {
|
||||
return AgvResult::failure(AgvErrorCode::NotConnected, "SRC1100 socket not connected");
|
||||
}
|
||||
|
||||
const auto exchange = [&]() {
|
||||
const std::string payload_text = payload.empty() ? std::string{} : toJsonString_(payload);
|
||||
const auto frame = buildFrame_(command, payload_text);
|
||||
if (::send(sock, frame.data(), frame.size(), MSG_NOSIGNAL) != static_cast<ssize_t>(frame.size())) {
|
||||
return AgvResult::failure(AgvErrorCode::CommandFailed, "SRC1100 send command failed: " + systemError());
|
||||
if (::send(sock, frame.data(), frame.size(), MSG_NOSIGNAL)
|
||||
!= static_cast<ssize_t>(frame.size())) {
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::CommandFailed,
|
||||
"SRC1100 send command failed: " + systemError());
|
||||
}
|
||||
|
||||
std::uint16_t response_command = 0;
|
||||
@ -1651,11 +1977,93 @@ AgvResult Src1100Agv::sendCommandRaw_(
|
||||
if (!result.ok()) {
|
||||
return result;
|
||||
}
|
||||
(void)response_command;
|
||||
const auto expected_response_command = static_cast<std::uint16_t>(
|
||||
command + 10000U);
|
||||
if (response_command != expected_response_command) {
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::CommandFailed,
|
||||
"SRC1100 response command mismatch: expected="
|
||||
+ std::to_string(expected_response_command)
|
||||
+ ", actual=" + std::to_string(response_command));
|
||||
}
|
||||
if (response_payload) {
|
||||
*response_payload = std::move(payload_text_response);
|
||||
}
|
||||
return AgvResult::success();
|
||||
};
|
||||
const auto close_matching_socket_locked = [this, sock]() {
|
||||
if (sock == sock_status_) {
|
||||
closeSocket_(sock_status_);
|
||||
} else if (sock == sock_control_) {
|
||||
closeSocket_(sock_control_);
|
||||
} else if (sock == sock_navigation_) {
|
||||
closeSocket_(sock_navigation_);
|
||||
} else if (sock == sock_config_) {
|
||||
closeSocket_(sock_config_);
|
||||
} else if (sock == sock_other_) {
|
||||
closeSocket_(sock_other_);
|
||||
}
|
||||
};
|
||||
const auto mark_channel_desynchronized = [](AgvResult result) {
|
||||
std::string detail = result.message.empty()
|
||||
? "unknown transport or frame error"
|
||||
: result.message;
|
||||
detail +=
|
||||
"; SRC1100 channel closed because the response stream may be "
|
||||
"desynchronized; reconnect before sending another command";
|
||||
return AgvResult::failure(result.code, detail);
|
||||
};
|
||||
|
||||
bool is_status_socket = false;
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
if (sock < 0) {
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::NotConnected,
|
||||
"SRC1100 socket not connected");
|
||||
}
|
||||
is_status_socket = sock == sock_status_;
|
||||
}
|
||||
|
||||
if (is_status_socket) {
|
||||
// A slow 1110 status response must never hold the lifecycle/global I/O
|
||||
// mutex needed by cancelNavigation() or emergencyStop(). The dedicated
|
||||
// status lock still serializes requests on port 19204. connect_() and
|
||||
// disconnect_() take this lock before changing the descriptor.
|
||||
std::lock_guard<std::mutex> status_lock(status_io_mutex_);
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
if (sock < 0 || sock != sock_status_) {
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::NotConnected,
|
||||
"SRC1100 status socket is no longer connected");
|
||||
}
|
||||
}
|
||||
auto result = exchange();
|
||||
if (!result.ok()) {
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
close_matching_socket_locked();
|
||||
return mark_channel_desynchronized(std::move(result));
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
if (sock < 0
|
||||
|| (sock != sock_control_
|
||||
&& sock != sock_navigation_
|
||||
&& sock != sock_config_
|
||||
&& sock != sock_other_)) {
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::NotConnected,
|
||||
"SRC1100 socket is no longer connected");
|
||||
}
|
||||
auto result = exchange();
|
||||
if (!result.ok()) {
|
||||
close_matching_socket_locked();
|
||||
return mark_channel_desynchronized(std::move(result));
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
AgvResult Src1100Agv::sendCommandNoResponse_(
|
||||
@ -1837,6 +2245,17 @@ void Src1100Agv::updateCachedRuntimeState_(const Json::Value& payload)
|
||||
|
||||
state.moving = std::hypot(state.velocity.vx, state.velocity.vy) > 1e-4 || std::abs(state.velocity.wz) > 1e-4;
|
||||
state.fault = hasFaultArray(payload, "fatals") || hasFaultArray(payload, "errors");
|
||||
if (state.fault) {
|
||||
std::ostringstream detail;
|
||||
detail << "SRC1100 controller fault";
|
||||
if (const auto* fatals = jsonFind(payload, "fatals"); fatals && !fatals->empty()) {
|
||||
detail << ": fatals=" << jsonValueToString(*fatals);
|
||||
}
|
||||
if (const auto* errors = jsonFind(payload, "errors"); errors && !errors->empty()) {
|
||||
detail << ": errors=" << jsonValueToString(*errors);
|
||||
}
|
||||
state.last_error = detail.str();
|
||||
}
|
||||
if (state.emergency_stopped) {
|
||||
state.mode = AgvMode::EmergencyStop;
|
||||
} else if (state.fault) {
|
||||
@ -1995,12 +2414,21 @@ void Src1100Agv::applyAdapterParams_(Json::Value& payload, const AgvAdapterParam
|
||||
|
||||
AgvResult Src1100Agv::resultFromResponse_(const Json::Value& response)
|
||||
{
|
||||
const int ret_code = jsonGet(response, "ret_code", 0).asInt();
|
||||
if (!hasNumericControllerRetCode(response)) {
|
||||
return AgvResult::failure(
|
||||
AgvErrorCode::CommandFailed,
|
||||
"SRC1100 controller response is missing a numeric ret_code");
|
||||
}
|
||||
const auto* ret_code_value = jsonFind(response, "ret_code");
|
||||
const bool success = ret_code_value->isUInt() || ret_code_value->isUInt64()
|
||||
? ret_code_value->asUInt64() == 0
|
||||
: ret_code_value->asInt64() == 0;
|
||||
const std::string ret_code = jsonValueToString(*ret_code_value);
|
||||
const std::string message = jsonGet(response, "err_msg", "").asString();
|
||||
if (ret_code == 0) {
|
||||
if (success) {
|
||||
return AgvResult::success();
|
||||
}
|
||||
std::string detail = "SRC1100 command failed: ret_code=" + std::to_string(ret_code);
|
||||
std::string detail = "SRC1100 command failed: ret_code=" + ret_code;
|
||||
if (!message.empty()) {
|
||||
detail += ", err_msg=" + message;
|
||||
}
|
||||
|
||||
@ -1,8 +1,11 @@
|
||||
#include <algorithm>
|
||||
#include <array>
|
||||
#include <atomic>
|
||||
#include <cerrno>
|
||||
#include <chrono>
|
||||
#include <cstdint>
|
||||
#include <cstring>
|
||||
#include <deque>
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
@ -27,27 +30,53 @@ class Src1100AgvTestPeer {
|
||||
public:
|
||||
static void installSockets(
|
||||
Src1100Agv& agv,
|
||||
const int status,
|
||||
const int control,
|
||||
const int navigation,
|
||||
const int config,
|
||||
const int other)
|
||||
{
|
||||
agv.sock_status_ = status;
|
||||
agv.sock_control_ = control;
|
||||
agv.sock_navigation_ = navigation;
|
||||
agv.sock_config_ = config;
|
||||
agv.sock_other_ = other;
|
||||
}
|
||||
|
||||
static void cacheRuntimeState(Src1100Agv& agv, const Json::Value& payload)
|
||||
{
|
||||
agv.updateCachedRuntimeState_(payload);
|
||||
}
|
||||
|
||||
static void setNavigationReceiveTimeout(
|
||||
Src1100Agv& agv,
|
||||
const std::chrono::milliseconds timeout)
|
||||
{
|
||||
timeval value{};
|
||||
value.tv_sec = static_cast<time_t>(timeout.count() / 1000);
|
||||
value.tv_usec = static_cast<suseconds_t>(
|
||||
(timeout.count() % 1000) * 1000);
|
||||
ASSERT_EQ(
|
||||
::setsockopt(
|
||||
agv.sock_navigation_,
|
||||
SOL_SOCKET,
|
||||
SO_RCVTIMEO,
|
||||
&value,
|
||||
sizeof(value)),
|
||||
0);
|
||||
}
|
||||
};
|
||||
|
||||
namespace {
|
||||
|
||||
constexpr std::uint16_t kRobotStatusTask = 1020;
|
||||
constexpr std::uint16_t kRobotStatusTaskPackage = 1110;
|
||||
constexpr std::uint16_t kRobotControlStop = 2000;
|
||||
constexpr std::uint16_t kRobotControlMotion = 2010;
|
||||
constexpr std::uint16_t kRobotControlLoadMap = 2022;
|
||||
constexpr std::uint16_t kRobotTaskPause = 3001;
|
||||
constexpr std::uint16_t kRobotTaskResume = 3002;
|
||||
constexpr std::uint16_t kRobotTaskCancel = 3003;
|
||||
constexpr std::uint16_t kRobotTaskGoPoint = 3050;
|
||||
constexpr std::uint16_t kRobotTaskGoTarget = 3051;
|
||||
constexpr std::uint16_t kRobotTaskGoTargetList = 3066;
|
||||
constexpr std::uint16_t kRobotConfigLock = 4005;
|
||||
@ -57,7 +86,8 @@ constexpr std::uint16_t kRobotOtherStartMapping = 6100;
|
||||
constexpr std::uint16_t kRobotOtherStopMapping = 6101;
|
||||
|
||||
enum class Channel : std::size_t {
|
||||
Control = 0,
|
||||
Status = 0,
|
||||
Control,
|
||||
Navigation,
|
||||
Config,
|
||||
Other,
|
||||
@ -142,13 +172,9 @@ bool sendAll(const int fd, const std::vector<std::uint8_t>& data)
|
||||
}
|
||||
|
||||
std::vector<std::uint8_t> responseFrame(
|
||||
const std::uint16_t request_command,
|
||||
const int ret_code)
|
||||
const std::uint16_t response_command,
|
||||
const std::string& payload)
|
||||
{
|
||||
const std::string payload = ret_code == 0
|
||||
? R"({"ret_code":0,"err_msg":""})"
|
||||
: "{\"ret_code\":" + std::to_string(ret_code)
|
||||
+ R"(,"err_msg":"simulated command failure"})";
|
||||
std::vector<std::uint8_t> frame(16 + payload.size(), 0);
|
||||
frame[0] = 0x5A;
|
||||
frame[1] = 0x01;
|
||||
@ -158,13 +184,45 @@ std::vector<std::uint8_t> responseFrame(
|
||||
frame[5] = static_cast<std::uint8_t>((length >> 16U) & 0xFFU);
|
||||
frame[6] = static_cast<std::uint8_t>((length >> 8U) & 0xFFU);
|
||||
frame[7] = static_cast<std::uint8_t>(length & 0xFFU);
|
||||
const auto response_command = static_cast<std::uint16_t>(request_command + 10000U);
|
||||
frame[8] = static_cast<std::uint8_t>((response_command >> 8U) & 0xFFU);
|
||||
frame[9] = static_cast<std::uint8_t>(response_command & 0xFFU);
|
||||
std::copy(payload.begin(), payload.end(), frame.begin() + 16);
|
||||
return frame;
|
||||
}
|
||||
|
||||
std::string injectRequestedTaskId(
|
||||
std::string response_payload,
|
||||
const std::string& request_payload)
|
||||
{
|
||||
constexpr char kTaskIdToken[] = "${TASK_ID}";
|
||||
const auto token_position = response_payload.find(kTaskIdToken);
|
||||
if (token_position == std::string::npos) {
|
||||
return response_payload;
|
||||
}
|
||||
|
||||
Json::Value request;
|
||||
Json::CharReaderBuilder builder;
|
||||
std::string error;
|
||||
std::unique_ptr<Json::CharReader> reader(builder.newCharReader());
|
||||
if (!reader->parse(
|
||||
request_payload.data(),
|
||||
request_payload.data() + request_payload.size(),
|
||||
&request,
|
||||
&error)) {
|
||||
return response_payload;
|
||||
}
|
||||
const auto* task_ids = request.find("task_ids", "task_ids" + std::strlen("task_ids"));
|
||||
if (!task_ids || !task_ids->isArray() || task_ids->empty()) {
|
||||
return response_payload;
|
||||
}
|
||||
|
||||
response_payload.replace(
|
||||
token_position,
|
||||
std::strlen(kTaskIdToken),
|
||||
(*task_ids)[0].asString());
|
||||
return response_payload;
|
||||
}
|
||||
|
||||
class FakeSrc1100Controller {
|
||||
public:
|
||||
FakeSrc1100Controller()
|
||||
@ -220,6 +278,37 @@ public:
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(response_codes_mutex_);
|
||||
response_codes_[command] = ret_code;
|
||||
response_payloads_.erase(command);
|
||||
}
|
||||
|
||||
void setResponsePayload(const std::uint16_t command, std::string payload)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(response_codes_mutex_);
|
||||
response_codes_.erase(command);
|
||||
response_payloads_[command] = {std::move(payload)};
|
||||
}
|
||||
|
||||
void queueResponsePayload(const std::uint16_t command, std::string payload)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(response_codes_mutex_);
|
||||
response_codes_.erase(command);
|
||||
response_payloads_[command].push_back(std::move(payload));
|
||||
}
|
||||
|
||||
void setResponseDelay(
|
||||
const std::uint16_t command,
|
||||
const std::chrono::milliseconds delay)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(response_codes_mutex_);
|
||||
response_delays_[command] = delay;
|
||||
}
|
||||
|
||||
void setResponseCommand(
|
||||
const std::uint16_t request_command,
|
||||
const std::uint16_t response_command)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(response_codes_mutex_);
|
||||
response_commands_[request_command] = response_command;
|
||||
}
|
||||
|
||||
void clearRecords()
|
||||
@ -261,17 +350,48 @@ private:
|
||||
}
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(records_mutex_);
|
||||
records_.push_back({command, std::move(payload)});
|
||||
records_.push_back({command, payload});
|
||||
}
|
||||
int ret_code = 0;
|
||||
std::string response_payload;
|
||||
std::chrono::milliseconds response_delay{0};
|
||||
std::uint16_t response_command = static_cast<std::uint16_t>(
|
||||
command + 10000U);
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(response_codes_mutex_);
|
||||
const auto response = response_codes_.find(command);
|
||||
if (response != response_codes_.end()) {
|
||||
ret_code = response->second;
|
||||
const auto payloads = response_payloads_.find(command);
|
||||
if (payloads != response_payloads_.end() && !payloads->second.empty()) {
|
||||
response_payload = payloads->second.front();
|
||||
if (payloads->second.size() > 1U) {
|
||||
payloads->second.pop_front();
|
||||
}
|
||||
}
|
||||
if (!sendAll(fd, responseFrame(command, ret_code))) {
|
||||
const auto response_code = response_codes_.find(command);
|
||||
if (response_code != response_codes_.end()) {
|
||||
ret_code = response_code->second;
|
||||
}
|
||||
const auto delay = response_delays_.find(command);
|
||||
if (delay != response_delays_.end()) {
|
||||
response_delay = delay->second;
|
||||
}
|
||||
const auto response_command_override = response_commands_.find(command);
|
||||
if (response_command_override != response_commands_.end()) {
|
||||
response_command = response_command_override->second;
|
||||
}
|
||||
}
|
||||
if (response_delay.count() > 0) {
|
||||
std::this_thread::sleep_for(response_delay);
|
||||
}
|
||||
if (response_payload.empty()) {
|
||||
response_payload = ret_code == 0
|
||||
? R"({"ret_code":0,"err_msg":""})"
|
||||
: "{\"ret_code\":" + std::to_string(ret_code)
|
||||
+ R"(,"err_msg":"simulated command failure"})";
|
||||
}
|
||||
response_payload = injectRequestedTaskId(
|
||||
std::move(response_payload),
|
||||
payload);
|
||||
if (!sendAll(fd, responseFrame(response_command, response_payload))) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
@ -282,6 +402,9 @@ private:
|
||||
std::vector<CommandRecord> records_;
|
||||
std::mutex response_codes_mutex_;
|
||||
std::unordered_map<std::uint16_t, int> response_codes_;
|
||||
std::unordered_map<std::uint16_t, std::deque<std::string>> response_payloads_;
|
||||
std::unordered_map<std::uint16_t, std::chrono::milliseconds> response_delays_;
|
||||
std::unordered_map<std::uint16_t, std::uint16_t> response_commands_;
|
||||
};
|
||||
|
||||
class Src1100ControlAuthorityTest : public ::testing::Test {
|
||||
@ -293,13 +416,25 @@ protected:
|
||||
cfg.set_ip("invalid-ip");
|
||||
cfg.set_recv_timeout_ms(100);
|
||||
cfg.set_control_nick_name("cmvr-test");
|
||||
controller_.setResponsePayload(
|
||||
kRobotStatusTask,
|
||||
R"({"ret_code":0,"err_msg":"","task_status":2,"task_type":1,"target_point":[1.0,2.0,0.5]})");
|
||||
controller_.setResponsePayload(
|
||||
kRobotStatusTaskPackage,
|
||||
R"({"ret_code":0,"err_msg":"","task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":1}]}})");
|
||||
agv_ = std::make_unique<Src1100Agv>(cfg);
|
||||
const int status_socket = controller_.takeClient(Channel::Status);
|
||||
const int control_socket = controller_.takeClient(Channel::Control);
|
||||
const int navigation_socket = controller_.takeClient(Channel::Navigation);
|
||||
const int config_socket = controller_.takeClient(Channel::Config);
|
||||
const int other_socket = controller_.takeClient(Channel::Other);
|
||||
Src1100AgvTestPeer::installSockets(
|
||||
*agv_,
|
||||
controller_.takeClient(Channel::Control),
|
||||
controller_.takeClient(Channel::Navigation),
|
||||
controller_.takeClient(Channel::Config),
|
||||
controller_.takeClient(Channel::Other));
|
||||
status_socket,
|
||||
control_socket,
|
||||
navigation_socket,
|
||||
config_socket,
|
||||
other_socket);
|
||||
}
|
||||
|
||||
void TearDown() override
|
||||
@ -352,7 +487,7 @@ protected:
|
||||
|
||||
TEST_F(Src1100ControlAuthorityTest, EveryImplementedMutatingOperationAcquiresAuthorityFirst)
|
||||
{
|
||||
expectControlled(kRobotTaskGoPoint, [this]() {
|
||||
expectControlledSequence({kRobotTaskGoTarget, kRobotStatusTaskPackage}, [this]() {
|
||||
return agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5});
|
||||
});
|
||||
expectControlled(kRobotTaskGoTarget, [this]() {
|
||||
@ -469,7 +604,7 @@ TEST_F(Src1100ControlAuthorityTest, ReadOnlyMapDownloadDoesNotAcquireAuthority)
|
||||
EXPECT_EQ(records[0].command, kRobotConfigDownloadMap);
|
||||
}
|
||||
|
||||
TEST_F(Src1100ControlAuthorityTest, NavigateToPoseUsesGoPointWithTypedMotionLimits)
|
||||
TEST_F(Src1100ControlAuthorityTest, NavigateToPoseUsesFreeGoWithTypedMotionLimits)
|
||||
{
|
||||
AgvMotionOptions options;
|
||||
options.max_speed = 0.6;
|
||||
@ -486,28 +621,579 @@ TEST_F(Src1100ControlAuthorityTest, NavigateToPoseUsesGoPointWithTypedMotionLimi
|
||||
|
||||
ASSERT_TRUE(result.ok()) << result.message;
|
||||
const auto records = controller_.records();
|
||||
ASSERT_EQ(records.size(), 2U);
|
||||
ASSERT_EQ(records.size(), 3U);
|
||||
EXPECT_EQ(records[0].command, kRobotConfigLock);
|
||||
EXPECT_EQ(records[1].command, kRobotTaskGoPoint);
|
||||
EXPECT_EQ(records[1].command, kRobotTaskGoTarget);
|
||||
EXPECT_EQ(records[2].command, kRobotStatusTaskPackage);
|
||||
|
||||
const auto payload = parsePayload(records[1]);
|
||||
EXPECT_TRUE(payloadValue(payload, "x").isNumeric());
|
||||
EXPECT_TRUE(payloadValue(payload, "y").isNumeric());
|
||||
EXPECT_TRUE(payloadValue(payload, "angle").isNumeric());
|
||||
EXPECT_DOUBLE_EQ(payloadValue(payload, "x").asDouble(), 1.0);
|
||||
EXPECT_DOUBLE_EQ(payloadValue(payload, "y").asDouble(), 2.0);
|
||||
EXPECT_DOUBLE_EQ(payloadValue(payload, "angle").asDouble(), 0.5);
|
||||
EXPECT_EQ(payloadValue(payload, "source_id").asString(), "SELF_POSITION");
|
||||
EXPECT_EQ(payloadValue(payload, "id").asString(), "SELF_POSITION");
|
||||
EXPECT_FALSE(payloadValue(payload, "task_id").asString().empty());
|
||||
const auto& free_go = payloadValue(payload, "freeGo");
|
||||
EXPECT_TRUE(payloadValue(free_go, "x").isNumeric());
|
||||
EXPECT_TRUE(payloadValue(free_go, "y").isNumeric());
|
||||
EXPECT_TRUE(payloadValue(free_go, "theta").isNumeric());
|
||||
EXPECT_DOUBLE_EQ(payloadValue(free_go, "x").asDouble(), 1.0);
|
||||
EXPECT_DOUBLE_EQ(payloadValue(free_go, "y").asDouble(), 2.0);
|
||||
EXPECT_DOUBLE_EQ(payloadValue(free_go, "theta").asDouble(), 0.5);
|
||||
EXPECT_DOUBLE_EQ(payloadValue(payload, "max_speed").asDouble(), 0.6);
|
||||
EXPECT_DOUBLE_EQ(payloadValue(payload, "max_wspeed").asDouble(), 0.7);
|
||||
EXPECT_DOUBLE_EQ(payloadValue(payload, "max_acc").asDouble(), 0.8);
|
||||
EXPECT_DOUBLE_EQ(payloadValue(payload, "max_wacc").asDouble(), 0.9);
|
||||
EXPECT_FALSE(payloadHas(payload, "id"));
|
||||
EXPECT_FALSE(payloadHas(payload, "source_id"));
|
||||
EXPECT_DOUBLE_EQ(payloadValue(payload, "reach_dist").asDouble(), 0.1);
|
||||
EXPECT_DOUBLE_EQ(payloadValue(payload, "reach_angle").asDouble(), 0.2);
|
||||
EXPECT_FALSE(payloadHas(payload, "skill_name"));
|
||||
EXPECT_FALSE(payloadHas(payload, "freeGo"));
|
||||
EXPECT_FALSE(payloadHas(payload, "reach_dist"));
|
||||
EXPECT_FALSE(payloadHas(payload, "reach_angle"));
|
||||
EXPECT_FALSE(payloadHas(payload, "x"));
|
||||
EXPECT_FALSE(payloadHas(payload, "y"));
|
||||
EXPECT_FALSE(payloadHas(payload, "angle"));
|
||||
EXPECT_FALSE(payloadHas(payload, "jack_height"));
|
||||
|
||||
const auto status_payload = parsePayload(records[2]);
|
||||
const auto& requested_task_ids = payloadValue(status_payload, "task_ids");
|
||||
ASSERT_TRUE(requested_task_ids.isArray());
|
||||
ASSERT_EQ(requested_task_ids.size(), 1U);
|
||||
ASSERT_TRUE(requested_task_ids[0].isString());
|
||||
EXPECT_EQ(
|
||||
requested_task_ids[0].asString(),
|
||||
payloadValue(payload, "task_id").asString());
|
||||
}
|
||||
|
||||
TEST_F(Src1100ControlAuthorityTest, NavigateToPoseWhitelistsAdapterFieldsAndKeepsOriginFreeGo)
|
||||
{
|
||||
controller_.setResponsePayload(
|
||||
kRobotStatusTaskPackage,
|
||||
R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":1}]}})");
|
||||
AgvAdapterParams adapter_params;
|
||||
adapter_params.values.emplace("source_id", "SELF_POSITION");
|
||||
adapter_params.values.emplace("target_id", "SELF_POSITION");
|
||||
adapter_params.values.emplace("task_id", "pose-task");
|
||||
adapter_params.values.emplace("skill_name", "GotoSpecifiedPose");
|
||||
adapter_params.values.emplace("operation", "JackHeight");
|
||||
adapter_params.values.emplace("jack_height", "0.5");
|
||||
adapter_params.values.emplace("script_name", "unsafe-script");
|
||||
adapter_params.values.emplace("unknown_field", "unsafe-value");
|
||||
controller_.clearRecords();
|
||||
|
||||
const auto result = agv_->navigateToPose(
|
||||
math::Pose2d{0.0, 0.0, 0.5},
|
||||
{},
|
||||
adapter_params);
|
||||
|
||||
ASSERT_TRUE(result.ok()) << result.message;
|
||||
const auto records = controller_.records();
|
||||
ASSERT_EQ(records.size(), 3U);
|
||||
EXPECT_EQ(records[0].command, kRobotConfigLock);
|
||||
EXPECT_EQ(records[1].command, kRobotTaskGoTarget);
|
||||
EXPECT_EQ(records[2].command, kRobotStatusTaskPackage);
|
||||
|
||||
const auto payload = parsePayload(records[1]);
|
||||
EXPECT_EQ(payloadValue(payload, "source_id").asString(), "SELF_POSITION");
|
||||
EXPECT_EQ(payloadValue(payload, "id").asString(), "SELF_POSITION");
|
||||
EXPECT_EQ(payloadValue(payload, "task_id").asString().find("pose-task_pose_"), 0U);
|
||||
EXPECT_EQ(payloadValue(payload, "skill_name").asString(), "GotoSpecifiedPose");
|
||||
const auto& free_go = payloadValue(payload, "freeGo");
|
||||
EXPECT_DOUBLE_EQ(payloadValue(free_go, "x").asDouble(), 0.0);
|
||||
EXPECT_DOUBLE_EQ(payloadValue(free_go, "y").asDouble(), 0.0);
|
||||
EXPECT_DOUBLE_EQ(payloadValue(free_go, "theta").asDouble(), 0.5);
|
||||
EXPECT_FALSE(payloadHas(payload, "operation"));
|
||||
EXPECT_FALSE(payloadHas(payload, "jack_height"));
|
||||
EXPECT_FALSE(payloadHas(payload, "script_name"));
|
||||
EXPECT_FALSE(payloadHas(payload, "unknown_field"));
|
||||
}
|
||||
|
||||
TEST_F(Src1100ControlAuthorityTest, NavigateToPoseRejectsUnsafeFallbackStationAndSkill)
|
||||
{
|
||||
AgvAdapterParams adapter_params;
|
||||
adapter_params.values.emplace("source_id", "station-0");
|
||||
controller_.clearRecords();
|
||||
|
||||
auto result = agv_->navigateToPose(
|
||||
math::Pose2d{1.0, 2.0, 0.5},
|
||||
{},
|
||||
adapter_params);
|
||||
|
||||
EXPECT_FALSE(result.ok());
|
||||
EXPECT_EQ(result.code, AgvErrorCode::InvalidArgument);
|
||||
EXPECT_NE(result.message.find("source_id must be SELF_POSITION"), std::string::npos);
|
||||
EXPECT_TRUE(controller_.records().empty());
|
||||
|
||||
adapter_params.values.clear();
|
||||
adapter_params.values.emplace("target_id", "station-1");
|
||||
controller_.clearRecords();
|
||||
|
||||
result = agv_->navigateToPose(
|
||||
math::Pose2d{1.0, 2.0, 0.5},
|
||||
{},
|
||||
adapter_params);
|
||||
|
||||
EXPECT_FALSE(result.ok());
|
||||
EXPECT_EQ(result.code, AgvErrorCode::InvalidArgument);
|
||||
EXPECT_NE(result.message.find("target_id must be SELF_POSITION"), std::string::npos);
|
||||
EXPECT_TRUE(controller_.records().empty());
|
||||
|
||||
adapter_params.values.clear();
|
||||
adapter_params.values.emplace("skill_name", "unsafe-custom-skill");
|
||||
controller_.clearRecords();
|
||||
|
||||
result = agv_->navigateToPose(
|
||||
math::Pose2d{1.0, 2.0, 0.5},
|
||||
{},
|
||||
adapter_params);
|
||||
|
||||
EXPECT_FALSE(result.ok());
|
||||
EXPECT_EQ(result.code, AgvErrorCode::InvalidArgument);
|
||||
EXPECT_NE(result.message.find("skill_name must be GotoSpecifiedPose"), std::string::npos);
|
||||
EXPECT_TRUE(controller_.records().empty());
|
||||
}
|
||||
|
||||
TEST_F(Src1100ControlAuthorityTest, NavigateToPoseIgnoresUncorrelatedStatusUntilPoseTaskAppears)
|
||||
{
|
||||
controller_.setResponsePayload(
|
||||
kRobotStatusTaskPackage,
|
||||
R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"old-pose-task","status":2,"type":1}]}})");
|
||||
controller_.queueResponsePayload(
|
||||
kRobotStatusTaskPackage,
|
||||
R"({"ret_code":0,"task_status_package":{"task_status_list":[{"task_id":"${TASK_ID}","status":2,"type":1}]}})");
|
||||
controller_.clearRecords();
|
||||
|
||||
const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5});
|
||||
|
||||
ASSERT_TRUE(result.ok()) << result.message;
|
||||
const auto records = controller_.records();
|
||||
ASSERT_EQ(records.size(), 4U);
|
||||
EXPECT_EQ(records[0].command, kRobotConfigLock);
|
||||
EXPECT_EQ(records[1].command, kRobotTaskGoTarget);
|
||||
EXPECT_EQ(records[2].command, kRobotStatusTaskPackage);
|
||||
EXPECT_EQ(records[3].command, kRobotStatusTaskPackage);
|
||||
}
|
||||
|
||||
TEST_F(Src1100ControlAuthorityTest, NavigateToPoseReturnsAsynchronousControllerFailure)
|
||||
{
|
||||
controller_.setResponsePayload(
|
||||
kRobotStatusTaskPackage,
|
||||
R"({"ret_code":0,"task_status_package":{"info":"planner rejected pose","task_status_list":[{"task_id":"${TASK_ID}","status":5,"type":1}]}})");
|
||||
controller_.clearRecords();
|
||||
|
||||
const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5});
|
||||
|
||||
EXPECT_FALSE(result.ok());
|
||||
EXPECT_EQ(result.code, AgvErrorCode::TaskFailed);
|
||||
EXPECT_NE(result.message.find("planner rejected pose"), std::string::npos);
|
||||
EXPECT_NE(result.message.find("task_status=5"), std::string::npos);
|
||||
EXPECT_NE(result.message.find("task_type=1"), std::string::npos);
|
||||
}
|
||||
|
||||
TEST_F(Src1100ControlAuthorityTest, NavigateToPosePreservesSynchronousControllerCode)
|
||||
{
|
||||
controller_.setResponseCode(kRobotTaskGoTarget, 43051);
|
||||
controller_.clearRecords();
|
||||
|
||||
const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5});
|
||||
|
||||
EXPECT_FALSE(result.ok());
|
||||
EXPECT_EQ(result.code, AgvErrorCode::CommandFailed);
|
||||
EXPECT_NE(result.message.find("ret_code=43051"), std::string::npos);
|
||||
EXPECT_NE(result.message.find("err_msg=simulated command failure"), std::string::npos);
|
||||
const auto records = controller_.records();
|
||||
ASSERT_EQ(records.size(), 2U);
|
||||
EXPECT_EQ(records[0].command, kRobotConfigLock);
|
||||
EXPECT_EQ(records[1].command, kRobotTaskGoTarget);
|
||||
}
|
||||
|
||||
TEST_F(Src1100ControlAuthorityTest, NavigateToPosePreservesStatusQueryControllerCode)
|
||||
{
|
||||
controller_.setResponseCode(kRobotStatusTaskPackage, 41110);
|
||||
controller_.clearRecords();
|
||||
|
||||
const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5});
|
||||
|
||||
EXPECT_FALSE(result.ok());
|
||||
EXPECT_EQ(result.code, AgvErrorCode::CommandFailed);
|
||||
EXPECT_NE(result.message.find("ret_code=41110"), std::string::npos);
|
||||
EXPECT_NE(result.message.find("err_msg=simulated command failure"), std::string::npos);
|
||||
}
|
||||
|
||||
TEST_F(Src1100ControlAuthorityTest, NavigateToPoseRejectsWrongResponseCommand)
|
||||
{
|
||||
controller_.setResponseCommand(kRobotTaskGoTarget, 13052);
|
||||
controller_.clearRecords();
|
||||
|
||||
const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5});
|
||||
|
||||
EXPECT_FALSE(result.ok());
|
||||
EXPECT_EQ(result.code, AgvErrorCode::CommandFailed);
|
||||
EXPECT_NE(result.message.find("expected=13051"), std::string::npos);
|
||||
EXPECT_NE(result.message.find("actual=13052"), std::string::npos);
|
||||
EXPECT_NE(result.message.find("channel closed"), std::string::npos);
|
||||
EXPECT_NE(result.message.find("controller outcome is unknown"), std::string::npos);
|
||||
const auto records = controller_.records();
|
||||
ASSERT_EQ(records.size(), 2U);
|
||||
EXPECT_EQ(records[1].command, kRobotTaskGoTarget);
|
||||
}
|
||||
|
||||
TEST_F(Src1100ControlAuthorityTest, NavigateToPoseRejectsMissingControllerCode)
|
||||
{
|
||||
controller_.setResponsePayload(
|
||||
kRobotTaskGoTarget,
|
||||
R"({"err_msg":"missing acknowledgment code"})");
|
||||
controller_.clearRecords();
|
||||
|
||||
const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5});
|
||||
|
||||
EXPECT_FALSE(result.ok());
|
||||
EXPECT_EQ(result.code, AgvErrorCode::CommandFailed);
|
||||
EXPECT_NE(result.message.find("missing a numeric ret_code"), std::string::npos);
|
||||
EXPECT_NE(result.message.find("controller outcome is unknown"), std::string::npos);
|
||||
const auto records = controller_.records();
|
||||
ASSERT_EQ(records.size(), 2U);
|
||||
EXPECT_EQ(records[1].command, kRobotTaskGoTarget);
|
||||
}
|
||||
|
||||
TEST_F(Src1100ControlAuthorityTest, NavigateToPoseReportsPausedTaskExplicitly)
|
||||
{
|
||||
controller_.setResponsePayload(
|
||||
kRobotStatusTaskPackage,
|
||||
R"({"ret_code":0,"task_status_package":{"info":"safety pause","task_status_list":[{"task_id":"${TASK_ID}","status":3,"type":1}]}})");
|
||||
controller_.clearRecords();
|
||||
|
||||
const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5});
|
||||
|
||||
EXPECT_FALSE(result.ok());
|
||||
EXPECT_EQ(result.code, AgvErrorCode::TaskRejected);
|
||||
EXPECT_NE(result.message.find("established but is paused"), std::string::npos);
|
||||
EXPECT_NE(result.message.find("safety pause"), std::string::npos);
|
||||
EXPECT_NE(result.message.find("do not retry automatically"), std::string::npos);
|
||||
}
|
||||
|
||||
TEST_F(Src1100ControlAuthorityTest, NavigateToPoseIncludesCachedControllerFaultCodes)
|
||||
{
|
||||
Json::Value push_payload(Json::objectValue);
|
||||
Json::Value errors(Json::arrayValue);
|
||||
Json::Value error(Json::objectValue);
|
||||
*error.demand("code", "code" + std::strlen("code")) = "E_NAV_42";
|
||||
*error.demand("message", "message" + std::strlen("message")) = "planner alarm";
|
||||
errors.append(error);
|
||||
*push_payload.demand("errors", "errors" + std::strlen("errors")) = errors;
|
||||
Src1100AgvTestPeer::cacheRuntimeState(*agv_, push_payload);
|
||||
controller_.setResponsePayload(
|
||||
kRobotStatusTaskPackage,
|
||||
R"({"ret_code":0,"task_status_package":{"info":"navigation failed","task_status_list":[{"task_id":"${TASK_ID}","status":5,"type":1}]}})");
|
||||
controller_.clearRecords();
|
||||
|
||||
const auto result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5});
|
||||
|
||||
EXPECT_FALSE(result.ok());
|
||||
EXPECT_EQ(result.code, AgvErrorCode::TaskFailed);
|
||||
EXPECT_NE(result.message.find("E_NAV_42"), std::string::npos);
|
||||
EXPECT_NE(result.message.find("planner alarm"), std::string::npos);
|
||||
}
|
||||
|
||||
TEST_F(Src1100ControlAuthorityTest, CancelSupersedesPoseStartConfirmation)
|
||||
{
|
||||
controller_.setResponsePayload(
|
||||
kRobotStatusTaskPackage,
|
||||
R"({"ret_code":0,"task_status_package":{"task_status_list":[]}})");
|
||||
controller_.clearRecords();
|
||||
AgvResult pose_result = AgvResult::success();
|
||||
|
||||
std::thread pose_thread([this, &pose_result]() {
|
||||
pose_result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5});
|
||||
});
|
||||
|
||||
bool status_query_observed = false;
|
||||
for (int attempt = 0; attempt < 200; ++attempt) {
|
||||
const auto records = controller_.records();
|
||||
status_query_observed = std::any_of(
|
||||
records.begin(),
|
||||
records.end(),
|
||||
[](const CommandRecord& record) {
|
||||
return record.command == kRobotStatusTaskPackage;
|
||||
});
|
||||
if (status_query_observed) {
|
||||
break;
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
||||
}
|
||||
|
||||
const auto cancel_result = agv_->cancelNavigation();
|
||||
pose_thread.join();
|
||||
|
||||
EXPECT_TRUE(status_query_observed);
|
||||
ASSERT_TRUE(cancel_result.ok()) << cancel_result.message;
|
||||
EXPECT_FALSE(pose_result.ok());
|
||||
EXPECT_EQ(pose_result.code, AgvErrorCode::CommandFailed);
|
||||
EXPECT_NE(pose_result.message.find("superseded"), std::string::npos);
|
||||
EXPECT_NE(pose_result.message.find("state is unknown"), std::string::npos);
|
||||
}
|
||||
|
||||
TEST_F(Src1100ControlAuthorityTest, FailedCancelDoesNotSupersedePoseStartConfirmation)
|
||||
{
|
||||
controller_.setResponsePayload(
|
||||
kRobotStatusTaskPackage,
|
||||
R"({"ret_code":0,"task_status_package":{"task_status_list":[]}})");
|
||||
controller_.clearRecords();
|
||||
AgvResult pose_result = AgvResult::success();
|
||||
std::atomic_bool pose_finished{false};
|
||||
|
||||
std::thread pose_thread([this, &pose_result, &pose_finished]() {
|
||||
pose_result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5});
|
||||
pose_finished.store(true, std::memory_order_release);
|
||||
});
|
||||
|
||||
bool status_query_observed = false;
|
||||
for (int attempt = 0; attempt < 200; ++attempt) {
|
||||
const auto records = controller_.records();
|
||||
status_query_observed = std::any_of(
|
||||
records.begin(),
|
||||
records.end(),
|
||||
[](const CommandRecord& record) {
|
||||
return record.command == kRobotStatusTaskPackage;
|
||||
});
|
||||
if (status_query_observed) {
|
||||
break;
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
||||
}
|
||||
ASSERT_TRUE(status_query_observed);
|
||||
|
||||
controller_.setResponseCode(kRobotConfigLock, 17);
|
||||
const auto authority_failure = agv_->cancelNavigation();
|
||||
EXPECT_FALSE(authority_failure.ok());
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(75));
|
||||
EXPECT_FALSE(pose_finished.load(std::memory_order_acquire));
|
||||
|
||||
controller_.setResponseCode(kRobotConfigLock, 0);
|
||||
controller_.setResponseCode(kRobotTaskCancel, 23);
|
||||
const auto command_failure = agv_->cancelNavigation();
|
||||
EXPECT_FALSE(command_failure.ok());
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(75));
|
||||
EXPECT_FALSE(pose_finished.load(std::memory_order_acquire));
|
||||
|
||||
controller_.setResponseCode(kRobotTaskCancel, 0);
|
||||
const auto successful_cancel = agv_->cancelNavigation();
|
||||
pose_thread.join();
|
||||
|
||||
ASSERT_TRUE(successful_cancel.ok()) << successful_cancel.message;
|
||||
EXPECT_TRUE(pose_finished.load(std::memory_order_acquire));
|
||||
EXPECT_FALSE(pose_result.ok());
|
||||
EXPECT_EQ(pose_result.code, AgvErrorCode::CommandFailed);
|
||||
EXPECT_NE(pose_result.message.find("superseded"), std::string::npos);
|
||||
EXPECT_NE(pose_result.message.find("state is unknown"), std::string::npos);
|
||||
}
|
||||
|
||||
TEST_F(Src1100ControlAuthorityTest, IndeterminateCancelSupersedesPoseStartConfirmation)
|
||||
{
|
||||
controller_.setResponsePayload(
|
||||
kRobotStatusTaskPackage,
|
||||
R"({"ret_code":0,"task_status_package":{"task_status_list":[]}})");
|
||||
controller_.clearRecords();
|
||||
AgvResult pose_result = AgvResult::success();
|
||||
|
||||
std::thread pose_thread([this, &pose_result]() {
|
||||
pose_result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5});
|
||||
});
|
||||
|
||||
bool status_query_observed = false;
|
||||
for (int attempt = 0; attempt < 200; ++attempt) {
|
||||
const auto records = controller_.records();
|
||||
status_query_observed = std::any_of(
|
||||
records.begin(),
|
||||
records.end(),
|
||||
[](const CommandRecord& record) {
|
||||
return record.command == kRobotStatusTaskPackage;
|
||||
});
|
||||
if (status_query_observed) {
|
||||
break;
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
||||
}
|
||||
ASSERT_TRUE(status_query_observed);
|
||||
|
||||
controller_.setResponsePayload(
|
||||
kRobotTaskCancel,
|
||||
R"({"err_msg":"acknowledgment lost"})");
|
||||
const auto cancel_result = agv_->cancelNavigation();
|
||||
pose_thread.join();
|
||||
|
||||
EXPECT_FALSE(cancel_result.ok());
|
||||
EXPECT_NE(cancel_result.message.find("missing a numeric ret_code"), std::string::npos);
|
||||
EXPECT_NE(cancel_result.message.find("controller outcome is unknown"), std::string::npos);
|
||||
EXPECT_NE(
|
||||
cancel_result.message.find("do not issue another motion command automatically"),
|
||||
std::string::npos);
|
||||
EXPECT_FALSE(pose_result.ok());
|
||||
EXPECT_EQ(pose_result.code, AgvErrorCode::CommandFailed);
|
||||
EXPECT_NE(pose_result.message.find("state is unknown"), std::string::npos);
|
||||
}
|
||||
|
||||
TEST_F(Src1100ControlAuthorityTest, TimedOutChannelIsClosedBeforeSameCommandCanRetry)
|
||||
{
|
||||
Src1100AgvTestPeer::setNavigationReceiveTimeout(
|
||||
*agv_,
|
||||
std::chrono::milliseconds(50));
|
||||
controller_.setResponseDelay(
|
||||
kRobotTaskCancel,
|
||||
std::chrono::milliseconds(200));
|
||||
controller_.clearRecords();
|
||||
|
||||
const auto first_result = agv_->cancelNavigation();
|
||||
const auto second_result = agv_->cancelNavigation();
|
||||
|
||||
EXPECT_FALSE(first_result.ok());
|
||||
EXPECT_EQ(first_result.code, AgvErrorCode::Timeout);
|
||||
EXPECT_NE(first_result.message.find("channel closed"), std::string::npos);
|
||||
EXPECT_NE(
|
||||
first_result.message.find("controller outcome is unknown"),
|
||||
std::string::npos);
|
||||
EXPECT_FALSE(second_result.ok());
|
||||
EXPECT_EQ(second_result.code, AgvErrorCode::NotConnected);
|
||||
EXPECT_NE(
|
||||
second_result.message.find("controller outcome is unknown"),
|
||||
std::string::npos);
|
||||
|
||||
const auto records = controller_.records();
|
||||
const auto cancel_count = std::count_if(
|
||||
records.begin(),
|
||||
records.end(),
|
||||
[](const CommandRecord& record) {
|
||||
return record.command == kRobotTaskCancel;
|
||||
});
|
||||
EXPECT_EQ(cancel_count, 1);
|
||||
}
|
||||
|
||||
TEST_F(Src1100ControlAuthorityTest, SlowTaskStatusDoesNotBlockEmergencyStop)
|
||||
{
|
||||
controller_.setResponsePayload(
|
||||
kRobotStatusTaskPackage,
|
||||
R"({"ret_code":0,"task_status_package":{"task_status_list":[]}})");
|
||||
controller_.setResponseDelay(
|
||||
kRobotStatusTaskPackage,
|
||||
std::chrono::milliseconds(300));
|
||||
controller_.clearRecords();
|
||||
AgvResult pose_result = AgvResult::success();
|
||||
|
||||
std::thread pose_thread([this, &pose_result]() {
|
||||
pose_result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5});
|
||||
});
|
||||
|
||||
bool status_query_observed = false;
|
||||
for (int attempt = 0; attempt < 200; ++attempt) {
|
||||
const auto records = controller_.records();
|
||||
status_query_observed = std::any_of(
|
||||
records.begin(),
|
||||
records.end(),
|
||||
[](const CommandRecord& record) {
|
||||
return record.command == kRobotStatusTaskPackage;
|
||||
});
|
||||
if (status_query_observed) {
|
||||
break;
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
||||
}
|
||||
ASSERT_TRUE(status_query_observed);
|
||||
|
||||
const auto start = std::chrono::steady_clock::now();
|
||||
const auto stop_result = agv_->emergencyStop();
|
||||
const auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(
|
||||
std::chrono::steady_clock::now() - start);
|
||||
pose_thread.join();
|
||||
|
||||
ASSERT_TRUE(stop_result.ok()) << stop_result.message;
|
||||
EXPECT_LT(elapsed.count(), 150);
|
||||
EXPECT_FALSE(pose_result.ok());
|
||||
EXPECT_EQ(pose_result.code, AgvErrorCode::CommandFailed);
|
||||
EXPECT_NE(pose_result.message.find("state is unknown"), std::string::npos);
|
||||
|
||||
const auto records = controller_.records();
|
||||
const auto control_stop = std::find_if(
|
||||
records.begin(),
|
||||
records.end(),
|
||||
[](const CommandRecord& record) {
|
||||
return record.command == kRobotControlStop;
|
||||
});
|
||||
const auto navigation_cancel = std::find_if(
|
||||
records.begin(),
|
||||
records.end(),
|
||||
[](const CommandRecord& record) {
|
||||
return record.command == kRobotTaskCancel;
|
||||
});
|
||||
ASSERT_NE(control_stop, records.end());
|
||||
ASSERT_NE(navigation_cancel, records.end());
|
||||
EXPECT_LT(control_stop, navigation_cancel);
|
||||
}
|
||||
|
||||
TEST_F(Src1100ControlAuthorityTest, EmergencyStopInvalidatesPoseAfterFirstAcceptedStop)
|
||||
{
|
||||
controller_.setResponseDelay(
|
||||
kRobotStatusTaskPackage,
|
||||
std::chrono::milliseconds(200));
|
||||
controller_.setResponseDelay(
|
||||
kRobotTaskCancel,
|
||||
std::chrono::milliseconds(400));
|
||||
controller_.clearRecords();
|
||||
AgvResult pose_result = AgvResult::success();
|
||||
AgvResult stop_result = AgvResult::success();
|
||||
std::atomic_bool pose_finished{false};
|
||||
std::atomic_bool stop_finished{false};
|
||||
|
||||
std::thread pose_thread([this, &pose_result, &pose_finished]() {
|
||||
pose_result = agv_->navigateToPose(math::Pose2d{1.0, 2.0, 0.5});
|
||||
pose_finished.store(true, std::memory_order_release);
|
||||
});
|
||||
|
||||
bool status_query_observed = false;
|
||||
for (int attempt = 0; attempt < 200; ++attempt) {
|
||||
const auto records = controller_.records();
|
||||
status_query_observed = std::any_of(
|
||||
records.begin(),
|
||||
records.end(),
|
||||
[](const CommandRecord& record) {
|
||||
return record.command == kRobotStatusTaskPackage;
|
||||
});
|
||||
if (status_query_observed) {
|
||||
break;
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
||||
}
|
||||
ASSERT_TRUE(status_query_observed);
|
||||
|
||||
std::thread stop_thread([this, &stop_result, &stop_finished]() {
|
||||
stop_result = agv_->emergencyStop();
|
||||
stop_finished.store(true, std::memory_order_release);
|
||||
});
|
||||
|
||||
bool control_stop_observed = false;
|
||||
for (int attempt = 0; attempt < 200; ++attempt) {
|
||||
const auto records = controller_.records();
|
||||
control_stop_observed = std::any_of(
|
||||
records.begin(),
|
||||
records.end(),
|
||||
[](const CommandRecord& record) {
|
||||
return record.command == kRobotControlStop;
|
||||
});
|
||||
if (control_stop_observed) {
|
||||
break;
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
||||
}
|
||||
ASSERT_TRUE(control_stop_observed);
|
||||
|
||||
for (int attempt = 0; attempt < 350; ++attempt) {
|
||||
if (pose_finished.load(std::memory_order_acquire)) {
|
||||
break;
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
||||
}
|
||||
ASSERT_TRUE(pose_finished.load(std::memory_order_acquire));
|
||||
EXPECT_FALSE(stop_finished.load(std::memory_order_acquire));
|
||||
EXPECT_FALSE(pose_result.ok());
|
||||
EXPECT_EQ(pose_result.code, AgvErrorCode::CommandFailed);
|
||||
EXPECT_NE(pose_result.message.find("state is unknown"), std::string::npos);
|
||||
|
||||
stop_thread.join();
|
||||
pose_thread.join();
|
||||
ASSERT_TRUE(stop_result.ok()) << stop_result.message;
|
||||
}
|
||||
|
||||
TEST_F(Src1100ControlAuthorityTest, NavigateToStationForwardsTypedMotionOptions)
|
||||
@ -604,5 +1290,17 @@ TEST_F(Src1100ControlAuthorityTest, ControllerErrorCodeIsPreservedInResultMessag
|
||||
EXPECT_NE(result.message.find("err_msg=simulated command failure"), std::string::npos);
|
||||
}
|
||||
|
||||
TEST_F(Src1100ControlAuthorityTest, NavigationStatusPreservesControllerErrorCode)
|
||||
{
|
||||
controller_.setResponseCode(kRobotStatusTask, 51020);
|
||||
controller_.clearRecords();
|
||||
|
||||
const auto status = agv_->navigationStatus();
|
||||
|
||||
EXPECT_EQ(status.state, AgvTaskState::Failed);
|
||||
EXPECT_NE(status.message.find("ret_code=51020"), std::string::npos);
|
||||
EXPECT_NE(status.message.find("err_msg=simulated command failure"), std::string::npos);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace cmvr::device
|
||||
|
||||
@ -14,6 +14,8 @@ namespace {
|
||||
|
||||
constexpr char kNativeErrorMessage[] =
|
||||
"SRC1100 command failed: ret_code=41200, err_msg=speed_illegal";
|
||||
constexpr char kNativeNavigationErrorMessage[] =
|
||||
"SRC1100 command failed: ret_code=43051, err_msg=planner_rejected_pose";
|
||||
|
||||
class FakeAgv final : public device::AbstractAGV {
|
||||
public:
|
||||
@ -31,7 +33,7 @@ public:
|
||||
{
|
||||
pose_ = pose;
|
||||
pose_options_ = options;
|
||||
return device::AgvResult::success();
|
||||
return pose_result_;
|
||||
}
|
||||
|
||||
device::AgvResult navigateToStation(
|
||||
@ -53,6 +55,7 @@ public:
|
||||
|
||||
math::Pose2d pose_;
|
||||
device::AgvMotionOptions pose_options_;
|
||||
device::AgvResult pose_result_{device::AgvResult::success()};
|
||||
std::string station_id_;
|
||||
device::AgvMotionOptions station_options_;
|
||||
};
|
||||
@ -159,5 +162,30 @@ TEST_F(GrpcAgvServiceTest, NativeControllerCodeIsReturnedInGrpcMessage)
|
||||
EXPECT_EQ(response.header().error_message(), kNativeErrorMessage);
|
||||
}
|
||||
|
||||
TEST_F(GrpcAgvServiceTest, NativeNavigationCodeIsReturnedInGrpcMessage)
|
||||
{
|
||||
agv_->pose_result_ = device::AgvResult::failure(
|
||||
device::AgvErrorCode::CommandFailed,
|
||||
kNativeNavigationErrorMessage);
|
||||
api::AgvNavigateToPoseCommand_Request request;
|
||||
request.mutable_header()->set_device_id("test-agv");
|
||||
request.mutable_pose()->set_x(1.0);
|
||||
request.mutable_pose()->set_y(2.0);
|
||||
api::AgvNavigateToPoseCommand_Feedback response;
|
||||
grpc::ServerContext context;
|
||||
|
||||
const auto status = service_->navigateToPose(
|
||||
&context,
|
||||
&request,
|
||||
&response);
|
||||
|
||||
EXPECT_EQ(status.error_code(), grpc::StatusCode::INTERNAL);
|
||||
EXPECT_EQ(status.error_message(), kNativeNavigationErrorMessage);
|
||||
EXPECT_FALSE(response.header().success());
|
||||
EXPECT_EQ(
|
||||
response.header().error_message(),
|
||||
kNativeNavigationErrorMessage);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
} // namespace cmvr::service
|
||||
|
||||
Loading…
Reference in New Issue
Block a user