Feat/new tasks and controller

CMakeLists.txt

@@ -51,6 +51,7 @@ target_sources(
     PRIVATE
         src/cxx/context.cc
         src/cxx/navigation.cc
+        src/cxx/navigation_gimbal_controller.cc
         src/cxx/component.cc
 )
 target_include_directories(

src/cxx/component.cc

@@ -6,8 +6,10 @@
 
 #include "cxx/context.hh"
 #include "cxx/navigation.hh"
+#include "cxx/navigation_gimbal_controller.hh"
 #include "cxx/util/node_mixin.hh"
 #include <filesystem>
+#include <tuple>
 
 #include <Eigen/Geometry>
 #include <ament_index_cpp/get_package_share_directory.hpp>
@@ -32,6 +34,9 @@ class Navigation
 
     bool mock_context = false;
 
+    bool chassis_vel_override_active_ = false;
+    Eigen::Vector2d chassis_vel_override_ = Eigen::Vector2d::Zero();
+
     std::atomic<std::uint16_t> lua_tick_count = 0;
     std::unique_ptr<sol::state> lua;
     sol::table lua_blackboard;
@@ -42,19 +47,28 @@ class Navigation
 
     details::Context context;
     details::Navigation navigation;
+    details::NavigationGimbalController gimbal_controller;
 
     struct Command {
         OutputInterface<Eigen::Vector2d> chassis_speed;
+        OutputInterface<rmcs_msgs::ChassisMode> chassis_mode;
+        OutputInterface<bool> enable_local_obstacle;
+        OutputInterface<double> base_link_yaw;
         OutputInterface<std::size_t> nod_count;
-        OutputInterface<bool> rotate_chassis;
         OutputInterface<bool> detect_targets;
         OutputInterface<bool> enable_autoaim;
 
         auto init(Navigation& component) -> void {
             component.register_output(
                 "/rmcs_navigation/chassis_velocity", chassis_speed, Eigen::Vector2d::Zero());
+            component.register_output(
+                "/rmcs_navigation/chassis_mode", chassis_mode, rmcs_msgs::ChassisMode::AUTO);
+            component.register_output(
+                "/rmcs_navigation/enable_local_obstacle", enable_local_obstacle, true);
+            component.register_output(
+                "/rmcs_navigation/base_link_yaw", base_link_yaw,
+                std::numeric_limits<double>::quiet_NaN());
             component.register_output("/rmcs_navigation/nod_count", nod_count, 0);
-            component.register_output("/rmcs_navigation/rotate_chassis", rotate_chassis, false);
             component.register_output("/rmcs_navigation/detect_targets", detect_targets, false);
             component.register_output("/rmcs_navigation/start_autoaim", enable_autoaim, false);
         }
@@ -93,14 +107,39 @@ class Navigation
             navigation.switch_topic_forward(enable);
         });
         api.set_function("update_gimbal_direction", [this](double angle) {
-            warn("unimplement: update_gimbal_direction({})", angle);
+            gimbal_controller.set_lua_target_yaw(angle);
         });
         api.set_function("update_chassis_mode", [this](const std::string& mode) {
-            warn("unimplement: update_chassis_mode(\"{}\")", mode);
+            if (mode == "auto") {
+                *command.chassis_mode = rmcs_msgs::ChassisMode::AUTO;
+            } else if (mode == "spin") {
+                *command.chassis_mode = rmcs_msgs::ChassisMode::SPIN;
+            } else if (mode == "step_down") {
+                *command.chassis_mode = rmcs_msgs::ChassisMode::STEP_DOWN;
+            } else if (mode == "launch_ramp") {
+                *command.chassis_mode = rmcs_msgs::ChassisMode::LAUNCH_RAMP;
+            } else {
+                warn("unknown chassis mode \"{}\", fallback to AUTO", mode);
+                *command.chassis_mode = rmcs_msgs::ChassisMode::AUTO;
+            }
         });
         api.set_function("update_chassis_vel", [this](double x, double y) {
+            if (chassis_vel_override_active_)
+                return;
             *command.chassis_speed = Eigen::Vector2d{x, y};
         });
