From 4b444c749ea056d150489680254d60b827be72de Mon Sep 17 00:00:00 2001
From: Sergei Grichine <msghub@hotmail.com>
Date: Sat, 1 Mar 2025 10:12:09 -0600
Subject: [PATCH] Changes per Issue #4

---
 battery_state_rviz_overlay/CMakeLists.txt     | 19 +++++-
 .../BatteryStateDisplay.hpp                   | 13 ++++
 battery_state_rviz_overlay/package.xml        |  1 +
 .../src/BatteryStateDisplay.cpp               | 63 ++++++++++++++-----
 ...battery_state_rviz_overlay_parameters.yaml | 50 +++++++++++++++
 battery_state_rviz_overlay/src/main.cpp       | 12 +++-
 6 files changed, 140 insertions(+), 18 deletions(-)
 create mode 100644 battery_state_rviz_overlay/src/battery_state_rviz_overlay_parameters.yaml

diff --git a/battery_state_rviz_overlay/CMakeLists.txt b/battery_state_rviz_overlay/CMakeLists.txt
index 07de38a..851d5c4 100644
--- a/battery_state_rviz_overlay/CMakeLists.txt
+++ b/battery_state_rviz_overlay/CMakeLists.txt
@@ -5,21 +5,34 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
+# add_compile_options(-v)
+
 # find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(rviz_2d_overlay_msgs REQUIRED)
 find_package(fmt REQUIRED)
+find_package(generate_parameter_library REQUIRED)
+
+# Parameters Library ================================================
+set(TARGET ${PROJECT_NAME}_parameters)
+generate_parameter_library(${TARGET} src/${TARGET}.yaml)
 
 add_executable(battery_state_rviz_overlay src/main.cpp src/BatteryStateDisplay.cpp)
 ament_target_dependencies(battery_state_rviz_overlay rclcpp sensor_msgs rviz_2d_overlay_msgs)
-target_include_directories(battery_state_rviz_overlay PRIVATE include)
-target_link_libraries(battery_state_rviz_overlay fmt::fmt)
 
+target_include_directories(battery_state_rviz_overlay PUBLIC
+  include
+  $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include/battery_state_rviz_overlay>
+)
+
+target_link_libraries(${PROJECT_NAME} battery_state_rviz_overlay_parameters fmt::fmt)
 
 install(
-  TARGETS battery_state_rviz_overlay
+  TARGETS
+  battery_state_rviz_overlay
   DESTINATION lib/${PROJECT_NAME}
 )
 
diff --git a/battery_state_rviz_overlay/include/battery_state_rviz_overlay/BatteryStateDisplay.hpp b/battery_state_rviz_overlay/include/battery_state_rviz_overlay/BatteryStateDisplay.hpp
index 16fb4a5..8492d22 100644
--- a/battery_state_rviz_overlay/include/battery_state_rviz_overlay/BatteryStateDisplay.hpp
+++ b/battery_state_rviz_overlay/include/battery_state_rviz_overlay/BatteryStateDisplay.hpp
@@ -6,15 +6,28 @@
 #include <sensor_msgs/msg/battery_state.hpp>
 #include <sensor_msgs/msg/detail/battery_state__struct.hpp>
 
+// auto-generated by generate_parameter_library
+#include "battery_state_rviz_overlay/battery_state_rviz_overlay_parameters.hpp"
+
+namespace battery_state_rviz_overlay
+{
 class BatteryStateDisplay : public rclcpp::Node
 {
 public:
   explicit BatteryStateDisplay(const std::string& name);
 
+  inline void setParamListener(std::shared_ptr<ParamListener> param_listener) { param_listener_ = param_listener; }
+
 private:
+  std::shared_ptr<ParamListener> param_listener_;
+  Params params_;
+
   rclcpp::Publisher<rviz_2d_overlay_msgs::msg::OverlayText>::SharedPtr overlayPublisher_;
 
   rclcpp::Subscription<sensor_msgs::msg::BatteryState>::SharedPtr batteryStateSubscription_;
 
   void batteryStateCallback(const sensor_msgs::msg::BatteryState& message);
+
+  std::vector<double> getRgba();
 };
+}  // namespace battery_state_rviz_overlay
diff --git a/battery_state_rviz_overlay/package.xml b/battery_state_rviz_overlay/package.xml
index 6ac2cfb..0ac319f 100644
--- a/battery_state_rviz_overlay/package.xml
+++ b/battery_state_rviz_overlay/package.xml
@@ -13,6 +13,7 @@
   <depend>sensor_msgs</depend>
   <depend>rclcpp</depend>
   <depend>rviz_2d_overlay_msgs</depend>
