Implemented the non-servo motion control of KUKA robot.

CMakeLists.txt

@@ -32,7 +32,7 @@ file(GLOB_RECURSE CORE_IMPL_FILES ./src/core/*.cpp)
 file(GLOB_RECURSE ROBOT_IMPL_FILES ./src/robot/*.cpp)
 
 file(GLOB_RECURSE SRC_FILES ./src/*.cpp)
-add_executable(${PROJECT_NAME} ${SRC_FILES})
+add_executable(${PROJECT_NAME} ${SRC_FILES} src/robot/kuka/kuka_interface.cpp src/utility/coro_unique_lock.hpp src/robot/ur/ur_defs.h src/robot/kuka/kuka_defs.h)
 
 IF (WIN32)
     list(APPEND EXTRA_LIBS ws2_32 wsock32 winmm bcrypt)

src/algorithm/landmark_registration.cpp

@@ -130,7 +130,7 @@ namespace sophiar {
     }
 
     awaitable<void> landmark_registration::on_stop() noexcept {
-        SAFE_CLOSE_CORO_WORKER(pimpl->main_worker)
+        SAFE_RESET_WORKER(pimpl->main_worker)
         co_return;
     }
 

src/core/basic_obj_types.hpp

@@ -96,7 +96,7 @@ namespace sophiar {
     using scalarxyz_obj = small_obj_wrapper<Eigen::Vector3d>;   // 11
     using transform_obj = small_obj_wrapper<Eigen::Isometry3d>; // 12
     using array6_obj = array_obj<6>; // 13
-//    using array7_obj = array_obj<7>; // 14
+    using array7_obj = array_obj<7>; // 14
 
     // specialization
 
@@ -122,6 +122,14 @@ namespace sophiar {
         }
     }
 
+    template<>
+    template<typename ReaderType>
+    inline void array7_obj::fill_from(ReaderType &reader) {
+        for (auto &val: value) {
+            reader >> val;
+        }
+    }
+
     template<>
     template<typename WriterType>
     inline void transform_obj::write_to(WriterType &writer) const {
@@ -145,6 +153,14 @@ namespace sophiar {
         }
     }
 
+    template<>
+    template<typename WriterType>
+    inline void array7_obj::write_to(WriterType &writer) const {
+        for (auto val: value) {
+            writer << val;
+        }
+    }
+
     template<>
     constexpr size_t transform_obj::binary_length() {
         return 7 * sizeof(double);
@@ -184,6 +200,13 @@ namespace sophiar {
         static constexpr size_t element_size = 6;
     };
 
+    template<>
+    struct basic_obj_traits<array7_obj> {
+        using value_type = array6_obj::value_type;
+        using element_type = double;
+        static constexpr size_t element_size = 7;
+    };
+
     template<typename T>
     inline void small_obj_wrapper<T>::fill_from_json_array(const nlohmann::json &config) {
 

src/core/global_defs.cpp

@@ -6,7 +6,6 @@
 #include "core/sophiar_pool.h"
 #include "core/timestamp_helper.hpp"
 #include "core/tristate_obj.h"
-#include "robot/ur/ur_interface.h"
 #include "utility/debug_utility.hpp"
 #include "utility/dynamic_pool.hpp"
 #include "utility/variable_utility.hpp"
@@ -26,6 +25,13 @@ DEFAULT_TRISTATE_OBJ_DEF(landmark_registration)
 
 #endif
 
+#if !SOPHIAR_TEST || SOPHIAR_TEST_ROBOT
+
+DEFAULT_TRISTATE_OBJ_DEF(ur_interface)
+DEFAULT_TRISTATE_OBJ_DEF(kuka_interface)
+
+#endif
+
 #if !SOPHIAR_TEST || SOPHIAR_TEST_NDI
 
 DEFAULT_TRISTATE_OBJ_DEF(ndi_interface)
@@ -60,6 +66,7 @@ namespace sophiar {
         REGISTER_VARIABLE_TYPE(scalarxyz_obj);
         REGISTER_VARIABLE_TYPE(transform_obj);
         REGISTER_VARIABLE_TYPE(array6_obj);
+        REGISTER_VARIABLE_TYPE(array7_obj);
 
 #undef REGISTER_VARIABLE_TYPE
     }
@@ -91,6 +98,7 @@ namespace sophiar {
 #if !SOPHIAR_TEST || SOPHIAR_TEST_ROBOT
 
         REGISTER_TYPE(ur_interface);
+        REGISTER_TYPE(kuka_interface);
 
 #endif
 

src/core/sophiar_pool.cpp

@@ -122,7 +122,7 @@ namespace sophiar {
             REGISTER_TYPE(scalarxyz_obj);
             REGISTER_TYPE(transform_obj);
             REGISTER_TYPE(array6_obj);
-//            REGISTER_TYPE(array7_obj);
+            REGISTER_TYPE(array7_obj);
 #undef REGISTER_TYPE
         }
 

src/robot/kuka/kuka_defs.h

@@ -0,0 +1,16 @@
+#ifndef SOPHIAR2_KUKA_DEFS_H
+#define SOPHIAR2_KUKA_DEFS_H
+
+#include <cstdint>
+
+namespace sophiar {
+
+    enum class kuka_control_mode_type : uint8_t {
+        NO_CONTROL = 0,
+        HARD_FLANGE_POSE = 1,
+        HARD_JOINT_ANGLE = 2,
+    };
+
+}
+
+#endif //SOPHIAR2_KUKA_DEFS_H

src/robot/kuka/kuka_interface.cpp

@@ -0,0 +1,634 @@
+#include "core/basic_obj_types.hpp"
+#include "robot/kuka/kuka_defs.h"
+#include "utility/bit_operations.hpp"
+#include "utility/config_utility.hpp"
+#include "utility/coro_worker.hpp"
+#include "utility/coro_worker_helper_func.hpp"
+#include "utility/coro_unique_lock.hpp"
+#include "utility/debug_utility.hpp"
+#include "utility/name_translator.hpp"
+#include "utility/variable_helper.hpp"
+#include "utility/versatile_buffer2.hpp"
+
+#include <boost/asio/ip/address.hpp>
+#include <boost/asio/ip/tcp.hpp>
+#include <boost/asio/co_spawn.hpp>
+#include <boost/asio/detached.hpp>
+#include <boost/asio/use_awaitable.hpp>
+#include <boost/crc.hpp>
+#include <boost/smart_ptr/scoped_ptr.hpp>
+#include <boost/system/error_code.hpp>
+
+#include <spdlog/spdlog.h>
+
+#include <unordered_map>
+#include <string_view>
+
+#include "utility/assert_utility.h"
+
+DEFAULT_TRISTATE_OBJ_DEF(kuka_interface);
+
+namespace sophiar {
+
+    using boost::asio::async_read;
+    using boost::asio::async_write;
+    using boost::asio::awaitable;
+    using boost::asio::buffer;
+    using boost::asio::co_spawn;
+    using boost::asio::detached;
+    using boost::asio::use_awaitable;
+    using boost::scoped_ptr;
+    using boost::system::error_code;
+
+    using namespace boost::asio::ip;
+    using namespace Eigen;
+
+    struct kuka_interface::impl {
+
+        using packet_size_type = uint16_t;
+        using packet_id_type = uint32_t;
+        using packet_crc_type = uint32_t;
+
+        enum class packet_type : uint8_t {
+            QUERY = 0x01,
+            CONTROL = 0x02
+        };
+
+        enum class query_packet_type : uint8_t {
+            FLANGE_POSE = 0x01,
+            FLANGE_FORCE_TORQUE = 0x02,
+            JOINT_ANGLE = 0x03,
+            JOINT_TORQUE = 0x04
+        };
+
+        enum class control_packet_type : uint8_t {
+            PTP = 0x01,
+            LIN = 0x02,
+            LIN_REL = 0x03,
+            SERVO = 0x10
+        };
+
+        using vector6d_type = array6_obj::value_type;
+
+        static constexpr uint16_t kuka_port = 30009;
+        static constexpr uint16_t servo_port = 30003;
+        static constexpr auto kuka_endian = boost::endian::order::big;
+
+        static constexpr uint8_t query_flange_pose_mask = 1 << 0;
+        static constexpr uint8_t query_joint_angle_mask = 1 << 1;
+        static constexpr uint8_t query_flange_force_torque_mask = 1 << 2;
+        static constexpr uint8_t query_joint_torque_mask = 1 << 3;
+
+        static constexpr auto hard_motion_params_size = 3 * sizeof(double)
+                                                        + 3 * array7_obj::binary_length();
+
+        using control_mode_type = kuka_control_mode_type;
+        name_translator<kuka_control_mode_type> control_mode_translator;
+
+        array7_obj::value_type invalid_array7;
+
+        kuka_interface *q_this = nullptr;
+
+        using ip_address_type = boost::asio::ip::address;
+        ip_address_type kuka_ip;
+        scoped_ptr<tcp::socket> kuka_socket;
+
+        using packet_hanlder_type = std::function<void(const_extern_memory)>;
+        using handler_map_type = std::unordered_map<packet_id_type, packet_hanlder_type>;
+        handler_map_type handler_map;
+
+        int query_interval_ms = 8;
+
+        // output variables
+        variable_index_type cur_flange_pose_index = -1;
+        variable_index_type cur_joint_angle_index = -1;
+        variable_index_type cur_flange_force_index = -1, cur_flange_force_uncertainty_index = -1;
+        variable_index_type cur_flange_torque_index = -1, cur_flange_torque_uncertainty_index = -1;
+        variable_index_type cur_joint_torque_index = -1;
+
+        variable_index_type is_moving_index = -1;
+        variable_index_type current_control_mode_index = -1;
+
+        // input variables
+        variable_index_type target_flange_pose_index = -1;
+        variable_index_type target_joint_angle_index = -1;
+        variable_index_type flange_speed_limit_index = -1;
+        variable_index_type flange_acceleration_limit_index = -1;
+        variable_index_type flange_jerk_limit_index = -1;
+        variable_index_type joint_speed_limit_index = -1;
+        variable_index_type joint_acceleration_limit_index = -1;
+        variable_index_type joint_jerk_limit_index = -1;
+
+        // temporary output variables
+        transform_obj::pointer cur_flange_pose;
+        array7_obj::pointer cur_joint_angle;
+        scalarxyz_obj::pointer cur_flange_force, cur_flange_force_uncertainty;
+        scalarxyz_obj::pointer cur_flange_torque, cur_flange_torque_uncertainty;
+        array7_obj::pointer cur_joint_torque;
+
+        coro_unique_lock send_packet_lock;
+        coro_unique_lock hard_motion_lock;
+
+        packet_id_type last_packet_id = 0;
+        uint8_t cur_query_mask = 0, cur_updated_mask = 0;
+        control_mode_type current_control_mode;
+
+        coro_signal2 status_updated_signal;
+        coro_worker::pointer status_worker;
+        coro_worker::pointer command_worker;
+        coro_worker::pointer kuka_reply_worker;
+
+        static void vector6d_to_transform(const vector6d_type &vec, Isometry3d &trans) {
+            auto translate = Translation3d(vec[0], vec[1], vec[2]);
+            auto rotation = AngleAxisd(vec[3], Vector3d::UnitZ())
+                            * AngleAxisd(vec[4], Vector3d::UnitY())
+                            * AngleAxisd(vec[5], Vector3d::UnitX());
+            trans = translate * rotation;
+        }
+
+        static void transform_to_vector6d(const Isometry3d &trans, vector6d_type &vec) {
+            auto transPart = trans.translation();
+            for (int i = 0; i < 3; ++i) {
+                vec[i] = transPart(i);
+            }
+            auto rotPart = trans.rotation().eulerAngles(2, 1, 0);
+            for (int i = 0; i < 3; ++i) {
+                vec[i + 3] = rotPart(i);
+            }
+        }
+
+        template<ReadableMemory MemType>
+        static packet_crc_type calc_crc32(const MemType &mem) {
+            auto crc32 = boost::crc_32_type{};
+            crc32.process_bytes(mem.data(), mem.size());
+            return crc32.checksum();
+        }
+
+        template<ReadableMemory MemType>
+        static dynamic_memory::pointer create_packet(const MemType &content, packet_id_type packet_id) {
+            packet_size_type packet_size = sizeof(packet_id_type)
+                                           + content.size()
+                                           + sizeof(packet_crc_type);
+            auto packet = dynamic_memory::new_instance(packet_size + sizeof(packet_size_type));
+            auto writer = versatile_writer<kuka_endian>(*packet);
+            writer << packet_size << packet_id;
+            writer.write_memory(content);
+            auto crc_content = const_extern_memory{packet->data() + sizeof(packet_size_type),
+                                                   sizeof(packet_id_type) + content.size()};
+            writer << calc_crc32(crc_content);
+            assert(writer.remaining_bytes() == 0);
+            return std::move(packet);
+        }
+
+        static const_extern_memory parse_packet(const dynamic_memory &packet, packet_id_type &packet_id) {
+            auto reader = versatile_reader<kuka_endian>(packet);
+            reader >> packet_id;
+            auto ok_sign = reader.read_value<bool>();
+            if (ok_sign != 0x00) {
+                // TODO show log
+            }
+            auto content_size = packet.size()
+                                - sizeof(packet_id_type)
+                                - 1 // for the ok sign
+                                - sizeof(packet_crc_type);
+            auto content = const_extern_memory{packet.data() + sizeof(packet_id_type) + 1,
+                                               content_size};
+            reader.manual_offset(content_size);
+            auto crc_receive = reader.read_value<uint32_t>();
+            assert(reader.empty());
+            auto crc_content = const_extern_memory{packet.data(),
+                                                   packet.size() - sizeof(packet_crc_type)};
+            auto crc_calc = calc_crc32(crc_content);
+            assert(crc_receive == crc_calc);
+            return content;
+        }
+
+        // not used for servo communication
+        awaitable<void> send_kuka_packet(dynamic_memory::pointer &&packet) {
+            auto lock_guard = coro_lock_guard{send_packet_lock};
+            co_await lock_guard.coro_require_lock();
+            assert(kuka_socket != nullptr);
+            co_await async_write_memory_to(*kuka_socket, *packet);
+            co_return;
+        }
+
+        awaitable<bool> receive_and_dispatch_kuka_packet() {
+            assert(kuka_socket != nullptr);
+            auto packet_size = co_await async_read_value<kuka_endian, packet_size_type>(*kuka_socket);
+            auto packet = dynamic_memory::new_instance(packet_size);
+            co_await async_fill_memory_from(*kuka_socket, *packet);
+            packet_id_type packet_id;
+            auto content = parse_packet(*packet, packet_id);
+            assert(handler_map.contains(packet_id));
+            handler_map.at(packet_id)(content);
+            auto delete_count = handler_map.erase(packet_id);
+            assert(delete_count == 1);
+            co_return true;
+        }
+
+        void check_and_update_status() {
+            assert((cur_updated_mask & (~cur_query_mask)) == 0); // ensure no extra bits are set
+            if (cur_updated_mask != cur_query_mask) [[likely]] return;
+            cur_updated_mask = 0b0000;
+            auto ts = current_timestamp();
+            try_update_variable<transform_obj>(cur_flange_pose_index, std::move(cur_flange_pose), ts);
+            try_update_variable<array7_obj>(cur_joint_angle_index, std::move(cur_joint_angle), ts);
+            try_update_variable<scalarxyz_obj>(cur_flange_force_index, std::move(cur_flange_force), ts);
+            try_update_variable<scalarxyz_obj>(cur_flange_force_uncertainty_index,
+                                               std::move(cur_flange_force_uncertainty), ts);
+            try_update_variable<scalarxyz_obj>(cur_flange_torque_index, std::move(cur_flange_torque), ts);
+            try_update_variable<scalarxyz_obj>(cur_flange_torque_uncertainty_index,
+                                               std::move(cur_flange_torque_uncertainty), ts);
+            try_update_variable<array7_obj>(cur_joint_torque_index, std::move(cur_joint_torque), ts);
+            status_updated_signal.try_notify_all(ts);
+        }
+
+        void handle_new_cur_flange_pose(const_extern_memory content) {
+            auto reader = versatile_reader<kuka_endian>(content);
+            vector6d_type pose_vec;
+            reader >> pose_vec;
+            assert(is_valid_id(cur_flange_pose_index));
+            cur_flange_pose = transform_obj::new_instance();
+            vector6d_to_transform(pose_vec, cur_flange_pose->value);
+            cur_updated_mask |= query_flange_pose_mask;
+            check_and_update_status();
+        }
+
+        void handle_new_cur_joint_angle(const_extern_memory content) {
+            auto reader = versatile_reader<kuka_endian>(content);
+            assert(is_valid_id(cur_joint_angle_index));
+            cur_joint_angle = array7_obj::new_instance();
+            reader >> cur_joint_angle->value;
+            cur_updated_mask |= query_joint_angle_mask;
+            check_and_update_status();
+        }
+
+        void handle_new_cur_flange_force_torque(const_extern_memory content) {
+            scalarxyz_obj::value_type ignored_value;
+            auto reader = versatile_reader<kuka_endian>(content);
+
+#define SIMPLE_CASE(item) \
+            if (is_valid_id(item##_index)) { \
+                item = scalarxyz_obj ::new_instance(); \
+                reader >> item->value; \
+            } else { \
+                reader >> ignored_value; \
+            }
+
+            SIMPLE_CASE(cur_flange_force)
+            SIMPLE_CASE(cur_flange_torque)
+            SIMPLE_CASE(cur_flange_force_uncertainty)
+            SIMPLE_CASE(cur_flange_torque_uncertainty)
+
+#undef SIMPLE_CASE
+
+            cur_updated_mask |= query_flange_force_torque_mask;
+            check_and_update_status();
+        }
+
+        void handle_new_cur_joint_torque(const_extern_memory content) {
+            auto reader = versatile_reader<kuka_endian>(content);
+            assert(is_valid_id(cur_joint_torque_index));
+            cur_joint_torque = array7_obj::new_instance();
+            reader >> cur_joint_torque->value;
+            cur_updated_mask |= query_joint_torque_mask;
+            check_and_update_status();
+        }
+
+        void create_query_worker() {
+            // content defs
+            auto query_flange_pose_content = static_memory<2>{
+                    (char) packet_type::QUERY,
+                    (char) query_packet_type::FLANGE_POSE
+            };
+            auto query_joint_angle_content = static_memory<2>{
+                    (char) packet_type::QUERY,
+                    (char) query_packet_type::JOINT_ANGLE
+            };
+            auto query_flange_force_torque_content = static_memory<2>{
+                    (char) packet_type::QUERY,
+                    (char) query_packet_type::FLANGE_FORCE_TORQUE
+            };
+            auto query_joint_torque_content = static_memory<2>{
+                    (char) packet_type::QUERY,
+                    (char) query_packet_type::JOINT_TORQUE
+            };
+
+            auto enable_query_flange_pose = is_valid_id(cur_flange_pose_index);
+            auto enable_query_joint_angle = is_valid_id(cur_joint_angle_index);
+            auto enable_query_flange_force_torque = is_valid_id(cur_flange_force_index)
+                                                    || is_valid_id(cur_flange_torque_index);
+            auto enable_query_joint_torque = is_valid_id(cur_joint_torque_index);
+
+            cur_query_mask = 0b0000;
+            cur_updated_mask = 0b0000;
+            if (enable_query_flange_pose) cur_query_mask |= query_flange_pose_mask;
+            if (enable_query_joint_angle) cur_query_mask |= query_joint_angle_mask;
+            if (enable_query_flange_force_torque) cur_query_mask |= query_flange_force_torque_mask;
+            if (enable_query_joint_torque) cur_query_mask |= query_joint_torque_mask;
+
+            if (cur_query_mask == 0) return;
+
+            auto worker_func = [=, this,
+                    reply_watcher = status_updated_signal.new_watcher()]() mutable
+                    -> awaitable<bool> {
+                reply_watcher.sync();
+
+#define SIMPLE_CASE(item) \
+                if (enable_query_##item) { \
+                    auto packet_id = last_packet_id++; \
+                    handler_map.emplace(packet_id, [this](const_extern_memory content) { \
+                        handle_new_cur_##item(content); \
+                    }); \
+                    auto packet = create_packet(query_##item##_content, packet_id); \
+                    co_await send_kuka_packet(std::move(packet)); \
+                }
+
+                SIMPLE_CASE(flange_pose)
+                SIMPLE_CASE(joint_angle)
+                SIMPLE_CASE(flange_force_torque)
+                SIMPLE_CASE(joint_torque)
+
+#undef SIMPLE_CASE
+
+                co_await reply_watcher.coro_wait(false); // manually synced before
+                co_return true;
+            };
+            auto exit_func = reset_on_exit_func(q_this);
+            auto noexcept_wrapper = make_noexcept_func(std::move(worker_func), [](std::exception &) {
+                // TODO show log
+            });
+            assert(status_worker == nullptr);
+            status_worker = make_interval_coro_worker(std::chrono::milliseconds(query_interval_ms),
+                                                      std::move(noexcept_wrapper), std::move(exit_func));
+            status_worker->run();
+        }
+
+        void create_kuka_reply_worker() {
+            auto noexcept_wrapper = make_noexcept_func([this]() -> awaitable<bool> {
+                return receive_and_dispatch_kuka_packet();
+            }, [](std::exception &) {
+                // TODO show log
+            });
+            assert(kuka_reply_worker == nullptr);
+            kuka_reply_worker = make_infinite_coro_worker(std::move(noexcept_wrapper));
+            kuka_reply_worker->run();
+        }
+
+        template<typename WriterType>
+        void fill_hard_motion_params(WriterType &writer) {
+            static constexpr auto nan_literal = std::numeric_limits<double>::quiet_NaN();
+            writer << try_query_variable_value<double_obj>(flange_speed_limit_index, nan_literal);
+            writer << try_query_variable_value<double_obj>(flange_acceleration_limit_index, nan_literal);
+            writer << try_query_variable_value<double_obj>(flange_jerk_limit_index, nan_literal);
+            writer << try_query_variable_value<array7_obj>(joint_speed_limit_index, invalid_array7);
+            writer << try_query_variable_value<array7_obj>(joint_acceleration_limit_index, invalid_array7);
+            writer << try_query_variable_value<array7_obj>(joint_jerk_limit_index, invalid_array7);
+        }
+
+        void handle_hard_motion_reply(const_extern_memory reply) {
+            assert(reply.size() == 0);
+            hard_motion_lock.release_lock();
+            try_update_variable_value<bool_obj>(is_moving_index, false);
+        }
+
+        auto create_hard_flange_pose_func() {
+            assert(current_control_mode == control_mode_type::HARD_FLANGE_POSE);
+            assert(is_valid_id(target_flange_pose_index));
+
+            auto worker_func = [=, this,
+                    flange_pose = VARIABLE_MANUAL_DELEGATE(transform_obj, target_flange_pose_index)]()mutable
+                    -> awaitable<bool> {
+
+                co_await flange_pose.coro_wait_update();
+                if (flange_pose.empty()) co_return true;
+
+                co_await hard_motion_lock.coro_lock();
+                try_update_variable_value<bool_obj>(is_moving_index, true);
+                flange_pose.manual_sync();
+                if (flange_pose.empty()) co_return true;
+
+                static constexpr auto content_size = 2 + 6 * sizeof(double) + hard_motion_params_size;
+                auto content = static_memory<content_size>{};
+                auto writer = versatile_writer<kuka_endian>{content};
+                writer << (uint8_t) packet_type::CONTROL
+                       << (uint8_t) control_packet_type::LIN;
+                vector6d_type pose_vec;
+                transform_to_vector6d(flange_pose->value, pose_vec);
+                writer << pose_vec;
+                fill_hard_motion_params(writer);
+                assert(writer.remaining_bytes() == 0);
+
+                auto packet_id = last_packet_id++;
+                handler_map.emplace(packet_id, [this](const_extern_memory reply) {
+                    handle_hard_motion_reply(reply);
+                });
+                auto packet = create_packet(content, packet_id);
+                co_await send_kuka_packet(std::move(packet));
+                co_return true;
+            };
+            return std::move(worker_func);
+        };
+
+        auto create_hard_joint_angle_func() {
+            assert(current_control_mode == control_mode_type::HARD_JOINT_ANGLE);
+            assert(is_valid_id(target_joint_angle_index));
+
+            auto worker_func = [=, this,
+                    joint_angle = VARIABLE_MANUAL_DELEGATE(array7_obj, target_joint_angle_index)]()mutable
+                    -> awaitable<bool> {
+
+                co_await joint_angle.coro_wait_update();
+                if (joint_angle.empty()) co_return true;
+
+                co_await hard_motion_lock.coro_lock();
+                try_update_variable_value<bool_obj>(is_moving_index, true);
+                joint_angle.manual_sync();
+                if (joint_angle.empty()) co_return true;
+
+                static constexpr auto content_size = 2 + 7 * sizeof(double) + hard_motion_params_size;
+                auto content = static_memory<content_size>{};
+                auto writer = versatile_writer<kuka_endian>{content};
+                writer << (uint8_t) packet_type::CONTROL
+                       << (uint8_t) control_packet_type::PTP
+                       << joint_angle->value;
+                fill_hard_motion_params(writer);
+                assert(writer.remaining_bytes() == 0);
+
+                auto packet_id = last_packet_id++;
+                handler_map.emplace(packet_id, [this](const_extern_memory reply) {
+                    handle_hard_motion_reply(reply);
+                });
+                auto packet = create_packet(content, packet_id);
+                co_await send_kuka_packet(std::move(packet));
+                co_return true;
+            };
+            return std::move(worker_func);
+        };
+
+        void create_command_worker() {
+            assert(command_worker == nullptr);
+            assert(current_control_mode != control_mode_type::NO_CONTROL);
+
+            auto exception_handle_func = [](std::exception &) {
+                // TODO show log
+            };
+
+            switch (current_control_mode) {
+                case control_mode_type::HARD_FLANGE_POSE: {
+                    auto noexcept_wrapper = make_noexcept_func(
+                            create_hard_flange_pose_func(), std::move(exception_handle_func));
+                    command_worker = make_infinite_coro_worker(std::move(noexcept_wrapper));
+                    break;
+                }
+                case control_mode_type::HARD_JOINT_ANGLE: {
+                    auto noexcept_wrapper = make_noexcept_func(
+                            create_hard_joint_angle_func(), std::move(exception_handle_func));
+                    command_worker = make_infinite_coro_worker(std::move(noexcept_wrapper));
+                    break;
+                }
+                default: {
+                    assert(false);
+                    return;
+                }
+            }
+
+            command_worker->run();
+
+            // set initial state
+            try_update_variable_value<bool_obj>(is_moving_index, false);
+        }
+
+        awaitable<void> on_init_impl() {
+            assert(kuka_socket == nullptr);
+            kuka_socket.reset(new tcp::socket{*global_context});
+            co_await kuka_socket->async_connect({kuka_ip, kuka_port}, use_awaitable);
+            kuka_socket->set_option(tcp::no_delay{true}); // decrease latency
+            handler_map.clear(); // remove existing handlers
+            create_kuka_reply_worker();
+            co_return;
+        }
+
+        void on_start_impl() {
+            create_query_worker();
+            if (current_control_mode != control_mode_type::NO_CONTROL) {
+                create_command_worker();
+            }
+        }
+
+        void initialize_translator() {
+            control_mode_translator.register_item("no_control", control_mode_type::NO_CONTROL);
+            control_mode_translator.register_item("hard_flange_pose", control_mode_type::HARD_FLANGE_POSE);
+            control_mode_translator.register_item("hard_joint_angle", control_mode_type::HARD_JOINT_ANGLE);
+        }
+
+        void load_init_config(const nlohmann::json &config) {
+            kuka_ip = boost::asio::ip::make_address(LOAD_STRING_ITEM("address"));
+        }
+
+        void load_output_config(const nlohmann::json &config) {
+            auto report_frequency = LOAD_FLOAT_ITEM("report_frequency");
+            query_interval_ms = 1000.0 / report_frequency;
+            if (config.contains("joint_info")) {
+                const auto &joint_config = config["joint_info"];
+                cur_joint_angle_index = TRY_LOAD_VARIABLE_INDEX2(joint_config, array7_obj, "angle");
+                cur_joint_torque_index = TRY_LOAD_VARIABLE_INDEX2(joint_config, array7_obj, "torque");
+            }
+            if (config.contains("flange_info")) {
+                const auto &flange_config = config["flange_info"];
+                cur_flange_pose_index = TRY_LOAD_VARIABLE_INDEX2(flange_config, transform_obj, "pose");
+                cur_flange_force_index = TRY_LOAD_VARIABLE_INDEX2(flange_config, scalarxyz_obj, "force");
+                cur_flange_torque_index = TRY_LOAD_VARIABLE_INDEX2(flange_config, scalarxyz_obj, "torque");
+                cur_flange_force_uncertainty_index = TRY_LOAD_VARIABLE_INDEX2(
+                        flange_config, scalarxyz_obj, "force_uncertainty");
+                cur_flange_torque_uncertainty_index = TRY_LOAD_VARIABLE_INDEX2(
+                        flange_config, scalarxyz_obj, "torque_uncertainty");
+            }
+            is_moving_index = TRY_LOAD_VARIABLE_INDEX(bool_obj, "is_moving");
+            // only one variable must be exported
+            current_control_mode_index = LOAD_VARIABLE_INDEX(u64int_obj, "current_control_mode");
+        }
+
+        void load_input_config(const nlohmann::json &config) {
+            auto control_mode_str = LOAD_STRING_ITEM("control_mode");
+            current_control_mode = control_mode_translator.translate(control_mode_str);
+            UPDATE_VARIABLE_VAL(u64int_obj, current_control_mode_index, (uint8_t) current_control_mode);
+            // common variables
+            flange_speed_limit_index = TRY_LOAD_VARIABLE_INDEX(double_obj, "flange_speed_limit");
+            flange_acceleration_limit_index = TRY_LOAD_VARIABLE_INDEX(double_obj, "flange_acceleration_limit");
+            flange_jerk_limit_index = TRY_LOAD_VARIABLE_INDEX(double_obj, "flange_jerk_limit");
+            joint_speed_limit_index = TRY_LOAD_VARIABLE_INDEX(array7_obj, "joint_speed_limit");
+            joint_acceleration_limit_index = TRY_LOAD_VARIABLE_INDEX(array7_obj, "joint_acceleration_limit");
+            joint_jerk_limit_index = TRY_LOAD_VARIABLE_INDEX(array7_obj, "joint_jerk_limit");
+            // specific variables
+            switch (current_control_mode) {
+                case control_mode_type::HARD_FLANGE_POSE: {
+                    target_flange_pose_index = LOAD_VARIABLE_INDEX(transform_obj, "flange_pose");
+                    break;
+                }
+                case control_mode_type::HARD_JOINT_ANGLE: {
+                    target_joint_angle_index = LOAD_VARIABLE_INDEX(array7_obj, "joint_angle");
+                    break;
+                }
+                case control_mode_type::NO_CONTROL: {
+                    break;
+                }
+                default: {
+                    assert(false);
+                    return;
+                }
+            }
+        }
+
+        impl() {
+            initialize_translator();
+
+            // initialize static variables
+            for (auto &val: invalid_array7) {
+                val = std::numeric_limits<double>::quiet_NaN();
+            }
+        }
+
+    };
+
+    awaitable<bool> kuka_interface::on_init(const nlohmann::json &config) noexcept {
+        pimpl->load_init_config(config);
+        try {
+            co_await pimpl->on_init_impl();
+        } catch (std::exception &e) {
+            SPDLOG_ERROR("Failed to init KUKA interface: {}", e.what());
+            co_return false;
+        }
+        co_return true;
+    }
+
+    awaitable<bool> kuka_interface::on_start(const nlohmann::json &config) noexcept {
+        // load configs
+        assert(config.contains("output_config"));
+        pimpl->load_output_config(config["output_config"]);
+        assert(config.contains("input_config"));
+        pimpl->load_input_config(config["input_config"]);
+        pimpl->on_start_impl();
+        co_return true;
+    }
+
+    awaitable<void> kuka_interface::on_stop() noexcept {
+        SAFE_RESET_WORKER(pimpl->status_worker)
+        SAFE_RESET_WORKER(pimpl->command_worker)
+        co_return;
+    }
+
+    awaitable<void> kuka_interface::on_reset() noexcept {
+        SAFE_RESET_WORKER(pimpl->kuka_reply_worker)
+        pimpl->kuka_socket.reset();
+        co_return;
+    }
+
+    kuka_interface::kuka_interface()
+            : pimpl(std::make_unique<impl>()) {
+        pimpl->q_this = this;
+    }
+
+    kuka_interface::~kuka_interface() = default;
+
+}

src/robot/ur/ur_defs.h

@@ -0,0 +1,18 @@
+#ifndef SOPHIAR2_UR_DEFS_H
+#define SOPHIAR2_UR_DEFS_H
+
+#include <cstdint>
+
+namespace sophiar {
+
+    enum class ur_control_mode_type : uint8_t {
+        NO_CONTROL = 0,
+        JOINT_ANGLE = 1,
+        JOINT_SPEED = 3,
+        TCP_POSE = 2,
+        TCP_SPEED = 4,
+    };
+
+}
+
+#endif //SOPHIAR2_UR_DEFS_H

src/robot/ur/ur_interface.cpp

@@ -1,6 +1,5 @@
-#include "ur_interface.h"
-
 #include "core/basic_obj_types.hpp"
+#include "robot/ur/ur_defs.h"
 #include "utility/bit_operations.hpp"
 #include "utility/config_utility.hpp"
 #include "utility/coro_worker.hpp"
@@ -30,6 +29,8 @@
 
 #include "utility/assert_utility.h"
 
+DEFAULT_TRISTATE_OBJ_DEF(ur_interface)
+
 namespace sophiar {
 
     using boost::asio::async_read;
@@ -333,14 +334,7 @@ namespace sophiar {
         static Eigen::Isometry3d identity_transform;
         static vector6d_type identity_transform_vec;
 
-        enum class control_mode_type : uint8_t {
-            NO_CONTROL = 0,
-            JOINT_ANGLE = 1,
-            JOINT_SPEED = 3,
-            TCP_POSE = 2,
-            TCP_SPEED = 4,
-        };
-
+        using control_mode_type = ur_control_mode_type;
         name_translator<control_mode_type> control_mode_translator;
 
         static constexpr uint16_t rtde_port = 30004;
@@ -812,6 +806,7 @@ namespace sophiar {
         void load_input_config(const nlohmann::json &config) {
             auto control_mode_str = LOAD_STRING_ITEM("control_mode");
             current_control_mode = control_mode_translator.translate(control_mode_str);
+            UPDATE_VARIABLE_VAL(u64int_obj, current_control_mode_index, (uint8_t) current_control_mode);
             switch (current_control_mode) {
                 case control_mode_type::JOINT_ANGLE: {
                     joint_angle_index = LOAD_VARIABLE_INDEX(array6_obj, "target_joint_q");
@@ -917,10 +912,10 @@ namespace sophiar {
     }
 
     awaitable<bool> ur_interface::on_start(const nlohmann::json &config) noexcept {
-        assert(config.contains("input_config"));
-        pimpl->load_input_config(config["input_config"]);
         assert(config.contains("output_config"));
         pimpl->load_output_config(config["output_config"]);
+        assert(config.contains("input_config"));
+        pimpl->load_input_config(config["input_config"]);
         try {
             CO_ENSURE(pimpl->on_start_impl())
         } catch (std::exception &e) {

src/robot/ur/ur_interface.h

@@ -1,60 +0,0 @@
-#ifndef SOPHIAR2_UR_INTERFACE_H
-#define SOPHIAR2_UR_INTERFACE_H
-
-#include "core/tristate_obj.h"
-
-DEFAULT_TRISTATE_OBJ_DEF(ur_interface)
-
-//namespace sophiar {
-//
-//    namespace ur_runtime_status {
-//        using ur_runtime_status_type = uint32_t;
-//        static constexpr ur_runtime_status_type STOPPING = 0;
-//        static constexpr ur_runtime_status_type STOPPED = 1;
-//        static constexpr ur_runtime_status_type PLAYING = 2;
-//        static constexpr ur_runtime_status_type PAUSING = 3;
-//        static constexpr ur_runtime_status_type PAUSED = 4;
-//        static constexpr ur_runtime_status_type RESUMING = 5;
-//    }
-//
-//    namespace ur_robot_mode {
-//        using ur_robot_mode_type = int32_t;
-//        static constexpr ur_robot_mode_type NO_CONTROLLER = -1;
-//        static constexpr ur_robot_mode_type DISCONNECTED = 0;
-//        static constexpr ur_robot_mode_type CONFIRM_SAFETY = 1;
-//        static constexpr ur_robot_mode_type BOOTING = 2;
-//        static constexpr ur_robot_mode_type POWER_OFF = 3;
-//        static constexpr ur_robot_mode_type POWER_ON = 4;
-//        static constexpr ur_robot_mode_type IDLE = 5;
-//        static constexpr ur_robot_mode_type BACKDRIVE = 6;
-//        static constexpr ur_robot_mode_type RUNNING = 7;
-//        static constexpr ur_robot_mode_type UPDATING_FIRMWARE = 8;
-//    }
-//
-//    namespace ur_safety_mode {
-//        using ur_safety_mode_type = int32_t;
-//        static constexpr ur_safety_mode_type NORMAL = 1;
-//        static constexpr ur_safety_mode_type REDUCED = 2;
-//        static constexpr ur_safety_mode_type PROTECTIVE_STOP = 3;
-//        static constexpr ur_safety_mode_type RECOVERY = 4;
-//        static constexpr ur_safety_mode_type SAFEGUARD_STOP = 5;
-//        static constexpr ur_safety_mode_type SYSTEM_EMERGENCY_STOP = 6;
-//        static constexpr ur_safety_mode_type ROBOT_EMERGENCY_STOP = 7;
-//        static constexpr ur_safety_mode_type VIOLATION = 8;
-//        static constexpr ur_safety_mode_type FAULT = 9;
-//        static constexpr ur_safety_mode_type VALIDATE_JOINT_ID = 10;
-//        static constexpr ur_safety_mode_type UNDEFINED_SAFETY_MODE = 11;
-//    }
-//
-//    namespace ur_move_mode {
-//        using ur_move_mode_type = int32_t;
-//        static constexpr ur_move_mode_type NO_MOTION = 0;
-//        static constexpr ur_move_mode_type JOINT_ANGLE = 1;
-//        static constexpr ur_move_mode_type JOINT_SPEED = 2;
-//        static constexpr ur_move_mode_type TOOL_POSE = 3;
-//        static constexpr ur_move_mode_type TOOL_SPEED = 4;
-//    }
-//
-
-
-#endif //SOPHIAR2_UR_INTERFACE_H

src/utility/config_utility.hpp

@@ -81,9 +81,4 @@
     var_name_out = LOAD_STRING_ITEM(var_name); \
     REQUIRE_VARIABLE(var_type, var_name_out); })
 
-#define SAFE_CLOSE_CORO_WORKER(worker) \
-    worker->cancel(); \
-    co_await worker->coro_wait_stop(); \
-    worker = nullptr;
-
 #endif //SOPHIAR2_CONFIG_UTILITY_HPP

src/utility/coro_unique_lock.hpp

@@ -0,0 +1,81 @@
+#ifndef SOPHIAR2_CORO_UNIQUE_LOCK_HPP
+#define SOPHIAR2_CORO_UNIQUE_LOCK_HPP
+
+#include "utility/coro_signal2.hpp"
+
+#include "utility/assert_utility.h"
+
+namespace sophiar {
+
+    class coro_unique_lock : private boost::noncopyable {
+    public:
+        coro_unique_lock() = default;
+
+        bool can_lock() const {
+            return !is_locked;
+        }
+
+        bool try_lock() {
+            if (is_locked) return false;
+            is_locked = true;
+            return true;
+        }
+
+        boost::asio::awaitable<void> coro_lock() {
+            if (try_lock()) co_return;
+            auto watcher = sig.new_watcher();
+            for (;;) {
+                if (try_lock()) co_return;
+                co_await watcher.coro_wait();
+            }
+        }
+
+        void release_lock() {
+            assert(is_locked);
+            is_locked = false;
+            sig.try_notify_all();
+        }
+
+    private:
+        coro_signal2 sig;
+        bool is_locked = false;
+
+        friend class coro_lock_guard;
+
+        boost::asio::awaitable<void> coro_require_lock(signal_watcher &watcher) {
+            for (;;) {
+                if (try_lock()) co_return;
+                co_await watcher.coro_wait();
+            }
+        }
+    };
+
+    class coro_lock_guard : private boost::noncopyable {
+    public:
+        explicit coro_lock_guard(coro_unique_lock &_lock)
+                : lock(std::addressof(_lock)),
+                  watcher(_lock.sig.new_watcher()) {}
+
+        coro_lock_guard(coro_lock_guard &&) = delete;
+
+        ~coro_lock_guard() {
+            if (is_locked) {
+                lock->release_lock();
+            }
+        }
+
+        boost::asio::awaitable<void> coro_require_lock() {
+            co_await lock->coro_require_lock(watcher);
+            is_locked = true;
+            co_return;
+        }
+
+    private:
+        bool is_locked = false;
+        coro_unique_lock *lock;
+        signal_watcher watcher;
+    };
+
+}
+
+#endif //SOPHIAR2_CORO_UNIQUE_LOCK_HPP

src/utility/debug_utility.hpp

@@ -17,19 +17,15 @@
 #include "utility/assert_utility.h"
 
 #define DEBUG_PRINT(msg) \
-    std::cout << fmt::format("F:{} L:{} {}", \
-                             __FILE__, __LINE__, msg) \
+    std::cout << fmt::format("F:{} L:{} T:{} {}", \
+                             __FILE__, __LINE__, current_timestamp(), msg) \
               << std::endl
 
-#define FILE_LINE_TRACE \
-    { \
-        auto __msg = fmt::format("T:{}", current_timestamp()); \
-        DEBUG_PRINT(__msg); \
-    }
+#define FILE_LINE_TRACE DEBUG_PRINT("");
 
 #define FILE_LINE_TRACE_WITH_THIS \
     { \
-        auto __msg = fmt::format("T:{} TH:{}", current_timestamp(), (void *) this); \
+        auto __msg = fmt::format("TH:{}", (void *) this); \
         DEBUG_PRINT(__msg); \
     }
 

src/utility/variable_helper.hpp

@@ -161,6 +161,19 @@ namespace sophiar {
         co_return;
     }
 
+    template<typename SmallObjType>
+    boost::asio::awaitable<void> coro_wait_for_not_empty(variable_index_type var_index) {
+        auto var_helper = VARIABLE_MANUAL_DELEGATE(SmallObjType, var_index);
+        var_helper.manual_sync();
+        for (;;) {
+            if (!var_helper.empty()) {
+                break;
+            }
+            co_await var_helper.coro_wait_update();
+        }
+        co_return;
+    }
+
 }
 
 #endif //SOPHIAR2_VARIABLE_HELPER_HPP

src/utility/versatile_buffer2.hpp

@@ -24,7 +24,14 @@
 namespace sophiar {
 
     template<size_t Length>
-    struct static_memory : private boost::noncopyable {
+    struct static_memory {
+
+        static_memory() = default;
+
+        static_memory(std::initializer_list<char> l) {
+            assert(l.size() == Length);
+            std::copy_n(l.begin(), Length, buf.begin());
+        }
 
         char *data() { return buf.data(); }
 
@@ -215,6 +222,14 @@ namespace sophiar {
             str = read_string_until(sep);
         }
 
+        template<WriteableMemory MemType>
+        void read_memory(MemType &mem) {
+            auto end_pos = pos + mem.size();
+            assert(end_pos <= buf.end());
+            std::copy_n(pos, mem.size(), mem.data());
+            pos = end_pos;
+        }
+
         template<typename T>
         auto &operator>>(T &val) {
             read_value(val);
@@ -283,6 +298,13 @@ namespace sophiar {
             }
         }
 
+        template<ReadableMemory MemType>
+        void write_memory(const MemType &mem) {
+            assert(pos + mem.size() <= buf.end());
+            std::copy_n(mem.data(), mem.size(), pos);
+            pos += mem.size();
+        }
+
         template<typename T>
         auto &operator<<(const T &val) {
             write_value(val);

tests/CMakeLists.txt

@@ -37,7 +37,7 @@ target_compile_definitions(test_robot PUBLIC SOPHIAR_TEST SOPHIAR_TEST_ROBOT)
 target_compile_definitions(test_robot PUBLIC CORO_SIGNAL2_USE_TIMER)
 target_link_libraries(test_robot ${Boost_LIBRARIES} ${EXTRA_LIBS})
 
-file(GLOB_RECURSE TEST_NDI_SRC_FILES ./interface/ndi*.cpp)
+file(GLOB_RECURSE TEST_NDI_SRC_FILES ./tracker/ndi*.cpp)
 add_executable(test_ndi
         ${TEST_NDI_SRC_FILES}
         ../src/tracker/ndi/ndi_interface.cpp

tests/data/kuka_interface_config.json

@@ -0,0 +1,134 @@
+{
+  "controller_port": 5277,
+  "variable_list": [
+    {
+      "name": "kuka_joint_angle",
+      "type": "array7_obj"
+    },
+    {
+      "name": "kuka_joint_torque",
+      "type": "array7_obj"
+    },
+    {
+      "name": "robot_flange_pose",
+      "type": "transform_obj"
+    },
+    {
+      "name": "robot_flange_force",
+      "type": "scalarxyz_obj"
+    },
+    {
+      "name": "robot_flange_torque",
+      "type": "scalarxyz_obj"
+    },
+    {
+      "name": "robot_flange_force_uncertainty",
+      "type": "scalarxyz_obj"
+    },
+    {
+      "name": "robot_flange_torque_uncertainty",
+      "type": "scalarxyz_obj"
+    },
+    {
+      "name": "robot_is_moving",
+      "type": "bool_obj"
+    },
+    {
+      "name": "kuka_control_mode",
+      "type": "u64int_obj"
+    },
+    {
+      "name": "target_flange_pose",
+      "type": "transform_obj"
+    },
+    {
+      "name": "target_joint_angle",
+      "type": "array7_obj"
+    },
+    {
+      "name": "robot_flange_speed_limit",
+      "type": "double_obj"
+    },
+    {
+      "name": "robot_flange_acceleration_limit",
+      "type": "double_obj"
+    },
+    {
+      "name": "robot_flange_jerk_limit",
+      "type": "double_obj"
+    },
+    {
+      "name": "kuka_joint_speed_limit",
+      "type": "array7_obj"
+    },
+    {
+      "name": "kuka_joint_acceleration_limit",
+      "type": "array7_obj"
+    },
+    {
+      "name": "kuka_joint_jerk_limit",
+      "type": "array7_obj"
+    }
+  ],
+  "object_list": [
+    {
+      "type": "kuka_interface",
+      "name": "kuka",
+      "init_config": {
+        "address": "10.0.0.10"
+      },
+      "start_config": {
+        "output_config": {
+          "report_frequency": 1,
+          "joint_info": {
+            "angle": "kuka_joint_angle",
+            "torque": "kuka_joint_torque"
+          },
+          "flange_info": {
+            "pose": "robot_flange_pose",
+            "force": "robot_flange_force",
+            "torque": "robot_flange_torque",
+            "force_uncertainty": "robot_flange_force_uncertainty",
+            "torque_uncertainty": "robot_flange_torque_uncertainty"
+          },
+          "is_moving": "robot_is_moving",
+          "current_control_mode": "kuka_control_mode"
+        },
+        "input_config": {
+          "control_mode": "hard_flange_pose",
+          "flange_pose": "target_flange_pose",
+          "joint_angle": "target_joint_angle",
+          "flange_speed_limit": "robot_flange_speed_limit",
+          "flange_acceleration_limit": "robot_flange_acceleration_limit",
+          "flange_jerk_limit": "robot_flange_jerk_limit",
+          "joint_speed_limit": "kuka_joint_speed_limit",
+          "joint_acceleration_limit": "kuka_joint_acceleration_limit",
+          "joint_jerk_limit": "kuka_joint_jerk_limit"
+        }
+      }
+    },
+    {
+      "type": "transform_obj_watcher",
+      "name": "flange_pose_watcher",
+      "start_config": {
+        "variable_name": "robot_flange_pose"
+      }
+    },
+    {
+      "type": "bool_obj_watcher",
+      "name": "is_moving_watcher",
+      "start_config": {
+        "variable_name": "robot_is_moving"
+      }
+    },
+    {
+      "type": "empty_object",
+      "name": "all",
+      "dependencies": [
+        "kuka",
+        "flange_pose_watcher",
+        "is_moving_watcher"
+      ]
+    }
+  ]
+}

tests/robot/kuka_interface.cpp

@@ -0,0 +1,87 @@
+#define BOOST_TEST_DYN_LINK
+
+#include "core/basic_obj_types.hpp"
+#include "core/global_defs.h"
+#include "core/sophiar_pool.h"
+#include "utility/debug_utility.hpp"
+#include "utility/variable_helper.hpp"
+
+#include <boost/asio/co_spawn.hpp>
+#include <boost/asio/detached.hpp>
+#include <boost/test/unit_test.hpp>
+
+#include <spdlog/spdlog.h>
+
+#include <fstream>
+#include <numbers>
+
+using namespace sophiar;
+using namespace std::chrono_literals;
+
+using boost::asio::co_spawn;
+using boost::asio::awaitable;
+using boost::asio::detached;
+
+awaitable<void> test_kuka_joint_angle_control() {
+    auto joint_angle_index = REQUIRE_VARIABLE(array7_obj, "kuka_joint_angle");
+    auto joint_angle = VARIABLE_AUTO_DELEGATE(array7_obj, joint_angle_index);
+    auto var_index = REQUIRE_VARIABLE(bool_obj, "robot_is_moving");
+    auto target_index = REQUIRE_VARIABLE(array7_obj, "target_joint_angle");
+    co_await coro_wait_for_not_empty<array7_obj>(joint_angle_index);
+    BOOST_TEST(QUERY_VARIABLE(bool_obj, var_index)->value == false);
+    auto next_joint_angle = array7_obj::new_instance();
+    next_joint_angle->value = joint_angle->value;
+    next_joint_angle->value[6] += std::numbers::pi / 4;
+    UPDATE_VARIABLE(array7_obj, target_index, next_joint_angle);
+    co_await coro_wait_for_variable_value<bool_obj>(var_index, true);
+    BOOST_TEST(QUERY_VARIABLE(bool_obj, var_index)->value == true);
+    co_await coro_wait_for_variable_value<bool_obj>(var_index, false);
+    BOOST_TEST(QUERY_VARIABLE(bool_obj, var_index)->value == false);
+    co_await joint_angle.coro_wait_update();
+    next_joint_angle = array7_obj::new_instance();
+    next_joint_angle->value = joint_angle->value;
+    next_joint_angle->value[6] -= std::numbers::pi / 4;
+    UPDATE_VARIABLE(array7_obj, target_index, next_joint_angle);
+    co_await coro_wait_for_variable_value<bool_obj>(var_index, true);
+    BOOST_TEST(QUERY_VARIABLE(bool_obj, var_index)->value == true);
+    co_await coro_wait_for_variable_value<bool_obj>(var_index, false);
+    BOOST_TEST(QUERY_VARIABLE(bool_obj, var_index)->value == false);
+    co_return;
+}
+
+awaitable<void> test_kuka_flange_pose_control() {
+    auto flange_pose_index = REQUIRE_VARIABLE(transform_obj, "robot_flange_pose");
+    auto flange_pose = VARIABLE_AUTO_DELEGATE(transform_obj, flange_pose_index);
+    auto var_index = REQUIRE_VARIABLE(bool_obj, "robot_is_moving");
+    auto target_index = REQUIRE_VARIABLE(transform_obj, "target_flange_pose");
+    co_await coro_wait_for_not_empty<transform_obj>(flange_pose_index);
+    BOOST_TEST(QUERY_VARIABLE(bool_obj, var_index)->value == false);
+    auto next_flange_pose = transform_obj::new_instance();
+    next_flange_pose->value = flange_pose->value;
+    next_flange_pose->value.translation().z() += 30;
+    UPDATE_VARIABLE(transform_obj, target_index, next_flange_pose);
+    co_await coro_wait_for_variable_value<bool_obj>(var_index, true);
+    BOOST_TEST(QUERY_VARIABLE(bool_obj, var_index)->value == true);
+    co_await coro_wait_for_variable_value<bool_obj>(var_index, false);
+    BOOST_TEST(QUERY_VARIABLE(bool_obj, var_index)->value == false);
+    co_await flange_pose.coro_wait_update();
+    next_flange_pose = transform_obj::new_instance();
+    next_flange_pose->value = flange_pose->value;
+    next_flange_pose->value.translation().z() -= 30;
+    UPDATE_VARIABLE(transform_obj, target_index, next_flange_pose);
+    co_await coro_wait_for_variable_value<bool_obj>(var_index, true);
+    BOOST_TEST(QUERY_VARIABLE(bool_obj, var_index)->value == true);
+    co_await coro_wait_for_variable_value<bool_obj>(var_index, false);
+    BOOST_TEST(QUERY_VARIABLE(bool_obj, var_index)->value == false);
+    co_return;
+}
+
+BOOST_AUTO_TEST_CASE(test_kuka_interface) {
+    spdlog::set_level(spdlog::level::trace);
+    std::ifstream config_file("data/kuka_interface_config.json");
+    BOOST_TEST(config_file.is_open());
+    auto config = nlohmann::json::parse(config_file);
+    BOOST_TEST(initialize(config));
+    co_spawn(*global_context, test_kuka_flange_pose_control(), detached);
+    global_context->run();
+}

tests/robot/ur_interface.cpp

@@ -4,7 +4,6 @@
 #include "core/basic_obj_types.hpp"
 #include "core/global_defs.h"
 #include "core/sophiar_pool.h"
-#include "robot/ur/ur_interface.h"
 #include "utility/debug_utility.hpp"
 #include "utility/variable_helper.hpp"
 

tests/utility/coro_unique_lock.cpp

@@ -0,0 +1,49 @@
+#define BOOST_TEST_DYN_LINK
+
+#include "core/sophiar_obj.hpp"
+#include "utility/coro_unique_lock.hpp"
+#include "utility/debug_utility.hpp"
+
+#include <boost/asio/co_spawn.hpp>
+#include <boost/asio/detached.hpp>
+#include <boost/asio/this_coro.hpp>
+#include <boost/asio/use_awaitable.hpp>
+#include <boost/test/unit_test.hpp>
+
+#include <chrono>
+#include <iostream>
+#include <vector>
+
+using boost::asio::awaitable;
+using boost::asio::co_spawn;
+using boost::asio::detached;
+using boost::asio::use_awaitable;
+
+using namespace sophiar;
+using namespace std::chrono_literals;
+
+coro_unique_lock *lock;
+
+awaitable<void> unlocked_worker(int id) {
+    DEBUG_PRINT(fmt::format("Worker {} starts to work.", id));
+    co_await coro_sleep(1s);
+    DEBUG_PRINT(fmt::format("Worker {} finished work.", id));
+}
+
+awaitable<void> locked_worker(int id) {
+    auto lock_guard = coro_lock_guard{*lock};
+    co_await lock_guard.coro_require_lock();
+    DEBUG_PRINT(fmt::format("Worker {} starts to work.", id));
+    co_await coro_sleep(1s);
+    DEBUG_PRINT(fmt::format("Worker {} finished work.", id));
+}
+
+BOOST_AUTO_TEST_CASE(test_coro_unique_lock) {
+    initialize({});
+
+    lock = new coro_unique_lock{};
+    for (int i = 0; i < 5; ++i) {
+        co_spawn(*global_context, locked_worker(i), detached);
+    }
+    global_context->run();
+}