Get servos to work

src/uaspire_controls/CMakeLists.txt

@@ -21,15 +21,32 @@ if (CMAKE_CXX_COMPILER_ID MATCHES "(GNU|AppleClang|Clang)")
     add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
+option(BUILD_RASPBERRY_PI "Build for Raspberry Pi" ON)
+
 # Find required packages
 find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(uaspire_msgs REQUIRED)
 
+if (BUILD_RASPBERRY_PI)
+    find_library(PIGPIO_LIB NAMES pigpiod_if2 HINTS ${CMAKE_SOURCE_DIR}/../pigpio)
+    find_path(PIGPIO_INCLUDE_DIR NAMES pigpiod_if2.h HINTS ${CMAKE_SOURCE_DIR}/../pigpio)
+    if (NOT PIGPIO_LIB OR NOT PIGPIO_INCLUDE_DIR)
+        message(FATAL_ERROR "Could not find pigpio: ${PIGPIO_LIB} ${PIGPIO_INCLUDE_DIR}")
+    else()
+        message(STATUS "Found pigpio: ${PIGPIO_LIB} ${PIGPIO_INCLUDE_DIR}")
+    endif()
+endif()
+
 # Create targets and add target dependencies
 add_executable(uaspire_controls
     main.cc
 )
+if (BUILD_RASPBERRY_PI)
+    target_link_libraries(uaspire_controls ${PIGPIO_LIB})
+    target_include_directories(uaspire_controls PUBLIC ${PIGPIO_INCLUDE_DIR})
+    target_compile_definitions(uaspire_controls PUBLIC BUILD_RASPBERRY_PI)
+endif()
 ament_target_dependencies(uaspire_controls
     rclcpp
     std_msgs

src/uaspire_controls/main.cc

@@ -1,21 +1,101 @@
-#include <functional>
-
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp/timer.hpp>
 #include <std_msgs/msg/string.hpp>
 #include <uaspire_msgs/msg/command.hpp>
 #include <uaspire_msgs/msg/sensors.hpp>
+#include <uaspire_msgs/msg/servo_positions.hpp>
+
+#ifdef BUILD_RASPBERRY_PI
+#include <pigpiod_if2.h>
+#endif
 
 namespace arg = std::placeholders;
 namespace ros = ::rclcpp;
 using UaspireCommand = ::uaspire_msgs::msg::Command;
 using UaspireSensors = ::uaspire_msgs::msg::Sensors;
+using UaspireServoPositions = ::uaspire_msgs::msg::ServoPositions;
+
+struct Servo {
+  unsigned int pin;
+};
+
+class IoNode : public ros::Node {
+private:
+  ros::Subscription<UaspireServoPositions>::SharedPtr _servo_positions_subscription;
+  std::array<Servo, 2> _servos;
+  int _pi;
+
+public:
+  RCLCPP_SHARED_PTR_DEFINITIONS(IoNode)
+
+  IoNode() : Node("uaspire_io") {
+#ifdef BUILD_RASPBERRY_PI
+    // use default address and port
+    _pi = pigpio_start(NULL, NULL);
+    if (_pi < 0) {
+      RCLCPP_ERROR(get_logger(), "Error initializing pigpiod_if: %d", _pi);
+    }
+#endif
+
+    _servo_positions_subscription = create_subscription<UaspireServoPositions>(
+        "uaspire/servo_positions", ros::QoS(10),
+        std::bind(&IoNode::on_servo_positions, this, arg::_1));
+
+    init_servos();
+  }
+
+  ~IoNode() {
+#ifdef BUILD_RASPBERRY_PI
+    pigpio_stop(_pi);
+#endif
+  }
+
+  void init_servos() {
+    _servos = { Servo { 23 }, Servo { 24 } };
+    for (auto& servo : _servos) {
+#ifdef BUILD_RASPBERRY_PI
+      int result = set_mode(_pi, servo.pin, PI_OUTPUT);
+      if (result != 0) {
+        RCLCPP_ERROR(get_logger(), "Error setting pin mode: %d", result);
+      }
+      result = gpio_write(_pi, servo.pin, PI_LOW);
+      if (result != 0) {
+        RCLCPP_ERROR(get_logger(), "Error writing pin: %d", result);
+      }
+#endif
+    }
+  }
+
+  void on_servo_positions(UaspireServoPositions::SharedPtr msg) {
+    std::array<float, 2> values = {msg->yaw, msg->pitch};
+
+    for (size_t i = 0; i < _servos.size(); ++i) {
+      Servo& servo = _servos[i];
+      float value = values[i];
+      value = std::min(value, 90.f);
+      value = std::max(value, -90.f);
+      // convert value from -90 to 90 to int from 1000 to 2000 (us)
+      unsigned int decoded_value = static_cast<unsigned int>(
+          ((value / 90.f) * 500.f) + 1500);
+#ifdef BUILD_RASPBERRY_PI
+      int result = set_servo_pulsewidth(_pi, servo.pin, decoded_value);
+      if (result != 0) {
+        RCLCPP_ERROR(get_logger(), "error setting servo pulse width: %d", result);
+      }
+#endif
+    }
+  }
+};
 
 class ControlsNode : public ros::Node {
 private:
   ros::Subscription<UaspireCommand>::SharedPtr _command_subscription;
   ros::Publisher<UaspireSensors>::SharedPtr _sensors_publisher;
+  ros::Publisher<UaspireServoPositions>::SharedPtr _servo_publisher;
   ros::TimerBase::SharedPtr _timer;
+  ros::TimerBase::SharedPtr _servo_timer;
+
+  int _last_value;
 
 public:
   RCLCPP_SHARED_PTR_DEFINITIONS(ControlsNode)
@@ -26,12 +106,27 @@ public:
         std::bind(&ControlsNode::on_message, this, arg::_1));
     _sensors_publisher =
         create_publisher<UaspireSensors>("uaspire/sensors", ros::QoS(10));
+    _servo_publisher =
+        create_publisher<UaspireServoPositions>("uaspire/servo_positions", ros::QoS(10));
     _timer = create_wall_timer(std::chrono::milliseconds(1000),
                                std::bind(&ControlsNode::on_loop, this));
+
+    _last_value = 0;
   }
 