+  <depend>generate_parameter_library</depend>
 
   <export>
     <build_type>ament_cmake</build_type>
diff --git a/battery_state_rviz_overlay/src/BatteryStateDisplay.cpp b/battery_state_rviz_overlay/src/BatteryStateDisplay.cpp
index ec1cc7c..893942c 100644
--- a/battery_state_rviz_overlay/src/BatteryStateDisplay.cpp
+++ b/battery_state_rviz_overlay/src/BatteryStateDisplay.cpp
@@ -7,6 +7,8 @@
 #include <rviz_2d_overlay_msgs/msg/overlay_text.hpp>
 #include <sensor_msgs/msg/battery_state.hpp>
 
+namespace battery_state_rviz_overlay
+{
 BatteryStateDisplay::BatteryStateDisplay(const std::string& name)
   : rclcpp::Node(name)
   , overlayPublisher_{ create_publisher<rviz_2d_overlay_msgs::msg::OverlayText>("battery_display_text",
@@ -30,10 +32,12 @@ void BatteryStateDisplay::batteryStateCallback(const sensor_msgs::msg::BatterySt
   overlay.action = OverlayText::ADD;
   overlay.text = "<pre>";
 
-  overlay.width = 450;
-  overlay.height = 10;
+  params_ = param_listener_->get_params();
+
+  overlay.width = params_.width;
+  overlay.height = params_.height;
 
-  constexpr auto line_height = 25;
+  auto line_height = params_.line_height;
 
   overlay.text += fmt::format(FMT_COMPILE("Battery voltage: {:>8.2f}V\n"), message.voltage);
   overlay.height += line_height;
@@ -50,19 +54,50 @@ void BatteryStateDisplay::batteryStateCallback(const sensor_msgs::msg::BatterySt
     overlay.height += line_height;
   }
 
-  overlay.horizontal_distance = 20;
-  overlay.vertical_distance = 20;
-  overlay.horizontal_alignment = OverlayText::LEFT;
-  overlay.vertical_alignment = OverlayText::TOP;
+  overlay.horizontal_distance = params_.horizontal_distance;
+  overlay.vertical_distance = params_.vertical_distance;
+  overlay.horizontal_alignment = params_.horizontal_alignment;
+  overlay.vertical_alignment = params_.vertical_alignment;
+
+  overlay.bg_color.a = params_.bg_color_a;
 
-  overlay.bg_color.a = 0.5;
+  overlay.text_size = params_.text_size;
+  overlay.font = params_.font;
 
-  overlay.text_size = 20.0;
-  overlay.font = "DejaVu Sans Mono";
-  overlay.fg_color.a = 1.0;
-  overlay.fg_color.r = 0;
-  overlay.fg_color.g = 0.576470588;
-  overlay.fg_color.b = 0.454901961;
+  try {
+    std::vector<double> rgba = getRgba();
+
+    overlay.fg_color.a = rgba[3];
+    overlay.fg_color.r = rgba[0];
+    overlay.fg_color.g = rgba[1];
+    overlay.fg_color.b = rgba[2];
+  
+  } catch (const std::exception & e) {
+
+    overlay.fg_color.a = 1.0;
+    overlay.fg_color.r = 0;
+    overlay.fg_color.g = 0.576470588;
+    overlay.fg_color.b = 0.454901961;
+  
+    RCLCPP_ERROR(
+      get_logger(), "Exception thrown while parsing RGBA parameter fg_color_rgba: %s\n%s \n",
+      params_.fg_color_rgba.c_str(),
+      e.what());
+  }
 
   overlayPublisher_->publish(overlay);
 }
+
+std::vector<double> BatteryStateDisplay::getRgba()
+{
+  //const std::string s_rgba = params_.fg_color_rgba;
+  std::stringstream ss(params_.fg_color_rgba);
+  std::vector<double> double_array;
+  std::string value;
+
+  while (std::getline(ss, value, ' ')) {
+    double_array.push_back(std::stod(value));
+  }
+  return double_array;
+}
+}  // namespace battery_state_rviz_overlay
diff --git a/battery_state_rviz_overlay/src/battery_state_rviz_overlay_parameters.yaml b/battery_state_rviz_overlay/src/battery_state_rviz_overlay_parameters.yaml
new file mode 100644
index 0000000..3cb6851
--- /dev/null
+++ b/battery_state_rviz_overlay/src/battery_state_rviz_overlay_parameters.yaml
@@ -0,0 +1,50 @@
+battery_state_rviz_overlay:
+
+  # See https://github.com/teamspatzenhirn/rviz_2d_overlay_plugins/blob/main/rviz_2d_overlay_msgs/msg/OverlayText.msg
+
+  # the following parameters can be defined in a launch file:
+  width:
+    type: int
+    description: "Width in pixels of the overlay"
+    default_value: 450
+  height:
+    type: int
+    description: "Height in pixels of the overlay"
+    default_value: 10
+  line_height:
+    type: int
+    description: "Line Height in pixels of the overlay"
+    default_value: 25
+  horizontal_distance:
+    type: int
+    description: "Position: Horizontal distance from left/right border or center, depending on alignment"
+    default_value: 20
+  vertical_distance:
+    type: int
+    description: "Position: Vertical distance between from top/bottom border or center, depending on alignment"
+    default_value: 20
+  font:
+    type: string
+    description: "Font"
+    default_value: "DejaVu Sans Mono"
+  text_size:
+    type: double
+    description: "Font size"
+    default_value: 20.0
+  horizontal_alignment:
+    type: int
+    description: "Horizontal alignment, one of LEFT=0, CENTER=2, RIGHT=1"
+    default_value: 0
+  vertical_alignment:
+    type: int
+    description: "Vertical alignment, one of TOP=3, BOTTOM=4"
+    default_value: 3
+  bg_color_a:
+    type: double
+    description: "Background transparency"
+    default_value: 0.5
+  fg_color_rgba:
+    type: string
+    description: "R,G,B and A of the font color"
+    default_value: "0 0.576470588 0.454901961 1.0"
+
diff --git a/battery_state_rviz_overlay/src/main.cpp b/battery_state_rviz_overlay/src/main.cpp
index daaf919..bb8dada 100644
--- a/battery_state_rviz_overlay/src/main.cpp
+++ b/battery_state_rviz_overlay/src/main.cpp
@@ -7,7 +7,17 @@ int main(int argc, char* argv[])
 {
   rclcpp::init(argc, argv);
 
-  auto node = std::make_shared<BatteryStateDisplay>("battery_state_rviz_overlay");
+  auto node = std::make_shared<battery_state_rviz_overlay::BatteryStateDisplay>("battery_state_rviz_overlay");
+
+  try {
+    auto param_listener = std::make_shared<battery_state_rviz_overlay::ParamListener>(node);
+    node->setParamListener(param_listener);
+  } catch (const std::exception & e) {
+    RCLCPP_ERROR(
+      node->get_logger(), "Exception thrown during BatteryStateDisplay init stage with message: %s \n",
+      e.what());
+  }
+
   rclcpp::spin(node);
   rclcpp::shutdown();
 }