fix compilation

luxonis · Jan 8, 2024 · fea2938 · fea2938
1 parent 9f26900
commit fea2938
Show file tree

Hide file tree

Showing 3 changed files with 45 additions and 57 deletions.
diff --git a/rae_hw/CMakeLists.txt b/rae_hw/CMakeLists.txt
@@ -12,7 +12,6 @@ set(CMAKE_POSITION_INDEPENDENT_CODE ON)
 find_package(ament_cmake_auto REQUIRED)
 ament_auto_find_build_dependencies()
 find_package(ALSA REQUIRED)
-find_package(std_srvs REQUIRED)
 
 find_library(GPIOD_LIBRARY NAMES libgpiodcxx.so)
 if(NOT GPIOD_LIBRARY)
@@ -88,7 +87,6 @@ add_executable(test_speed test/test_speed.cpp src/rae_motors.cpp)
 target_link_libraries(test_speed ${GPIOD_LIBRARY})
 ament_target_dependencies(test_speed rclcpp geometry_msgs)
 
-# ament_target_dependencies(battery rclcpp sensor_msgs)
 
 install(TARGETS
 

diff --git a/rae_hw/package.xml b/rae_hw/package.xml
@@ -24,6 +24,7 @@
   <depend>sensor_msgs</depend>
   <depend>geometry_msgs</depend>
   <depend>rae_msgs</depend>
+  <depend>std_srvs</depend>
   <depend>cv_bridge</depend>
   <depend>audio_msgs</depend>
   <depend>robot_localization</depend>

diff --git a/rae_hw/src/peripherals/led.cpp b/rae_hw/src/peripherals/led.cpp
@@ -95,82 +95,71 @@ void LEDNode::transmitSPI() {
 
     int ret = ioctl(fd, SPI_IOC_MESSAGE(1), &tr);
 
-    int ret = ioctl(fd, SPI_IOC_MESSAGE(1), &tr);
-
     if(ret < 1) RCLCPP_ERROR(this->get_logger(), "can't send spi message");
 }
-void LEDNode::fillBuffer(uint8_t color) {
+void LEDNode::fillBuffer(uint8_t color, float intensity) {
+    uint8_t scaledColor = static_cast<uint8_t>(round(color * intensity));
     for(uint8_t mask = 0x80; mask; mask >>= 1) {
-        if(color & mask) {
+        if(scaledColor & mask) {
             *ptr = 0xfc;
         } else {
             *ptr = 0x80;
         }
         ptr++;
     }
-    void LEDNode::fillBuffer(uint8_t color, float intensity) {
-        uint8_t scaledColor = static_cast<uint8_t>(round(color * intensity));
-        for(uint8_t mask = 0x80; mask; mask >>= 1) {
-            if(scaledColor & mask) {
-                *ptr = 0xfc;
-            } else {
-                *ptr = 0x80;
-            }
-            ptr++;
-        }
+}
+void LEDNode::setSinglePixel(uint16_t pixel, uint8_t r, uint8_t g, uint8_t b, float frequency) {
+    auto mapping_pair = logicalToPhysicalMapping.find(pixel);
+    const float phaseOffset = 3.14 / 4;  // Initial phase offset, if needed
+    const float amplitudeOffset = 1.0f;  // Add 1 to sine wave to avoid negative values
+    const float scaler = 2.0f;           // Scale the sine wave to 0-1
+    const float intensity = (std::sin(frame / (2 * M_PI) * frequency + phaseOffset) + amplitudeOffset) / scaler;
+    if(mapping_pair != logicalToPhysicalMapping.end()) {
+        uint16_t physicalID = mapping_pair->second;
+        ptr = &ws2812b_buffer[24 * physicalID];
+    } else {
+        RCLCPP_INFO(this->get_logger(), "One of the logical LED IDs doesn't have a corresponding physical LED. ");
     }
-    void LEDNode::setSinglePixel(uint16_t pixel, uint8_t r, uint8_t g, uint8_t b, float frequency) {
-        auto mapping_pair = logicalToPhysicalMapping.find(pixel);
-        const float phaseOffset = 3.14 / 4;  // Initial phase offset, if needed
-        const float amplitudeOffset = 1.0f;  // Add 1 to sine wave to avoid negative values
-        const float scaler = 2.0f;           // Scale the sine wave to 0-1
-        const float intensity = (std::sin(frame / (2 * M_PI) * frequency + phaseOffset) + amplitudeOffset) / scaler;
-        if(mapping_pair != logicalToPhysicalMapping.end()) {
-            uint16_t physicalID = mapping_pair->second;
-            ptr = &ws2812b_buffer[24 * physicalID];
-        } else {
-            RCLCPP_INFO(this->get_logger(), "One of the logical LED IDs doesn't have a corresponding physical LED. ");
-        }
 
+    fillBuffer(g, intensity);
+    fillBuffer(r, intensity);
+    fillBuffer(b, intensity);
+}
+
+void LEDNode::setAllPixels(uint8_t r, uint8_t g, uint8_t b, float frequency) {
+    const float phaseOffset = 3.14 / 4;  // Initial phase offset, if needed
+    const float amplitudeOffset = 1.0f;  // Add 1 to sine wave to avoid negative values
+    const float scaler = 2.0f;           // Scale the sine wave to 0-1
+    const float intensity = (std::sin(frame / (2 * M_PI) * frequency + phaseOffset) + amplitudeOffset) / scaler;
+    ptr = ws2812b_buffer;
+
+    for(uint16_t i = 0; i < WS2812B_NUM_LEDS; ++i) {
         fillBuffer(g, intensity);
         fillBuffer(r, intensity);
         fillBuffer(b, intensity);
     }
+}
 
-    void LEDNode::setAllPixels(uint8_t r, uint8_t g, uint8_t b, float frequency) {
-        const float phaseOffset = 3.14 / 4;  // Initial phase offset, if needed
-        const float amplitudeOffset = 1.0f;  // Add 1 to sine wave to avoid negative values
-        const float scaler = 2.0f;           // Scale the sine wave to 0-1
-        const float intensity = (std::sin(frame / (2 * M_PI) * frequency + phaseOffset) + amplitudeOffset) / scaler;
-        ptr = ws2812b_buffer;
-
-        for(uint16_t i = 0; i < WS2812B_NUM_LEDS; ++i) {
-            fillBuffer(g, intensity);
-            fillBuffer(r, intensity);
-            fillBuffer(b, intensity);
-        }
-    }
-
-    void LEDNode::spinner(uint8_t r, uint8_t g, uint8_t b, uint8_t size, uint8_t blades, float frequency) {
-        setAllPixels(0, 0, 0, 0);
-        for(uint8_t i = 0; i < blades; i++) {
-            for(uint32_t j = frame; j < frame + size; j++) {
-                setSinglePixel((j + i * (WS2812B_NUM_LEDS / blades)) % WS2812B_NUM_LEDS, r, g, b, frequency);
-            };
+void LEDNode::spinner(uint8_t r, uint8_t g, uint8_t b, uint8_t size, uint8_t blades, float frequency) {
+    setAllPixels(0, 0, 0, 0);
+    for(uint8_t i = 0; i < blades; i++) {
+        for(uint32_t j = frame; j < frame + size; j++) {
+            setSinglePixel((j + i * (WS2812B_NUM_LEDS / blades)) % WS2812B_NUM_LEDS, r, g, b, frequency);
         };
     };
+};
 
-    void LEDNode::fan(uint8_t r, uint8_t g, uint8_t b, bool opening, uint8_t blades, float frequency) {
-        uint8_t blade_length = WS2812B_NUM_LEDS / blades;
-        uint8_t tip = frame % blade_length;
-        setAllPixels(0, 0, 0, 0);
-        for(uint32_t i = 0; i < blades; i++) {
-            for(uint32_t j = 0; j < (opening ? tip : blade_length - tip); j++) {
-                setSinglePixel((j + i * blade_length) % WS2812B_NUM_LEDS, r, g, b, frequency);
-            };
+void LEDNode::fan(uint8_t r, uint8_t g, uint8_t b, bool opening, uint8_t blades, float frequency) {
+    uint8_t blade_length = WS2812B_NUM_LEDS / blades;
+    uint8_t tip = frame % blade_length;
+    setAllPixels(0, 0, 0, 0);
+    for(uint32_t i = 0; i < blades; i++) {
+        for(uint32_t j = 0; j < (opening ? tip : blade_length - tip); j++) {
+            setSinglePixel((j + i * blade_length) % WS2812B_NUM_LEDS, r, g, b, frequency);
         };
     };
-}
+};
+
 }  // namespace rae_hw
 #include "rclcpp_components/register_node_macro.hpp"
 RCLCPP_COMPONENTS_REGISTER_NODE(rae_hw::LEDNode);