diff --git a/examples/inference/franka.py b/examples/inference/franka.py index 4652fb77..c21052c0 100644 --- a/examples/inference/franka.py +++ b/examples/inference/franka.py @@ -390,6 +390,9 @@ def get_env(cfg: InferenceConfig) -> gym.Env: hw_cfg.robot_cfgs["right"].ignore_realtime = True hw_cfg.robot_cfgs["left"].speed_factor = 0.1 hw_cfg.robot_cfgs["right"].speed_factor = 0.1 + # interpolation window of the controllers must match the rate at which we stream actions + hw_cfg.robot_cfgs["left"].policy_rate = cfg.fps + hw_cfg.robot_cfgs["right"].policy_rate = cfg.fps hw_cfg.gripper_cfgs["left"].serial_number = ROBOTIQ_SERIAL["left"] hw_cfg.gripper_cfgs["right"].serial_number = ROBOTIQ_SERIAL["right"] env_rel = env_creator.create_env(hw_cfg) diff --git a/examples/teleop/franka.py b/examples/teleop/franka.py index 8c539fb7..7c76a211 100644 --- a/examples/teleop/franka.py +++ b/examples/teleop/franka.py @@ -156,6 +156,9 @@ def get_env(): hw_cfg.robot_cfgs["right"].ignore_realtime = True hw_cfg.robot_cfgs["left"].speed_factor = 0.3 hw_cfg.robot_cfgs["right"].speed_factor = 0.3 + # interpolation window of the controllers must match the rate at which we stream actions + hw_cfg.robot_cfgs["left"].policy_rate = RECORD_FPS + hw_cfg.robot_cfgs["right"].policy_rate = RECORD_FPS hw_cfg.gripper_cfgs["left"].serial_number = ROBOTIQ_SERIAL["left"] # type: ignore hw_cfg.gripper_cfgs["right"].serial_number = ROBOTIQ_SERIAL["right"] # type: ignore env_rel = env_creator.create_env(hw_cfg) diff --git a/extensions/rcs_fr3/src/hw/Franka.cpp b/extensions/rcs_fr3/src/hw/Franka.cpp index efdc9d11..ca34468d 100644 --- a/extensions/rcs_fr3/src/hw/Franka.cpp +++ b/extensions/rcs_fr3/src/hw/Franka.cpp @@ -86,8 +86,7 @@ FrankaState* Franka::get_state() { if (this->running_controller.load() == Controller::none) { current_robot_state = this->robot.readOnce(); } else { - std::lock_guard lock(this->interpolator_mutex); - current_robot_state = this->curr_state; + current_robot_state = this->curr_state.load(); } auto* state = new FrankaState(); state->robot_state = current_robot_state; @@ -111,12 +110,10 @@ common::Pose Franka::get_cartesian_position() { this->check_for_background_errors(); franka::RobotState robot_state; if (this->running_controller.load() == Controller::none) { - this->curr_state = this->robot.readOnce(); - robot_state = this->curr_state; + robot_state = this->robot.readOnce(); + this->curr_state.store(robot_state); } else { - this->interpolator_mutex.lock(); - robot_state = this->curr_state; - this->interpolator_mutex.unlock(); + robot_state = this->curr_state.load(); } return GetTCPInBaseFrame(robot_state, this->m_cfg.tcp_offset); } @@ -125,11 +122,10 @@ common::Pose Franka::get_cartesian_flange_position() { this->check_for_background_errors(); franka::RobotState robot_state; if (this->running_controller.load() == Controller::none) { - this->curr_state = this->robot.readOnce(); - robot_state = this->curr_state; + robot_state = this->robot.readOnce(); + this->curr_state.store(robot_state); } else { - std::lock_guard lock(this->interpolator_mutex); - robot_state = this->curr_state; + robot_state = this->curr_state.load(); } return GetFlangeInBaseFrame(robot_state); } @@ -146,16 +142,14 @@ void Franka::set_joint_position(const common::VectorXd& q) { common::VectorXd Franka::get_joint_position() { this->check_for_background_errors(); - common::Vector7d joints; + franka::RobotState robot_state; if (this->running_controller.load() == Controller::none) { - this->curr_state = this->robot.readOnce(); - joints = common::Vector7d(this->curr_state.q.data()); + robot_state = this->robot.readOnce(); + this->curr_state.store(robot_state); } else { - this->interpolator_mutex.lock(); - joints = common::Vector7d(this->curr_state.q.data()); - this->interpolator_mutex.unlock(); + robot_state = this->curr_state.load(); } - return joints; + return common::Vector7d(robot_state.q.data()); } void Franka::set_guiding_mode(bool x, bool y, bool z, bool roll, bool pitch, @@ -199,11 +193,14 @@ void Franka::controller_set_joint_position(const common::Vector7d& desired_q) { // from deoxys/config/osc-position-controller.yml double traj_interpolation_time_fraction = 1.0; // in s // form deoxys/config/charmander.yml - int policy_rate = 20; int traj_rate = 500; - if (this->running_controller.load() == Controller::none) { + const bool starting_fresh = + this->running_controller.load() == Controller::none; + + if (starting_fresh) { this->controller_time = 0.0; + this->m_active_policy_rate = this->m_cfg.policy_rate; this->get_joint_position(); this->joint_interpolator = common::LinearJointPositionTrajInterpolator(); } else if (this->running_controller.load() != Controller::jsc) { @@ -216,33 +213,72 @@ void Franka::controller_set_joint_position(const common::Vector7d& desired_q) { this->interpolator_mutex.lock(); } + franka::RobotState state_now = this->curr_state.load(); + const common::Vector7d q_now = + Eigen::Map(state_now.q.data()); + + const bool approach_on_start = + starting_fresh && this->m_cfg.blocking_move_on_start; + double approach_time = -1.0; + if (approach_on_start) { + const double kMinApproachTime = 0.3; // s + const double kMaxApproachTime = 5.0; // s + const double max_gap = (desired_q - q_now).cwiseAbs().maxCoeff(); + const double speed = std::max(this->m_cfg.approach_joint_speed, 1e-6); + approach_time = + std::clamp(max_gap / speed, kMinApproachTime, kMaxApproachTime); + } + this->joint_interpolator.reset( - this->controller_time, - Eigen::Map(this->curr_state.q.data()), desired_q, - policy_rate, traj_rate, traj_interpolation_time_fraction); + this->controller_time, q_now, desired_q, this->m_active_policy_rate, + traj_rate, traj_interpolation_time_fraction, approach_time); // if not thread is running, then start - if (this->running_controller.load() == Controller::none) { + if (starting_fresh) { this->running_controller.store(Controller::jsc); this->control_thread = std::thread(&Franka::joint_controller, this); } else { this->interpolator_mutex.unlock(); } + + if (approach_on_start) { + // Block until the controller has driven the robot to desired_q (or a + // safety timeout elapses). + const double pos_tol = 0.02; // rad + const double vel_tol = 0.05; // rad/s + const double timeout = approach_time + 2.0; + const auto start = std::chrono::steady_clock::now(); + while (true) { + this->check_for_background_errors(); + franka::RobotState state = this->curr_state.load(); + const common::Vector7d q = Eigen::Map(state.q.data()); + const common::Vector7d dq = Eigen::Map(state.dq.data()); + const double elapsed = std::chrono::duration( + std::chrono::steady_clock::now() - start) + .count(); + const double pos_err = (q - desired_q).cwiseAbs().maxCoeff(); + const double vel = dq.cwiseAbs().maxCoeff(); + if (elapsed >= approach_time && pos_err < pos_tol && vel < vel_tol) { + break; + } + if (elapsed > timeout) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + } + } } void Franka::check_for_background_errors() { - std::lock_guard lock(this->exception_mutex); - if (this->background_exception) { + std::exception_ptr ex = this->background_exception.load_and_clear(); + if (ex) { this->stop_control_thread(); - std::exception_ptr ex = this->background_exception; - this->background_exception = nullptr; std::rethrow_exception(ex); } } void Franka::clear_background_error() { - std::lock_guard lock(this->exception_mutex); - this->background_exception = nullptr; + this->background_exception.store(nullptr); } void Franka::osc_set_cartesian_position( @@ -251,15 +287,15 @@ void Franka::osc_set_cartesian_position( // from deoxys/config/osc-position-controller.yml double traj_interpolation_time_fraction = 1.0; // form deoxys/config/charmander.yml - int policy_rate = 20; int traj_rate = 500; - if (this->running_controller.load() == Controller::none) { + const bool starting_fresh = + this->running_controller.load() == Controller::none; + + if (starting_fresh) { this->controller_time = 0.0; - { - std::lock_guard lock(this->interpolator_mutex); - this->curr_state = this->robot.readOnce(); - } + this->m_active_policy_rate = this->m_cfg.policy_rate; + this->curr_state.store(this->robot.readOnce()); this->traj_interpolator = common::LinearPoseTrajInterpolator(); } else if (this->running_controller.load() != Controller::osc) { throw std::runtime_error( @@ -271,20 +307,80 @@ void Franka::osc_set_cartesian_position( } common::Pose curr_pose = - GetTCPInBaseFrame(this->curr_state, this->m_cfg.tcp_offset); + GetTCPInBaseFrame(this->curr_state.load(), this->m_cfg.tcp_offset); + + const bool approach_on_start = + starting_fresh && this->m_cfg.blocking_move_on_start; + double approach_time = -1.0; + if (approach_on_start) { + const double kMinApproachTime = 0.3; // s + const double kMaxApproachTime = 5.0; // s + const double trans_gap = + (desired_pose_EE_in_base_frame.translation() - curr_pose.translation()) + .norm(); + double dot = std::abs(curr_pose.quaternion().normalized().dot( + desired_pose_EE_in_base_frame.quaternion().normalized())); + dot = std::min(1.0, dot); + const double rot_gap = 2.0 * std::acos(dot); + const double trans_speed = + std::max(this->m_cfg.approach_cartesian_speed, 1e-6); + const double rot_speed = + std::max(this->m_cfg.approach_rotation_speed, 1e-6); + approach_time = + std::clamp(std::max(trans_gap / trans_speed, rot_gap / rot_speed), + kMinApproachTime, kMaxApproachTime); + } + this->traj_interpolator.reset( this->controller_time, curr_pose.translation(), curr_pose.quaternion(), desired_pose_EE_in_base_frame.translation(), - desired_pose_EE_in_base_frame.quaternion(), policy_rate, traj_rate, - traj_interpolation_time_fraction); + desired_pose_EE_in_base_frame.quaternion(), this->m_active_policy_rate, + traj_rate, traj_interpolation_time_fraction, approach_time); // if not thread is running, then start - if (this->running_controller.load() == Controller::none) { + if (starting_fresh) { this->running_controller.store(Controller::osc); this->control_thread = std::thread(&Franka::osc, this); } else { this->interpolator_mutex.unlock(); } + + if (approach_on_start) { + // Block until the controller has driven the robot to the desired pose (or a + // safety timeout elapses). + const double pos_tol = 0.005; // m + const double ori_tol = 0.02; // rad + const double vel_tol = 0.05; // rad/s (joint-space proxy for "stopped") + const double timeout = approach_time + 2.0; + const auto start = std::chrono::steady_clock::now(); + const Eigen::Vector3d target_p = + desired_pose_EE_in_base_frame.translation(); + const Eigen::Quaterniond target_q = + desired_pose_EE_in_base_frame.quaternion().normalized(); + while (true) { + this->check_for_background_errors(); + franka::RobotState state = this->curr_state.load(); + const common::Vector7d dq = Eigen::Map(state.dq.data()); + const common::Pose meas_pose = + GetTCPInBaseFrame(state, this->m_cfg.tcp_offset); + const double pos_err = (target_p - meas_pose.translation()).norm(); + double dot = std::abs(meas_pose.quaternion().normalized().dot(target_q)); + dot = std::min(1.0, dot); + const double ori_err = 2.0 * std::acos(dot); + const double vel = dq.cwiseAbs().maxCoeff(); + const double elapsed = std::chrono::duration( + std::chrono::steady_clock::now() - start) + .count(); + if (elapsed >= approach_time && pos_err < pos_tol && ori_err < ori_tol && + vel < vel_tol) { + break; + } + if (elapsed > timeout) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + } + } } // method to stop thread @@ -377,8 +473,9 @@ void Franka::osc() { int policy_rate = 20; int traj_rate = 500; + this->curr_state.store(robot_state); + this->interpolator_mutex.lock(); - this->curr_state = robot_state; this->controller_time += period.toSec(); this->traj_interpolator.next_step(this->controller_time, desired_pos_EE_in_base_frame, @@ -521,8 +618,7 @@ void Franka::osc() { return tau_d_rate_limited; }); } catch (...) { - std::lock_guard lock(this->exception_mutex); - this->background_exception = std::current_exception(); + this->background_exception.store(std::current_exception()); } // Ensure we mark the controller as stopped so we can restart later @@ -568,8 +664,9 @@ void Franka::joint_controller() { common::Vector7d desired_q; + this->curr_state.store(robot_state); + this->interpolator_mutex.lock(); - this->curr_state = robot_state; this->controller_time += period.toSec(); this->joint_interpolator.next_step(this->controller_time, desired_q); this->interpolator_mutex.unlock(); @@ -617,8 +714,7 @@ void Franka::joint_controller() { return tau_d_rate_limited; }); } catch (...) { - std::lock_guard lock(this->exception_mutex); - this->background_exception = std::current_exception(); + this->background_exception.store(std::current_exception()); } this->running_controller.store(Controller::none); @@ -650,8 +746,9 @@ void Franka::zero_torque_controller() { try { this->robot.control([&](const franka::RobotState& robot_state, franka::Duration period) -> franka::Torques { + this->curr_state.store(robot_state); + this->interpolator_mutex.lock(); - this->curr_state = robot_state; this->controller_time += period.toSec(); this->interpolator_mutex.unlock(); if (this->running_controller.load() == Controller::none) { @@ -661,8 +758,7 @@ void Franka::zero_torque_controller() { return franka::Torques({0, 0, 0, 0, 0, 0, 0}); }); } catch (...) { - std::lock_guard lock(this->exception_mutex); - this->background_exception = std::current_exception(); + this->background_exception.store(std::current_exception()); } this->running_controller.store(Controller::none); diff --git a/extensions/rcs_fr3/src/hw/Franka.h b/extensions/rcs_fr3/src/hw/Franka.h index 3906f49f..410ffe8c 100644 --- a/extensions/rcs_fr3/src/hw/Franka.h +++ b/extensions/rcs_fr3/src/hw/Franka.h @@ -38,6 +38,9 @@ struct FrankaConfig : common::RobotConfig { common::RobotPlatform robot_platform = common::RobotPlatform::HARDWARE; IKSolver ik_solver = IKSolver::rcs_ik; double speed_factor = DEFAULT_SPEED_FACTOR; + // Rate (Hz) at which set-point commands are streamed from the Python side. + // Used to size the interpolation window between successive targets. + int policy_rate = 20; // values from deoxys/config/joint-impedance-controller.yml common::Vector7d kp = (common::Vector7d() << 100., 100., 100., 100., 75., 150., 50.).finished(); @@ -54,6 +57,24 @@ struct FrankaConfig : common::RobotConfig { // Indicates that Cartesian control uses tcp_offset. bool tcp_offset_explicit = false; bool async_control = false; + // When true, on every (re)start of the joint controller the controller first + // holds the current measured position (~zero error) and ramps its target to + // the first commanded target over a gap-scaled window, blocking until it has + // converged before streaming resumes. Avoids a torque/velocity jump when + // (re)starting far from the target (e.g. after a PD-gain switch). + bool blocking_move_on_start = false; + // Max joint speed (rad/s) used to size the blocking approach window on + // (re)start of the joint controller: + // approach_time = max|q_target - q_now| / approach_joint_speed (clamped). + // Only used when blocking_move_on_start is true. + double approach_joint_speed = 0.4; + // Max Cartesian translation (m/s) and rotation (rad/s) speeds used to size + // the blocking approach window on (re)start of the OSC controller: + // approach_time = max(trans_gap / approach_cartesian_speed, + // rot_gap / approach_rotation_speed) (clamped). + // Only used when blocking_move_on_start is true. + double approach_cartesian_speed = 0.1; + double approach_rotation_speed = 0.5; bool ignore_realtime = false; size_t dof = 7; Eigen::Matrix joint_limits = @@ -94,12 +115,14 @@ class Franka : public common::Robot { std::optional control_thread = std::nullopt; common::LinearPoseTrajInterpolator traj_interpolator; double controller_time = 0.0; + // Snapshot of m_cfg.policy_rate taken when a controller thread is started, so + // the interpolation window stays consistent for the controller's lifetime. + int m_active_policy_rate = 20; common::LinearJointPositionTrajInterpolator joint_interpolator; - franka::RobotState curr_state; + common::ThreadSafeValue curr_state; std::mutex interpolator_mutex; std::atomic running_controller{Controller::none}; - std::exception_ptr background_exception = nullptr; - std::mutex exception_mutex; + common::ThreadSafeValue background_exception; void osc(); void joint_controller(); void zero_torque_controller(); diff --git a/extensions/rcs_fr3/src/pybind/rcs.cpp b/extensions/rcs_fr3/src/pybind/rcs.cpp index 9b2f4ece..fc3ddc18 100644 --- a/extensions/rcs_fr3/src/pybind/rcs.cpp +++ b/extensions/rcs_fr3/src/pybind/rcs.cpp @@ -128,6 +128,7 @@ PYBIND11_MODULE(_core, m) { py::class_(hw, "FrankaConfig", robot_config) .def_readwrite("ik_solver", &rcs::hw::FrankaConfig::ik_solver) .def_readwrite("speed_factor", &rcs::hw::FrankaConfig::speed_factor) + .def_readwrite("policy_rate", &rcs::hw::FrankaConfig::policy_rate) .def_readwrite("kp", &rcs::hw::FrankaConfig::kp) .def_readwrite("kd", &rcs::hw::FrankaConfig::kd) .def_readwrite("torque_limit", &rcs::hw::FrankaConfig::torque_limit) @@ -148,6 +149,14 @@ PYBIND11_MODULE(_core, m) { .def_readwrite("tcp_offset_explicit", &rcs::hw::FrankaConfig::tcp_offset_explicit) .def_readwrite("async_control", &rcs::hw::FrankaConfig::async_control) + .def_readwrite("blocking_move_on_start", + &rcs::hw::FrankaConfig::blocking_move_on_start) + .def_readwrite("approach_joint_speed", + &rcs::hw::FrankaConfig::approach_joint_speed) + .def_readwrite("approach_cartesian_speed", + &rcs::hw::FrankaConfig::approach_cartesian_speed) + .def_readwrite("approach_rotation_speed", + &rcs::hw::FrankaConfig::approach_rotation_speed) .def_readwrite("ignore_realtime", &rcs::hw::FrankaConfig::ignore_realtime) .def_readwrite("ip", &rcs::hw::FrankaConfig::ip); diff --git a/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi b/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi index af761256..5b9ab3eb 100644 --- a/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi +++ b/extensions/rcs_fr3/src/rcs_fr3/_core/hw/__init__.pyi @@ -105,7 +105,11 @@ class Franka(rcs._core.common.Robot): class FrankaConfig(rcs._core.common.RobotConfig): allow_high_collision: bool + approach_cartesian_speed: float + approach_joint_speed: float + approach_rotation_speed: float async_control: bool + blocking_move_on_start: bool ignore_realtime: bool ik_solver: IKSolver ip: str @@ -114,6 +118,7 @@ class FrankaConfig(rcs._core.common.RobotConfig): kp_p: numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]] kp_r: float load_parameters: FrankaLoad | None + policy_rate: int speed_factor: float tcp_offset: rcs._core.common.Pose tcp_offset_explicit: bool diff --git a/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi b/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi index af761256..5b9ab3eb 100644 --- a/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi +++ b/extensions/rcs_panda/src/rcs_panda/_core/hw/__init__.pyi @@ -105,7 +105,11 @@ class Franka(rcs._core.common.Robot): class FrankaConfig(rcs._core.common.RobotConfig): allow_high_collision: bool + approach_cartesian_speed: float + approach_joint_speed: float + approach_rotation_speed: float async_control: bool + blocking_move_on_start: bool ignore_realtime: bool ik_solver: IKSolver ip: str @@ -114,6 +118,7 @@ class FrankaConfig(rcs._core.common.RobotConfig): kp_p: numpy.ndarray[tuple[typing.Literal[3]], numpy.dtype[numpy.float64]] kp_r: float load_parameters: FrankaLoad | None + policy_rate: int speed_factor: float tcp_offset: rcs._core.common.Pose tcp_offset_explicit: bool diff --git a/include/rcs/LinearPoseTrajInterpolator.h b/include/rcs/LinearPoseTrajInterpolator.h index cecaa754..ed126f42 100644 --- a/include/rcs/LinearPoseTrajInterpolator.h +++ b/include/rcs/LinearPoseTrajInterpolator.h @@ -40,11 +40,17 @@ class LinearPoseTrajInterpolator { const Eigen::Vector3d &p_goal, const Eigen::Quaterniond &q_goal, const int &policy_rate, const int &rate, - const double &traj_interpolator_time_fraction) { + const double &traj_interpolator_time_fraction, + const double &explicit_max_time = -1.0) { dt_ = 1. / static_cast(rate); last_time_ = time_sec; + // A positive explicit_max_time overrides the default (1 / policy_rate) + // interpolation window; used for a longer, gap-scaled approach on (re)start. max_time_ = - 1. / static_cast(policy_rate) * traj_interpolator_time_fraction; + explicit_max_time > 0.0 + ? explicit_max_time + : 1. / static_cast(policy_rate) * + traj_interpolator_time_fraction; start_time_ = time_sec; start_ = false; @@ -131,12 +137,18 @@ class LinearJointPositionTrajInterpolator { const Eigen::Matrix &q_start, const Eigen::Matrix &q_goal, const int &policy_rate, const int &rate, - const double &traj_interpolator_time_fraction) { + const double &traj_interpolator_time_fraction, + const double &explicit_max_time = -1.0) { dt_ = 1. / static_cast(rate); last_time_ = time_sec; + // A positive explicit_max_time overrides the default (1 / policy_rate) + // interpolation window; used for a longer, gap-scaled approach on (re)start. max_time_ = - 1. / static_cast(policy_rate) * traj_interpolator_time_fraction; + explicit_max_time > 0.0 + ? explicit_max_time + : 1. / static_cast(policy_rate) * + traj_interpolator_time_fraction; start_time_ = time_sec; start_ = false; diff --git a/include/rcs/utils.h b/include/rcs/utils.h index 5ebc8cce..2baf0e08 100644 --- a/include/rcs/utils.h +++ b/include/rcs/utils.h @@ -2,7 +2,12 @@ #define RCS_UTIL_H #include +#include +#include #include +#include +#include +#include namespace rcs { namespace common { @@ -39,6 +44,83 @@ void bootstrap_egl(std::uintptr_t fn_addr, std::uintptr_t display, std::uintptr_t context); void ensure_current(); +/*** + * @brief thread safe holder for a single value, e.g. to hand data from a + * non-realtime thread to a control loop + */ +template +class ThreadSafeValue { + private: + T value_; + mutable std::mutex mutex_; + + public: + ThreadSafeValue() = default; + explicit ThreadSafeValue(const T& value) : value_(value) {} + + void store(const T& value) { + std::lock_guard lock(mutex_); + value_ = value; + } + + T load() const { + std::lock_guard lock(mutex_); + return value_; + } + + /*** + * @brief atomically read the value and reset it to a default constructed one + */ + T load_and_clear() { + std::lock_guard lock(mutex_); + return std::exchange(value_, T{}); + } +}; + +/*** + * @brief thread safe ring buffer of fixed maximum size, drops the oldest + * element once the buffer is full + */ +template +class ThreadSafeFixedBuffer { + private: + std::deque deque_; + size_t max_size_; + mutable std::mutex mutex_; + + public: + explicit ThreadSafeFixedBuffer(size_t max_size) : max_size_(max_size) {} + + void push_back(const T& value) { + std::lock_guard lock(mutex_); + + deque_.push_back(value); + if (deque_.size() > max_size_) { + deque_.pop_front(); + } + } + + T get(size_t index) const { + std::lock_guard lock(mutex_); + return deque_[index]; + } + + size_t size() const { + std::lock_guard lock(mutex_); + return deque_.size(); + } + + void clear() { + std::lock_guard lock(mutex_); + deque_.clear(); + } + + std::vector to_vector() const { + std::lock_guard lock(mutex_); + return std::vector(deque_.begin(), deque_.end()); + } +}; + } // namespace common } // namespace rcs