src/uaspire_controls/CMakeLists.txt

@@ -21,32 +21,15 @@ 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,101 +1,21 @@
+#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)
@@ -106,27 +26,12 @@ 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() {
@@ -135,25 +40,13 @@ 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);
   {
-    ros::executors::MultiThreadedExecutor executor;
+    auto node = ControlsNode::make_shared();
-    auto io_node = IoNode::make_shared();
+    ros::spin(node);
-    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,7 +18,6 @@ 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

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