add a navigator controller

src/navigator/gnc/navigator_controller/CMakeLists.txt

@@ -0,0 +1,33 @@
+cmake_minimum_required(VERSION 3.8)
+project(navigator_controller)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+set(CMAKE_CXX_STANDARD 20)
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(navigator_msgs REQUIRED)
+find_package(ftxui REQUIRED)
+
+include_directories(include)
+
+set(DEPS rclcpp geometry_msgs std_msgs navigator_msgs)
+
+add_executable(server_node src/server_node.cpp)
+ament_target_dependencies(server_node ${DEPS})
+
+add_executable(keyboard_client_node src/keyboard_client_node.cpp)
+ament_target_dependencies(keyboard_client_node ${DEPS})
+target_link_libraries(keyboard_client_node ftxui::component ftxui::dom
+                      ftxui::screen)
+
+install(TARGETS server_node keyboard_client_node
+        DESTINATION lib/${PROJECT_NAME})
+
+ament_package()

src/navigator/gnc/navigator_controller/include/navigator_controller/client.h

@@ -0,0 +1,38 @@
+#pragma once
+
+#include <rclcpp/node.hpp>
+#include <rclcpp/publisher.hpp>
+
+#include <navigator_msgs/msg/wrench_named.hpp>
+
+namespace navigator_controller
+{
+
+class Client : public rclcpp::Node
+{
+  public:
+    Client(std::string const& name) : rclcpp::Node(name)
+    {
+        wrench_pub_ = create_publisher<navigator_msgs::msg::WrenchNamed>("/wrench", 10);
+    }
+
+  protected:
+    bool control_wrench(double fx, double fy, double fz, double tx, double ty, double tz)
+    {
+        navigator_msgs::msg::WrenchNamed msg;
+        msg.source_node = get_name();
+        msg.wrench.force.x = fx;
+        msg.wrench.force.y = fy;
+        msg.wrench.force.z = fz;
+        msg.wrench.torque.x = tx;
+        msg.wrench.torque.y = ty;
+        msg.wrench.torque.z = tz;
+        wrench_pub_->publish(msg);
+        return true;
+    }
+
+  private:
+    rclcpp::Publisher<navigator_msgs::msg::WrenchNamed>::SharedPtr wrench_pub_;
+};
+
+}  // namespace navigator_controller

src/navigator/gnc/navigator_controller/package.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>navigator_controller</name>
+  <version>0.0.0</version>
+  <description>The controller for the Navigator</description>
+  <maintainer email="zhangrenzhongzheng@outlook.com">zhongzheng</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>ftxui</depend>
+  <depend>geometry_msgs</depend>
+  <depend>navigator_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/navigator/gnc/navigator_controller/src/keyboard_client_node.cpp

