Add script for auto brightness

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 28f0b0953b8dd5ddd70866a0c37738062ad6448d..72c9340e169fbfe6ea29b95d33bb49c905b32d43 100644 (file)
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -38,10 +38,17 @@ add_executable(display src/display.cpp)
 ament_target_dependencies(display rclcpp sensor_msgs)
 target_compile_features(display PUBLIC cxx_std_20)
 
+add_executable(light_brightness src/light_brightness.cpp)
+target_link_libraries(light_brightness "${cpp_typesupport_target}")
+ament_target_dependencies(light_brightness rclcpp sensor_msgs)
+target_compile_features(light_brightness PUBLIC cxx_std_20)
+
 install(TARGETS hw_node
   DESTINATION lib/${PROJECT_NAME})
 install(TARGETS display
   DESTINATION lib/${PROJECT_NAME})
+install(TARGETS light_brightness
+  DESTINATION lib/${PROJECT_NAME})
 install(DIRECTORY launch
   DESTINATION share/${PROJECT_NAME})
 install(DIRECTORY params

diff --git a/src/hw_node.cpp b/src/hw_node.cpp
index bc0e2a216b7e37ac37639a5329152d6b37526b3c..7f3cfabba51eddce90f334dc36cca0d402259fef 100644 (file)
--- a/src/hw_node.cpp
+++ b/src/hw_node.cpp
@@ -343,6 +343,9 @@ class A4wd3 : public rclcpp::Node {
                        auto msg = sensor_msgs::msg::Illuminance();
                        msg.header.stamp = this->get_clock()->now();
                        msg.illuminance = lux*2.0183-5.2079;
+                       if (msg.illuminance < 0) {
+                               msg.illuminance = 0;
+                       }
                        pub_light->publish(msg);
                }
 

diff --git a/src/light_brightness.cpp b/src/light_brightness.cpp
new file mode 100644 (file)
index 0000000..cdcfdf9
--- /dev/null
+++ b/src/light_brightness.cpp
@@ -0,0 +1,50 @@
+#include <memory>
+#include "sensor_msgs/msg/illuminance.hpp"
+#include "a4wd3/msg/led_stripe.hpp"
+#include "a4wd3/msg/led.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+using std::placeholders::_1;
+
+class LightBrightness : public rclcpp::Node
+{
+       public:
+               LightBrightness() : Node("light_brightness") {
+                       sub_light = this->create_subscription<sensor_msgs::msg::Illuminance>("/light", 10, std::bind(&LightBrightness::light_callback, this, _1));
+                       pub_led = this->create_publisher<a4wd3::msg::LedStripe>("/led_stripe", 10);
+               }
+
+       private:
+               void light_callback(const sensor_msgs::msg::Illuminance msg) {
+                       bool changed=false;
+                       if (msg.illuminance == 0 && val < 254) {
+                               val++;
+                               changed=true;
+                       } else if (msg.illuminance > 0 && val > 0) {
+                               val--;
+                               changed=true;
+                       }
+                       RCLCPP_INFO(this->get_logger(), "'%.2lflx -> light=%d'", msg.illuminance, val);
+                       if (changed) {
+                               auto ledmsg = a4wd3::msg::LedStripe();
+                               auto led=a4wd3::msg::Led();
+                               led.num=0;
+                               led.cw=val;
+                               led.ww=val;
+                               ledmsg.leds.push_back(led);
+                               pub_led->publish(ledmsg);
+                       }
+               }
+
+               rclcpp::Subscription<sensor_msgs::msg::Illuminance>::SharedPtr sub_light;
+               rclcpp::Publisher<a4wd3::msg::LedStripe>::SharedPtr pub_led;
+               uint8_t val=0;
+};
+
+int main(int argc, char * argv[])
+{
+       rclcpp::init(argc, argv);
+       rclcpp::spin(std::make_shared<LightBrightness>());
+       rclcpp::shutdown();
+       return 0;
+}