576 lines
20 KiB
C++
576 lines
20 KiB
C++
#include "service/grpc/action/include/action_queue.h"
|
|
|
|
#include <algorithm>
|
|
#include <atomic>
|
|
#include <chrono>
|
|
#include <cmath>
|
|
#include <cstddef>
|
|
#include <cstdint>
|
|
#include <deque>
|
|
#include <functional>
|
|
#include <memory>
|
|
#include <mutex>
|
|
#include <string>
|
|
#include <unordered_map>
|
|
#include <utility>
|
|
#include <vector>
|
|
|
|
#include <google/protobuf/util/time_util.h>
|
|
|
|
#include "common/base/logging/logger.h"
|
|
#include "devices/arm/robot_arm.h"
|
|
#include "service/grpc/action/include/control_command_arbiter.h"
|
|
|
|
namespace cmvr::service {
|
|
|
|
namespace {
|
|
|
|
constexpr std::size_t kMaximumStepCount = 100;
|
|
constexpr auto kDefaultTotalTimeout = std::chrono::minutes(5);
|
|
constexpr auto kMaximumTimeout = std::chrono::minutes(30);
|
|
constexpr auto kMaximumTimeoutMs =
|
|
std::chrono::duration_cast<std::chrono::milliseconds>(kMaximumTimeout)
|
|
.count();
|
|
|
|
bool finiteNonNegative(const double value)
|
|
{
|
|
return std::isfinite(value) && value >= 0.0;
|
|
}
|
|
|
|
device::JointPositionCommand toJointPosition(
|
|
const api::JointPositionCommand& source)
|
|
{
|
|
device::JointPositionCommand target;
|
|
target.position.assign(source.position().begin(), source.position().end());
|
|
return target;
|
|
}
|
|
|
|
device::MotionOptions toMotionOptions(const api::MotionOptions& source)
|
|
{
|
|
device::MotionOptions options;
|
|
options.velocity = source.velocity();
|
|
options.acceleration = source.acceleration();
|
|
options.blend_radius = source.blend_radius();
|
|
options.jerk = source.jerk() > 0.0 ? source.jerk() : 5.0;
|
|
options.joint_velocity_limits.assign(
|
|
source.joint_velocity_limits().begin(),
|
|
source.joint_velocity_limits().end());
|
|
options.asynchronous = source.asynchronous();
|
|
return options;
|
|
}
|
|
|
|
device::CartesianPose toCartesianPose(const api::CartesianPose& source)
|
|
{
|
|
return {source.x(), source.y(), source.z(),
|
|
source.rx(), source.ry(), source.rz()};
|
|
}
|
|
|
|
device::FrameType toFrameType(const api::ArmFrameType frame)
|
|
{
|
|
switch (frame) {
|
|
case api::ARM_FRAME_TOOL:
|
|
return device::FrameType::Tool;
|
|
case api::ARM_FRAME_WORLD:
|
|
return device::FrameType::World;
|
|
case api::ARM_FRAME_USER:
|
|
return device::FrameType::User;
|
|
case api::ARM_FRAME_BASE:
|
|
default:
|
|
return device::FrameType::Base;
|
|
}
|
|
}
|
|
|
|
bool validMotionOptions(const api::MotionOptions& options, std::string& error)
|
|
{
|
|
if (options.asynchronous()) {
|
|
error = "ActionQueue requires synchronous RobotArm motion";
|
|
return false;
|
|
}
|
|
if (!finiteNonNegative(options.velocity()) ||
|
|
!finiteNonNegative(options.acceleration()) ||
|
|
!finiteNonNegative(options.blend_radius()) ||
|
|
!finiteNonNegative(options.jerk())) {
|
|
error = "motion options must be finite and non-negative";
|
|
return false;
|
|
}
|
|
for (const double limit : options.joint_velocity_limits()) {
|
|
if (!finiteNonNegative(limit)) {
|
|
error = "joint velocity limits must be finite and non-negative";
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
void finishFeedback(api::ActionQueueCommand_Feedback& feedback,
|
|
const api::ActionResultCode result,
|
|
const std::uint32_t completed_steps,
|
|
const std::string& error,
|
|
const int failed_step_index = -1)
|
|
{
|
|
feedback.Clear();
|
|
feedback.set_result(result);
|
|
feedback.set_completed_steps(completed_steps);
|
|
if (failed_step_index >= 0) {
|
|
feedback.set_failed_step_index(
|
|
static_cast<std::uint32_t>(failed_step_index));
|
|
}
|
|
auto* header = feedback.mutable_header();
|
|
header->set_success(result == api::ACTION_RESULT_CODE_COMPLETED);
|
|
header->set_error_message(error);
|
|
*header->mutable_timestamp() =
|
|
google::protobuf::util::TimeUtil::GetCurrentTime();
|
|
}
|
|
|
|
} // namespace
|
|
|
|
struct ActionQueue::Impl {
|
|
struct PreparedStep {
|
|
api::ActionStep source;
|
|
std::shared_ptr<device::RobotArm> arm;
|
|
std::size_t source_index{0};
|
|
};
|
|
|
|
struct CommandHandler {
|
|
std::function<bool(const api::ActionStep&,
|
|
std::size_t,
|
|
PreparedStep&,
|
|
std::string&)> prepare;
|
|
std::function<device::Result(
|
|
const PreparedStep&,
|
|
const std::function<bool()>&)> execute;
|
|
};
|
|
|
|
explicit Impl(ActionQueue::ArmResolver resolver)
|
|
: arm_resolver(std::move(resolver))
|
|
{
|
|
registerCommands();
|
|
}
|
|
|
|
void registerCommands()
|
|
{
|
|
handlers.emplace(
|
|
api::ActionStep::kArmMoveJ,
|
|
CommandHandler{
|
|
[this](const api::ActionStep& step,
|
|
const std::size_t index,
|
|
PreparedStep& prepared,
|
|
std::string& error) {
|
|
const auto& command = step.arm_move_j();
|
|
return prepareArmStep(
|
|
step, command.header().device_id(), index,
|
|
command.options(), prepared, error,
|
|
[&command, &error](const device::RobotArm& arm) {
|
|
if (command.target().position_size() !=
|
|
static_cast<int>(arm.getDof())) {
|
|
error = "MoveJ target size does not match arm DOF";
|
|
return false;
|
|
}
|
|
for (const double value : command.target().position()) {
|
|
if (!std::isfinite(value)) {
|
|
error = "MoveJ target must contain finite values";
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
});
|
|
},
|
|
[](const PreparedStep& prepared,
|
|
const std::function<bool()>& canceled) {
|
|
const auto& command = prepared.source.arm_move_j();
|
|
auto options = toMotionOptions(command.options());
|
|
options.cancellation_requested = canceled;
|
|
return prepared.arm->moveJ(
|
|
toJointPosition(command.target()), options);
|
|
}});
|
|
|
|
handlers.emplace(
|
|
api::ActionStep::kArmMoveL,
|
|
CommandHandler{
|
|
[this](const api::ActionStep& step,
|
|
const std::size_t index,
|
|
PreparedStep& prepared,
|
|
std::string& error) {
|
|
const auto& command = step.arm_move_l();
|
|
return prepareArmStep(
|
|
step, command.header().device_id(), index,
|
|
command.options(), prepared, error,
|
|
[&command, &error](const device::RobotArm&) {
|
|
const auto& pose = command.target();
|
|
if (!std::isfinite(pose.x()) ||
|
|
!std::isfinite(pose.y()) ||
|
|
!std::isfinite(pose.z()) ||
|
|
!std::isfinite(pose.rx()) ||
|
|
!std::isfinite(pose.ry()) ||
|
|
!std::isfinite(pose.rz())) {
|
|
error = "MoveL target must contain finite values";
|
|
return false;
|
|
}
|
|
return true;
|
|
});
|
|
},
|
|
[](const PreparedStep& prepared,
|
|
const std::function<bool()>& canceled) {
|
|
const auto& command = prepared.source.arm_move_l();
|
|
auto options = toMotionOptions(command.options());
|
|
options.cancellation_requested = canceled;
|
|
const auto target = toCartesianPose(command.target());
|
|
const bool named_frame =
|
|
(command.has_base_frame() &&
|
|
!command.base_frame().empty()) ||
|
|
(command.has_tcp_frame() &&
|
|
!command.tcp_frame().empty());
|
|
if (named_frame) {
|
|
return prepared.arm->moveL(
|
|
target,
|
|
options,
|
|
command.has_base_frame()
|
|
? command.base_frame()
|
|
: std::string{},
|
|
command.has_tcp_frame()
|
|
? command.tcp_frame()
|
|
: std::string{});
|
|
}
|
|
return prepared.arm->moveL(
|
|
target, options, toFrameType(command.frame()));
|
|
}});
|
|
}
|
|
|
|
template <typename ExtraValidator>
|
|
bool prepareArmStep(const api::ActionStep& step,
|
|
const std::string& device_id,
|
|
const std::size_t index,
|
|
const api::MotionOptions& options,
|
|
PreparedStep& prepared,
|
|
std::string& error,
|
|
ExtraValidator&& validate)
|
|
{
|
|
if (device_id.empty()) {
|
|
error = "RobotArm device_id is required";
|
|
return false;
|
|
}
|
|
auto arm = arm_resolver(device_id);
|
|
if (!arm) {
|
|
error = "RobotArm device not found: " + device_id;
|
|
return false;
|
|
}
|
|
if (!arm->supportsActionQueueMotion()) {
|
|
error = "RobotArm backend does not support ActionQueue motion: " +
|
|
device_id;
|
|
return false;
|
|
}
|
|
if (!validMotionOptions(options, error) || !validate(*arm)) {
|
|
return false;
|
|
}
|
|
prepared.source = step;
|
|
prepared.arm = std::move(arm);
|
|
prepared.source_index = index;
|
|
return true;
|
|
}
|
|
|
|
bool prepare(const api::ActionQueueCommand_Request& request,
|
|
std::vector<PreparedStep>& prepared,
|
|
std::string& error,
|
|
int& failed_index)
|
|
{
|
|
if (request.steps_size() == 0) {
|
|
error = "ActionQueue requires at least one step";
|
|
return false;
|
|
}
|
|
if (request.steps_size() > static_cast<int>(kMaximumStepCount)) {
|
|
error = "ActionQueue step count exceeds the server limit";
|
|
return false;
|
|
}
|
|
if (request.total_timeout_ms() >
|
|
static_cast<std::uint64_t>(kMaximumTimeoutMs)) {
|
|
error = "ActionQueue total timeout exceeds the server limit";
|
|
return false;
|
|
}
|
|
|
|
prepared.reserve(static_cast<std::size_t>(request.steps_size()));
|
|
for (int index = 0; index < request.steps_size(); ++index) {
|
|
const auto& step = request.steps(index);
|
|
if (step.timeout_ms() >
|
|
static_cast<std::uint64_t>(kMaximumTimeoutMs)) {
|
|
error = "step timeout exceeds the server limit";
|
|
failed_index = index;
|
|
return false;
|
|
}
|
|
const auto handler = handlers.find(step.command_case());
|
|
if (handler == handlers.end()) {
|
|
error = "ActionQueue command type is not supported";
|
|
failed_index = index;
|
|
return false;
|
|
}
|
|
PreparedStep item;
|
|
if (!handler->second.prepare(
|
|
step, static_cast<std::size_t>(index), item, error)) {
|
|
failed_index = index;
|
|
return false;
|
|
}
|
|
prepared.push_back(std::move(item));
|
|
}
|
|
return true;
|
|
}
|
|
|
|
void setState(const State next)
|
|
{
|
|
std::lock_guard lock(mutex);
|
|
state = next;
|
|
}
|
|
|
|
ActionQueue::ArmResolver arm_resolver;
|
|
std::unordered_map<int, CommandHandler> handlers;
|
|
|
|
mutable std::mutex mutex;
|
|
State state{State::Idle};
|
|
std::deque<PreparedStep> pending;
|
|
std::shared_ptr<device::RobotArm> active_arm;
|
|
std::atomic<bool> stop_requested{false};
|
|
std::atomic<bool> shutting_down{false};
|
|
};
|
|
|
|
ActionQueue::ActionQueue(ArmResolver arm_resolver)
|
|
: impl_(std::make_unique<Impl>(std::move(arm_resolver)))
|
|
{
|
|
}
|
|
|
|
ActionQueue::~ActionQueue()
|
|
{
|
|
shutdown();
|
|
}
|
|
|
|
void ActionQueue::execute(
|
|
const api::ActionQueueCommand_Request& request,
|
|
api::ActionQueueCommand_Feedback& feedback)
|
|
{
|
|
if (impl_->shutting_down.load()) {
|
|
finishFeedback(feedback, api::ACTION_RESULT_CODE_REJECTED, 0,
|
|
"ActionQueue is shutting down");
|
|
return;
|
|
}
|
|
|
|
auto action_lease =
|
|
ControlCommandArbiter::instance().tryAcquireActionQueue();
|
|
if (!action_lease) {
|
|
finishFeedback(
|
|
feedback, api::ACTION_RESULT_CODE_REJECTED, 0,
|
|
"ActionQueue cannot start while another control command is active");
|
|
return;
|
|
}
|
|
|
|
{
|
|
std::lock_guard lock(impl_->mutex);
|
|
impl_->stop_requested.store(false);
|
|
impl_->state = State::Validating;
|
|
}
|
|
std::vector<Impl::PreparedStep> prepared;
|
|
std::string error;
|
|
int failed_index = -1;
|
|
if (!impl_->prepare(request, prepared, error, failed_index)) {
|
|
if (impl_->stop_requested.load() || impl_->shutting_down.load()) {
|
|
impl_->setState(State::Canceled);
|
|
finishFeedback(feedback, api::ACTION_RESULT_CODE_CANCELED, 0,
|
|
"ActionQueue was stopped");
|
|
impl_->setState(impl_->shutting_down.load()
|
|
? State::ShuttingDown
|
|
: State::Idle);
|
|
return;
|
|
}
|
|
impl_->setState(State::Rejected);
|
|
finishFeedback(feedback, api::ACTION_RESULT_CODE_REJECTED, 0,
|
|
error, failed_index);
|
|
impl_->setState(State::Idle);
|
|
return;
|
|
}
|
|
|
|
{
|
|
std::lock_guard lock(impl_->mutex);
|
|
if (impl_->stop_requested.load() || impl_->shutting_down.load()) {
|
|
impl_->state = impl_->shutting_down.load()
|
|
? State::ShuttingDown
|
|
: State::Idle;
|
|
finishFeedback(feedback, api::ACTION_RESULT_CODE_CANCELED, 0,
|
|
"ActionQueue was stopped");
|
|
return;
|
|
}
|
|
impl_->pending.assign(
|
|
std::make_move_iterator(prepared.begin()),
|
|
std::make_move_iterator(prepared.end()));
|
|
impl_->active_arm.reset();
|
|
impl_->state = State::Running;
|
|
}
|
|
|
|
const auto total_timeout = request.total_timeout_ms() == 0
|
|
? kDefaultTotalTimeout
|
|
: std::chrono::milliseconds(request.total_timeout_ms());
|
|
const auto total_deadline = std::chrono::steady_clock::now() +
|
|
total_timeout;
|
|
std::uint32_t completed_steps = 0;
|
|
|
|
while (true) {
|
|
Impl::PreparedStep step;
|
|
{
|
|
std::lock_guard lock(impl_->mutex);
|
|
if (impl_->stop_requested.load() || impl_->shutting_down.load()) {
|
|
impl_->state = State::Canceled;
|
|
finishFeedback(feedback, api::ACTION_RESULT_CODE_CANCELED,
|
|
completed_steps, "ActionQueue was stopped");
|
|
impl_->active_arm.reset();
|
|
impl_->state = impl_->shutting_down.load()
|
|
? State::ShuttingDown
|
|
: State::Idle;
|
|
return;
|
|
}
|
|
if (impl_->pending.empty()) {
|
|
impl_->state = State::Completed;
|
|
finishFeedback(feedback, api::ACTION_RESULT_CODE_COMPLETED,
|
|
completed_steps, {});
|
|
impl_->active_arm.reset();
|
|
impl_->state = State::Idle;
|
|
return;
|
|
}
|
|
if (std::chrono::steady_clock::now() >= total_deadline) {
|
|
impl_->pending.clear();
|
|
impl_->state = State::TimedOut;
|
|
finishFeedback(feedback, api::ACTION_RESULT_CODE_TIMED_OUT,
|
|
completed_steps,
|
|
"ActionQueue total timeout expired");
|
|
impl_->state = State::Idle;
|
|
return;
|
|
}
|
|
step = std::move(impl_->pending.front());
|
|
impl_->pending.pop_front();
|
|
impl_->active_arm = step.arm;
|
|
}
|
|
|
|
auto step_deadline = total_deadline;
|
|
if (step.source.timeout_ms() != 0) {
|
|
step_deadline = std::min(
|
|
step_deadline,
|
|
std::chrono::steady_clock::now() +
|
|
std::chrono::milliseconds(step.source.timeout_ms()));
|
|
}
|
|
const auto canceled = [this, step_deadline]() {
|
|
return impl_->stop_requested.load() ||
|
|
impl_->shutting_down.load() ||
|
|
std::chrono::steady_clock::now() >= step_deadline;
|
|
};
|
|
|
|
device::Result result;
|
|
try {
|
|
const auto handler = impl_->handlers.find(
|
|
step.source.command_case());
|
|
result = handler->second.execute(step, canceled);
|
|
} catch (const std::exception& exception) {
|
|
result = device::Result::failure(
|
|
device::ArmErrorCode::CommandFailed, exception.what());
|
|
} catch (...) {
|
|
result = device::Result::failure(
|
|
device::ArmErrorCode::CommandFailed,
|
|
"ActionQueue command threw an unknown exception");
|
|
}
|
|
|
|
{
|
|
std::lock_guard lock(impl_->mutex);
|
|
impl_->active_arm.reset();
|
|
if (impl_->stop_requested.load() || impl_->shutting_down.load()) {
|
|
impl_->pending.clear();
|
|
impl_->state = State::Canceled;
|
|
finishFeedback(
|
|
feedback, api::ACTION_RESULT_CODE_CANCELED,
|
|
completed_steps, "ActionQueue was stopped",
|
|
static_cast<int>(step.source_index));
|
|
impl_->state = impl_->shutting_down.load()
|
|
? State::ShuttingDown
|
|
: State::Idle;
|
|
return;
|
|
}
|
|
if (std::chrono::steady_clock::now() >= step_deadline) {
|
|
impl_->pending.clear();
|
|
impl_->state = State::TimedOut;
|
|
finishFeedback(
|
|
feedback, api::ACTION_RESULT_CODE_TIMED_OUT,
|
|
completed_steps, "ActionQueue step timeout expired",
|
|
static_cast<int>(step.source_index));
|
|
impl_->state = State::Idle;
|
|
return;
|
|
}
|
|
if (!result.ok()) {
|
|
impl_->pending.clear();
|
|
impl_->state = State::Failed;
|
|
finishFeedback(
|
|
feedback, api::ACTION_RESULT_CODE_FAILED,
|
|
completed_steps, result.message,
|
|
static_cast<int>(step.source_index));
|
|
impl_->state = State::Idle;
|
|
return;
|
|
}
|
|
++completed_steps;
|
|
}
|
|
}
|
|
}
|
|
|
|
void ActionQueue::cancelAndClear()
|
|
{
|
|
std::shared_ptr<device::RobotArm> active_arm;
|
|
{
|
|
std::lock_guard lock(impl_->mutex);
|
|
if (impl_->state == State::Idle ||
|
|
impl_->state == State::Rejected ||
|
|
impl_->state == State::Completed ||
|
|
impl_->state == State::Failed ||
|
|
impl_->state == State::Canceled ||
|
|
impl_->state == State::TimedOut) {
|
|
return;
|
|
}
|
|
impl_->stop_requested.store(true);
|
|
impl_->pending.clear();
|
|
impl_->state = impl_->shutting_down.load()
|
|
? State::ShuttingDown
|
|
: State::Stopping;
|
|
active_arm = impl_->active_arm;
|
|
}
|
|
|
|
// Never call a device SDK while holding the ActionQueue mutex. This keeps
|
|
// hardware emergency-stop and auto-enable monitor callbacks from forming a
|
|
// lock cycle with StopAll.
|
|
if (active_arm) {
|
|
try {
|
|
const auto result = active_arm->stopMotion();
|
|
if (!result.ok()) {
|
|
CMVR_LOG(WARNING)
|
|
<< "[ActionQueue] active arm stop was not confirmed: "
|
|
<< result.message;
|
|
}
|
|
} catch (const std::exception& error) {
|
|
CMVR_LOG(WARNING)
|
|
<< "[ActionQueue] active arm stop threw: " << error.what();
|
|
} catch (...) {
|
|
CMVR_LOG(WARNING)
|
|
<< "[ActionQueue] active arm stop threw an unknown exception";
|
|
}
|
|
}
|
|
}
|
|
|
|
void ActionQueue::shutdown()
|
|
{
|
|
if (!impl_) {
|
|
return;
|
|
}
|
|
impl_->shutting_down.store(true);
|
|
cancelAndClear();
|
|
std::lock_guard lock(impl_->mutex);
|
|
if (!impl_->active_arm) {
|
|
impl_->state = State::ShuttingDown;
|
|
}
|
|
}
|
|
|
|
ActionQueue::State ActionQueue::state() const
|
|
{
|
|
std::lock_guard lock(impl_->mutex);
|
|
return impl_->state;
|
|
}
|
|
|
|
} // namespace cmvr::service
|