@@ -0,0 +1,195 @@
+#include <atomic>
+#include <thread>
+
+#include <ftxui/component/component.hpp>
+#include <ftxui/component/event.hpp>
+#include <ftxui/component/screen_interactive.hpp>
+#include <ftxui/dom/elements.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include "navigator_controller/client.h"
+
+namespace navigator_controller
+{
+
+class KeyboardClient : public Client
+{
+  public:
+    KeyboardClient() : Client("navigator_keyboard_controller")
+    {
+        declare_parameter("force_x", 100.0);
+        declare_parameter("force_y", 100.0);
+        declare_parameter("torque_z", 100.0);
+        declare_parameter("publish_frequency", 50.0);
+
+        force_x_ = get_parameter("force_x").as_double();
+        force_y_ = get_parameter("force_y").as_double();
+        torque_z_ = get_parameter("torque_z").as_double();
+
+        param_cb_ = add_on_set_parameters_callback(
+            [this](std::vector<rclcpp::Parameter> const& params)
+            {
+                for (auto const& p : params)
+                {
+                    if (p.get_name() == "force_x")
+                        force_x_ = p.as_double();
+                    else if (p.get_name() == "force_y")
+                        force_y_ = p.as_double();
+                    else if (p.get_name() == "torque_z")
+                        torque_z_ = p.as_double();
+                }
+                rcl_interfaces::msg::SetParametersResult result;
+                result.successful = true;
+                return result;
+            });
+
+        double freq = get_parameter("publish_frequency").as_double();
+        auto period = std::chrono::duration<double>(1.0 / freq);
+        timer_ = create_wall_timer(
+            period, [this]() { control_wrench(active(fx_, fx_t_), active(fy_, fy_t_), 0, 0, 0, active(tz_, tz_t_)); });
+    }
+
+    void w_pressed()
+    {
+        fx_ = static_cast<float>(force_x_);
+        fx_t_ = now_ns();
+    }
+    void s_pressed()
+    {
+        fx_ = -static_cast<float>(force_x_);
+        fx_t_ = now_ns();
+    }
+    void a_pressed()
+    {
+        fy_ = static_cast<float>(force_y_);
+        fy_t_ = now_ns();
+    }
+    void d_pressed()
+    {
+        fy_ = -static_cast<float>(force_y_);
+        fy_t_ = now_ns();
+    }
+    void left_arrow_pressed()
+    {
+        tz_ = static_cast<float>(torque_z_);
+        tz_t_ = now_ns();
+    }
+    void right_arrow_pressed()
+    {
+        tz_ = -static_cast<float>(torque_z_);
+        tz_t_ = now_ns();
+    }
+
+  private:
+    static constexpr int64_t KEY_TIMEOUT_NS = 150'000'000;  // 150 ms
+
+    static int64_t now_ns()
+    {
+        return std::chrono::steady_clock::now().time_since_epoch().count();
+    }
+
+    static float active(std::atomic<float> const& val, std::atomic<int64_t> const& t)
+    {
+        return (now_ns() - t.load()) < KEY_TIMEOUT_NS ? val.load() : 0.0f;
+    }
+
+    double force_x_{ 100.0 };
+    double force_y_{ 100.0 };
+    double torque_z_{ 100.0 };
+    rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_cb_;
+
+    std::atomic<float> fx_{ 0.0f };
+    std::atomic<int64_t> fx_t_{ 0 };
+    std::atomic<float> fy_{ 0.0f };
+    std::atomic<int64_t> fy_t_{ 0 };
+    std::atomic<float> tz_{ 0.0f };
+    std::atomic<int64_t> tz_t_{ 0 };
+    rclcpp::TimerBase::SharedPtr timer_;
+};
+
+}  // namespace navigator_controller
+
+void run_ui(std::shared_ptr<navigator_controller::KeyboardClient> client)
+{
+    using namespace ftxui;
+
+    std::vector<std::string> const help_lines = {
+        "Move Forward:         w          ", "Move Backward:        s          ", "Move Port:            a          ",
+        "Move Starboard:       d          ", "Yaw Counterclockwise: arrow left ", "Yaw Clockwise:        arrow right",
+        "Quit:                 q          ",
+    };
+
+    auto screen = ScreenInteractive::Fullscreen();
+    auto exit = screen.ExitLoopClosure();
+
+    auto renderer = Renderer(
+        [&help_lines]()
+        {
+            Elements rows;
+            for (auto const& line : help_lines)
+                rows.push_back(text(line));
+
+            return vbox({
+                text(" Navigator Keyboard Controller ") | bold | center,
+                separator(),
+                vbox(rows) | border,
+            });
+        });
+
+    auto component = CatchEvent(renderer,
+                                [=](Event event) -> bool
+                                {
+                                    if (event == Event::Character('q'))
+                                    {
+                                        rclcpp::shutdown();
+                                        exit();
+                                        return true;
+                                    }
+                                    if (event == Event::Character('w'))
+                                    {
+                                        client->w_pressed();
+                                        return true;
+                                    }
+                                    if (event == Event::Character('s'))
+                                    {
+                                        client->s_pressed();
+                                        return true;
+                                    }
+                                    if (event == Event::Character('a'))
+                                    {
+                                        client->a_pressed();
+                                        return true;
+                                    }
+                                    if (event == Event::Character('d'))
+                                    {
+                                        client->d_pressed();
+                                        return true;
+                                    }
+                                    if (event == Event::ArrowLeft)
+                                    {
+                                        client->left_arrow_pressed();
+                                        return true;
+                                    }
+                                    if (event == Event::ArrowRight)
+                                    {
+                                        client->right_arrow_pressed();
+                                        return true;
+                                    }
+                                    return false;
+                                });
+
+    screen.Loop(component);
+}
+
+int main(int argc, char* argv[])
+{
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<navigator_controller::KeyboardClient>();
+
+    std::thread ui_thread(run_ui, node);
+    rclcpp::spin(node);
+    rclcpp::shutdown();
+
+    ui_thread.join();
+    return 0;
+}

