搁置 UR 接口的开发

src/core/basic_obj_types.hpp

@@ -5,6 +5,8 @@
 
 #include <Eigen/Geometry>
 
+#include <array>
+
 namespace sophiar {
 
     template<typename T>
@@ -45,6 +47,9 @@ namespace sophiar {
 
     };
 
+    template<size_t Length>
+    using array_obj = small_obj_wrapper<std::array<double, Length>>;
+
     using bool_obj = small_obj_wrapper<bool>;       // 0
     using u8int_obj = small_obj_wrapper<uint8_t>;   // 1
     using u16int_obj = small_obj_wrapper<uint16_t>; // 2
@@ -58,6 +63,8 @@ namespace sophiar {
     using double_obj = small_obj_wrapper<double>;   // 10
     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
 
     // specialization
 

src/core/sophiar_manager.cpp

@@ -163,7 +163,9 @@ namespace sophiar {
             register_global_obj_type<double_obj>();
             register_global_obj_type<scalarxyz_obj>();
             register_global_obj_type<transform_obj>();
-            assert(global_obj_type_to_index_pool[typeid(transform_obj)] == 12);
+            register_global_obj_type<array6_obj>();
+            register_global_obj_type<array7_obj>();
+            assert(global_obj_type_to_index_pool[typeid(array7_obj)] == 14);
         }
 
         auto get_tristate_ptr(obj_index_type obj_index) const {

src/robot/ur/scripts/URScript.script

@@ -0,0 +1,84 @@
+def myProg():
+    def noMotion():
+        write_output_boolean_register(64, False)
+        stopj(1.4)
+        sync()
+        return None
+    end
+
+    a = [0, 0, 0, 0, 0, 0]
+    b = [0, 0, 0, 0, 0, 0]
+
+    resTime = 0.1
+    lastCmdId = 0
+
+	while True:
+	    enter_critical
+		moveType = read_input_integer_register(24)
+		cmdId = read_input_integer_register(25)
+
+		i = 0
+		while i < 6:
+			a[i] = read_input_float_register(i + 24)
+			i = i + 1
+		end
+
+        i = 0
+        while i < 6:
+            b[i] = read_input_float_register(i + 30)
+            i = i + 1
+        end
+
+        paramA = read_input_float_register(36)
+        paramV = read_input_float_register(37)
+        exit_critical
+
+        tcpPose = p[a[0], a[1], a[2], a[3], a[4], a[5]]
+		paramPose = p[b[0], b[1], b[2], b[3], b[4], b[5]]
+		paramArray = [b[0], b[1], b[2], b[3], b[4], b[5]]
+
+        if cmdId != lastCmdId:
+            set_tcp(tcpPose)
+            resTime = 0.1
+        end
+
+        if moveType == 0:
+            noMotion()
+        elif moveType == 1:
+            if cmdId == lastCmdId:
+                noMotion()
+            else:
+                write_output_boolean_register(64, True)
+                movej(paramArray, paramA, paramV)
+            end
+        elif moveType == 2:
+            if cmdId == lastCmdId:
+                noMotion()
+            else:
+                write_output_boolean_register(64, True)
+                movel(paramPose, paramA, paramV)
+            end
+        elif moveType == 3:
+            if resTime < 0:
+                noMotion()
+            else:
+                write_output_boolean_register(64, True)
+                speedj(paramArray, paramA, get_steptime())
+                resTime = resTime - get_steptime()
+            end
+        elif moveType == 4:
+            if resTime < 0:
+                noMotion()
+            else:
+                write_output_boolean_register(64, True)
+                speedl(paramArray, paramA, get_steptime())
+                resTime = resTime - get_steptime()
+            end
+        end
+
+        lastCmdId = cmdId
+        write_output_integer_register(24, lastCmdId)
+    end
+end
+
+myProg()

src/robot/ur/ur_interface.cpp

@@ -1,11 +1,11 @@
 #include "ur_interface.h"
 
+#include "core/basic_obj_types.hpp"
 #include "utility/debug_utility.hpp"
-#include "utility/coro_signal.hpp"
-#include "utility/tiny_signal.hpp"
-#include "utility/versatile_buffer.hpp"
+#include "utility/coro_worker.hpp"
+#include "utility/coro_worker_helper_func.hpp"
+#include "utility/versatile_buffer2.hpp"
 
-#include <boost/algorithm/string/join.hpp>
 #include <boost/asio/experimental/awaitable_operators.hpp>
 #include <boost/asio/experimental/channel.hpp>
 #include <boost/asio/ip/address.hpp>
@@ -28,18 +28,17 @@
 
 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::experimental::channel;
     using boost::asio::use_awaitable;
     using boost::iostreams::mapped_file;
     using boost::scoped_ptr;
     using boost::system::error_code;
 
-    using namespace boost::asio::experimental::awaitable_operators;
     using namespace boost::asio::ip;
     using namespace Eigen;
 
@@ -55,194 +54,247 @@ namespace sophiar {
         static constexpr uint8_t RTDE_CONTROL_PACKAGE_START = 83;
         static constexpr uint8_t RTDE_CONTROL_PACKAGE_PAUSE = 80;
 
-        struct rtde_packet_header {
-            uint16_t packet_size;
-            uint8_t packet_type;
+        struct string_list {
+            using container_type = std::vector<std::string>;
 
-            static constexpr uint16_t length() {
-                return sizeof(uint16_t) + sizeof(uint8_t);
-            }
+            container_type elements;
+
+            string_list() = default;
+
+            explicit string_list(const container_type &other)
+                    : elements(other) {}
 
-            void write_to_buffer(versatile_buffer &buffer) const {
-                buffer << packet_size << packet_type;
+            explicit string_list(container_type &&other)
+                    : elements(std::move(other)) {}
+
+            template<typename WriterType>
+            void write_to(WriterType &writer) const {
+                auto n = elements.size();
+                for (const auto &elem: elements) {
+                    writer << elem;
+                    if ((--n) != 0) {
+                        writer << ',';
+                    }
+                }
             }
         };
 
+        struct empty_content {
+            template<typename WriterType>
+            void write_to(WriterType &writer) const {}
+        };
+
+        struct rtde_packet_header {
+            uint16_t packet_size;
+            uint8_t packet_type;
+        };
+
         struct rtde_request_protocol_version_content {
+            static constexpr auto packet_type = RTDE_REQUEST_PROTOCOL_VERSION;
             uint16_t protocol_version;
 
-            static constexpr uint16_t length() {
-                return sizeof(uint16_t);
-            }
-
-            void write_to_buffer(versatile_buffer &buffer) const {
-                buffer << protocol_version;
+            template<typename WriterType>
+            void write_to(WriterType &writer) const {
+                writer << protocol_version;
             }
         };
 
         struct rtde_request_protocol_version_result_content {
+            static constexpr auto packet_type = RTDE_REQUEST_PROTOCOL_VERSION;
             uint8_t accepted;
 
-            void fill_from_buffer(versatile_buffer &buffer) {
-                buffer >> accepted;
+            template<typename ReaderType>
+            void fill_from(ReaderType &reader) {
+                reader >> accepted;
             }
         };
 
+        struct rtde_get_urcontrol_version_content :
+                public empty_content {
+            static constexpr auto packet_type = RTDE_GET_URCONTROL_VERSION;
+        };
+
         struct rtde_get_urcontrol_version_result_content {
+            static constexpr auto packet_type = RTDE_GET_URCONTROL_VERSION;
             uint32_t major, minor, bugfix, build;
 
-            void fill_from_buffer(versatile_buffer &buffer) {
-                buffer >> major >> minor >> bugfix >> build;
+            template<typename ReaderType>
+            void fill_from(ReaderType &reader) {
+                reader >> major >> minor >> bugfix >> build;
             }
         };
 
         struct rtde_text_message_content {
+            static constexpr auto packet_type = RTDE_TEXT_MESSAGE;
             uint8_t message_length, source_length, warning_level;
             std::string message, source;
 
-            uint16_t length() const {
-                auto ret = sizeof(uint8_t) * 3;
-                ret += message_length;
-                ret += source_length;
-                return ret;
+            template<typename ReaderType>
+            void fill_from(ReaderType &reader) {
+                reader >> message_length;
+                reader.read_string(message, message_length);
+                reader >> source_length;
+                reader.read_string(source, source_length);
+                reader >> warning_level;
             }
 
-            void fill_from_buffer(versatile_buffer &buffer) {
-                buffer >> message_length;
-                buffer.read_string(message, message_length);
-                buffer >> source_length;
-                buffer.read_string(source, source_length);
-                buffer >> warning_level;
-            }
-
-            void write_to_buffer(versatile_buffer &buffer) const {
+            template<typename WriterType>
+            void write_to(WriterType &writer) const {
                 assert(message.length() == message_length);
                 assert(source.length() == source_length);
-                buffer << message_length << message;
-                buffer << source_length << source;
-                buffer << warning_level;
+                writer << message_length << message;
+                writer << source_length << source;
+                writer << warning_level;
             }
         };
 
         template<typename RtdeDataContent>
         struct rtde_data_package_content {
+            static constexpr auto packet_type = RTDE_DATA_PACKAGE;
             uint8_t recipe_id;
             RtdeDataContent content;
 
-            uint16_t length() const {
-                return sizeof(uint8_t) + content.length();
+            template<typename ReaderType>
+            void fill_from(ReaderType &reader) {
+                reader >> recipe_id;
+                content.fill_from(reader);
             }
 
-            void fill_from_buffer(versatile_buffer &buffer) {
-                buffer >> recipe_id >> content;
-            }
-
-            void write_to_buffer(versatile_buffer &buffer) const {
-                buffer << recipe_id << content;
+            template<typename WriterType>
+            void write_to(WriterType &writer) const {
+                writer << recipe_id;
+                content.write_to(writer);
             }
         };
 
         struct rtde_setup_inputs_content {
-            std::vector<std::string> variable_names;
-
-            uint16_t length() const {
-                uint16_t ret = 0;
-                for (const auto &item: variable_names) {
-                    ret += item.length();
-                }
-                ret += variable_names.size() - 1;
-                return ret;
-            }
+            static constexpr auto packet_type = RTDE_CONTROL_PACKAGE_SETUP_INPUTS;
+            string_list variable_names;
 
-            void write_to_buffer(versatile_buffer &buffer) const {
-                for (auto iter = variable_names.cbegin(), eiter = variable_names.cend(); iter != eiter; ++iter) {
-                    buffer << *iter;
-                    if (iter + 1 != eiter) {
-                        buffer << ',';
-                    }
-                }
+            template<typename WriterType>
+            void write_to(WriterType &writer) const {
+                variable_names.write_to(writer);
             }
         };
 
         struct rtde_setup_inputs_result_content {
+            static constexpr auto packet_type = RTDE_CONTROL_PACKAGE_SETUP_INPUTS;
             uint8_t output_recipe_id;
             std::vector<std::string> variable_types;
 
-            void fill_from_buffer(versatile_buffer &buffer) {
-                buffer >> output_recipe_id;
-                while (buffer) {
-                    variable_types.push_back(buffer.read_string_until(','));
+            template<typename ReaderType>
+            void fill_from(ReaderType &reader) {
+                reader >> output_recipe_id;
+                while (!reader.empty()) {
+                    variable_types.push_back(reader.read_string_until(','));
                 }
             }
         };
 
-        struct rtde_setup_outputs_content
-                : public rtde_setup_inputs_content {
+        struct rtde_setup_outputs_content {
+            static constexpr auto packet_type = RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS;
             double output_frequency;
+            string_list variable_names;
 
-            uint16_t length() const {
-                return sizeof(double) + rtde_setup_inputs_content::length();
-            }
-
-            void write_to_buffer(versatile_buffer &buffer) const {
-                buffer << output_frequency;
-                rtde_setup_inputs_content::write_to_buffer(buffer);
+            template<typename WriterType>
+            void write_to(WriterType &writer) const {
+                writer << output_frequency;
+                variable_names.write_to(writer);
             }
         };
 
-        using rtde_setup_outputs_result_content = rtde_setup_inputs_result_content;
-        using rtde_start_result_content = rtde_request_protocol_version_result_content;
-        using rtde_pause_result_content = rtde_request_protocol_version_result_content;
-
-        struct ur_status_wrapper : public ur_status_content {
-
-            void fill_from_buffer(versatile_buffer &buffer) {
-                buffer >> target_q >> target_qd >> target_qdd;
-                buffer >> actual_q >> actual_qd;
-                buffer >> actual_TCP_pose >> actual_TCP_speed;
-                buffer >> target_TCP_pose >> target_TCP_speed;
-                buffer >> timestamp;
-                buffer >> robot_mode;
-                buffer >> runtime_state;
-                buffer >> safety_mode;
-                buffer >> last_cmd_id;
-                buffer >> is_moving;
-            }
+        struct rtde_setup_outputs_result_content :
+                public rtde_setup_inputs_result_content {
+            static constexpr auto packet_type = RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS;
         };
 
-        struct ur_command_wrapper : public ur_command_content {
-
-            static constexpr uint16_t length() {
-                return sizeof(int32_t) * 2 +
-                       sizeof(Vector6d) * 2 +
-                       sizeof(double) * 2;
-            }
+        struct rtde_start_content :
+                public empty_content {
+            static constexpr auto packet_type = RTDE_CONTROL_PACKAGE_START;
+        };
 
-            void write_to_buffer(versatile_buffer &buffer) const {
-                buffer << move_type << cmd_id;
-                buffer << tcp_pose << target;
-                buffer << param_a << param_v;
-            }
+        struct rtde_start_result_content :
+                public rtde_request_protocol_version_result_content {
+            static constexpr auto packet_type = RTDE_CONTROL_PACKAGE_START;
         };
 
-        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 rtde_pause_content :
+                public empty_content {
+            static constexpr auto packet_type = RTDE_CONTROL_PACKAGE_PAUSE;
+        };
 
-        struct ur_command_slot_impl_type : public ur_command_slot_type {
-            impl *p_this = nullptr;
+        struct rtde_pause_result_content :
+                public rtde_request_protocol_version_result_content {
+            static constexpr auto packet_type = RTDE_CONTROL_PACKAGE_PAUSE;
+        };
 
-            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
-                }
+        using vector6d_type = array6_obj::value_type;
+
+        struct ur_status_content {
+            vector6d_type target_q, target_qd, target_qdd;
+            vector6d_type actual_q, actual_qd;
+            vector6d_type actual_TCP_pose, actual_TCP_speed;
+            vector6d_type target_TCP_pose, target_TCP_speed;
+            double timestamp;
+            int32_t robot_mode;
+            uint32_t runtime_state;
+            int32_t safety_mode;
+            int32_t last_cmd_id;
+            bool is_moving;
+
+            template<typename ReaderType>
+            void fill_from(ReaderType &reader) {
+                reader >> target_q >> target_qd >> target_qdd;
+                reader >> actual_q >> actual_qd;
+                reader >> actual_TCP_pose >> actual_TCP_speed;
+                reader >> target_TCP_pose >> target_TCP_speed;
+                reader >> timestamp;
+                reader >> robot_mode;
+                reader >> runtime_state;
+                reader >> safety_mode;
+                reader >> last_cmd_id;
+                reader >> is_moving;
             }
         };
 
+        struct ur_status_packet : public rtde_data_package_content<ur_status_content>,
+                                  public small_obj<ur_status_packet> {
+        };
+
+//
+//        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
@@ -282,9 +334,15 @@ namespace sophiar {
         static constexpr uint16_t rtde_port = 30004;
         static constexpr uint16_t urscript_port = 30002;
         static constexpr uint16_t rtde_protocol_version = 2;
-        static constexpr size_t ur_command_queue_size = 4;
         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> {
+        };
+
         ur_interface *q_this = nullptr;
 
         double report_frequency = 125;
@@ -294,108 +352,99 @@ namespace sophiar {
         ip_address_type ur_ip;
         scoped_ptr<tcp::socket> ur_socket; // https://stackoverflow.com/questions/3062803/
 
-        ur_status_signal_type ur_status_signal;
-        ur_command_slot_impl_type ur_command_handler;
-        coro_signal stop_requested_signal;
-        coro_signal handle_command_request_finished_signal;
-        coro_signal handle_packages_finished_signal;
-        scoped_ptr<coro_signal::signal_token> handle_command_request_finished_token;
-        scoped_ptr<coro_signal::signal_token> handle_packages_finished_token;
-
-        using ur_command_queue_type = channel<void(error_code, ur_command::pointer)>;
-        ur_command_queue_type ur_command_queue;
-
-        // send packet with no content
-        awaitable<void> send_rtde_packet(versatile_buffer &buffer, uint8_t packet_type) {
-            rtde_packet_header header{
-                    .packet_size = rtde_packet_header::length(),
-                    .packet_type = packet_type,
-            };
-            assert(buffer.empty());
-            buffer.reset();
-            buffer << header;
-            assert(ur_socket->is_open());
-            co_await buffer.async_write_to(*ur_socket);
-            co_return;
-        }
+        coro_worker::pointer status_worker;
+        coro_worker::pointer command_worker;
 
-        template<typename RtdeContent>
-        awaitable<void> send_rtde_packet(versatile_buffer &buffer,
-                                         uint8_t packet_type, const RtdeContent &content) {
-            rtde_packet_header header{
-                    .packet_size = rtde_packet_header::length() + content.length(),
-                    .packet_type = packet_type,
-            };
-            assert(buffer.empty());
-            buffer.reset();
-            buffer << header << content;
+        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;
+            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(ur_socket->is_open());
-            co_await buffer.async_write_to(*ur_socket);
+            co_await writer.async_write_to(*ur_socket);
             co_return;
         }
 
-        // contents will be read into rtde_buffer
-        awaitable<rtde_packet_header> receive_rtde_packet(versatile_buffer &buffer) {
-            rtde_packet_header header;
+        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{};
             assert(ur_socket->is_open());
-            co_await async_read_value(*ur_socket, header.packet_size);
-            if (!buffer.empty()) {
-                buffer.reset();
-            }
-            assert(buffer.empty());
-            co_await buffer.async_read_from(*ur_socket,
-                                            header.packet_size - sizeof(header.packet_size));
-            buffer >> header.packet_type;
-            co_return header;
+            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);
         }
 
-        // return packet type
-        template<typename RtdeContent>
-        awaitable<uint8_t> receive_rtde_packet(versatile_buffer &buffer, RtdeContent &content) {
-            auto header = co_await receive_rtde_packet(buffer);
-            buffer >> content;
-            co_return header.packet_type;
+        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);
+            content.fill_from(reader);
+            co_return;
         }
 
-        awaitable<bool> negotiate_protocol(versatile_buffer &buffer) {
-            co_await send_rtde_packet(buffer,
-                                      RTDE_REQUEST_PROTOCOL_VERSION,
-                                      rtde_request_protocol_version_content{
-                                              .protocol_version=rtde_protocol_version,
-                                      });
-            rtde_request_protocol_version_result_content protocol_result;
-            auto ret_packet_type = co_await receive_rtde_packet(buffer, protocol_result);
-            assert(ret_packet_type == RTDE_REQUEST_PROTOCOL_VERSION);
-            if (protocol_result.accepted != 1) {
+        awaitable<bool> negotiate_protocol() {
+            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);
+            if (reply_content.accepted != 1) {
                 // TODO show log
                 co_return false;
             }
             co_return true;
         }
 
-        awaitable<void> print_ur_control_version(versatile_buffer &buffer) {
-            co_await send_rtde_packet(buffer, RTDE_GET_URCONTROL_VERSION);
-            rtde_get_urcontrol_version_result_content control_version_result;
-            auto ret_packet_type = co_await receive_rtde_packet(buffer, control_version_result);
-            assert(ret_packet_type == RTDE_GET_URCONTROL_VERSION);
+        awaitable<void> print_ur_control_version() {
+            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{}.{}.{}.{}",
-                        control_version_result.major, control_version_result.minor,
-                        control_version_result.bugfix, control_version_result.build);
+                        reply_content.major, reply_content.minor,
+                        reply_content.bugfix, reply_content.build);
             co_return;
         }
 
-        awaitable<bool> setup_inputs(versatile_buffer &buffer) {
-            rtde_setup_inputs_content setup_inputs_content;
-            std::copy(input_variable_list.cbegin(),
-                      input_variable_list.cend(),
-                      std::back_inserter(setup_inputs_content.variable_names));
-            co_await send_rtde_packet(buffer,
-                                      RTDE_CONTROL_PACKAGE_SETUP_INPUTS,
-                                      setup_inputs_content);
-            rtde_setup_inputs_result_content setup_inputs_result;
-            auto ret_packet_type = co_await receive_rtde_packet(buffer, setup_inputs_result);
-            assert(ret_packet_type == RTDE_CONTROL_PACKAGE_SETUP_INPUTS);
-            inputs_recipe_id = setup_inputs_result.output_recipe_id;
+        awaitable<bool> setup_inputs() {
+            rtde_setup_inputs_content content{
+                    .variable_names = string_list(input_variable_list)};
+            rtde_setup_inputs_result_content reply_content;
+            co_await send_rtde_packet(content);
+            co_await receive_rtde_packet(reply_content);
+            inputs_recipe_id = reply_content.output_recipe_id;
             if (inputs_recipe_id == 0) {
                 // TODO show log
                 co_return false;
@@ -403,19 +452,14 @@ namespace sophiar {
             co_return true;
         }
 
-        awaitable<bool> setup_outputs(versatile_buffer &buffer) {
-            rtde_setup_outputs_content setup_outputs_content;
-            setup_outputs_content.output_frequency = report_frequency;
-            std::copy(output_variable_list.cbegin(),
-                      output_variable_list.cend(),
-                      std::back_inserter(setup_outputs_content.variable_names));
-            co_await send_rtde_packet(buffer,
-                                      RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS,
-                                      setup_outputs_content);
-            rtde_setup_outputs_result_content setup_outputs_result;
-            auto ret_packet_type = co_await receive_rtde_packet(buffer, setup_outputs_result);
-            assert(ret_packet_type == RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS);
-            outputs_recipe_id = setup_outputs_result.output_recipe_id;
+        awaitable<bool> setup_outputs() {
+            rtde_setup_outputs_content content{
+                    .output_frequency = report_frequency,
+                    .variable_names = string_list(output_variable_list)};
+            rtde_setup_outputs_result_content reply_content;
+            co_await send_rtde_packet(content);
+            co_await receive_rtde_packet(reply_content);
+            outputs_recipe_id = reply_content.output_recipe_id;
             if (outputs_recipe_id == 0) {
                 // TODO show log
                 co_return false;
@@ -423,144 +467,127 @@ namespace sophiar {
             co_return true;
         }
 
-        awaitable<bool> start_report(versatile_buffer &buffer) {
-            co_await send_rtde_packet(buffer, RTDE_CONTROL_PACKAGE_START);
-            rtde_start_result_content start_result;
-            auto ret_packet_type = co_await receive_rtde_packet(buffer, start_result);
-            assert(ret_packet_type == RTDE_CONTROL_PACKAGE_START);
-            if (start_result.accepted != 1) {
+        awaitable<bool> start_report() {
+            rtde_start_content content;
+            rtde_start_result_content reply_content;
+            co_await send_rtde_packet(content);
+            co_await receive_rtde_packet(reply_content);
+            if (reply_content.accepted != 1) {
                 // TODO show log
                 co_return false;
             }
             co_return true;
         }
 
-        awaitable<void> install_program() {
-            // 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() { // 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> pause_report(versatile_buffer &buffer) {
-            co_await send_rtde_packet(buffer, RTDE_CONTROL_PACKAGE_PAUSE);
+        awaitable<void> pause_report() {
+            rtde_pause_content content;
+            co_await send_rtde_packet(content);
             // let handle_packages() read the result
             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);
-        }
+//        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_readable_buffer<ur_endian, extern_memory>;
+
+        void handle_data_package(income_reader_type &reader) {
+            auto status = ur_status_packet::new_instance();
+            status->fill_from(reader);
 
-        void handle_data_package(versatile_buffer &buffer) {
-            auto status = ur_status::new_instance();
-            auto status_content_ptr = static_cast<ur_status_content *>(status.get());
-            auto status_wrapper_ptr = reinterpret_cast<ur_status_wrapper *>(status_content_ptr);
-            rtde_data_package_content<ur_status_wrapper &> data_packet{
-                    .content = *status_wrapper_ptr,
-            };
-            buffer >> data_packet;
-            assert(data_packet.recipe_id == outputs_recipe_id);
-            ur_status_signal.emit(status);
-        }
 
-        void handle_text_message(versatile_buffer &buffer) {
-            rtde_text_message_content text_message;
-            buffer >> text_message;
-            // TODO show log
         }
 
-        awaitable<void> handle_packages() {
-            versatile_buffer buffer;
-            auto stop_token = stop_requested_signal.new_token();
-            for (;;) {
-                // check if exit
-                if (stop_requested_signal.try_wait(stop_token)) break;
+        void handle_text_message(income_reader_type &reader) {
+            rtde_text_message_content reply_content;
+            reply_content.fill_from(reader);
+            // TODO show content of the message
+        }
 
-                // receive package
+        // 处理来自 UR 的报文
+        void create_income_worker() {
+            auto worker_func = [this]() -> awaitable<bool> {
                 rtde_packet_header header;
-                try {
-                    header = co_await receive_rtde_packet(buffer);
-                } catch (std::exception &e) {
-                    // TODO show log
-                    // TODO try auto re-connect
-                    // TODO check status
-                    co_spawn(q_this->get_context(), q_this->reset(), detached);
-                    break;
-                }
-
+                auto content = co_await receive_rtde_packet(header);
+                auto reader = income_reader_type(extern_memory(*content));
                 switch (header.packet_type) {
                     case RTDE_CONTROL_PACKAGE_PAUSE: {
-                        rtde_pause_result_content pause_result;
-                        buffer >> pause_result;
-                        assert(pause_result.accepted == 1);
-                        break;
+                        rtde_pause_result_content reply_content;
+                        reply_content.fill_from(reader);
+                        assert(reply_content.accepted == 1);
+                        co_return false; // stop this worker
                     }
-                    case RTDE_DATA_PACKAGE:
-                        handle_data_package(buffer);
+                    case RTDE_DATA_PACKAGE: {
+                        handle_data_package(reader);
                         break;
-                    case RTDE_TEXT_MESSAGE:
-                        handle_text_message(buffer);
+                    }
+                    case RTDE_TEXT_MESSAGE: {
+                        handle_text_message(reader);
                         break;
-                    default:
-                        buffer.reset();
-                        // TODO show log
+                    }
+                    default: {
                         assert(false);
                         break;
+                    }
                 }
-
-                // check if exit
-                if (header.packet_type == RTDE_CONTROL_PACKAGE_PAUSE) break;
-            }
-
-            handle_packages_finished_signal.try_notify_all();
-            co_return;
+                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->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;
-        }
-
-        void register_signal_slot() {
-//            get_manager().register_signal(q_this, "status", ur_status_signal);
-//            get_manager().register_slot<ur_command::pointer>(q_this, "command", ur_command_handler);
-        }
+//        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;
+//        }
 
         void load_init_config(const nlohmann::json &config) {
             assert(config.contains("address"));
@@ -572,57 +599,57 @@ namespace sophiar {
         }
 
         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();
+//            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();
             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);
+//            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);
             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);
+//            // 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);
             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()
+//                : 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;
+//        }
 
     };
 
@@ -634,8 +661,7 @@ namespace sophiar {
             SPDLOG_ERROR("Failed to init UR interface: {}", e.what());
             co_return false;
         }
-
-        co_return co_await tristate_obj::on_init(config);
+        co_return true;
     }
 
     awaitable<bool> ur_interface::on_start(const nlohmann::json &config) {
@@ -645,8 +671,7 @@ namespace sophiar {
             SPDLOG_ERROR("Failed to start UR interface: {}", e.what());
             co_return false;
         }
-
-        co_return co_await tristate_obj::on_start(config);
+        co_return true;
     }
 
     boost::asio::awaitable<void> ur_interface::on_stop() {
@@ -666,11 +691,6 @@ namespace sophiar {
         pimpl->q_this = this;
     }
 
-    void ur_interface::load_construct_config(const nlohmann::json &config) {
-        assert(config.empty());
-        pimpl->register_signal_slot();
-    }
-
     ur_interface::~ur_interface() = default;
 
 }

src/robot/ur/ur_interface.h

@@ -1,196 +1,186 @@
 #ifndef SOPHIAR2_UR_INTERFACE_H
 #define SOPHIAR2_UR_INTERFACE_H
 
-#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 {
-        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;
-    }
-
-    struct ur_status_content {
-        Eigen::Vector6d target_q, target_qd, target_qdd;
-        Eigen::Vector6d actual_q, actual_qd;
-        Eigen::Vector6d actual_TCP_pose, actual_TCP_speed;
-        Eigen::Vector6d target_TCP_pose, target_TCP_speed;
-        double timestamp;
-        int32_t robot_mode;
-        uint32_t runtime_state;
-        int32_t safety_mode;
-        int32_t last_cmd_id;
-        bool is_moving;
-    };
-
-    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;
-
-    };
-
-}
+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 {
+//        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;
+//    }
+//
+//
+//    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/sensor/optoforce/optoforce_daq.cpp

@@ -2,6 +2,7 @@
 
 #include "core/basic_obj_types.hpp"
 #include "utility/coro_worker.hpp"
+#include "utility/coro_worker_helper_func.hpp"
 #include "utility/versatile_buffer2.hpp"
 
 #include <boost/asio/serial_port.hpp>

src/utility/debug_utility.hpp

@@ -172,6 +172,7 @@ namespace sophiar {
         assert(ifs.is_open());
         // create worker
         auto worker = make_infinite_coro_worker(global_context, [
+                timer = boost::asio::high_resolution_timer(global_context),
                 obj_index, buffer = string_reader(), ifs = std::move(ifs)]() mutable
                 -> awaitable<bool> {
             std::string str_buf;
@@ -182,7 +183,8 @@ namespace sophiar {
             auto next_ts = static_cast<timestamp_type>(next_ts_ms * 1000); // ms -> us
             timestamp_type cur_ts = current_timestamp();
             if (next_ts > cur_ts) {
-                co_await coro_sleep(std::chrono::microseconds(next_ts - cur_ts));
+                timer.expires_at(get_time_from_timestamp(next_ts));
+                co_await timer.async_wait(boost::asio::use_awaitable);
             } else if (next_ts != cur_ts) { // if time is missed, simply ignore
                 co_return true;
             }

src/utility/versatile_buffer2.hpp

@@ -156,6 +156,13 @@ namespace sophiar {
             }
         }
 
+        template<typename T, size_t Length>
+        void read_value(std::array<T, Length> &arr) {
+            for (auto &val: arr) {
+                read_value(val);
+            }
+        }
+
         std::string read_string(size_t read_length) {
             assert(cur_pos + read_length <= cur_length);
             auto ret = std::string{cur_data(), cur_data() + read_length};
@@ -172,7 +179,9 @@ namespace sophiar {
             while (cur_pos != cur_length && *cur_data() != sep) {
                 ++cur_pos;
             }
-            return std::string{buffer.data() + start_pos, cur_data()};
+            // solve char * vs const char * problem
+            const char *start_ptr = buffer.data() + start_pos;
+            return std::string{start_ptr, cur_data()};
         }
 
         void read_string_until(std::string &str, char sep) {
@@ -272,6 +281,13 @@ namespace sophiar {
             }
         }
 
+        template<typename T, size_t Length>
+        void write_value(const std::array<T, Length> &arr) {
+            for (auto val: arr) {
+                write_value(val);
+            }
+        }
+
         template<typename T>
         auto &operator<<(const T &val) {
             write_value(val);

tests/data/optoforce_daq_config.json

@@ -3,7 +3,7 @@
   "object_list": [
     {
       "type": "optoforce_daq",
-      "name": "optoforce_dqp",
+      "name": "optoforce_daq",
       "start_config": {
         "com_port": "COM5",
         "device_type": 3,

tests/data/ur_interface_config.json

@@ -0,0 +1,44 @@
+{
+  "listen_port": 5277,
+  "object_list": [
+    {
+      "type": "ur_interface",
+      "name": "ur_interface",
+      "init_config": {
+        "address": "192.168.38.141",
+        "report_frequency": 125
+      },
+      "start_config": {
+        "output_objs": {
+          "joint_info": {
+            "target_q": "ur_joint_target_q",
+            "target_qd": "ur_joint_target_qd",
+            "target_qdd": "ur_joint_target_qdd",
+            "actual_q": "ur_joint_actual_q",
+            "actual_qd": "ur_joint_actual_qd"
+          },
+          "tcp_info": {
+            "target_pose": "robot_tcp_target_pose",
+            "target_speed": "robot_tcp_target_speed",
+            "actual_pose": "robot_tcp_actual_pose",
+            "actual_speed": "robot_tcp_actual_speed"
+          },
+          "is_controllable": "robot_is_controllable",
+          "is_moving": "robot_is_moving"
+        },
+        "input_objs": {
+          "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",
+          "target_joint_qd": "ur_target_joint_qd",
+          "tcp_speed_limit": "robot_tcp_speed_limit",
+          "tcp_acceleration_limit": "robot_tcp_acceleration_limit",
+          "joint_speed_limit": "ur_joint_speed_limit",
+          "joint_acceleration_limit": "ur_joint_acceleration_limit"
+        }
+      }
+    }
+  ]
+}