+        api.set_function(
+            "set_local_obstacle", [this](bool enable) { *command.enable_local_obstacle = enable; });
+        api.set_function("cancel_target", [this]() { navigation.cancel_target(); });
+        api.set_function("set_chassis_vel_override", [this](double vx, double vy) {
+            chassis_vel_override_ = Eigen::Vector2d{vx, vy};
+            chassis_vel_override_active_ = true;
+            *command.chassis_speed = chassis_vel_override_;
+        });
+        api.set_function("clear_chassis_vel_override", [this]() {
+            chassis_vel_override_active_ = false;
+            *command.chassis_speed = Eigen::Vector2d::Zero();
+        });
     }
 
     auto make_option_injection() {
@@ -149,22 +188,43 @@ class Navigation
     auto lua_sync() {
         const auto [x, y, yaw] = navigation.check_position();
 
-        auto user = lua_blackboard["user"].get<sol::table>();
-        user["health"] = *context.robot_health;
-        user["bullet"] = *context.robot_bullet;
-        user["chassis_power_limit"] = *context.chassis_power_limit_referee;
+        if (!lua_blackboard.valid() || lua_blackboard.get_type() != sol::type::table) {
+            warn("lua blackboard is invalid, skip sync");
+            return;
+        }
+
+        auto user_object = lua_blackboard["user"];
+        auto game_object = lua_blackboard["game"];
+        auto play_object = lua_blackboard["play"];
+        auto meta_object = lua_blackboard["meta"];
+        if (user_object.get_type() != sol::type::table || game_object.get_type() != sol::type::table
+            || play_object.get_type() != sol::type::table
+            || meta_object.get_type() != sol::type::table) {
+            warn("lua blackboard schema is invalid, skip sync");
+            return;
+        }
+
+        auto user = user_object.get<sol::table>();
+        user["health"] = context.robot_health.ready() ? *context.robot_health : std::uint16_t{0};
+        user["bullet"] = context.robot_bullet.ready() ? *context.robot_bullet : std::uint16_t{0};
+        user["chassis_power_limit"] = context.chassis_power_limit_referee.ready()
+                                        ? *context.chassis_power_limit_referee
+                                        : 0.0;
         user["x"] = x;
         user["y"] = y;
         user["yaw"] = yaw;
 
-        auto game = lua_blackboard["game"].get<sol::table>();
-        game["stage"] = rmcs_msgs::to_string(*context.game_stage);
+        auto game = game_object.get<sol::table>();
+        game["stage"] =
+            context.game_stage.ready() ? rmcs_msgs::to_string(*context.game_stage) : "UNKNOWN";
 
-        auto play = lua_blackboard["play"].get<sol::table>();
-        play["rswitch"] = rmcs_msgs::to_string(*context.switch_right);
-        play["lswitch"] = rmcs_msgs::to_string(*context.switch_left);
+        auto play = play_object.get<sol::table>();
+        play["rswitch"] =
+            context.switch_right.ready() ? rmcs_msgs::to_string(*context.switch_right) : "UNKNOWN";
+        play["lswitch"] =
+            context.switch_left.ready() ? rmcs_msgs::to_string(*context.switch_left) : "UNKNOWN";
 
-        auto meta = lua_blackboard["meta"].get<sol::table>();
+        auto meta = meta_object.get<sol::table>();
         meta["timestamp"] = this->now().seconds();
     }
 
@@ -197,6 +257,10 @@ class Navigation
         lua_on_init = (*lua)["on_init"];
         lua_on_tick = (*lua)["on_tick"];
 
+        if (!lua_blackboard.valid() || lua_blackboard.get_type() != sol::type::table) {
+            throw std::runtime_error("lua endpoint must define blackboard table");
+        }
+
         const auto situation = std::array{
             lua_on_init.valid(),
             lua_on_tick.valid(),
@@ -228,12 +292,16 @@ class Navigation
     explicit Navigation()
         : rclcpp::Node{get_component_name(), option()}
         , context{*this, *this}
-        , navigation{*this} {
+        , navigation{*this}
+        , gimbal_controller{*this, navigation} {
 
         mock_context = param<bool>("mock_context");
 
+        gimbal_controller.init_params();
+
         context.init(io_mutex, mock_context);
         command.init(*this);
+        gimbal_controller.init_output(*this);
 
         lua_init();
 
@@ -258,6 +326,8 @@ class Navigation
             lua_sync();
             lua_tick();
         }
+
+        gimbal_controller.update();
     }
 };