src/navigator/gnc/navigator_controller/src/server_node.cpp

@@ -0,0 +1,63 @@
+#include <unordered_map>
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <geometry_msgs/msg/wrench_stamped.hpp>
+#include <navigator_msgs/msg/wrench_named.hpp>
+
+namespace navigator_controller
+{
+
+class Server : public rclcpp::Node
+{
+  public:
+    Server() : rclcpp::Node("navigator_controller_server")
+    {
+        declare_parameter("publish_frequency", 50.0);
+        double freq = get_parameter("publish_frequency").as_double();
+
+        wrench_sub_ = create_subscription<navigator_msgs::msg::WrenchNamed>(
+            "/wrench", 10, [this](navigator_msgs::msg::WrenchNamed::SharedPtr msg)
+            { latest_wrenches_[msg->source_node] = msg->wrench; });
+
+        wrench_pub_ = create_publisher<geometry_msgs::msg::WrenchStamped>("/wrench_combined", 10);
+
+        auto period = std::chrono::duration<double>(1.0 / freq);
+        timer_ = create_wall_timer(period, [this]() { publish_summed_wrench(); });
+    }
+
+  private:
+    void publish_summed_wrench()
+    {
+        geometry_msgs::msg::WrenchStamped out;
+        out.header.stamp = now();
+        out.header.frame_id = "base_link";
+
+        for (auto const& [name, w] : latest_wrenches_)
+        {
+            out.wrench.force.x += w.force.x;
+            out.wrench.force.y += w.force.y;
+            out.wrench.force.z += w.force.z;
+            out.wrench.torque.x += w.torque.x;
+            out.wrench.torque.y += w.torque.y;
+            out.wrench.torque.z += w.torque.z;
+        }
+
+        wrench_pub_->publish(out);
+    }
+
+    std::unordered_map<std::string, geometry_msgs::msg::Wrench> latest_wrenches_;
+    rclcpp::Subscription<navigator_msgs::msg::WrenchNamed>::SharedPtr wrench_sub_;
+    rclcpp::Publisher<geometry_msgs::msg::WrenchStamped>::SharedPtr wrench_pub_;
+    rclcpp::TimerBase::SharedPtr timer_;
+};
+
+}  // namespace navigator_controller
+
+int main(int argc, char* argv[])
+{
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<navigator_controller::Server>());
+    rclcpp::shutdown();
+    return 0;
+}

src/navigator/gnc/navigator_thruster_mapper/src/thruster_mapper_node.cpp

@@ -122,7 +122,7 @@ class ThrusterMapperNode : public rclcpp::Node
 
         // Subscribe to the wrench
         wrench_sub_ = this->create_subscription<geometry_msgs::msg::WrenchStamped>(
-            "/wrench/cmd", 1, std::bind(&ThrusterMapperNode::wrench_cb, this, std::placeholders::_1));
+            "/wrench_combined", 1, std::bind(&ThrusterMapperNode::wrench_cb, this, std::placeholders::_1));
 
         // Subscribe to kill alarm
         kill_sub_ = this->create_subscription<std_msgs::msg::Bool>(

src/navigator/navigator_bringup/launch/gazebo.launch.py

@@ -274,6 +274,7 @@ def generate_launch_description():
                 " --render-engine",
                 " ogre2",
             ],
+            "on_exit_shutdown": "true",
         }.items(),
     )
 

src/navigator/navigator_bringup/launch/navigator_setup.launch.py

@@ -119,6 +119,16 @@ def generate_launch_description():
         ],
     )
 
+    controller_server = Node(
+        package="navigator_controller",
+        executable="server_node",
+        name="navigator_controller_server",
+        output="screen",
+        parameters=[
+            {"use_sim_time": LaunchConfiguration("use_sim_time")},
+        ],
+    )
+
     # !!! Uncomment once navigator_localization is created !!!
     # localization = IncludeLaunchDescription(
     #     PythonLaunchDescriptionSource(
@@ -186,6 +196,7 @@ def generate_launch_description():
             OpaqueFunction(function=make_robot_state),
             rviz,
             thruster_mapper,
+            controller_server,
             # !!! Uncomment once navigator_localization is created !!!
             # thruster_manager,
             # localization,