From 4b08a7755292ae8b756e387d4df80505f420f791 Mon Sep 17 00:00:00 2001 From: Alex Mikhalev Date: Fri, 8 Nov 2019 14:32:10 -0800 Subject: [PATCH] Tests with servos --- src/uaspire_controls/CMakeLists.txt | 24 ++++++ src/uaspire_controls/main.cc | 99 ++++++++++++++++++++++++- src/uaspire_msgs/CMakeLists.txt | 1 + src/uaspire_msgs/msg/ServoPositions.msg | 3 + 4 files changed, 123 insertions(+), 4 deletions(-) create mode 100644 src/uaspire_msgs/msg/ServoPositions.msg diff --git a/src/uaspire_controls/CMakeLists.txt b/src/uaspire_controls/CMakeLists.txt index 21fd101..236833f 100644 --- a/src/uaspire_controls/CMakeLists.txt +++ b/src/uaspire_controls/CMakeLists.txt @@ -21,15 +21,39 @@ 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(WIRINGPI_LIB NAMES wiringPi HINTS ${CMAKE_SOURCE_DIR}/../WiringPi/wiringPi) + find_path(WIRINGPI_INCLUDE_DIR NAMES wiringPi.h HINTS ${CMAKE_SOURCE_DIR}/../WiringPi/wiringPi) + if (NOT WIRINGPI_LIB OR NOT WIRINGPI_INCLUDE_DIR) + message(FATAL_ERROR "Could not find wiringPi: ${WIRINGPI_LIB} ${WIRINGPI_INCLUDE_DIR}") + else() + message(STATUS "Found wiringPi: ${WIRINGPI_LIB} ${WIRINGPI_INCLUDE_DIR}") + endif() + find_library(PIGPIO_LIB NAMES pigpiod_if HINTS ${CMAKE_SOURCE_DIR}/../pigpio) + find_path(PIGPIO_INCLUDE_DIR NAMES pigpiod_if.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 diff --git a/src/uaspire_controls/main.cc b/src/uaspire_controls/main.cc index d08f2a7..fae2246 100644 --- a/src/uaspire_controls/main.cc +++ b/src/uaspire_controls/main.cc @@ -1,21 +1,92 @@ -#include - #include #include #include #include #include +#include + +#ifdef BUILD_RASPBERRY_PI +#include +#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::SharedPtr _servo_positions_subscription; + std::array _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( + "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 { 9 }, Servo { 25 } }; + for (auto& servo : _servos) { +#ifdef BUILD_RASPBERRY_PI + set_mode(_pi, servo.pin, PI_OUTPUT); + gpio_write(_pi, servo.pin, PI_LOW); +#endif + } + } + + void on_servo_positions(UaspireServoPositions::SharedPtr msg) { + std::array 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( + ((value / 90.f) * 500.f) + 1000); +#ifdef BUILD_RASPBERRY_PI + set_servo_pulsewidth(_pi, servo.pin, decoded_value); +#endif + } + } +}; class ControlsNode : public ros::Node { private: ros::Subscription::SharedPtr _command_subscription; ros::Publisher::SharedPtr _sensors_publisher; + ros::Publisher::SharedPtr _servo_publisher; ros::TimerBase::SharedPtr _timer; + ros::TimerBase::SharedPtr _servo_timer; + + int _last_value; public: RCLCPP_SHARED_PTR_DEFINITIONS(ControlsNode) @@ -26,10 +97,18 @@ public: std::bind(&ControlsNode::on_message, this, arg::_1)); _sensors_publisher = create_publisher("uaspire/sensors", ros::QoS(10)); + _servo_publisher = + create_publisher("uaspire/servo_positions", ros::QoS(10)); _timer = create_wall_timer(std::chrono::milliseconds(1000), std::bind(&ControlsNode::on_loop, this)); + + _last_value = 0; + // start 100Hz servo timer + _servo_timer = create_wall_timer(std::chrono::milliseconds(10), + std::bind(&ControlsNode::on_servo_update, this)); } + void on_message(const UaspireCommand::SharedPtr cmd) { RCLCPP_INFO(get_logger(), "Command received: %s", cmd->data.c_str()); } @@ -40,13 +119,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::spin(node); + ros::executors::MultiThreadedExecutor executor; + 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; diff --git a/src/uaspire_msgs/CMakeLists.txt b/src/uaspire_msgs/CMakeLists.txt index 947a8f9..450fac6 100644 --- a/src/uaspire_msgs/CMakeLists.txt +++ b/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 diff --git a/src/uaspire_msgs/msg/ServoPositions.msg b/src/uaspire_msgs/msg/ServoPositions.msg new file mode 100644 index 0000000..c71c55e --- /dev/null +++ b/src/uaspire_msgs/msg/ServoPositions.msg @@ -0,0 +1,3 @@ +float32 yaw +float32 pitch +