Sophiar2/src/algorithm/transform_tree.cpp

#include "core/basic_obj_types.hpp"
#include "core/global_defs.h"
#include "utility/bit_operations.hpp"
#include "utility/config_utility.hpp"
#include "utility/coro_worker.hpp"
#include "utility/coro_worker_helper_func.hpp"
#include "utility/coro_signal_group.hpp"
#include "utility/named_vector.hpp"

#include <boost/asio/co_spawn.hpp>
#include <boost/asio/detached.hpp>

#include <Eigen/Geometry>

#include <list>
#include <optional>
#include <string>
#include <vector>
#include <utility>

#include "utility/assert_utility.h"

DEFAULT_TRISTATE_OBJ_DEF(transform_tree)

namespace sophiar {

    using namespace Eigen;

    using boost::asio::co_spawn;
    using boost::asio::detached;

    struct transform_tree::impl {

        using node_index_type = uint8_t;
        using trans_type = Isometry3d;
        static_assert(std::is_same<trans_type, decltype(std::declval<transform_obj>().value)>());

        struct node_type { // 在父亲下观察自己
            node_index_type parent = -1, depth = 0; // 父亲的编号, 节点深度
            variable_index_type transform_var_index = -1;
            trans_type fixed_transform; // 如果不提供 transform_var_index, 则表示这是一个固定变换
        };

        struct query_scheme {
            std::list<node_index_type> left, right;
        };

        named_vector<node_index_type, node_type> nodes;
        std::vector<coro_worker::pointer> worker_list;

        std::optional<trans_type> get_transform_from_path(const std::list<node_index_type> &path) const {
            auto ret = trans_type::Identity();
            for (auto index: path) {
                const auto &node_info = nodes[index];
                if (!is_valid_id(node_info.transform_var_index)) {
                    ret = ret * node_info.fixed_transform;
                } else {
                    auto trans_obj = QUERY_VARIABLE(transform_obj, node_info.transform_var_index);
                    if (trans_obj == nullptr) return std::nullopt;
                    ret = ret * trans_obj->value;
                }
            }
            return ret;
        }

        std::optional<trans_type> get_transform_from_scheme(const query_scheme &scheme) const {
            auto left_trans = get_transform_from_path(scheme.left);
            auto right_trans = get_transform_from_path(scheme.right);
            if (!left_trans.has_value() || !right_trans.has_value()) return std::nullopt;
            return left_trans.value().inverse() * right_trans.value();
        }

        void create_worker(node_index_type observer,
                           node_index_type target,
                           variable_index_type out_var_index) {
            assert(is_valid_id(observer) && is_valid_id(target));
            assert(observer != target);

            query_scheme scheme;
            auto signal_group = coro_signal_any_group::new_instance();
            while (observer != target) {
                const auto &obs_info = nodes[observer];
                const auto &tar_info = nodes[target];
                if (obs_info.depth > tar_info.depth) {
                    assert(is_valid_id(obs_info.parent));
                    scheme.left.push_front(observer);
                    if (is_valid_id(obs_info.transform_var_index)) {
                        signal_group->add_watcher(REQUIRE_VARIABLE_WATCHER(obs_info.transform_var_index));
                    }
                    observer = obs_info.parent;
                } else { // target is deeper
                    assert(is_valid_id(tar_info.parent));
                    scheme.right.push_front(target);
                    if (is_valid_id(tar_info.transform_var_index)) {
                        signal_group->add_watcher(REQUIRE_VARIABLE_WATCHER(tar_info.transform_var_index));
                    }
                    target = tar_info.parent;
                }
            }

            signal_group->start();
            auto signal_watcher = signal_group->new_watcher();
            auto exit_func = SIGNAL_GROUP_AUTO_CLOSER(signal_group);

            auto worker_func = [=, this,
                    first_run = true,
                    scheme = std::move(scheme),
                    signal_watcher = std::move(signal_watcher)]() mutable
                    -> boost::asio::awaitable<bool> {
                if (!first_run) { // set initial value for watched transform
                    co_await signal_watcher.coro_wait();
                }
                auto new_trans = get_transform_from_scheme(scheme);
                auto new_trans_obj = new_trans.has_value() ?
                                     transform_obj::new_instance(std::move(new_trans.value())) : nullptr;
                UPDATE_VARIABLE(transform_obj, out_var_index, new_trans_obj);
                first_run = false;
                co_return true;
            };
            auto worker = make_infinite_coro_worker(std::move(worker_func),
                                                    std::move(exit_func)); // signal group will be deleted here
            worker->run();
            worker_list.push_back(std::move(worker));
        }

        boost::asio::awaitable<void> stop_workers() {
            for (auto &worker: worker_list) {
                worker->cancel();
            }
            for (auto &worker: worker_list) {
                co_await worker->coro_wait_stop();
            }
            worker_list.clear();
            co_return;
        }

        void build_tree(const nlohmann::json &config) {
            ENSURE_ARRAY("node_list");
            for (auto &node: config["node_list"]) {

                auto name = LOAD_STRING_ITEM2(node, "name");
                auto index = nodes.new_elem(name);

                node_index_type parent_index = -1;
                if (node.contains("parent")) {
                    auto parent_name = LOAD_STRING_ITEM2(node, "parent");
                    parent_index = nodes.to_index_by_name(parent_name);
                }

                variable_index_type transform_var_index = invalid_variable_index;
                if (node.contains("transform_var_name")) {
                    transform_var_index = LOAD_VARIABLE_INDEX2(node, transform_obj, "transform_var_name");
                }

                trans_type default_trans = trans_type::Identity();
                if (node.contains("transform")) {
                    default_trans = transform_obj::read_value_from_json_array(node["transform"]);
                } else if (is_valid_id(parent_index)) {
                    assert(is_valid_id(transform_var_index));
                }

                auto &node_info = nodes[index];
                node_info.parent = parent_index;
                node_info.transform_var_index = transform_var_index;
                node_info.fixed_transform = default_trans;
                if (is_valid_id(parent_index)) {
                    node_info.depth = nodes[parent_index].depth + 1;
                }
            }
        }

        void config_watcher(const nlohmann::json &config) {
            ENSURE_ARRAY("watch_list");
            for (auto &watch_config: config["watch_list"]) {
                auto target_name = LOAD_STRING_ITEM2(watch_config, "target");
                auto observer_name = LOAD_STRING_ITEM2(watch_config, "observer");
                auto target_index = nodes.to_index_by_name(target_name);
                auto observer_index = nodes.to_index_by_name(observer_name);
                auto transform_obj_index = LOAD_VARIABLE_INDEX2(watch_config, transform_obj, "transform_var_name");
                create_worker(observer_index, target_index, transform_obj_index);
            }
        }

    };

    transform_tree::transform_tree()
            : pimpl(std::make_unique<impl>()) {}

    transform_tree::~transform_tree() = default;

    boost::asio::awaitable<bool> transform_tree::on_init(const nlohmann::json &config) noexcept {
        pimpl->build_tree(config);
        co_return true;
    }

    boost::asio::awaitable<bool> transform_tree::on_start(const nlohmann::json &config) noexcept {
        pimpl->config_watcher(config);
        co_return true;
    }

    boost::asio::awaitable<void> transform_tree::on_stop() noexcept {
        co_await pimpl->stop_workers();
        co_return;
    }

    boost::asio::awaitable<void> transform_tree::on_reset() noexcept {
        pimpl->nodes.clear();
        co_return;
    }

}