修改了信号的传递方式

CMakeLists.txt

@@ -7,7 +7,8 @@ project(${PROJECT_NAME})
 
 include_directories(./src)
 
-find_package(Boost REQUIRED)
+find_package(Boost REQUIRED COMPONENTS iostreams)
+list(APPEND EXTRA_LIBS ${Boost_LIBRARIES} bcrypt)
 include_directories(${Boost_INCLUDE_DIRS})
 
 find_package(fmt REQUIRED)
@@ -19,12 +20,16 @@ list(APPEND EXTRA_LIBS spdlog::spdlog)
 find_package(Eigen3 REQUIRED)
 list(APPEND EXTRA_LIBS Eigen3::Eigen)
 
+find_package(yaml-cpp REQUIRED)
+list(APPEND EXTRA_LIBS ${YAML_CPP_LIBRARIES})
+include_directories(${YAML_CPP_INCLUDE_DIR})
+
 file(GLOB_RECURSE SRC_FILES ./src/*.cpp)
 add_executable(${PROJECT_NAME} ${SRC_FILES})
 
 IF (WIN32)
     list(APPEND EXTRA_LIBS ws2_32 winmm)
-ENDIF()
+ENDIF ()
 
 target_link_libraries(${PROJECT_NAME} ${EXTRA_LIBS})
 

src/core/types/vectorx.hpp

@@ -4,9 +4,15 @@
 #include "core/small_obj.hpp"
 #include "utility/versatile_buffer.hpp"
 
+#include <fmt/format.h>
+
 #include <Eigen/Core>
 
+#include <algorithm>
+#include <array>
 #include <cassert>
+#include <initializer_list>
+#include <stdexcept>
 #include <type_traits>
 #include <utility>
 
@@ -37,12 +43,12 @@ namespace sophiar {
                 : data(other_data) {}
 
         vectorx_view(vectorx<RealFloatType, Length> &other)
-                : data(other.data) {}
+                : data(other.m.data()) {}
 
         template<typename ThisFloatType = FloatType,
                 typename = std::enable_if_t<std::is_const_v<ThisFloatType>>>
         vectorx_view(const vectorx<RealFloatType, Length> &other)
-                : data(other.data) {}
+                : data(other.m.data()) {}
 
         vectorx_view(const vectorx_view<RealFloatType *, Length> &other)
                 : data(other.data) {}
@@ -97,22 +103,23 @@ namespace sophiar {
             return at(pos);
         }
 
-        template<typename OtherFloatPointerType, size_t OtherLength,
+        template<typename OtherFloatType, size_t OtherLength,
                 typename = std::enable_if_t<OtherLength < Length>>
-        auto fill(const vectorx_view<OtherFloatPointerType, OtherLength> &other) {
-            copy_from_vector_view<OtherLength>(other);
+        auto fill(const vectorx<OtherFloatType, OtherLength> &other) {
+            copy_from_vectorx<OtherLength>(other);
             return vectorx_view<FloatPointerType, Length - OtherLength>(data + OtherLength);
         };
 
-        template<typename OtherFloatType, size_t OtherLength,
+        template<typename OtherFloatPointerType, size_t OtherLength,
                 typename = std::enable_if_t<OtherLength < Length>>
-        auto fill(const vectorx<OtherFloatType, OtherLength> &other) {
-            return fill(other.get_view());
+        auto fill(const vectorx_view<OtherFloatPointerType, OtherLength> &other) {
+            copy_from_vectorx_view<OtherLength>(other);
+            return vectorx_view<FloatPointerType, Length - OtherLength>(data + OtherLength);
         };
 
         template<typename OtherFloatType, int OtherRows, int OtherColumns,
                 size_t CopyLength = OtherRows * OtherColumns,
-                typename = std::enable_if_t<OtherRows * OtherColumns < Length>>
+                typename = std::enable_if_t<CopyLength < Length>>
         auto fill(const Eigen::Matrix<OtherFloatType, OtherRows, OtherColumns> &other) {
             copy_from_eigen_matrix<CopyLength>(other);
             return vectorx_view<FloatPointerType, Length - CopyLength>(data + CopyLength);
@@ -125,22 +132,23 @@ namespace sophiar {
             return vectorx_view<FloatPointerType, Length - OtherLength>(data + OtherLength);
         };
 
-        template<typename OtherFloatPointerType, size_t OtherLength,
+        template<typename OtherFloatType, size_t OtherLength,
                 typename = std::enable_if_t<OtherLength == Length>>
-        void fill(const vectorx_view<OtherFloatPointerType, OtherLength> &other) {
-            copy_from_vector_view<OtherLength>(other);
+        void fill(const vectorx<OtherFloatType, OtherLength> &other) {
+            copy_from_vectorx<OtherLength>(other);
         };
 
-        template<typename OtherFloatType, size_t OtherLength,
+        template<typename OtherFloatPointerType, size_t OtherLength,
                 typename = std::enable_if_t<OtherLength == Length>>
-        void fill(const vectorx<OtherFloatType, OtherLength> &other) {
-            fill(other.get_view());
+        void fill(const vectorx_view<OtherFloatPointerType, OtherLength> &other) {
+            copy_from_vectorx_view<OtherLength>(other);
         };
 
         template<typename OtherFloatType, int OtherRows, int OtherColumns,
-                typename = std::enable_if_t<OtherRows * OtherColumns == Length>>
+                size_t CopyLength = OtherRows * OtherColumns,
+                typename = std::enable_if_t<CopyLength == Length>>
         void fill(const Eigen::Matrix<OtherFloatType, OtherRows, OtherColumns> &other) {
-            copy_from_eigen_matrix<OtherRows * OtherColumns>(other);
+            copy_from_eigen_matrix<CopyLength>(other);
         }
 
         template<typename OtherFloatType, size_t OtherLength,
@@ -196,34 +204,39 @@ namespace sophiar {
         template<typename, size_t> friend
         class vectorx;
 
+        template<size_t CopyLength, typename OtherFloatType, size_t OtherLength,
+                typename ThisFloatType = FloatType,
+                typename = std::enable_if_t<!std::is_const_v<ThisFloatType>>>
+        void copy_from_vectorx(const vectorx<OtherFloatType, OtherLength> &other) {
+            static_assert(std::is_convertible_v<OtherFloatType, RealFloatType>);
+            static_assert(CopyLength <= Length);
+            std::copy_n(other.m.begin(), CopyLength, data);
+        }
+
         template<size_t CopyLength, typename OtherFloatPointerType, size_t OtherLength,
                 typename ThisFloatType = FloatType,
-                typename = std::enable_if_t<!std::is_const_v<ThisFloatType>>,
-                typename = std::enable_if_t<std::is_convertible_v<
-                        std::remove_pointer_t<OtherFloatPointerType>, RealFloatType>>>
-        void copy_from_vector_view(const vectorx_view<OtherFloatPointerType, OtherLength> &other) {
+                typename = std::enable_if_t<!std::is_const_v<ThisFloatType>>>
+        void copy_from_vectorx_view(const vectorx_view<OtherFloatPointerType, OtherLength> &other) {
+            static_assert(std::is_convertible_v<
+                    std::remove_pointer_t<OtherFloatPointerType>, RealFloatType>);
             static_assert(CopyLength <= Length);
-            for (size_t i = 0; i < CopyLength; ++i) {
-                data[i] = other.data[i];
-            }
+            std::copy_n(other.data, CopyLength, data);
         }
 
         template<size_t CopyLength, typename OtherFloatType, size_t OtherLength,
                 typename ThisFloatType = FloatType,
-                typename = std::enable_if_t<!std::is_const_v<ThisFloatType>>,
-                typename = std::enable_if_t<std::is_convertible_v<OtherFloatType, RealFloatType>>>
+                typename = std::enable_if_t<!std::is_const_v<ThisFloatType>>>
         void copy_from_list(const OtherFloatType(&list)[OtherLength]) {
+            static_assert(std::is_convertible_v<OtherFloatType, RealFloatType>);
             static_assert(CopyLength <= Length);
-            for (size_t i = 0; i < CopyLength; ++i) {
-                data[i] = list[i];
-            }
+            std::copy_n(list, CopyLength, data);
         }
 
         template<size_t CopyLength, typename OtherFloatType, int OtherRows, int OtherColumns,
                 typename ThisFloatType = FloatType,
-                typename = std::enable_if_t<!std::is_const_v<ThisFloatType>>,
-                typename = std::enable_if_t<std::is_convertible_v<OtherFloatType, RealFloatType>>>
+                typename = std::enable_if_t<!std::is_const_v<ThisFloatType>>>
         void copy_from_eigen_matrix(const Eigen::Matrix<OtherFloatType, OtherRows, OtherColumns> &other) {
+            static_assert(std::is_convertible_v<OtherFloatType, RealFloatType>);
             static_assert(OtherRows == 1 || OtherColumns == 1);
             static_assert(OtherRows * OtherColumns == Length);
             static_assert(CopyLength <= Length);
@@ -241,33 +254,38 @@ namespace sophiar {
 
         vectorx() = default;
 
+        vectorx(std::initializer_list<FloatType> list) {
+            copy_from_initial_list(list);
+        }
+
         template<typename OtherFloatType>
-        explicit vectorx(const OtherFloatType(&list)[Length]) {
-            copy_from_list(list);
+        explicit vectorx(const vectorx<OtherFloatType, Length> &other) {
+            copy_from_vectorx(other);
         }
 
         template<typename OtherFloatPointerType>
         explicit vectorx(const vectorx_view<OtherFloatPointerType, Length> &other) {
-            copy_from_vector_view(other);
+            copy_from_vectorx_view(other);
         }
 
-        template<typename OtherFloatType>
-        explicit vectorx(const vectorx<OtherFloatType, Length> &other)
-                :vectorx(other.get_view()) {}
-
         template<typename OtherFloatType, size_t OtherRows, size_t OtherColumns>
         explicit vectorx(const Eigen::Matrix<OtherFloatType, OtherRows, OtherColumns> &other) {
             copy_from_eigen_matrix(other);
         }
 
-        template<typename OtherFloatType>
-        vectorx &operator=(const OtherFloatType(&list)[Length]) {
-            copy_from_list(list);
+        vectorx &operator=(std::initializer_list<FloatType> list) {
+            copy_from_initial_list(list);
             return *this;
         }
 
         template<typename OtherFloatType>
-        vectorx &operator=(const vectorx_view<OtherFloatType, Length> &other) {
+        vectorx &operator=(const vectorx<OtherFloatType, Length> &other) {
+            copy_from_vectorx(other);
+            return *this;
+        }
+
+        template<typename OtherFloatPointerType>
+        vectorx &operator=(const vectorx_view<OtherFloatPointerType, Length> &other) {
             copy_from_vector_view(other);
             return *this;
         }
@@ -281,43 +299,45 @@ namespace sophiar {
         template<size_t Pos>
         FloatType &at() {
             static_assert(Pos < Length);
-            return data[Pos];
+            return m[Pos];
         }
 
         template<size_t Pos>
         const FloatType &at() const {
             static_assert(Pos < Length);
-            return data[Pos];
+            return m[Pos];
         }
 
         FloatType &at(size_t pos) {
             assert(pos < Length);
-            return data[pos];
+            return m.at(pos);
         }
 
         const FloatType &at(size_t pos) const {
             assert(pos < Length);
-            return data[pos];
+            return m.at(pos);
         }
 
         FloatType &operator[](size_t pos) {
-            return at(pos);
+            assert(pos < Length);
+            return m[pos];
         }
 
         const FloatType &operator[](size_t pos) const {
-            return at(pos);
+            assert(pos < Length);
+            return m[pos];
         }
 
         template<size_t StartPos = 0, size_t EndPos = Length>
         auto get_view() {
             static_assert(StartPos < EndPos && EndPos <= Length);
-            return vectorx_view<FloatType *, EndPos - StartPos>(data + StartPos);
+            return vectorx_view<FloatType *, EndPos - StartPos>(m.data() + StartPos);
         };
 
         template<size_t StartPos = 0, size_t EndPos = Length>
         auto get_view() const {
             static_assert(StartPos < EndPos && EndPos <= Length);
-            return vectorx_view<const FloatType *, EndPos - StartPos>(data + StartPos);
+            return vectorx_view<const FloatType *, EndPos - StartPos>(m.data() + StartPos);
         };
 
         static constexpr uint16_t length() {
@@ -325,48 +345,49 @@ namespace sophiar {
         }
 
         void write_to_buffer(versatile_buffer &buffer) const {
-            for (const auto &item: data) {
+            for (const auto &item: m) {
                 buffer << item;
             }
         }
 
         void fill_from_buffer(versatile_buffer &buffer) {
-            for (auto &item: data) {
+            for (auto &item: m) {
                 buffer >> item;
             }
         }
 
     private:
-        FloatType data[Length];
+        std::array<FloatType, Length> m;
 
         template<typename, size_t> friend
         class vectorx_view;
 
-        template<typename OtherFloatType,
-                typename = std::enable_if_t<std::is_convertible_v<OtherFloatType, FloatType>>>
-        void copy_from_list(const OtherFloatType(&list)[Length]) {
-            for (size_t i = 0; i < Length; ++i) {
-                data[i] = list[i];
-            }
+        void copy_from_initial_list(const std::initializer_list<FloatType> &list) {
+            assert(list.size() == Length);
+            std::copy_n(list.begin(), Length, m.begin());
         }
 
-        template<typename OtherFloatType, int OtherRows, int OtherColumns,
-                typename = std::enable_if_t<std::is_convertible_v<OtherFloatType, FloatType>>>
+        template<typename OtherFloatType, int OtherRows, int OtherColumns>
         void copy_from_eigen_matrix(const Eigen::Matrix<OtherFloatType, OtherRows, OtherColumns> &other) {
+            static_assert(std::is_convertible_v<OtherFloatType, FloatType>);
             static_assert(OtherRows == 1 || OtherColumns == 1);
             static_assert(OtherRows * OtherColumns == Length);
             for (size_t i = 0; i < Length; ++i) {
-                data[i] = other(i);
+                m[i] = other(i);
             }
         }
 
-        template<typename OtherFloatPointerType,
-                typename = std::enable_if_t<std::is_convertible_v<
-                        std::remove_pointer_t<OtherFloatPointerType>, FloatType>>>
-        void copy_from_vector_view(const vectorx_view<OtherFloatPointerType, Length> &other) {
-            for (size_t i = 0; i < Length; ++i) {
-                data[i] = other.data[i];
-            }
+        template<typename OtherFloatType>
+        void copy_from_vectorx(const vectorx<OtherFloatType, Length> &other) {
+            static_assert(std::is_convertible_v<OtherFloatType, FloatType>);
+            m = other.m;
+        }
+
+        template<typename OtherFloatPointerType>
+        void copy_from_vectorx_view(const vectorx_view<OtherFloatPointerType, Length> &other) {
+            static_assert(std::is_convertible_v<
+                    std::remove_pointer_t<OtherFloatPointerType>, FloatType>);
+            std::copy_n(other.data, Length, m.begin());
         }
 
     };

src/main.cpp

@@ -7,6 +7,7 @@
 
 #include <boost/asio/co_spawn.hpp>
 #include <boost/asio/detached.hpp>
+#include <boost/uuid/uuid_generators.hpp>
 
 using boost::asio::co_spawn;
 using boost::asio::detached;
@@ -28,9 +29,14 @@ int main() {
     ur.add_status_listener(listener);
     ur.set_report_frequency(125);
     auto func = [&]() -> boost::asio::awaitable<void> {
+        auto &cmd_listener = ur.get_command_listener(boost::uuids::random_generator()());
         co_await ur.init();
         co_await ur.start();
-        co_await coro_sleep(std::chrono::seconds(1));
+        for (int i = 0; i < 125; ++i) {
+            auto move = ur_command::new_tcp_speed({0, 0, 0.01, .0, .0, .0});
+            cmd_listener.on_signal_received(std::move(move));
+            co_await coro_sleep(std::chrono::milliseconds(8));
+        }
         co_await ur.reset();
         co_return;
     };

src/robot/ur/ur_interface.cpp

@@ -1,14 +1,20 @@
 #include "ur_interface.h"
 
 #include "utility/debug_utility.hpp"
+#include "utility/coro_signal.hpp"
 #include "utility/versatile_buffer.hpp"
 
 #include <boost/algorithm/string/join.hpp>
+#include <boost/asio/experimental/awaitable_operators.hpp>
+#include <boost/asio/experimental/channel.hpp>
 #include <boost/asio/ip/tcp.hpp>
 #include <boost/asio/co_spawn.hpp>
 #include <boost/asio/detached.hpp>
 #include <boost/asio/use_awaitable.hpp>
+#include <boost/asio/write.hpp>
+#include <boost/iostreams/device/mapped_file.hpp>
 #include <boost/smart_ptr/scoped_ptr.hpp>
+#include <boost/system/error_code.hpp>
 
 #include <spdlog/spdlog.h>
 
@@ -20,12 +26,18 @@
 
 namespace sophiar {
 
+    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;
 
     struct ur_interface::impl {
@@ -181,24 +193,11 @@ namespace sophiar {
 
         struct ur_status_wrapper : public ur_status_content {
 
-            ur_status_wrapper() {
-                static_assert(sizeof(ur_status_wrapper) == sizeof(ur_status_content));
-                target_q = ur_vector6d_obj::new_instance();
-                target_qd = ur_vector6d_obj::new_instance();
-                target_qdd = ur_vector6d_obj::new_instance();
-                actual_q = ur_vector6d_obj::new_instance();
-                actual_qd = ur_vector6d_obj::new_instance();
-                actual_TCP_pose = ur_vector6d_obj::new_instance();
-                actual_TCP_speed = ur_vector6d_obj::new_instance();
-                target_TCP_pose = ur_vector6d_obj::new_instance();
-                target_TCP_speed = ur_vector6d_obj::new_instance();
-            }
-
             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 >> 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;
@@ -208,33 +207,37 @@ namespace sophiar {
             }
         };
 
-        struct ur_command_content {
-            int32_t move_type, cmd_id;
-            ur_vector6d_pointer tcp_pose, target;
-            double param_a, param_v;
-
-            ur_command_content() {
-                static int32_t cur_cmd_id = 0;
-                cmd_id = cur_cmd_id++;
-            }
+        struct ur_command_wrapper : public ur_command_content {
 
-            uint16_t length() const {
+            static constexpr uint16_t length() {
                 return sizeof(int32_t) * 2 +
-                       ur_vector6d_obj::length() * 2 +
+                       vector6d::length() * 2 +
                        sizeof(double) * 2;
             }
 
             void write_to_buffer(versatile_buffer &buffer) const {
                 buffer << move_type << cmd_id;
-                buffer << *tcp_pose << *target;
+                buffer << tcp_pose << target;
                 buffer << param_a << param_v;
             }
         };
 
-        struct ur_command_obj : public ur_command_content,
-                                public small_obj<ur_command_obj> {
+        struct ur_command_handler_type : public ur_command_listener_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
+                }
+            }
         };
 
+        using ur_command_muxer_type = signal_muxer<ur_command::pointer>;
+
         std::vector<std::string> input_variable_list = {
                 "input_int_register_24", // Move Type
                 "input_int_register_25", // Cmd Id
@@ -272,7 +275,10 @@ 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";
 
         ur_interface *q_this = nullptr;
 
@@ -281,67 +287,80 @@ namespace sophiar {
 
         ip_address_type ur_ip;
         scoped_ptr<tcp::socket> ur_socket; // https://stackoverflow.com/questions/3062803/
-        ur_vector6d_pointer ur_tcp;
-
-        versatile_buffer rtde_buffer;
+        vector6d ur_tcp{0, 0, 0, 0, 0, 0}; // TODO 让外部代码自己提供 tcp_pose
 
         ur_status_signal_type ur_status_signal;
+        ur_command_handler_type ur_command_handler;
+        ur_command_muxer_type ur_command_muxer;
+        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(uint8_t packet_type) {
+        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(rtde_buffer.empty());
-            rtde_buffer.reset();
-            rtde_buffer << header;
+            assert(buffer.empty());
+            buffer.reset();
+            buffer << header;
             assert(ur_socket->is_open());
-            co_await rtde_buffer.async_write_to(*ur_socket);
+            co_await buffer.async_write_to(*ur_socket);
             co_return;
         }
 
         template<typename RtdeContent>
-        awaitable<void> send_rtde_packet(uint8_t packet_type, const RtdeContent &content) {
+        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(rtde_buffer.empty());
-            rtde_buffer.reset();
-            rtde_buffer << header << content;
+            assert(buffer.empty());
+            buffer.reset();
+            buffer << header << content;
             assert(ur_socket->is_open());
-            co_await rtde_buffer.async_write_to(*ur_socket);
+            co_await buffer.async_write_to(*ur_socket);
             co_return;
         }
 
         // contents will be read into rtde_buffer
-        awaitable<rtde_packet_header> receive_rtde_packet() {
+        awaitable<rtde_packet_header> receive_rtde_packet(versatile_buffer &buffer) {
             rtde_packet_header header;
             assert(ur_socket->is_open());
             co_await async_read_value(*ur_socket, header.packet_size);
-            assert(rtde_buffer.empty());
-            co_await rtde_buffer.async_read_from(*ur_socket,
-                                                 header.packet_size - sizeof(header.packet_size));
-            rtde_buffer >> header.packet_type;
+            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;
         }
 
         // return packet type
         template<typename RtdeContent>
-        awaitable<uint8_t> receive_rtde_packet(RtdeContent &content) {
-            auto header = co_await receive_rtde_packet();
-            rtde_buffer >> content;
+        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;
         }
 
-        awaitable<bool> negotiate_protocol() {
-            co_await send_rtde_packet(RTDE_REQUEST_PROTOCOL_VERSION,
+        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(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) {
                 // TODO show log
@@ -350,10 +369,10 @@ namespace sophiar {
             co_return true;
         }
 
-        awaitable<void> print_ur_control_version() {
-            co_await send_rtde_packet(RTDE_GET_URCONTROL_VERSION);
+        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(control_version_result);
+            auto ret_packet_type = co_await receive_rtde_packet(buffer, control_version_result);
             assert(ret_packet_type == RTDE_GET_URCONTROL_VERSION);
             SPDLOG_INFO("Get URControl version: V{}.{}.{}.{}",
                         control_version_result.major, control_version_result.minor,
@@ -361,15 +380,16 @@ namespace sophiar {
             co_return;
         }
 
-        awaitable<bool> setup_inputs() {
+        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(RTDE_CONTROL_PACKAGE_SETUP_INPUTS,
+            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(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;
             if (inputs_recipe_id == 0) {
@@ -379,16 +399,17 @@ namespace sophiar {
             co_return true;
         }
 
-        awaitable<bool> setup_outputs() {
+        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(RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS,
+            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(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;
             if (outputs_recipe_id == 0) {
@@ -398,10 +419,10 @@ namespace sophiar {
             co_return true;
         }
 
-        awaitable<bool> start_report() {
-            co_await send_rtde_packet(RTDE_CONTROL_PACKAGE_START);
+        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(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) {
                 // TODO show log
@@ -410,64 +431,125 @@ namespace sophiar {
             co_return true;
         }
 
-        awaitable<void> pause_report() {
-            co_await send_rtde_packet(RTDE_CONTROL_PACKAGE_PAUSE);
+        awaitable<void> install_program() {
+            // stop moving
+            auto no_motion = ur_command::new_stop_motion();
+            co_await ur_command_queue.async_send(error_code{}, std::move(no_motion), use_awaitable);
+            // open urscript socket
+            tcp::socket urscript_socket(q_this->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);
             // let handle_packages() read the result
             co_return;
         }
 
-        void handle_data_package() {
+        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++;
+            cmd->tcp_pose = ur_tcp; // TODO 让外部提供 tcp_pose
+            // 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);
+        }
+
+        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);
-            std::construct_at(status_wrapper_ptr);
             rtde_data_package_content<ur_status_wrapper &> data_packet{
                     .content = *status_wrapper_ptr,
             };
-            rtde_buffer >> data_packet;
+            buffer >> data_packet;
             assert(data_packet.recipe_id == outputs_recipe_id);
             ur_status_signal.emit_signal(status);
         }
 
-        void handle_text_message() {
+        void handle_text_message(versatile_buffer &buffer) {
             rtde_text_message_content text_message;
-            rtde_buffer >> 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;
+
                 // receive package
                 rtde_packet_header header;
                 try {
-                    header = co_await receive_rtde_packet();
+                    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);
-                    co_return;
+                    break;
                 }
 
                 switch (header.packet_type) {
                     case RTDE_CONTROL_PACKAGE_PAUSE: {
                         rtde_pause_result_content pause_result;
-                        rtde_buffer >> pause_result;
+                        buffer >> pause_result;
                         assert(pause_result.accepted == 1);
-                        co_return;
+                        break;
                     }
                     case RTDE_DATA_PACKAGE:
-                        handle_data_package();
+                        handle_data_package(buffer);
                         break;
                     case RTDE_TEXT_MESSAGE:
-                        handle_text_message();
+                        handle_text_message(buffer);
                         break;
                     default:
-                        rtde_buffer.reset();
+                        buffer.reset();
                         // TODO show log
                         assert(false);
                         break;
                 }
+
+                // check if exit
+                if (header.packet_type == RTDE_CONTROL_PACKAGE_PAUSE) break;
+            }
+
+            handle_packages_finished_signal.try_notify_all();
+            co_return;
+        }
+
+        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;
         }
 
         awaitable<bool> on_init_impl() {
@@ -477,22 +559,48 @@ namespace sophiar {
             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())
+            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() {
-            CO_ENSURE(start_report())
+            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;
         }
 
-        impl() {
-            ur_tcp = ur_vector6d_obj::new_instance();
-            ur_tcp->get_view().fill({0, 0, 0, 0, 0, 0});
+        awaitable<void> on_stop_impl() {
+            versatile_buffer buffer;
+            try {
+                co_await pause_report(buffer);
+            } catch (std::exception &e) {
+                SPDLOG_ERROR("Failed to stop UR interface: {}", e.what());
+            }
+            stop_requested_signal.try_notify_all();
+            co_await (handle_packages_finished_signal.coro_wait(*handle_packages_finished_token) &&
+                      handle_command_request_finished_signal.coro_wait(*handle_command_request_finished_token));
+            co_return;
+        }
+
+        impl()
+                : ur_command_queue(ur_interface::get_context(), ur_command_queue_size),
+                  ur_command_muxer(ur_command_handler),
+                  stop_requested_signal(ur_interface::get_context()),
+                  handle_packages_finished_signal(ur_interface::get_context()),
+                  handle_command_request_finished_signal(ur_interface::get_context()) {
+            ur_command_handler.p_this = this;
         }
 
     };
@@ -507,14 +615,27 @@ namespace sophiar {
         pimpl->report_frequency = freq;
     }
 
-    void ur_interface::set_tcp(ur_vector6d_pointer tcp) {
-        pimpl->ur_tcp = std::move(tcp);
+    void ur_interface::set_tcp(const vector6d &tcp) {
+        pimpl->ur_tcp = tcp;
     }
 
-    void ur_interface::add_status_listener(ur_status_signal_type::listener_type &node) {
+    void ur_interface::add_status_listener(ur_status_listener_type &node) {
         pimpl->ur_status_signal.add_listener(node);
     }
 
+    ur_command_listener_type &ur_interface::get_command_listener(const switchable_muxer::uuid_type &eid) {
+        return pimpl->ur_command_muxer.get_listener(eid);
+    }
+
+    switchable_muxer *ur_interface::get_signal_switcher(uint8_t sid) {
+        switch (sid) {
+            case 0:
+                return &pimpl->ur_command_muxer;
+            default:
+                return nullptr;
+        }
+    }
+
     awaitable<bool> ur_interface::on_init() {
         try {
             CO_ENSURE(pimpl->on_init_impl())
@@ -538,11 +659,7 @@ namespace sophiar {
     }
 
     boost::asio::awaitable<void> ur_interface::on_stop() {
-        try {
-            co_await pimpl->pause_report();
-        } catch (std::exception &e) {
-            SPDLOG_ERROR("Failed to start UR interface: {}", e.what());
-        }
+        co_await pimpl->on_stop_impl();
         co_await tristate_obj::on_stop();
         co_return;
     }

src/robot/ur/ur_interface.h

@@ -5,9 +5,11 @@
 #include "core/small_obj.hpp"
 #include "core/tristate_obj.h"
 #include "core/types/vectorx.hpp"
+#include "utility/signal_muxer.hpp"
 #include "utility/tiny_signal.hpp"
 
 #include <boost/asio/ip/address.hpp>
+#include <boost/uuid/uuid.hpp>
 
 #include <memory>
 
@@ -54,14 +56,11 @@ namespace sophiar {
         static constexpr ur_safety_mode_type UNDEFINED_SAFETY_MODE = 11;
     }
 
-    using ur_vector6d_obj = vector6d_obj<ur_freq_tag>;
-    using ur_vector6d_pointer = ur_vector6d_obj::pointer;
-
     struct ur_status_content {
-        ur_vector6d_pointer target_q, target_qd, target_qdd;
-        ur_vector6d_pointer actual_q, actual_qd;
-        ur_vector6d_pointer actual_TCP_pose, actual_TCP_speed;
-        ur_vector6d_pointer target_TCP_pose, target_TCP_speed;
+        vector6d target_q, target_qd, target_qdd;
+        vector6d actual_q, actual_qd;
+        vector6d actual_TCP_pose, actual_TCP_speed;
+        vector6d target_TCP_pose, target_TCP_speed;
         double timestamp;
         int32_t robot_mode;
         uint32_t runtime_state;
@@ -74,30 +73,98 @@ namespace sophiar {
                        public small_obj<ur_status> {
     };
 
-    using ur_status_signal_type = tiny_signal<ur_status::pointer>;
+    // tcp_pose and cmd_id will be filled by ur_interface
+    struct ur_command_content {
+        int32_t move_type, cmd_id;
+        vector6d tcp_pose, target;
+        double param_a, param_v;
+    };
 
-    class ur_interface : public tristate_obj<ur_freq_tag> {
-    public:
+    struct ur_command : public ur_command_content,
+                        public small_obj<ur_command> {
 
         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
 
+        static pointer new_stop_motion() {
+            auto ret = new_instance();
+            ret->move_type = 0;
+            ret->param_a = 0;
+            ret->param_v = 0;
+            return ret;
+        }
+
+        static pointer new_joint_angle(const vector6d &angle,
+                                       double ja = a_joint_default,
+                                       double jv = v_joint_default) {
+            auto ret = new_instance();
+            ret->move_type = 1;
+            ret->target = angle;
+            ret->param_a = ja;
+            ret->param_v = jv;
+            return ret;
+        }
+
+        static pointer new_joint_speed(const vector6d &speed,
+                                       double ja = a_joint_default) {
+            auto ret = new_instance();
+            ret->move_type = 3;
+            ret->target = speed;
+            ret->param_a = ja;
+            ret->param_v = 0;
+            return ret;
+        }
+
+        static pointer new_tool_pose(const vector6d &pose,
+                                     double ta = a_tool_default,
+                                     double tv = v_tool_default) {
+            auto ret = new_instance();
+            ret->move_type = 2;
+            ret->target = pose;
+            ret->param_a = ta;
+            ret->param_v = tv;
+            return ret;
+        }
+
+        static pointer new_tcp_speed(const vector6d &speed,
+                                     double ta = a_tool_default) {
+            auto ret = new_instance();
+            ret->move_type = 4;
+            ret->target = speed;
+            ret->param_a = ta;
+            ret->param_v = 0;
+            return ret;
+        }
+    };
+
+    using ur_status_signal_type = tiny_signal<ur_status::pointer>;
+    using ur_command_signal_type = tiny_signal<ur_command::pointer>;
+    using ur_status_listener_type = ur_status_signal_type::listener_type;
+    using ur_command_listener_type = ur_command_signal_type::listener_type;
+
+    class ur_interface : public tristate_obj<ur_freq_tag> {
+    public:
+
         using ip_address_type = boost::asio::ip::address;
 
         ur_interface();
 
         ~ur_interface() override;
 
-        void set_ur_ip(const ip_address_type &ip);
+        void set_ur_ip(const ip_address_type &ip); // TODO be private
 
         // default 125Hz
-        void set_report_frequency(double freq);
+        void set_report_frequency(double freq); // TODO be private
+
+        void add_status_listener(ur_status_listener_type &node);
+
+        ur_command_listener_type &get_command_listener(const switchable_muxer::uuid_type &eid);
 
-        void add_status_listener(ur_status_signal_type::listener_type &node);
+        switchable_muxer *get_signal_switcher(uint8_t sid); // signal id
 
-        void set_tcp(ur_vector6d_pointer tcp);
+        void set_tcp(const vector6d &tcp); // TODO 让外部提供
 
     protected:
 

src/utility/coro_signal.hpp

@@ -45,6 +45,8 @@ namespace sophiar {
             coro_signal &signal;
             bool is_fulfilled = false;
 
+            friend class coro_signal;
+
         };
 
         coro_signal(boost::asio::io_context &context)
@@ -54,7 +56,12 @@ namespace sophiar {
             return signal_token{*this};
         }
 
+        auto new_token_by_ptr() {
+            return new signal_token{*this};
+        }
+
         boost::asio::awaitable<void> coro_wait(signal_token &token) {
+            if (token.is_fulfilled) co_return;
             co_await channel.async_receive(boost::asio::use_awaitable);
             token.fulfill();
             try_notify_more();

src/utility/signal_muxer.hpp

@@ -0,0 +1,72 @@
+#ifndef SOPHIAR2_SIGNAL_MUXER_HPP
+#define SOPHIAR2_SIGNAL_MUXER_HPP
+
+#include "utility/tiny_signal.hpp"
+
+#include <boost/uuid/uuid.hpp>
+#include <boost/container_hash/hash.hpp>
+
+#include <typeindex>
+#include <unordered_map>
+
+namespace sophiar {
+
+    // 只做引用，所以不定义虚的析构函数
+    struct switchable_muxer {
+        using uuid_type = boost::uuids::uuid;
+
+        virtual void set_switch_state(const uuid_type &eid, bool is_enabled) = 0;
+    };
+
+    template<typename... Args>
+    class signal_muxer : public switchable_muxer {
+    public:
+
+        using signal_type = tiny_signal<Args...>;
+        using listener_type = typename signal_type::listener_type;
+
+        class proxy_listener : public listener_type {
+        public:
+            proxy_listener() = default;
+
+            explicit proxy_listener(listener_type &target)
+                    : listener(&target) {}
+
+            virtual void on_signal_received(Args... args) {
+                if (!is_enabled) return;
+                listener->on_signal_received(std::move(args)...);
+            }
+
+        private:
+            listener_type *listener;
+            bool is_enabled = true;
+
+            friend class signal_muxer<Args...>;
+        };
+
+        explicit signal_muxer(listener_type &target)
+                : listener(target) {}
+
+        listener_type &get_listener(const uuid_type &eid) {
+            if (!listener_map.contains(eid)) {
+                listener_map.emplace(std::piecewise_construct,
+                                     std::forward_as_tuple(eid),
+                                     std::forward_as_tuple(listener));
+            }
+            return listener_map[eid];
+        }
+
+        void set_switch_state(const uuid_type &eid, bool is_enabled) override {
+            assert(listener_map.contains(eid));
+            listener_map[eid].is_enabled = is_enabled;
+        }
+
+    private:
+        listener_type &listener;
+        std::unordered_map<boost::uuids::uuid, proxy_listener, boost::hash<uuid_type>> listener_map;
+
+    };
+
+}
+
+#endif //SOPHIAR2_SIGNAL_MUXER_HPP

src/utility/tiny_signal.hpp

@@ -5,37 +5,51 @@
 
 #include <type_traits>
 
-template<typename... Args>
-class tiny_signal {
-public:
+namespace sophiar {
 
-    using hook_type = boost::intrusive::list_base_hook<
-            boost::intrusive::link_mode<
-                    boost::intrusive::auto_unlink>>;
+    template<typename... Args>
+    class signal_muxer;
 
-    struct listener_type : public hook_type {
-        virtual void on_signal_received(Args... args) = 0;
-    };
+    template<typename... Args>
+    class tiny_signal {
+    public:
+
+        using muxer_type = signal_muxer<Args...>;
+
+        using hook_type = boost::intrusive::list_base_hook<
+                boost::intrusive::link_mode<
+                        boost::intrusive::auto_unlink>>;
+
+        // 其他对象不得持有 listener
+        // 所有调用应该以引用进行
+        struct listener_type : public hook_type {
 
-    using list_type = boost::intrusive::list<
-            listener_type, boost::intrusive::constant_time_size<false>>;
+            virtual ~listener_type() = default;
 
-    void add_listener(listener_type &node) {
-        if (node.is_linked()) {
-            node.unlink();
+            virtual void on_signal_received(Args... args) = 0;
+        };
+
+        using list_type = boost::intrusive::list<
+                listener_type, boost::intrusive::constant_time_size<false>>;
+
+        void add_listener(listener_type &node) {
+            if (node.is_linked()) {
+                node.unlink();
+            }
+            node_list.push_back(node);
         }
-        node_list.push_back(node);
-    }
 
-    void emit_signal(Args... args) {
-        for (auto &node: node_list) {
-            node.on_signal_received(args...);
+        void emit_signal(Args... args) {
+            for (auto &node: node_list) {
+                node.on_signal_received(args...);
+            }
         }
-    }
 
-private:
-    list_type node_list;
+    private:
+        list_type node_list;
+
+    };
 
-};
+}
 
 #endif //SOPHIAR2_TINY_SIGNAL_HPP