实现并测试了 ur_interface

CMakeLists.txt

@@ -27,8 +27,9 @@ list(APPEND EXTRA_LIBS Eigen3::Eigen)
 find_package(nlohmann_json REQUIRED)
 list(APPEND EXTRA_LIBS nlohmann_json::nlohmann_json)
 
-file(GLOB_RECURSE CORE_IMPL_FILES ./src/core/*.cpp)
 file(GLOB_RECURSE ALGORITHM_IMPL_FILES ./src/algorithm/*.cpp)
+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})

src/core/global_defs.cpp

@@ -4,6 +4,7 @@
 #include "core/sophiar_manager.h"
 #include "core/sophiar_pool.h"
 #include "core/timestamp_helper.hpp"
+#include "robot/ur/ur_interface.h"
 #include "utility/debug_utility.hpp"
 #include "utility/dynamic_pool.hpp"
 #include "utility/variable_utility.hpp"
@@ -44,12 +45,24 @@ namespace sophiar {
 #undef REGISTER_VARIABLE_TYPE
     }
 
+    struct empty_object : public tristate_obj {
+        DEFAULT_NEW_INSTANCE(empty_object);
+    };
+
     void register_object_types() {
 
+        REGISTER_TYPE(empty_object);
+
 #if !SOPHIAR_TEST || SOPHIAR_TEST_ALGORITHM
 
         REGISTER_TYPE(transform_tree);
 
+#endif
+
+#if !SOPHIAR_TEST || SOPHIAR_TEST_ROBOT
+
+        REGISTER_TYPE(ur_interface);
+
 #endif
 
         register_variable_utility_types();

src/core/global_defs.h

@@ -43,10 +43,10 @@ namespace sophiar {
     global_sophiar_pool->require_variable_watcher(var_index)
 
 #define UPDATE_VARIABLE(var_type, var_index, val_ptr) \
-    global_sophiar_pool->update_variable<var_type>(var_index, val_ptr)
+    global_sophiar_pool->update_variable<var_type>(var_index, std::move(val_ptr))
 
 #define UPDATE_VARIABLE_WITH_TS(var_type, var_index, val_ptr, ts) \
-    global_sophiar_pool->update_variable<var_type>(var_index, val_ptr, ts)
+    global_sophiar_pool->update_variable<var_type>(var_index, std::move(val_ptr), ts)
 
 #define UPDATE_VARIABLE_VAL(var_type, var_index, val) \
     global_sophiar_pool->update_variable<var_type>(var_index, var_type::new_instance(val))

src/core/sophiar_pool.cpp

@@ -185,10 +185,7 @@ namespace sophiar {
                                                           timestamp_type *ts_out) {
         assert(pimpl->variable_pool.contains(var_index));
         const auto &info = pimpl->variable_pool[var_index];
-        if (info.type_info->type != var_type) { // ensure type consistency
-            assert(false);
-            return nullptr;
-        }
+        assert(info.type_info->type == var_type); // ensure type consistency
         if (ts_out != nullptr) {
             *ts_out = info.last_update_ts;
         }

src/core/sophiar_pool.h

@@ -33,13 +33,13 @@ namespace sophiar {
 
         template<typename SmallObjType>
         void update_variable(variable_index_type var_index,
-                             const typename SmallObjType::pointer &value,
+                             typename SmallObjType::pointer &&value,
                              timestamp_type ts = current_timestamp()) {
             using ptr_type = typename SmallObjType::pointer;
             auto placeholder = require_variable_placeholder_impl(var_index, typeid(SmallObjType), nullptr);
             auto &inner_ptr = *static_cast<ptr_type *>(placeholder);
             if (inner_ptr == nullptr && value == nullptr) return; // nullptr value will not be duplicated
-            inner_ptr = value;
+            inner_ptr = std::move(value);
             update_variable_timestamp_impl(var_index, ts);
         }
 

src/core/tristate_obj.cpp

@@ -1,6 +1,7 @@
 #include "tristate_obj.h"
 #include "core/global_defs.h"
 #include "utility/coro_signal2.hpp"
+#include "utility/debug_utility.hpp"
 #include "utility/name_translator.hpp"
 
 #include <spdlog/spdlog.h>
@@ -30,10 +31,7 @@ namespace sophiar {
         coro_signal2 reset_finished_signal;
 
         static void initialize_translator() {
-            static bool is_called = false;
-            if (is_called) return;
-            is_called = true;
-
+            RUN_ONCE
             state_name_translator = new name_translator<uint8_t>;
             state_name_translator->register_item("Initial", (uint8_t) tristate_obj::state_type::INITIAL);
             state_name_translator->register_item("Initializing", (uint8_t) tristate_obj::state_type::INITIALIZING);
@@ -46,7 +44,7 @@ namespace sophiar {
         }
 
         void log_state_change(state_type old_state, state_type new_state) const {
-            SPDLOG_TRACE("Object [name = {}], State: {} => {}.",
+            SPDLOG_INFO("Object [name = {}], State: {} => {}.",
                          QUERY_OBJECT_NAME(q_this),
                          state_name_translator->translate((uint8_t) old_state),
                          state_name_translator->translate((uint8_t) new_state));

src/robot/ur/ur_interface.cpp

@@ -1,12 +1,15 @@
 #include "ur_interface.h"
 
 #include "core/basic_obj_types.hpp"
+#include "utility/bit_operations.hpp"
+#include "utility/config_utility.hpp"
 #include "utility/coro_worker.hpp"
 #include "utility/coro_worker_helper_func.hpp"
+#include "utility/debug_utility.hpp"
+#include "utility/name_translator.hpp"
+#include "utility/variable_helper.hpp"
 #include "utility/versatile_buffer2.hpp"
 
-#include <boost/asio/experimental/awaitable_operators.hpp>
-#include <boost/asio/experimental/channel.hpp>
 #include <boost/asio/ip/address.hpp>
 #include <boost/asio/ip/tcp.hpp>
 #include <boost/asio/co_spawn.hpp>
@@ -23,6 +26,7 @@
 #include <cassert>
 #include <exception>
 #include <string>
+#include <string_view>
 #include <vector>
 
 namespace sophiar {
@@ -43,6 +47,11 @@ namespace sophiar {
 
     struct ur_interface::impl {
 
+        using packet_size_type = uint16_t;
+        using packet_type_type = uint8_t;
+        static constexpr auto packet_size_offset = sizeof(packet_size_type);
+        static constexpr auto packet_header_offset = packet_size_offset + sizeof(packet_type_type);
+
         // Header types
         static constexpr uint8_t RTDE_REQUEST_PROTOCOL_VERSION = 86;
         static constexpr uint8_t RTDE_GET_URCONTROL_VERSION = 118;
@@ -53,30 +62,52 @@ namespace sophiar {
         static constexpr uint8_t RTDE_CONTROL_PACKAGE_START = 83;
         static constexpr uint8_t RTDE_CONTROL_PACKAGE_PAUSE = 80;
 
-        struct string_list {
-            using container_type = std::vector<std::string>;
-
-            container_type elements;
-
-            string_list() = default;
-
-            explicit string_list(const container_type &other)
-                    : elements(other) {}
-
-            explicit string_list(container_type &&other)
-                    : elements(std::move(other)) {}
+        static constexpr std::string_view input_variable_list[] = {
+                "input_int_register_24", // Move Type
+                "input_int_register_25", // Cmd Id
+                "input_double_register_24", // 6 个参数表示 TCP Pose
+                "input_double_register_25",
+                "input_double_register_26",
+                "input_double_register_27",
+                "input_double_register_28",
+                "input_double_register_29",
+                "input_double_register_30", // 6 个参数表示 Target
+                "input_double_register_31",
+                "input_double_register_32",
+                "input_double_register_33",
+                "input_double_register_34",
+                "input_double_register_35",
+                "input_double_register_36", // 加速度参数
+                "input_double_register_37", // 速度参数
+        };
+        static constexpr std::string_view output_variable_list[] = {
+                "target_q", // VECTOR6D
+                "target_qd", // VECTOR6D
+                "target_qdd", // VECTOR6D
+                "actual_q", // VECTOR6D
+                "actual_qd", // VECTOR6D
+                "actual_TCP_pose", // VECTOR6D
+                "actual_TCP_speed", // VECTOR6D
+                "target_TCP_pose", // VECTOR6D
+                "target_TCP_speed", // VECTOR6D
+                "timestamp", // DOUBLE
+                "robot_mode", // INT32
+                "runtime_state", // UINT32
+                "safety_mode", // INT32
+                "output_int_register_24", // INT32, last cmd Id
+                "output_bit_register_64", // BOOL, is moving
+        };
 
-            template<typename WriterType>
-            void write_to(WriterType &writer) const {
-                auto n = elements.size();
-                for (const auto &elem: elements) {
-                    writer << elem;
-                    if ((--n) != 0) {
-                        writer << ',';
-                    }
+        template<typename WriterType, typename StringListType>
+        static void write_string_list_to(WriterType &writer, const StringListType &str_list) {
+            auto n = std::size(str_list);
+            for (const auto &str: str_list) {
+                writer << str;
+                if ((--n) != 0) {
+                    writer << ',';
                 }
             }
-        };
+        }
 
         struct empty_content {
             template<typename WriterType>
@@ -86,6 +117,11 @@ namespace sophiar {
         struct rtde_packet_header {
             uint16_t packet_size;
             uint8_t packet_type;
+
+            template<typename ReaderType>
+            void fill_from(ReaderType &reader) {
+                reader >> packet_size >> packet_type;
+            }
         };
 
         struct rtde_request_protocol_version_content {
@@ -168,11 +204,10 @@ namespace sophiar {
 
         struct rtde_setup_inputs_content {
             static constexpr auto packet_type = RTDE_CONTROL_PACKAGE_SETUP_INPUTS;
-            string_list variable_names;
 
             template<typename WriterType>
             void write_to(WriterType &writer) const {
-                variable_names.write_to(writer);
+                write_string_list_to(writer, input_variable_list);
             }
         };
 
@@ -185,7 +220,7 @@ namespace sophiar {
             void fill_from(ReaderType &reader) {
                 reader >> output_recipe_id;
                 while (!reader.empty()) {
-                    variable_types.push_back(reader.read_string_until(','));
+                    variable_types.emplace_back(reader.read_string_until(','));
                 }
             }
         };
@@ -193,12 +228,11 @@ namespace sophiar {
         struct rtde_setup_outputs_content {
             static constexpr auto packet_type = RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS;
             double output_frequency;
-            string_list variable_names;
 
             template<typename WriterType>
             void write_to(WriterType &writer) const {
                 writer << output_frequency;
-                variable_names.write_to(writer);
+                write_string_list_to(writer, output_variable_list);
             }
         };
 
@@ -229,7 +263,7 @@ namespace sophiar {
 
         using vector6d_type = array6_obj::value_type;
 
-        struct ur_status_content {
+        struct ur_status_data {
             vector6d_type target_q, target_qd, target_qdd;
             vector6d_type actual_q, actual_qd;
             vector6d_type actual_TCP_pose, actual_TCP_speed;
@@ -256,166 +290,143 @@ namespace sophiar {
             }
         };
 
-        struct ur_status_packet : public rtde_data_package_content<ur_status_content>,
-                                  public small_obj<ur_status_packet> {
-        };
+        struct ur_command_data {
+            int32_t move_type, cmd_id;
+            vector6d_type tcp_pose, target;
+            double param_a, param_v;
 
-//
-//        struct ur_command_wrapper : public ur_command_content {
-//
-//            static constexpr uint16_t length() {
-//                return sizeof(int32_t) * 2 +
-//                       sizeof(Vector6d) * 2 +
-//                       sizeof(double) * 2;
-//            }
-//
-//            void write_to_buffer(versatile_buffer &buffer) const {
-//                buffer << move_type << cmd_id;
-//                buffer << tcp_pose << target;
-//                buffer << param_a << param_v;
-//            }
-//        };
-//
-//        using ur_command_signal_type = tiny_signal<ur_command::pointer>;
-//        using ur_status_signal_type = tiny_signal<ur_status::pointer>;
-//        using ur_command_slot_type = ur_command_signal_type::slot_type;
-//
-//        struct ur_command_slot_impl_type : public ur_command_slot_type {
-//            impl *p_this = nullptr;
-//
-//            void on_signal_received(ur_command::pointer cmd) override {
-//                assert(p_this != nullptr);
-//                assert(p_this->q_this->get_state() == state_type::RUNNING);
-//                auto ok = p_this->ur_command_queue.try_send(error_code{}, std::move(cmd));
-//                if (!ok) {
-//                    SPDLOG_WARN("Command queue is full.");
-//                    // TODO show log, cannot handle
-//                }
-//            }
-//        };
-
-        std::vector<std::string> input_variable_list = {
-                "input_int_register_24", // Move Type
-                "input_int_register_25", // Cmd Id
-                "input_double_register_24", // 6 个参数表示 TCP Pose
-                "input_double_register_25",
-                "input_double_register_26",
-                "input_double_register_27",
-                "input_double_register_28",
-                "input_double_register_29",
-                "input_double_register_30", // 6 个参数表示 Target
-                "input_double_register_31",
-                "input_double_register_32",
-                "input_double_register_33",
-                "input_double_register_34",
-                "input_double_register_35",
-                "input_double_register_36", // 加速度参数
-                "input_double_register_37", // 速度参数
+            template<typename WriterType>
+            void write_to(WriterType &writer) const {
+                writer << move_type << cmd_id;
+                writer << tcp_pose << target;
+                writer << param_a << param_v;
+            }
         };
-        std::vector<std::string> output_variable_list = {
-                "target_q", // VECTOR6D
-                "target_qd", // VECTOR6D
-                "target_qdd", // VECTOR6D
-                "actual_q", // VECTOR6D
-                "actual_qd", // VECTOR6D
-                "actual_TCP_pose", // VECTOR6D
-                "actual_TCP_speed", // VECTOR6D
-                "target_TCP_pose", // VECTOR6D
-                "target_TCP_speed", // VECTOR6D
-                "timestamp", // DOUBLE
-                "robot_mode", // INT32
-                "runtime_state", // UINT32
-                "safety_mode", // INT32
-                "output_int_register_24", // INT32, last cmd Id
-                "output_bit_register_64", // BOOL, is moving
+
+        using ur_status_content = rtde_data_package_content<ur_status_data>;
+        using ur_command_content = rtde_data_package_content<ur_command_data>;
+
+        static void vector6d_to_transform(const vector6d_type &vec, Eigen::Isometry3d &trans) {
+            auto translate = Eigen::Translation3d(vec[0], vec[1], vec[2]);
+            auto rotateAxis = Eigen::Vector3d(vec[3], vec[4], vec[5]);
+            auto rotateAngle = rotateAxis.norm();
+            rotateAxis /= rotateAngle;
+            auto rotation = Eigen::AngleAxisd(rotateAngle, rotateAxis);
+            trans = translate * rotation;
+        }
+
+        static void transform_to_vector6d(const Eigen::Isometry3d &trans, vector6d_type &vec) {
+            auto transPart = trans.translation();
+            for (int i = 0; i < 3; ++i) {
+                vec[i] = transPart(i);
+            }
+            auto rotPart = Eigen::AngleAxisd(trans.rotation());
+            auto rotAngle = rotPart.angle();
+            auto rotAxis = rotPart.axis();
+            rotAxis *= rotAngle;
+            for (int i = 0; i < 3; ++i) {
+                vec[i + 3] = rotAxis(i);
+            }
+        }
+
+        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,
         };
 
+        name_translator<control_mode_type> control_mode_translator;
+
         static constexpr uint16_t rtde_port = 30004;
         static constexpr uint16_t urscript_port = 30002;
         static constexpr uint16_t rtde_protocol_version = 2;
         static constexpr auto urscript_path = "scripts/URScript.script";
 
         static constexpr auto ur_endian = boost::endian::order::big;
-        static constexpr size_t max_packet_size = 1024;
-        using packet_content_type = static_memory<max_packet_size>;
-        struct packet_type : public packet_content_type,
-                             public small_obj<packet_type> {
-        };
+
+        static constexpr double joint_acceleration_default = 1.4; // rad/s^2
+        static constexpr double tcp_acceleration_default = 1.2; // m/s^2
+        static constexpr double joint_speed_default = 1.05; // rad/s
+        static constexpr double tcp_speed_default = 0.25; // m/s
 
         ur_interface *q_this = nullptr;
 
         double report_frequency = 125;
         uint8_t inputs_recipe_id = 0, outputs_recipe_id = 0;
 
+        control_mode_type current_control_mode = control_mode_type::NO_CONTROL;
+        int32_t current_cmd_id = 0;
+
         using ip_address_type = boost::asio::ip::address;
         ip_address_type ur_ip;
         scoped_ptr<tcp::socket> ur_socket; // https://stackoverflow.com/questions/3062803/
 
+        // output variables
+        variable_index_type target_q_index = -1, target_qd_index = -1, target_qdd_index = -1;
+        variable_index_type actual_q_index = -1, actual_qd_index = -1;
+        variable_index_type target_pose_index = -1, target_speed_index = -1;
+        variable_index_type actual_pose_index = -1, actual_speed_index = -1;
+        variable_index_type is_controllable_index = -1, is_moving_index = -1;
+        variable_index_type current_control_mode_index = -1;
+
+        // input variables
+        variable_index_type tcp_offset_index = -1, tcp_pose_index = -1, tcp_speed_index = -1;
+        variable_index_type tcp_speed_limit_index = -1, tcp_acceleration_limit_index = -1;
+        variable_index_type joint_angle_index = -1, joint_speed_index = -1;
+        variable_index_type joint_speed_limit_index = -1, joint_acceleration_limit_index = -1;
+
         coro_worker::pointer status_worker;
         coro_worker::pointer command_worker;
 
         template<typename ContentType>
         awaitable<void> send_rtde_packet(const ContentType &content) {
-            using writer_type = versatile_writable_buffer<ur_endian, extern_memory>;
-            auto packet = packet_type::new_instance();
-            auto writer = writer_type(extern_memory(*packet));
-
-            writer.manual_offset(sizeof(uint16_t)); // skip for packet size
-            writer << content.packet_type;
+            PREVENT_TWICE_ENTER
+            static auto buf = dynamic_memory::new_instance();
+            auto writer = dynamic_memory_writer<ur_endian>(buf.get(), packet_size_offset);
+            writer << (uint8_t) content.packet_type;
             content.write_to(writer);
-
-            // fill packet size
-            auto &packet_size = *reinterpret_cast<uint16_t *>(packet->data());
-            assert(writer.get_cur_pos() <= std::numeric_limits<uint16_t>::max());
-            packet_size = writer.get_cur_pos();
-            swap_net_loc_endian<ur_endian>(packet_size);
-
-            // write packet
+            assert(buf->size() <= std::numeric_limits<packet_size_type>::max());
+            write_binary_value<ur_endian>(buf->data(), (packet_size_type) buf->size());
             assert(ur_socket->is_open());
-            co_await writer.async_write_to(*ur_socket);
+            co_await async_write_memory_to(*ur_socket, *buf);
             co_return;
         }
 
-        awaitable<packet_type::pointer> receive_rtde_packet(rtde_packet_header &header) {
-            // read the header
-            using reader_type = versatile_readable_buffer<ur_endian, static_memory<3>>;
-            auto reader = reader_type{};
+        awaitable<dynamic_memory::pointer> receive_rtde_packet(rtde_packet_header &header) {
+            PREVENT_TWICE_ENTER
+            auto header_buf = static_memory<packet_header_offset>{};
             assert(ur_socket->is_open());
-            co_await reader.async_read_from(*ur_socket, 3);
-            reader >> header.packet_size >> header.packet_type;
-            assert(header.packet_size >= 3);
-            if (header.packet_size == 3) co_return nullptr;
-
-            // read the content
-            auto content = packet_type::new_instance();
-            auto content_buffer = buffer(content->data(), header.packet_size - 3);
-            co_await async_read(*ur_socket, content_buffer, use_awaitable);
-            co_return std::move(content);
+            co_await async_fill_memory_from(*ur_socket, header_buf);
+            auto header_reader = versatile_reader<ur_endian>(header_buf);
+            header.fill_from(header_reader);
+            assert(header.packet_size >= packet_header_offset);
+            auto content_length = header.packet_size - packet_header_offset;
+            if (content_length == 0) co_return nullptr;
+            auto content_buf = dynamic_memory::new_instance(content_length);
+            co_await async_fill_memory_from(*ur_socket, *content_buf);
+            co_return std::move(content_buf);
         }
 
         template<typename ContentType>
         awaitable<void> receive_rtde_packet(ContentType &content) {
-            using reader_type = versatile_readable_buffer<ur_endian, extern_memory>;
-            auto packet = packet_type::new_instance();
-            auto reader = reader_type(extern_memory(*packet));
-
-            // read packet size
-            assert(ur_socket->is_open());
-            uint16_t packet_size;
-            co_await async_read_value(*ur_socket, packet_size);
-            assert(packet_size >= 3); // uint16_t + uint8_t
-
-            // read the content
-            co_await reader.async_read_from(*ur_socket, packet_size - 2); // uint16_t
-            auto packet_type = reader.read_value<uint8_t>();
-            assert(packet_type == content.packet_type);
+            PREVENT_TWICE_ENTER
+            auto header = rtde_packet_header{};
+            auto content_buf = co_await receive_rtde_packet(header);
+            assert(header.packet_type == content.packet_type);
+            auto reader = versatile_reader<ur_endian>(*content_buf);
             content.fill_from(reader);
+            assert(reader.empty());
             co_return;
         }
 
         awaitable<bool> negotiate_protocol() {
-            rtde_request_protocol_version_content content;
-            rtde_request_protocol_version_result_content reply_content;
+            rtde_request_protocol_version_content content{};
+            rtde_request_protocol_version_result_content reply_content{};
             content.protocol_version = rtde_protocol_version;
             co_await send_rtde_packet(content);
             co_await receive_rtde_packet(reply_content);
@@ -427,8 +438,8 @@ namespace sophiar {
         }
 
         awaitable<void> print_ur_control_version() {
-            rtde_get_urcontrol_version_content content;
-            rtde_get_urcontrol_version_result_content reply_content;
+            rtde_get_urcontrol_version_content content{};
+            rtde_get_urcontrol_version_result_content reply_content{};
             co_await send_rtde_packet(content);
             co_await receive_rtde_packet(reply_content);
             SPDLOG_INFO("Get URControl version: V{}.{}.{}.{}",
@@ -438,8 +449,7 @@ namespace sophiar {
         }
 
         awaitable<bool> setup_inputs() {
-            rtde_setup_inputs_content content{
-                    .variable_names = string_list(input_variable_list)};
+            rtde_setup_inputs_content content{};
             rtde_setup_inputs_result_content reply_content;
             co_await send_rtde_packet(content);
             co_await receive_rtde_packet(reply_content);
@@ -453,8 +463,8 @@ namespace sophiar {
 
         awaitable<bool> setup_outputs() {
             rtde_setup_outputs_content content{
-                    .output_frequency = report_frequency,
-                    .variable_names = string_list(output_variable_list)};
+                    .output_frequency = report_frequency
+            };
             rtde_setup_outputs_result_content reply_content;
             co_await send_rtde_packet(content);
             co_await receive_rtde_packet(reply_content);
@@ -478,20 +488,29 @@ namespace sophiar {
             co_return true;
         }
 
-        awaitable<void> install_program() { // TODO
-//            // stop moving
-//            auto no_motion = ur_command::new_instance();
-//            no_motion->set_motion_type(ur_move_mode::NO_MOTION);
-//            co_await ur_command_queue.async_send(error_code{}, std::move(no_motion), use_awaitable);
-//            // open urscript socket
-//            tcp::socket urscript_socket(get_context());
-//            co_await urscript_socket.async_connect({ur_ip, urscript_port}, use_awaitable);
-//            // send script file
-//            auto script_file = mapped_file(urscript_path, boost::iostreams::mapped_file::readonly);
-//            co_await async_write(urscript_socket,
-//                                 boost::asio::buffer(script_file.const_data(), script_file.size()),
-//                                 use_awaitable);
-//            co_return;
+        awaitable<void> install_program() {
+            // create no motion command
+            static auto no_motion_content = ur_command_content{};
+            static auto &no_motion_data = no_motion_content.content;
+            no_motion_content.recipe_id = inputs_recipe_id;
+            no_motion_data.move_type = (int32_t) control_mode_type::NO_CONTROL;
+            no_motion_data.cmd_id = current_cmd_id++;
+            no_motion_data.tcp_pose = identity_transform_vec;
+            no_motion_data.target.fill({});
+            no_motion_data.param_a = 0;
+            no_motion_data.param_v = 0;
+            // stop moving
+            co_await send_rtde_packet(no_motion_content);
+            // open urscript socket
+            tcp::socket urscript_socket(*global_context);
+            co_await urscript_socket.async_connect({ur_ip, urscript_port}, use_awaitable);
+            // send script file
+            auto script_file = mapped_file(urscript_path, boost::iostreams::mapped_file::readonly);
+            assert(script_file.is_open());
+            co_await async_write(urscript_socket,
+                                 boost::asio::buffer(script_file.const_data(), script_file.size()),
+                                 use_awaitable);
+            co_return;
         }
 
         awaitable<void> pause_report() {
@@ -501,28 +520,34 @@ namespace sophiar {
             co_return;
         }
 
-//        awaitable<void> send_command_impl(versatile_buffer &buffer,
-//                                          const ur_command::pointer &cmd) {
-//            // fill data
-//            static int32_t last_cmd_id = 0;
-//            cmd->cmd_id = last_cmd_id++;
-//            // send command
-//            auto command_content_ptr = static_cast<ur_command_content *>(cmd.get());
-//            auto command_wrapper_ptr = reinterpret_cast<ur_command_wrapper *>(command_content_ptr);
-//            rtde_data_package_content<ur_command_wrapper &> data_packet{
-//                    .recipe_id = inputs_recipe_id,
-//                    .content = *command_wrapper_ptr,
-//            };
-//            co_await send_rtde_packet(buffer, RTDE_DATA_PACKAGE, data_packet);
-//        }
+        using income_reader_type = versatile_reader<ur_endian>;
 
-        using income_reader_type = versatile_readable_buffer<ur_endian, extern_memory>;
+        bool determine_is_controllable(const ur_status_data &status) {
+            return true; // TODO 根据状态参数判断是否可以控制
+        }
 
         void handle_data_package(income_reader_type &reader) {
-            auto status = ur_status_packet::new_instance();
-            status->fill_from(reader);
-
-
+            PREVENT_TWICE_ENTER
+            static auto content = ur_status_content{};
+            static const auto &status = content.content;
+            content.fill_from(reader);
+            assert(content.recipe_id == outputs_recipe_id);
+
+            auto trans_tmp = Eigen::Isometry3d{};
+            auto ts = current_timestamp();
+            try_update_variable_value<array6_obj>(target_q_index, status.target_q, ts);
+            try_update_variable_value<array6_obj>(target_qd_index, status.target_qd, ts);
+            try_update_variable_value<array6_obj>(target_qdd_index, status.target_qdd, ts);
+            try_update_variable_value<array6_obj>(actual_q_index, status.actual_q, ts);
+            try_update_variable_value<array6_obj>(actual_qd_index, status.actual_qd, ts);
+            vector6d_to_transform(status.target_TCP_pose, trans_tmp);
+            try_update_variable_value<transform_obj>(target_pose_index, trans_tmp, ts);
+            try_update_variable_value<array6_obj>(target_speed_index, status.target_TCP_speed, ts);
+            vector6d_to_transform(status.actual_TCP_pose, trans_tmp);
+            try_update_variable_value<transform_obj>(actual_pose_index, trans_tmp, ts);
+            try_update_variable_value<array6_obj>(actual_speed_index, status.actual_TCP_speed, ts);
+            try_update_variable_value<bool_obj>(is_controllable_index, determine_is_controllable(status), ts);
+            try_update_variable_value<bool_obj>(is_moving_index, status.is_moving, ts);
         }
 
         void handle_text_message(income_reader_type &reader) {
@@ -534,12 +559,12 @@ namespace sophiar {
         // 处理来自 UR 的报文
         void create_income_worker() {
             auto worker_func = [this]() -> awaitable<bool> {
-                rtde_packet_header header;
-                auto content = co_await receive_rtde_packet(header);
-                auto reader = income_reader_type(extern_memory(*content));
+                auto header = rtde_packet_header{};
+                auto content_buf = co_await receive_rtde_packet(header);
+                auto reader = income_reader_type(*content_buf);
                 switch (header.packet_type) {
                     case RTDE_CONTROL_PACKAGE_PAUSE: {
-                        rtde_pause_result_content reply_content;
+                        rtde_pause_result_content reply_content{};
                         reply_content.fill_from(reader);
                         assert(reply_content.accepted == 1);
                         co_return false; // stop this worker
@@ -557,102 +582,337 @@ namespace sophiar {
                         break;
                     }
                 }
+                assert(reader.empty());
                 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
             });
-            status_worker = make_infinite_coro_worker(
-                    get_context(), std::move(noexcept_wrapper), std::move(exit_func));
+            status_worker = make_infinite_coro_worker(std::move(noexcept_wrapper), std::move(exit_func));
             status_worker->run();
         }
 
-        // TODO 这个写法不对, 开一个专门的协程开来控制这玩意
-//        awaitable<void> handle_command_request() {
-//            versatile_buffer buffer;
-//            auto stop_token = stop_requested_signal.new_token();
-//            for (;;) {
-//                auto result = co_await (ur_command_queue.async_receive(use_awaitable) ||
-//                                        stop_requested_signal.coro_wait(stop_token));
-//                if (result.index() == 1) break; // exit
-//                auto cmd = std::get<ur_command::pointer>(result);
-//                try {
-//                    co_await send_command_impl(buffer, cmd);
-//                } catch (std::exception &e) {
-//                    // TODO show log
-//                    break;
-//                }
-//            }
-//            handle_command_request_finished_signal.try_notify_all();
-//            co_return;
-//        }
+        auto create_joint_angle_control_func() {
+            assert(current_control_mode == control_mode_type::JOINT_ANGLE);
+            assert(is_valid_id(joint_angle_index));
+
+            auto content = std::make_unique<ur_command_content>();
+            auto cmd_content = std::addressof(content->content);
+            content->recipe_id = inputs_recipe_id;
+            cmd_content->move_type = (int32_t) control_mode_type::JOINT_ANGLE;
+            cmd_content->tcp_pose = identity_transform_vec;
+
+            auto worker_func = [=, this,
+                    content = std::move(content),
+                    joint_angle = VARIABLE_MANUAL_DELEGATE(array6_obj, joint_angle_index)]() mutable
+                    -> awaitable<bool> {
+
+                co_await joint_angle.coro_wait_update();
+                if (joint_angle.empty()) co_return true;
+
+                cmd_content->cmd_id = current_cmd_id++;
+                cmd_content->target = joint_angle->value;
+                cmd_content->param_a =
+                        try_query_variable_value<double_obj>(
+                                joint_acceleration_limit_index, joint_acceleration_default);
+                cmd_content->param_v =
+                        try_query_variable_value<double_obj>(
+                                joint_speed_limit_index, joint_speed_default);
+
+                co_await send_rtde_packet(*content);
+                co_return true;
+            };
+            return worker_func;
+        }
+
+        auto create_joint_speed_control_func() {
+            assert(current_control_mode == control_mode_type::JOINT_SPEED);
+            assert(is_valid_id(joint_speed_index));
+
+            auto content = std::make_unique<ur_command_content>();
+            auto cmd_content = std::addressof(content->content);
+            content->recipe_id = inputs_recipe_id;
+            cmd_content->move_type = (int32_t) control_mode_type::JOINT_SPEED;
+            cmd_content->tcp_pose = identity_transform_vec;
+            cmd_content->param_v = 0;
+
+            auto worker_func = [=, this,
+                    content = std::move(content),
+                    joint_speed = VARIABLE_MANUAL_DELEGATE(array6_obj, joint_speed_index)]() mutable
+                    -> awaitable<bool> {
+
+                co_await joint_speed.coro_wait_update();
+                if (joint_speed.empty()) co_return true;
+
+                cmd_content->cmd_id = current_cmd_id++;
+                cmd_content->target = joint_speed->value;
+                cmd_content->param_a =
+                        try_query_variable_value<double_obj>(
+                                joint_acceleration_limit_index, joint_acceleration_default);
+
+                co_await send_rtde_packet(*content);
+                co_return true;
+            };
+            return worker_func;
+        }
+
+        auto create_tcp_pose_control_func() {
+            assert(current_control_mode == control_mode_type::TCP_POSE);
+            assert(is_valid_id(tcp_pose_index));
+
+            auto content = std::make_unique<ur_command_content>();
+            auto cmd_content = std::addressof(content->content);
+            content->recipe_id = inputs_recipe_id;
+            cmd_content->move_type = (int32_t) control_mode_type::TCP_POSE;
+
+            auto worker_func = [=, this,
+                    content = std::move(content),
+                    tcp_pose = VARIABLE_MANUAL_DELEGATE(transform_obj, tcp_pose_index)]() mutable
+                    -> awaitable<bool> {
+
+                co_await tcp_pose.coro_wait_update();
+                if (tcp_pose.empty()) co_return true;
+
+                cmd_content->cmd_id = current_cmd_id++;
+                transform_to_vector6d(
+                        try_query_variable_value<transform_obj>(
+                                tcp_offset_index, identity_transform),
+                        cmd_content->tcp_pose);
+                transform_to_vector6d(tcp_pose->value, cmd_content->target);
+                cmd_content->param_a =
+                        try_query_variable_value<double_obj>(
+                                tcp_acceleration_limit_index, tcp_acceleration_default);
+                cmd_content->param_v =
+                        try_query_variable_value<double_obj>(
+                                tcp_speed_limit_index, tcp_speed_default);
+
+                co_await send_rtde_packet(*content);
+                co_return true;
+            };
+            return worker_func;
+        }
+
+        auto create_tcp_speed_control_func() {
+            assert(current_control_mode == control_mode_type::TCP_SPEED);
+            assert(is_valid_id(tcp_speed_index));
+
+            auto content = std::make_unique<ur_command_content>();
+            auto cmd_content = std::addressof(content->content);
+            content->recipe_id = inputs_recipe_id;
+            cmd_content->move_type = (int32_t) control_mode_type::TCP_SPEED;
+            cmd_content->param_v = 0;
+
+            auto worker_func = [=, this,
+                    content = std::move(content),
+                    tcp_speed = VARIABLE_MANUAL_DELEGATE(array6_obj, tcp_speed_index)]() mutable
+                    -> awaitable<bool> {
+
+                co_await tcp_speed.coro_wait_update();
+                if (tcp_speed.empty()) co_return true;
+
+                cmd_content->cmd_id = current_cmd_id++;
+                transform_to_vector6d(
+                        try_query_variable_value<transform_obj>(
+                                tcp_offset_index, identity_transform),
+                        cmd_content->tcp_pose);
+                cmd_content->target = tcp_speed->value;
+                cmd_content->param_a =
+                        try_query_variable_value<double_obj>(
+                                tcp_acceleration_limit_index, tcp_acceleration_default);
+
+                co_await send_rtde_packet(*content);
+                co_return true;
+            };
+            return 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::JOINT_ANGLE: {
+                    auto noexcept_wrapper =
+                            make_noexcept_func(
+                                    create_joint_angle_control_func(),
+                                    std::move(exception_handle_func));
+                    command_worker = make_infinite_coro_worker(std::move(noexcept_wrapper));
+                    break;
+                }
+                case control_mode_type::JOINT_SPEED: {
+                    auto noexcept_wrapper =
+                            make_noexcept_func(
+                                    create_joint_speed_control_func(),
+                                    std::move(exception_handle_func));
+                    command_worker = make_infinite_coro_worker(std::move(noexcept_wrapper));
+                    break;
+                }
+                case control_mode_type::TCP_POSE: {
+                    auto noexcept_wrapper =
+                            make_noexcept_func(
+                                    create_tcp_pose_control_func(),
+                                    std::move(exception_handle_func));
+                    command_worker = make_infinite_coro_worker(std::move(noexcept_wrapper));
+                    break;
+                }
+                case control_mode_type::TCP_SPEED: {
+                    auto noexcept_wrapper =
+                            make_noexcept_func(
+                                    create_tcp_speed_control_func(),
+                                    std::move(exception_handle_func));
+                    command_worker = make_infinite_coro_worker(std::move(noexcept_wrapper));
+                    break;
+                }
+                default: {
+                    assert(false);
+                    return;
+                }
+            }
+
+            command_worker->run();
+        }
+
+        void initialize_translator() {
+            control_mode_translator.register_item("no_control", control_mode_type::NO_CONTROL);
+            control_mode_translator.register_item("joint_angle", control_mode_type::JOINT_ANGLE);
+            control_mode_translator.register_item("joint_speed", control_mode_type::JOINT_SPEED);
+            control_mode_translator.register_item("tcp_pose", control_mode_type::TCP_POSE);
+            control_mode_translator.register_item("tcp_speed", control_mode_type::TCP_SPEED);
+        }
 
         void load_init_config(const nlohmann::json &config) {
-            assert(config.contains("address"));
-            assert(config["address"].is_string());
-            ur_ip = boost::asio::ip::make_address(config["address"].get<std::string>());
-            assert(config.contains("report_frequency"));
-            assert(config["report_frequency"].is_number());
-            report_frequency = config["report_frequency"].get<double>();
+            ur_ip = boost::asio::ip::make_address(LOAD_STRING_ITEM("address"));
+        }
+
+        void load_output_config(const nlohmann::json &config) {
+            report_frequency = LOAD_FLOAT_ITEM("report_frequency");
+            if (config.contains("joint_info")) {
+                const auto &joint_config = config["joint_info"];
+                target_q_index = TRY_LOAD_VARIABLE_INDEX2(joint_config, array6_obj, "target_q");
+                target_qd_index = TRY_LOAD_VARIABLE_INDEX2(joint_config, array6_obj, "target_qd");
+                target_qdd_index = TRY_LOAD_VARIABLE_INDEX2(joint_config, array6_obj, "target_qdd");
+                actual_q_index = TRY_LOAD_VARIABLE_INDEX2(joint_config, array6_obj, "actual_q");
+                actual_qd_index = TRY_LOAD_VARIABLE_INDEX2(joint_config, array6_obj, "actual_qd");
+            }
+            if (config.contains("tcp_info")) {
+                const auto &tcp_config = config["tcp_info"];
+                target_pose_index = TRY_LOAD_VARIABLE_INDEX2(tcp_config, transform_obj, "target_pose");
+                target_speed_index = TRY_LOAD_VARIABLE_INDEX2(tcp_config, array6_obj, "target_speed");
+                actual_pose_index = TRY_LOAD_VARIABLE_INDEX2(tcp_config, transform_obj, "actual_pose");
+                actual_speed_index = TRY_LOAD_VARIABLE_INDEX2(tcp_config, array6_obj, "actual_speed");
+            }
+            is_controllable_index = TRY_LOAD_VARIABLE_INDEX(bool_obj, "is_controllable");
+            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);
+            switch (current_control_mode) {
+                case control_mode_type::JOINT_ANGLE: {
+                    joint_angle_index = LOAD_VARIABLE_INDEX(array6_obj, "target_joint_q");
+                    joint_speed_limit_index = TRY_LOAD_VARIABLE_INDEX(double_obj, "joint_speed_limit");
+                    joint_acceleration_limit_index = TRY_LOAD_VARIABLE_INDEX(double_obj, "joint_acceleration_limit");
+                    break;
+                }
+                case control_mode_type::JOINT_SPEED: {
+                    joint_speed_index = LOAD_VARIABLE_INDEX(array6_obj, "target_joint_qd");
+                    joint_acceleration_limit_index = TRY_LOAD_VARIABLE_INDEX(double_obj, "joint_acceleration_limit");
+                    break;
+                }
+                case control_mode_type::TCP_POSE: {
+                    tcp_pose_index = LOAD_VARIABLE_INDEX(transform_obj, "target_tcp_pose");
+                    tcp_offset_index = TRY_LOAD_VARIABLE_INDEX(transform_obj, "tcp_offset_pose");
+                    tcp_speed_limit_index = TRY_LOAD_VARIABLE_INDEX(double_obj, "tcp_speed_limit");
+                    tcp_acceleration_limit_index = TRY_LOAD_VARIABLE_INDEX(double_obj, "tcp_acceleration_limit");
+                    break;
+                }
+                case control_mode_type::TCP_SPEED: {
+                    tcp_speed_index = LOAD_VARIABLE_INDEX(array6_obj, "target_tcp_speed");
+                    tcp_offset_index = TRY_LOAD_VARIABLE_INDEX(transform_obj, "tcp_offset_pose");
+                    tcp_acceleration_limit_index = TRY_LOAD_VARIABLE_INDEX(double_obj, "tcp_acceleration_limit");
+                    break;
+                }
+                case control_mode_type::NO_CONTROL: {
+                    break;
+                }
+                default: {
+                    assert(false);
+                    return;
+                }
+            }
         }
 
         awaitable<bool> on_init_impl() {
-//            ur_socket.reset(new tcp::socket(get_context()));
-//            co_await ur_socket->async_connect({ur_ip, rtde_port}, use_awaitable);
-//            // decrease delay
-//            tcp::no_delay no_delay_option(true);
-//            ur_socket->set_option(no_delay_option);
-//            // setup steps
-//            versatile_buffer buffer;
-//            CO_ENSURE(negotiate_protocol(buffer))
-//            co_await print_ur_control_version(buffer);
-//            CO_ENSURE(setup_inputs(buffer))
-//            CO_ENSURE(setup_outputs(buffer))
-//            co_await install_program();
+            assert(ur_socket == nullptr);
+            ur_socket.reset(new tcp::socket(*global_context));
+            co_await ur_socket->async_connect({ur_ip, rtde_port}, use_awaitable);
+            // decrease delay
+            tcp::no_delay no_delay_option(true);
+            ur_socket->set_option(no_delay_option);
+            // setup steps
+            CO_ENSURE(negotiate_protocol())
+            co_await print_ur_control_version();
+            CO_ENSURE(setup_inputs())
+            CO_ENSURE(setup_outputs())
             co_return true;
         }
 
         awaitable<bool> on_start_impl() {
-//            versatile_buffer buffer;
-//            CO_ENSURE(start_report(buffer))
-//            ur_command_queue.reset();
-//            handle_command_request_finished_token.reset(
-//                    handle_command_request_finished_signal.new_token_by_ptr());
-//            handle_packages_finished_token.reset(
-//                    handle_packages_finished_signal.new_token_by_ptr());
-//            co_spawn(q_this->get_context(), handle_packages(), detached);
-//            co_spawn(q_this->get_context(), handle_command_request(), detached);
+            // monitor status
+            CO_ENSURE(start_report());
+            create_income_worker();
+            // handle command
+            if (current_control_mode != control_mode_type::NO_CONTROL) {
+                co_await install_program();
+                create_command_worker();
+            }
             co_return true;
         }
 
         awaitable<void> on_stop_impl() {
-//            // wait command sending stop
-//            stop_requested_signal.try_notify_all();
-//            co_await handle_command_request_finished_signal.coro_wait(*handle_command_request_finished_token);
-//            // stop sending status
-//            versatile_buffer buffer;
-//            try {
-//                co_await pause_report(buffer);
-//            } catch (std::exception &e) {
-//                SPDLOG_ERROR("Failed to stop UR interface: {}", e.what());
-//            }
-//            // wait package handler stop
-//            co_await handle_packages_finished_signal.coro_wait(*handle_packages_finished_token);
+            // stop sending command
+            if (current_control_mode != control_mode_type::NO_CONTROL) {
+                if (command_worker != nullptr) {
+                    command_worker->cancel();
+                    co_await command_worker->coro_wait_stop();
+                    command_worker = nullptr;
+                }
+            } else {
+                assert(command_worker == nullptr);
+            }
+            // stop receiving status
+            try {
+                co_await pause_report();
+            } catch (std::exception &) {
+                // TODO maybe the socket is already closed.
+            }
+            co_await status_worker->coro_wait_stop(); // let it stop itself.
+            status_worker = nullptr;
             co_return;
         }
 
-//        impl()
-//                : ur_command_queue(get_context(), ur_command_queue_size),
-//                  stop_requested_signal(get_context()),
-//                  handle_packages_finished_signal(get_context()),
-//                  handle_command_request_finished_signal(get_context()) {
-//            ur_command_handler.p_this = this;
-//        }
+        impl() {
+            initialize_translator();
+
+            // initialize identity transforms
+            identity_transform = Eigen::Isometry3d::Identity();
+            transform_to_vector6d(identity_transform, identity_transform_vec);
+        }
 
     };
 
-    awaitable<bool> ur_interface::on_init(const nlohmann::json &config) {
+    Eigen::Isometry3d ur_interface::impl::identity_transform;
+    array6_obj::value_type ur_interface::impl::identity_transform_vec;
+
+    awaitable<bool> ur_interface::on_init(const nlohmann::json &config) noexcept {
         pimpl->load_init_config(config);
         try {
             CO_ENSURE(pimpl->on_init_impl())
@@ -663,7 +923,11 @@ namespace sophiar {
         co_return true;
     }
 
-    awaitable<bool> ur_interface::on_start(const nlohmann::json &config) {
+    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"]);
         try {
             CO_ENSURE(pimpl->on_start_impl())
         } catch (std::exception &e) {
@@ -673,15 +937,13 @@ namespace sophiar {
         co_return true;
     }
 
-    boost::asio::awaitable<void> ur_interface::on_stop() {
+    boost::asio::awaitable<void> ur_interface::on_stop() noexcept {
         co_await pimpl->on_stop_impl();
-        co_await tristate_obj::on_stop();
         co_return;
     }
 
-    boost::asio::awaitable<void> ur_interface::on_reset() {
+    boost::asio::awaitable<void> ur_interface::on_reset() noexcept {
         pimpl->ur_socket.reset();
-        co_await tristate_obj::on_reset();
         co_return;
     }
 

src/robot/ur/ur_interface.h

@@ -5,13 +5,6 @@
 
 DEFAULT_TRISTATE_OBJ_DEF(ur_interface)
 
-//#include "core/small_obj.hpp"
-//#include "core/tristate_obj.h"
-//#include "core/types/versatile_data.hpp"
-//
-//#include <cassert>
-//#include <memory>
-//
 //namespace sophiar {
 //
 //    namespace ur_runtime_status {
@@ -62,125 +55,6 @@ DEFAULT_TRISTATE_OBJ_DEF(ur_interface)
 //        static constexpr ur_move_mode_type TOOL_SPEED = 4;
 //    }
 //
-//
-//    struct ur_status : public ur_status_content,
-//                       public small_obj<ur_status> {
-//    };
-//
-//    class ur_interface;
-//
-//    // tcp_pose and cmd_id will be filled by ur_interface
-//    struct ur_command_content {
-//    public:
-//
-//        static constexpr double a_joint_default = 1.4; // rad/s^2
-//        static constexpr double a_tool_default = 1.2; // m/s^2
-//        static constexpr double v_joint_default = 1.05; // rad/s
-//        static constexpr double v_tool_default = 0.25; // m/s
-//
-//    protected:
-//
-//        int32_t move_type = 0, cmd_id = -1;
-//        Eigen::Vector6d tcp_pose = Eigen::Vector6d::Zero();
-//        Eigen::Vector6d target;
-//        double param_a = 0, param_v = 0;
-//
-//    private:
-//
-//        friend class ur_interface;
-//
-//    };
-//
-//    struct ur_command : public ur_command_content,
-//                        public small_obj<ur_command> {
-//
-//        ur_command &set_motion_type(ur_move_mode::ur_move_mode_type move_type) {
-//            assert(move_type >= 0 && move_type <= 4);
-//            this->move_type = move_type;
-//            return *this;
-//        }
-//
-//        ur_command &set_tcp_pose(const Eigen::Vector6d &tcp) {
-//            assert(move_type != ur_move_mode::NO_MOTION); // != no motion
-//            this->tcp_pose = tcp;
-//            return *this;
-//        }
-//
-//        ur_command &set_joint_angle(const Eigen::Vector6d &angles) {
-//            assert(move_type == ur_move_mode::JOINT_ANGLE); // movej
-//            this->target = angles;
-//            return *this;
-//        }
-//
-//        ur_command &set_tool_pose(const Eigen::Vector6d &pose) {
-//            assert(move_type == ur_move_mode::TOOL_POSE); // movel
-//            this->target = pose;
-//            return *this;
-//        }
-//
-//        ur_command &set_joint_speed(const Eigen::Vector6d &speeds) {
-//            assert(move_type == ur_move_mode::JOINT_SPEED); // speedj
-//            this->target = speeds;
-//            return *this;
-//        }
-//
-//        ur_command &set_tool_speed(const Eigen::Vector6d &speed) {
-//            assert(move_type == ur_move_mode::TOOL_SPEED); // speedl
-//            this->target = speed;
-//            return *this;
-//        }
-//
-//        ur_command &set_joint_max_speed(double joint_speed) {
-//            assert(move_type == ur_move_mode::JOINT_ANGLE); // movej
-//            this->param_v = joint_speed;
-//            return *this;
-//        }
-//
-//        ur_command &set_joint_max_acceleration(double joint_acceleration) {
-//            assert(move_type == ur_move_mode::JOINT_ANGLE || // movej
-//                   move_type == ur_move_mode::JOINT_SPEED);  // speedj
-//            this->param_a = joint_acceleration;
-//            return *this;
-//        }
-//
-//        ur_command &set_tool_max_speed(double tool_speed) {
-//            assert(move_type == ur_move_mode::TOOL_POSE); // movel
-//            this->param_v = tool_speed;
-//            return *this;
-//        }
-//
-//        ur_command &set_tools_max_acceleration(double tool_acceleration) {
-//            assert(move_type == ur_move_mode::TOOL_POSE || // movel
-//                   move_type == ur_move_mode::TOOL_SPEED); // speedl
-//            this->param_a = tool_acceleration;
-//            return *this;
-//        }
-//    };
-//
-//    class ur_interface : public tristate_obj {
-//    public:
-//
-//        ur_interface();
-//
-//        ~ur_interface() override;
-//
-//        void load_construct_config(const nlohmann::json &config) override;
-//
-//    protected:
-//
-//        boost::asio::awaitable<bool> on_init(const nlohmann::json &config) override;
-//
-//        boost::asio::awaitable<bool> on_start(const nlohmann::json &config) override;
-//
-//        boost::asio::awaitable<void> on_stop() override;
-//
-//        boost::asio::awaitable<void> on_reset() override;
-//
-//    private:
-//        struct impl;
-//        std::unique_ptr<impl> pimpl;
-//
-//    };
 
 
 #endif //SOPHIAR2_UR_INTERFACE_H

src/utility/config_utility.hpp

@@ -5,19 +5,30 @@
     assert(config.contains(item_name)); \
     assert(config[item_name].is_array());
 
+#define ENSURE_STRING(item_name) \
+    assert(config.contains(item_name)); \
+    assert(config[item_name].is_string());
+
+#define ENSURE_STRING2(config, item_name) \
+    assert(config.contains(item_name)); \
+    assert(config[item_name].is_string());
+
+#define LOAD_FLOAT_ITEM(item_name) ({ \
+    assert(config.contains(item_name)); \
+    assert(config[item_name].is_number()); \
+    config[item_name].get<double>(); })
+
 #define LOAD_UINT_ITEM(item_name) ({ \
     assert(config.contains(item_name)); \
     assert(config[item_name].is_number_unsigned()); \
     config[item_name].get<std::uint64_t>(); })
 
 #define LOAD_STRING_ITEM(item_name) ({ \
-    assert(config.contains(item_name)); \
-    assert(config[item_name].is_string()); \
+    ENSURE_STRING(item_name) \
     config[item_name].get<std::string>(); })
 
 #define LOAD_STRING_ITEM2(config, item_name) ({ \
-    assert(config.contains(item_name)); \
-    assert(config[item_name].is_string()); \
+    ENSURE_STRING2(config, item_name) \
     config[item_name].get<std::string>(); })
 
 #define LOAD_VARIABLE_INDEX(var_type, var_name) ({ \
@@ -28,6 +39,30 @@
     auto _name = LOAD_STRING_ITEM2(config, var_name); \
     REQUIRE_VARIABLE(var_type, _name); })
 
+#define TRY_LOAD_VARIABLE_INDEX(var_type, var_name) ({ \
+    variable_index_type _index = -1; \
+    if (config.contains(var_name)) { \
+        assert(config[var_name].is_string()); \
+        auto _name = config[var_name].get<std::string>(); \
+        _index = REQUIRE_VARIABLE(var_type, _name); \
+    } else { \
+        _index = invalid_variable_index; \
+    } \
+        _index; \
+    })
+
+#define TRY_LOAD_VARIABLE_INDEX2(config, var_type, var_name) ({ \
+    variable_index_type _index = -1; \
+    if (config.contains(var_name)) { \
+        assert(config[var_name].is_string()); \
+        auto _name = config[var_name].get<std::string>(); \
+        _index = REQUIRE_VARIABLE(var_type, _name); \
+    } else { \
+        _index = invalid_variable_index; \
+    } \
+        _index; \
+    })
+
 #define LOAD_VARIABLE_INDEX_WITH_NAME(var_type, var_name, var_name_out) ({ \
     var_name_out = LOAD_STRING_ITEM(var_name); \
     REQUIRE_VARIABLE(var_type, var_name_out); })

src/utility/debug_utility.hpp

@@ -2,6 +2,7 @@
 #define SOPHIAR2_DEBUG_MACRO_H
 
 #include "core/timestamp_helper.hpp"
+#include "third_party/scope_guard.hpp"
 
 #include <boost/asio/awaitable.hpp>
 #include <boost/asio/high_resolution_timer.hpp>
@@ -10,6 +11,7 @@
 
 #include <spdlog/spdlog.h>
 
+#include <cassert>
 #include <iostream>
 
 #define FILE_LINE_TRACE {\
@@ -35,9 +37,31 @@
 #define CO_ENSURE(func) \
     { \
         bool ok = co_await (func); \
-        if (!ok) co_return false; \
+        if (!ok) { \
+            assert(false);  \
+            co_return false; \
+        } \
     }
 
+#define RUN_ONCE \
+    static bool __is_called = false; \
+    if (__is_called) [[likely]] return; \
+    __is_called = true;
+
+#ifdef NDEBUG
+
+#define PREVENT_TWICE_ENTER
+
+#else // NDEBUG
+
+#define PREVENT_TWICE_ENTER \
+    static bool __running = false; \
+    assert(!__running); \
+    __running = true; \
+    auto __closer = sg::make_scope_guard([&] { __running = false; });
+
+#endif // NDEBUG
+
 namespace sophiar {
 
     using boost::asio::awaitable;

src/utility/variable_helper.hpp

@@ -18,8 +18,8 @@ namespace sophiar {
                 : watcher(REQUIRE_VARIABLE_WATCHER(_var_index)),
                   var_index(_var_index) {}
 
-        explicit variable_delegate(const std::string &obj_name)
-                : variable_delegate(REQUIRE_VARIABLE<SmallObjType>(obj_name)) {}
+        explicit variable_delegate(std::string_view obj_name)
+                : variable_delegate(REQUIRE_VARIABLE(SmallObjType, obj_name)) {}
 
         variable_delegate(variable_delegate &&other) noexcept = default;
 
@@ -76,7 +76,7 @@ namespace sophiar {
             return get_value().get();
         }
 
-        boost::asio::awaitable<void> coro_wait_update(bool auto_sync = true) {
+        boost::asio::awaitable<void> coro_wait_update(bool auto_sync = AutoSync) {
             co_await watcher.coro_wait(auto_sync);
             val_ptr = QUERY_VARIABLE(SmallObjType, var_index);
             co_return;
@@ -101,6 +101,58 @@ namespace sophiar {
 #define VARIABLE_MANUAL_DELEGATE(var_type, var_index) \
     variable_manual_sync_delegate<var_type>(var_index)
 
+    template<typename SmallObjType>
+    auto try_query_variable_value(variable_index_type var_index,
+                                  const typename SmallObjType::value_type &default_value) {
+        if (!is_valid_id(var_index)) return default_value;
+        auto var_ptr = QUERY_VARIABLE(SmallObjType, var_index);
+        if (var_ptr == nullptr) return default_value;
+        return var_ptr->value;
+    }
+
+    template<typename SmallObjType>
+    auto try_update_variable_value(variable_index_type var_index,
+                                   const typename SmallObjType::value_type &value,
+                                   timestamp_type ts = current_timestamp()) {
+        if (!is_valid_id(var_index)) return;
+        UPDATE_VARIABLE_VAL_WITH_TS(SmallObjType, var_index, value, ts);
+    }
+
+    template<typename SmallObjType, typename CondFuncType>
+    std::enable_if_t<std::is_convertible_v<
+            decltype(std::declval<CondFuncType>()(
+                    std::declval<typename SmallObjType::value_type>())),
+            bool>, boost::asio::awaitable<void>>
+    coro_wait_for_variable_value(variable_index_type var_index,
+                                 CondFuncType &&cond_func) {
+        auto var_helper = VARIABLE_MANUAL_DELEGATE(SmallObjType, var_index);
+        var_helper.manual_sync();
+        for (;;) {
+            if (!var_helper.empty() &&
+                cond_func(var_helper->value)) {
+                break;
+            }
+            co_await var_helper.coro_wait_update();
+        }
+        co_return;
+    }
+
+    template<typename SmallObjType,
+            typename ValueType = typename SmallObjType::value_type>
+    boost::asio::awaitable<void> coro_wait_for_variable_value(variable_index_type var_index,
+                                                              const ValueType &val_tar) {
+        auto var_helper = VARIABLE_MANUAL_DELEGATE(SmallObjType, var_index);
+        var_helper.manual_sync();
+        for (;;) {
+            if (!var_helper.empty() &&
+                var_helper->value == val_tar) {
+                break;
+            }
+            co_await var_helper.coro_wait_update();
+        }
+        co_return;
+    }
+
 }
 
 #endif //SOPHIAR2_VARIABLE_HELPER_HPP

src/utility/versatile_buffer2.hpp

@@ -29,7 +29,7 @@ namespace sophiar {
 
         const char *data() const { return buf.data(); }
 
-        constexpr size_t size() { return Length; }
+        constexpr size_t size() const { return Length; }
 
         static auto new_instance() {
             using this_type = static_memory<Length>;
@@ -314,15 +314,15 @@ namespace sophiar {
         template<typename T>
         std::enable_if_t<std::is_arithmetic_v<T>>
         write_value(T val) {
-            swap_net_loc_endian<net_order>(val);
-            auto ptr = end_ptr();
             buf->increase_size(sizeof(T));
+            swap_net_loc_endian<net_order>(val);
+            auto ptr = start_ptr(sizeof(T));
             memcpy(ptr, &val, sizeof(T));
         }
 
         void write_value(std::string_view str) {
-            auto ptr = end_ptr();
             buf->increase_size(str.length());
+            auto ptr = start_ptr(str.length());
             std::copy(str.begin(), str.end(), ptr);
         }
 
@@ -349,8 +349,9 @@ namespace sophiar {
     private:
         dynamic_memory *buf;
 
-        char *end_ptr() {
-            return buf->data() + buf->size();
+        char *start_ptr(size_t offset) { // 从这个位置开始写入数据
+            assert(buf->size() >= offset);
+            return buf->data() + buf->size() - offset;
         }
     };
 

tests/CMakeLists.txt

@@ -22,12 +22,21 @@ file(GLOB_RECURSE TEST_ALGORITHM_SRC_FILES ./algorithm/*.cpp)
 add_executable(test_algorithm
         ${TEST_ALGORITHM_SRC_FILES}
         ../src/algorithm/transform_tree.cpp
-#        ${ALGORITHM_IMPL_FILES}
+        #        ${ALGORITHM_IMPL_FILES}
         ${CORE_IMPL_FILES})
 target_compile_definitions(test_algorithm PUBLIC SOPHIAR_TEST SOPHIAR_TEST_ALGORITHM)
 target_compile_definitions(test_algorithm PUBLIC CORO_SIGNAL2_USE_TIMER)
 target_link_libraries(test_algorithm ${Boost_LIBRARIES} ${EXTRA_LIBS})
 
+file(GLOB_RECURSE TEST_ROBOT_SRC_FILES ./robot/*.cpp)
+add_executable(test_robot
+        ${TEST_ROBOT_SRC_FILES}
+        ${ROBOT_IMPL_FILES}
+        ${CORE_IMPL_FILES})
+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})
+
 add_executable(test_ndi
         ./interface/ndi.cpp
         ../src/tracker/ndi/ndi_interface.cpp

tests/data/ur_interface_config.json

@@ -1,15 +1,101 @@
 {
-  "listen_port": 5277,
+  "controller_port": 5277,
+  "variable_list": [
+    {
+      "name": "ur_joint_target_q",
+      "type": "array6_obj"
+    },
+    {
+      "name": "ur_joint_target_qd",
+      "type": "array6_obj"
+    },
+    {
+      "name": "ur_joint_target_qdd",
+      "type": "array6_obj"
+    },
+    {
+      "name": "ur_joint_actual_q",
+      "type": "array6_obj"
+    },
+    {
+      "name": "ur_joint_actual_qd",
+      "type": "array6_obj"
+    },
+    {
+      "name": "robot_tcp_target_pose",
+      "type": "transform_obj"
+    },
+    {
+      "name": "robot_tcp_target_speed",
+      "type": "array6_obj"
+    },
+    {
+      "name": "robot_tcp_actual_pose",
+      "type": "transform_obj"
+    },
+    {
+      "name": "robot_tcp_actual_speed",
+      "type": "array6_obj"
+    },
+    {
+      "name": "robot_is_controllable",
+      "type": "bool_obj"
+    },
+    {
+      "name": "robot_is_moving",
+      "type": "bool_obj"
+    },
+    {
+      "name": "robot_control_mode",
+      "type": "u64int_obj"
+    },
+    {
+      "name": "robot_tcp_offset_pose",
+      "type": "transform_obj"
+    },
+    {
+      "name": "robot_target_tcp_pose",
+      "type": "transform_obj"
+    },
+    {
+      "name": "robot_target_tcp_speed",
+      "type": "array6_obj"
+    },
+    {
+      "name": "ur_target_joint_q",
+      "type": "array6_obj"
+    },
+    {
+      "name": "ur_target_joint_qd",
+      "type": "array6_obj"
+    },
+    {
+      "name": "robot_tcp_speed_limit",
+      "type": "double_obj"
+    },
+    {
+      "name": "robot_tcp_acceleration_limit",
+      "type": "double_obj"
+    },
+    {
+      "name": "ur_joint_speed_limit",
+      "type": "double_obj"
+    },
+    {
+      "name": "ur_joint_acceleration_limit",
+      "type": "double_obj"
+    }
+  ],
   "object_list": [
     {
       "type": "ur_interface",
-      "name": "ur_interface",
+      "name": "ur",
       "init_config": {
-        "address": "192.168.38.141",
-        "report_frequency": 125
+        "address": "192.168.38.141"
       },
       "start_config": {
-        "output_objs": {
+        "output_config": {
+          "report_frequency": 125,
           "joint_info": {
             "target_q": "ur_joint_target_q",
             "target_qd": "ur_joint_target_qd",
@@ -24,11 +110,12 @@
             "actual_speed": "robot_tcp_actual_speed"
           },
           "is_controllable": "robot_is_controllable",
-          "is_moving": "robot_is_moving"
+          "is_moving": "robot_is_moving",
+          "current_control_mode": "robot_control_mode"
         },
-        "input_objs": {
+        "input_config": {
+          "control_mode": "tcp_pose",
           "tcp_offset_pose": "robot_tcp_offset_pose",
-          "control_mode": "robot_control_mode",
           "target_tcp_pose": "robot_target_tcp_pose",
           "target_tcp_speed": "robot_target_tcp_speed",
           "target_joint_q": "ur_target_joint_q",
@@ -39,6 +126,14 @@
           "joint_acceleration_limit": "ur_joint_acceleration_limit"
         }
       }
+    },
+    {
+      "type": "bool_obj_recorder",
+      "name": "moving_recorder",
+      "start_config": {
+        "variable_name": "robot_is_moving",
+        "save_file": "is_moving.txt"
+      }
     }
   ]
 }

tests/robot/ur_interface.cpp

@@ -0,0 +1,141 @@
+#define BOOST_TEST_DYN_LINK
+#define BOOST_TEST_MAIN  // in only one cpp file
+
+#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"
+
+#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_joint_angle_control() {
+    auto joint_angle = VARIABLE_AUTO_DELEGATE(array6_obj, "ur_joint_actual_q");
+    auto var_index = REQUIRE_VARIABLE(bool_obj, "robot_is_moving");
+    auto target_index = REQUIRE_VARIABLE(array6_obj, "ur_target_joint_q");
+    co_await coro_wait_for_variable_value<bool_obj>(var_index, false);
+    BOOST_TEST(QUERY_VARIABLE(bool_obj, var_index)->value == false);
+    auto next_joint_angle = array6_obj::new_instance();
+    next_joint_angle->value = joint_angle->value;
+    next_joint_angle->value[0] += std::numbers::pi;
+    UPDATE_VARIABLE(array6_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);
+    next_joint_angle = array6_obj::new_instance();
+    next_joint_angle->value = joint_angle->value;
+    next_joint_angle->value[0] -= std::numbers::pi;
+    UPDATE_VARIABLE(array6_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_joint_speed_control() {
+    auto var_index = REQUIRE_VARIABLE(bool_obj, "robot_is_moving");
+    auto target_index = REQUIRE_VARIABLE(array6_obj, "ur_target_joint_qd");
+    co_await coro_wait_for_variable_value<bool_obj>(var_index, false);
+    BOOST_TEST(QUERY_VARIABLE(bool_obj, var_index)->value == false);
+    for (int i = 0; i < 40; ++i) {
+        auto next_joint_speed = array6_obj::new_instance();
+        next_joint_speed->value.fill({});
+        next_joint_speed->value[0] = 0.2;
+        UPDATE_VARIABLE(array6_obj, target_index, next_joint_speed);
+        co_await coro_sleep(50ms);
+    }
+    co_await coro_wait_for_variable_value<bool_obj>(var_index, false);
+    BOOST_TEST(QUERY_VARIABLE(bool_obj, var_index)->value == false);
+    for (int i = 0; i < 40; ++i) {
+        auto next_joint_speed = array6_obj::new_instance();
+        next_joint_speed->value.fill({});
+        next_joint_speed->value[0] = -0.2;
+        UPDATE_VARIABLE(array6_obj, target_index, next_joint_speed);
+        co_await coro_sleep(50ms);
+    }
+    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_tcp_pose_control() {
+    auto tcp_pose = VARIABLE_AUTO_DELEGATE(transform_obj, "robot_tcp_actual_pose");
+    auto var_index = REQUIRE_VARIABLE(bool_obj, "robot_is_moving");
+    auto target_index = REQUIRE_VARIABLE(transform_obj, "robot_target_tcp_pose");
+    co_await coro_wait_for_variable_value<bool_obj>(var_index, false);
+    BOOST_TEST(QUERY_VARIABLE(bool_obj, var_index)->value == false);
+    auto next_tcp_pose = transform_obj::new_instance();
+    next_tcp_pose->value = tcp_pose->value;
+    next_tcp_pose->value.translation().z() += 0.1;
+    UPDATE_VARIABLE(transform_obj, target_index, next_tcp_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);
+    next_tcp_pose = transform_obj::new_instance();
+    next_tcp_pose->value = tcp_pose->value;
+    next_tcp_pose->value.translation().z() -= 0.1;
+    UPDATE_VARIABLE(transform_obj, target_index, next_tcp_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;
+}
+
+awaitable<void> test_tcp_speed_control() {
+    // set custom tcp
+    auto tcp_offset_index = REQUIRE_VARIABLE(transform_obj, "robot_tcp_offset_pose");
+    UPDATE_VARIABLE_VAL(transform_obj, tcp_offset_index, Eigen::Translation3d(0, 0, 0.15));
+
+    auto var_index = REQUIRE_VARIABLE(bool_obj, "robot_is_moving");
+    auto target_index = REQUIRE_VARIABLE(array6_obj, "robot_target_tcp_speed");
+    co_await coro_wait_for_variable_value<bool_obj>(var_index, false);
+    BOOST_TEST(QUERY_VARIABLE(bool_obj, var_index)->value == false);
+    for (int i = 0; i < 40; ++i) {
+        auto next_tcp_speed = array6_obj::new_instance();
+        next_tcp_speed->value.fill({});
+        next_tcp_speed->value[3] = 0.2;
+        UPDATE_VARIABLE(array6_obj, target_index, next_tcp_speed);
+        co_await coro_sleep(50ms);
+    }
+    co_await coro_wait_for_variable_value<bool_obj>(var_index, false);
+    BOOST_TEST(QUERY_VARIABLE(bool_obj, var_index)->value == false);
+    for (int i = 0; i < 40; ++i) {
+        auto next_tcp_speed = array6_obj::new_instance();
+        next_tcp_speed->value.fill({});
+        next_tcp_speed->value[3] = -0.2;
+        UPDATE_VARIABLE(array6_obj, target_index, next_tcp_speed);
+        co_await coro_sleep(50ms);
+    }
+    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_ur_interface) {
+    spdlog::set_level(spdlog::level::trace);
+    std::ifstream config_file("data/ur_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_tcp_pose_control(), detached);
+    global_context->run();
+}