Online bagger

src/mil_common/mil_msgs/CMakeLists.txt

@@ -38,7 +38,7 @@ set(msg_files
     "msg/ObjectDetections.msg")
 
 set(srv_files "srv/CameraToLidarTransform.srv" "srv/ObjectDBQuery.srv"
-              "srv/SetGeometry.srv")
+              "srv/SetGeometry.srv" "srv/BagTopics.srv")
 
 rosidl_generate_interfaces(
   ${PROJECT_NAME}

src/mil_common/mil_msgs/action/BagOnline.action

@@ -9,15 +9,15 @@
 string bag_name
 
 # Space separated list of topics to bag. If empty string, all buffered topics will be bagged
-string topics
+string[] topics
 
 # Time in seconds
 float32 bag_time
 ---
 
 bool success
 string status # Contains error information if success is False
-string filename # Full path to file where bag was written
+# string filename # Full path to file where bag was written
 
 ---
 

src/mil_common/mil_msgs/srv/BagTopics.srv

@@ -0,0 +1,5 @@
+# Onling bagger client call this service to obtain a list of topics to be baggered
+# The topics will be display on client ui
+
+---
+string[] topics # Contains all topic names to bag in the online bagger server.

src/mil_common/mil_tools/CMakeLists.txt

@@ -20,6 +20,7 @@ find_package(backward_ros REQUIRED)
 find_package(Eigen3 REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(ftxui REQUIRED)
 
 set(SOURCE_FILES src/geometry/Rotation.cpp src/fs/path.cpp src/os.cpp
                  src/os/FileDescriptor.cpp src/string.cpp)
@@ -29,8 +30,12 @@ target_include_directories(
                          $<INSTALL_INTERFACE:include> ${EIGEN3_INCLUDE_DIR})
 target_include_directories(${PROJECT_NAME} PUBLIC $<BUILD_INTERFACE:${AU_LIB}>
                                                   $<INSTALL_INTERFACE:include>)
-ament_target_dependencies(${PROJECT_NAME} rclcpp Eigen3 tf2_geometry_msgs)
-ament_export_dependencies(rclcpp Eigen3 tf2_geometry_msgs)
+
+target_link_libraries(${PROJECT_NAME} PUBLIC ftxui::screen ftxui::dom
+                                             ftxui::component)
+ament_target_dependencies(${PROJECT_NAME} PUBLIC rclcpp Eigen3
+                          tf2_geometry_msgs)
+ament_export_dependencies(rclcpp Eigen3 tf2_geometry_msgs ftxui)
 ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
 ament_export_include_directories(include)
 include_directories(include)

src/mil_common/mil_tools/include/mil_tools/tui/dialog.hpp

@@ -0,0 +1,56 @@
+#pragma once
+// Show a dialog on the terminal powered by ftxui
+
+#include "ftxui/component/component.hpp"
+#include "ftxui/component/screen_interactive.hpp"
+#include "ftxui/dom/elements.hpp"
+
+namespace mil_tools::tui
+{
+class Dialog
+{
+  public:
+    Dialog()
+    {
+    }
+
+    int exec(std::string const& title, ftxui::Component content, std::vector<std::string> const& buttons = { "OK" })
+    {
+        using namespace ftxui;
+        Component close_button = Button("X", [&] { screen.Exit(); }, ButtonOption::Ascii());
+
+        Components button_comps;
+        button_comps.push_back(Renderer([] { return filler(); }));
+        for (size_t i = 0; i < buttons.size(); i++)
+        {
+            button_comps.push_back(Button(
+                buttons[i],
+                [this, i = i]
+                {
+                    exit_code = i;
+                    screen.Exit();
+                },
+                ButtonOption::Border()));
+        }
+
+        Component dialog_ui =
+            Container::Vertical(
+                { Renderer(close_button, [=] { return hbox({ text(title) | center | flex, close_button->Render() }); }),
+                  Renderer([] { return separator(); }), content | vcenter | flex, Renderer([] { return separator(); }),
+                  Container::Horizontal(button_comps) }) |
+            border;
+
+        screen.Loop(dialog_ui);
+        return exit_code;
+    }
+
+    void close()
+    {
+        screen.Exit();
+    }
+
+  private:
+    int exit_code = -1;
+    ftxui::ScreenInteractive screen = ftxui::ScreenInteractive::Fullscreen();
+};
+}  // namespace mil_tools::tui

src/mil_common/mil_tools/package.xml

@@ -13,6 +13,7 @@
   <build_export_depend>rclcpp</build_export_depend>
 
   <depend>backward_ros</depend>
+  <depend>ftxui</depend>
   <buildtool_depend>ament</buildtool_depend>
   <buildtool_depend>ament_cmake</buildtool_depend>
   <buildtool_depend>ament_cmake_python</buildtool_depend>

src/mil_common/online_bagger/CMakeLists.txt

@@ -0,0 +1,48 @@
+cmake_minimum_required(VERSION 3.8)
+project(online_bagger)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(mil_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(mil_tools REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(rosbag2_cpp REQUIRED)
+find_package(Boost REQUIRED COMPONENTS filesystem system program_options)
+find_package(ftxui REQUIRED COMPONENTS screen dom component)
+
+include_directories(include ${Boost_INCLUDE_DIRS})
+add_executable(${PROJECT_NAME}_server_node src/server.cpp)
+add_library(${PROJECT_NAME}_client src/client.cpp)
+add_executable(${PROJECT_NAME} src/online_bagger.cpp)
+
+ament_target_dependencies(
+  ${PROJECT_NAME}_server_node
+  rclcpp
+  mil_msgs
+  std_msgs
+  rclcpp_action
+  rosbag2_cpp
+  Boost)
+
+ament_target_dependencies(${PROJECT_NAME}_client rclcpp mil_msgs std_msgs
+                          rclcpp_action)
+
+ament_target_dependencies(${PROJECT_NAME} mil_tools rclcpp Boost)
+
+target_link_libraries(${PROJECT_NAME} ${PROJECT_NAME}_client ftxui::screen
+                      ftxui::dom ftxui::component)
+
+install(TARGETS ${PROJECT_NAME}_server_node ${PROJECT_NAME}_client
+        DESTINATION lib/${PROJECT_NAME})
+
+install(TARGETS ${PROJECT_NAME} DESTINATION bin)
+
+install(DIRECTORY launch/ DESTINATION share/${PROJECT_NAME}/launch)
+
+ament_package()

src/mil_common/online_bagger/include/online_bagger/client.hpp

@@ -0,0 +1,58 @@
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+
+#include "mil_msgs/action/bag_online.hpp"
+#include "mil_msgs/srv/bag_topics.hpp"
+
+#include <rclcpp_action/client.hpp>
+
+namespace online_bagger
+{
+class Client : public rclcpp::Node
+{
+  public:
+    using BagOnlineGoalHandle = rclcpp_action::ClientGoalHandle<mil_msgs::action::BagOnline>;
+    using TopicsFuture = std::shared_future<std::vector<std::string>>;
+    using BagFuture = std::shared_future<std::pair<bool, std::string>>;
+
+    enum class State
+    {
+        Waiting,
+        Ready,
+        Bagging
+    };
+
+    struct BagOptions
+    {
+        BagOptions()
+        {
+            goal.bag_time = 1.0f;
+        }
+
+        mil_msgs::action::BagOnline::Goal goal;
+        std::function<void(float)> on_progress = nullptr;
+        std::function<void(BagFuture)> on_finish = nullptr;
+    };
+
+    Client();
+    ~Client();
+
+    State get_state();
+
+    TopicsFuture get_bag_topics(std::function<void(TopicsFuture)> callback = nullptr);
+    BagFuture bag(BagOptions const& options);
+
+  private:
+    using TopicsPromise = std::promise<std::vector<std::string>>;
+    using BagPromise = std::promise<std::pair<bool, std::string>>;
+
+    std::atomic<bool> bagging;
+    rclcpp_action::Client<mil_msgs::action::BagOnline>::SharedPtr action_client;
+    rclcpp::Client<mil_msgs::srv::BagTopics>::SharedPtr srv_client;
+    rclcpp::TimerBase::SharedPtr alive_timer;
+
+    std::function<void(State old_state, State new_state)> on_state_change;
+};
+
+}  // namespace online_bagger

src/mil_common/online_bagger/include/online_bagger/common.hpp

@@ -0,0 +1,7 @@
+#pragma once
+
+namespace online_bagger
+{
+static constexpr char BAG_ACTION_NAME[] = "/online_bagger/bag";
+static constexpr char TOPIC_SERVICE_NAME[] = "/online_bagger/topics";
+}  // namespace online_bagger

src/mil_common/online_bagger/launch/test.launch.py

@@ -0,0 +1,16 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    return LaunchDescription(
+        [
+            # Launch online_bagger node with topics list
+            Node(
+                package="online_bagger",
+                executable="online_bagger_server",
+                output="screen",
+                parameters=[{"topics": ["rand_int", "rand_float", "rand_str"]}],
+            ),
+        ],
+    )

src/mil_common/online_bagger/package.xml

@@ -0,0 +1,26 @@
+<?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>online_bagger</name>
+  <version>0.0.0</version>
+  <description>The online bagger is a tool that allows user to store bagged information about important events, after they happen.</description>
+  <maintainer email="zhangrenzhongzheng@outlook.com">zhongzheng</maintainer>
+  <license>MIL</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>ftxui</depend>
+  <depend>mil_msgs</depend>
+  <depend>mil_tools</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_action</depend>
+  <depend>rosbag2_cpp</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/mil_common/online_bagger/src/client.cpp

@@ -0,0 +1,100 @@
+#include "online_bagger/client.hpp"
+
+#include <atomic>
+
+#include <rclcpp/rclcpp.hpp>
+
+#include "mil_msgs/action/bag_online.hpp"
+#include "mil_msgs/srv/bag_topics.hpp"
+#include "online_bagger/common.hpp"
+
+#include <rclcpp_action/rclcpp_action.hpp>
+
+namespace online_bagger
+{
+
+Client::Client() : rclcpp::Node("online_bagger_client")
+{
+    action_client = rclcpp_action::create_client<mil_msgs::action::BagOnline>(this, BAG_ACTION_NAME);
+    srv_client = create_client<mil_msgs::srv::BagTopics>(TOPIC_SERVICE_NAME);
+
+    action_client->wait_for_action_server(std::chrono::milliseconds(500));
+}
+
+Client::~Client()
+{
+}
+
+Client::State Client::get_state()
+{
+    if (!action_client->action_server_is_ready())
+        return State::Waiting;
+
+    if (!bagging)
+        return State::Ready;
+
+    return State::Bagging;
+}
+
+Client::TopicsFuture Client::get_bag_topics(std::function<void(Client::TopicsFuture)> callback)
+{
+    std::shared_ptr<TopicsPromise> promise = std::make_shared<TopicsPromise>();
+    TopicsFuture future = promise->get_future().share();
+
+    auto request = std::make_shared<mil_msgs::srv::BagTopics::Request>();
+    srv_client->async_send_request(
+        request,
+        [promise, future, callback](rclcpp::Client<mil_msgs::srv::BagTopics>::SharedFuture response_future)
+        {
+            promise->set_value(std::move(response_future.get()->topics));
+            if (callback)
+                callback(future);
+        });
+
+    return future;
+}
+
+Client::BagFuture Client::bag(Client::BagOptions const& options)
+{
+    std::shared_ptr<BagPromise> promise = std::make_shared<BagPromise>();
+    BagFuture future = promise->get_future().share();
+
+    bagging = true;
+
+    rclcpp_action::Client<mil_msgs::action::BagOnline>::SendGoalOptions goal_options;
+    goal_options.goal_response_callback =
+        [this, on_finish = options.on_finish, promise, future](Client::BagOnlineGoalHandle::SharedPtr goal_handle)
+    {
+        if (!goal_handle)
+        {
+            promise->set_value({ false, "Bag request is rejected by the online bagger server." });
+            if (on_finish)
+                on_finish(future);
+            bagging = false;
+        }
+    };
+
+    if (options.on_progress)
+    {
+        goal_options.feedback_callback =
+            [on_progress =
+                 options.on_progress]([[maybe_unused]] Client::BagOnlineGoalHandle::SharedPtr goal_handle,
+                                      std::shared_ptr<mil_msgs::action::BagOnline::Feedback const> const feedback)
+        { on_progress(feedback->progress); };
+    }
+
+    goal_options.result_callback =
+        [this, on_finish = options.on_finish, promise, future](Client::BagOnlineGoalHandle::WrappedResult const& result)
+    {
+        promise->set_value({ result.result->success, std::move(result.result->status) });
+        if (on_finish)
+            on_finish(future);
+        bagging = false;
+    };
+
+    action_client->async_send_goal(options.goal, goal_options);
+
+    return future;
+}
+
+}  // namespace online_bagger