feat: rotation estimator and scalar publish visualization

CMakeLists.txt

@@ -31,6 +31,7 @@ find_package(ament_index_cpp REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(visualization_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(pluginlib REQUIRED)
 
@@ -71,6 +72,7 @@ target_include_directories(
     PRIVATE
         ${visualization_msgs_INCLUDE_DIRS}
         ${geometry_msgs_INCLUDE_DIRS}
+        ${std_msgs_INCLUDE_DIRS}
         ${tf2_ros_INCLUDE_DIRS}
 )
 target_link_libraries(
@@ -84,6 +86,7 @@ target_link_libraries(
     PRIVATE
         ${visualization_msgs_LIBRARIES}
         ${geometry_msgs_LIBRARIES}
+        ${std_msgs_LIBRARIES}
         ${tf2_ros_LIBRARIES}
 )
 

config/config.yaml

@@ -20,8 +20,7 @@ capturer:
   reconnect_wait_interval: 100
 
   record_enable: true
-  saving_pathes:
-    ["/home/root/autoaim/", "/home/ubuntu/autoaim/", "/tmp/autoaim/"]
+  saving_pathes: ["/root/autoaim/", "/home/ubuntu/autoaim/", "/tmp/autoaim/"]
   max_duration_seconds: 60
   record_fps: 24
   max_videos_size_gb: 50

package.xml

@@ -17,6 +17,7 @@
   <depend>rmcs_executor</depend>
   <depend>rmcs_description</depend>
   <depend>fast_tf</depend>
+  <depend>std_msgs</depend>
   <depend>rmcs_msgs</depend>
   <depend>hikcamera</depend>
 

src/kernel/auto_aim.cpp

@@ -50,6 +50,8 @@ struct AutoAim::Impl {
             auto image = cap.fetch_image();
             if (!image) continue;
 
+            // @TODO:
+            //  避免拷贝，而是把需要的具体量拿出来
             auto context = SystemContext::kIdentity();
             {
                 std::lock_guard lock { self.context_mutex };
@@ -63,9 +65,9 @@ struct AutoAim::Impl {
 
             [[maybe_unused]] auto streamer = std::experimental::scope_exit { [&] {
                 visual.draw_later( // 录制开关
-                    Text { cap.recording() ? "RECORD ON" : "RECORD OFF", { 10, 700 } });
+                    Canvas::Text { cap.recording() ? "RECORD ON" : "RECORD OFF", { 10, 700 } });
                 visual.draw_later( // 自瞄帧率
-                    Text { std::format("FPS: {}", framerate.fps()), { 10, 680 } });
+                    Canvas::Text { std::format("FPS: {}", framerate.fps()), { 10, 680 } });
                 visual.update_image(*image);
             } };
 
@@ -148,8 +150,10 @@ struct AutoAim::Impl {
                         cmd.yaw_acc, cmd.pitch_acc);
                 }
             }
-            visual.draw_later(Text { "ATTACK", { 10, 660 }, cmd.should_shoot ? kRed : kWhite });
-            visual.draw_later(Text { "CONTROL", { 10, 640 }, cmd.should_control ? kRed : kWhite });
+            visual.draw_later(
+                Canvas::Text { "ATTACK", { 10, 660 }, cmd.should_shoot ? kRed : kWhite });
+            visual.draw_later(
+                Canvas::Text { "CONTROL", { 10, 640 }, cmd.should_control ? kRed : kWhite });
 
             {
                 std::lock_guard lock { self.command_mutex };

src/kernel/visualization.cpp

@@ -7,6 +7,7 @@
 #include "utility/rclcpp/visual/armor.hpp"
 #include "utility/rclcpp/visual/arrow.hpp"
 #include "utility/rclcpp/visual/lightbar.hpp"
+#include "utility/rclcpp/visual/scalar.hpp"
 #include "utility/rclcpp/visual/transform.hpp"
 #include "utility/serializable.hpp"
 
@@ -61,6 +62,7 @@ struct Visualization::Impl {
     std::unique_ptr<visual::Arrow> aiming_direction;
     std::unordered_map<std::string, visual::Armors> armor_publishers;
     std::unordered_map<std::string, visual::Lightbars> lightbar_publishers;
+    std::unordered_map<std::string, visual::Scalar> scalar_publishers;
 
     bool is_initialized  = false;
     bool size_determined = false;
@@ -141,7 +143,6 @@ struct Visualization::Impl {
 
     auto publish(std::span<const Armor3d> armors, const std::string& name) {
         if (!is_initialized || !config.publishable) return;
-
         auto iter = armor_publishers.find(name);
         if (iter == armor_publishers.end()) {
             auto config = visual::Armors::Config {
@@ -155,9 +156,7 @@ struct Visualization::Impl {
     }
 
     auto publish(std::span<const Lightbar3d> lightbars, const std::string& name) -> void {
-        if (!is_initialized) return;
-        if (!config.publishable) return;
-
+        if (!is_initialized || !config.publishable) return;
         auto iter = lightbar_publishers.find(name);
         if (iter == lightbar_publishers.end()) {
             auto config = visual::Lightbars::Config {
@@ -175,6 +174,15 @@ struct Visualization::Impl {
         odom_transform.publish(t);
     }
 
+    auto publish(double value, const std::string& name) -> void {
+        if (!is_initialized || !config.publishable) return;
+        auto iter = scalar_publishers.find(name);
+        if (iter == scalar_publishers.end()) {
+            iter = scalar_publishers.emplace(name, visual::Scalar { rclcpp, name }).first;
+        }
+        iter->second.publish(value);
+    }
+
     auto update_aiming_direction(double yaw, double pitch) const -> void {
         if (!is_initialized) return;
         if (!config.publishable) return;
@@ -226,6 +234,10 @@ auto Visualization::publish(const Transform& t, const std::string& name) -> void
     pimpl->publish(t, name);
 }
 
+auto Visualization::publish(double value, const std::string& name) -> void {
+    pimpl->publish(value, name);
+}
+
 Visualization::Visualization() noexcept
     : pimpl { std::make_unique<Impl>() } { }
 

src/kernel/visualization.hpp

@@ -38,6 +38,8 @@ class Visualization {
 
     auto publish(const Transform& t, const std::string& name) -> void;
 
+    auto publish(double value, const std::string& name) -> void;
+
     auto update_aiming_direction(double yaw, double pitch) const -> void;
 
     auto update_mpc_plan(double yaw, double pitch, double yaw_rate, double pitch_rate,