+
   void on_message(const UaspireCommand::SharedPtr cmd) {
     RCLCPP_INFO(get_logger(), "Command received: %s", cmd->data.c_str());
+    if (cmd->data == "start") {
+      // start 25Hz servo timer
+      _servo_timer = create_wall_timer(std::chrono::milliseconds(40),
+          std::bind(&ControlsNode::on_servo_update, this));
+    } else if (cmd->data == "stop") {
+      if (_servo_timer) {
+        _servo_timer->cancel();
+      }
+      _servo_timer.reset();
+    }
   }
 
   void on_loop() {
@@ -40,13 +135,25 @@ public:
     _sensors_publisher->publish(sensors);
     RCLCPP_INFO(get_logger(), "loop");
   }
+
+  void on_servo_update() {
+    UaspireServoPositions positions;
+    positions.pitch = _last_value - 90;
+    positions.yaw = 90 - _last_value;
+    _servo_publisher->publish(positions);
+    _last_value = (_last_value + 10) % 180;
+  }
 };
 
 int main(int argc, char **argv) {
   ros::init(argc, argv);
   {
-    auto node = ControlsNode::make_shared();
+    ros::executors::MultiThreadedExecutor executor;
-    ros::spin(node);
+    auto io_node = IoNode::make_shared();
+    auto controls_node = ControlsNode::make_shared();
+    executor.add_node(io_node);
+    executor.add_node(controls_node);
+    executor.spin();
   }
   ros::shutdown();
   return 0;

src/uaspire_msgs/CMakeLists.txt

@@ -18,6 +18,7 @@ find_package(rosidl_default_generators REQUIRED)
 rosidl_generate_interfaces(${PROJECT_NAME}
     "msg/Command.msg"
     "msg/Sensors.msg"
+    "msg/ServoPositions.msg"
 )
 
 # Install files to correct locations

src/uaspire_msgs/msg/ServoPositions.msg

@@ -0,0 +1,3 @@
+float32 yaw
+float32 pitch
+