diff --git a/.gitmodules b/.gitmodules index 6fb9bad..5f2fa2b 100644 --- a/.gitmodules +++ b/.gitmodules @@ -14,3 +14,6 @@ [submodule "decomposition/wheel_leg_rl"] path = decomposition/wheel_leg_rl url = https://github.com/Yao-Xinchen/Meta-WL +[submodule "perception/cserialport_wrapper/CSerialPort"] + path = perception/cserialport_wrapper/CSerialPort + url = https://github.com/itas109/CSerialPort.git diff --git a/interfaces/operation_interface/CMakeLists.txt b/interfaces/operation_interface/CMakeLists.txt index 3959e18..53f72aa 100644 --- a/interfaces/operation_interface/CMakeLists.txt +++ b/interfaces/operation_interface/CMakeLists.txt @@ -17,6 +17,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/GameInfo.msg" "msg/PowerState.msg" "msg/DbusControl.msg" + "msg/WflyControl.msg" "msg/CustomController.msg" "msg/RobotState.msg" ) diff --git a/interfaces/operation_interface/msg/WflyControl.msg b/interfaces/operation_interface/msg/WflyControl.msg new file mode 100644 index 0000000..56dec2a --- /dev/null +++ b/interfaces/operation_interface/msg/WflyControl.msg @@ -0,0 +1,8 @@ +float64 ls_x # stick +float64 ls_y +float64 rs_x +float64 rs_y +string sa # switch +string sb +string sc +string sd \ No newline at end of file diff --git a/perception/cserialport_wrapper/CMakeLists.txt b/perception/cserialport_wrapper/CMakeLists.txt new file mode 100644 index 0000000..c4e3759 --- /dev/null +++ b/perception/cserialport_wrapper/CMakeLists.txt @@ -0,0 +1,48 @@ +cmake_minimum_required(VERSION 3.8) +project(cserialport_wrapper) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set (CMAKE_CXX_STANDARD 20) + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +set(CSerialPortRootPath "${CMAKE_CURRENT_SOURCE_DIR}/CSerialPort") +list(APPEND CSerialPortSourceFiles ${CSerialPortRootPath}/src/SerialPort.cpp ${CSerialPortRootPath}/src/SerialPortBase.cpp ${CSerialPortRootPath}/src/SerialPortInfo.cpp ${CSerialPortRootPath}/src/SerialPortInfoBase.cpp) +list(APPEND CSerialPortSourceFiles ${CSerialPortRootPath}/src/SerialPortInfoUnixBase.cpp ${CSerialPortRootPath}/src/SerialPortUnixBase.cpp) + +ament_auto_add_library( ${PROJECT_NAME} SHARED ${CSerialPortSourceFiles}) +target_include_directories(${PROJECT_NAME} PUBLIC ${CSerialPortRootPath}/include) + +# remove prefix +# set_target_properties( ${PROJECT_NAME} PROPERTIES PREFIX "") + +# preprocessor definitions for compiling CSerialPort library +# set_target_properties( ${PROJECT_NAME} PROPERTIES COMPILE_DEFINITIONS BUILDING_LIBCSERIALPORT) + +ament_export_include_directories("${CSerialPortRootPath}/include") +install( + DIRECTORY ${CSerialPortRootPath}/include + DESTINATION include +) + +target_link_libraries( ${PROJECT_NAME} pthread) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package() diff --git a/perception/cserialport_wrapper/CSerialPort b/perception/cserialport_wrapper/CSerialPort new file mode 160000 index 0000000..9495d0c --- /dev/null +++ b/perception/cserialport_wrapper/CSerialPort @@ -0,0 +1 @@ +Subproject commit 9495d0c92839c95763c098ce38a89f1f3a04b4c6 diff --git a/perception/cserialport_wrapper/package.xml b/perception/cserialport_wrapper/package.xml new file mode 100644 index 0000000..ec5ab0d --- /dev/null +++ b/perception/cserialport_wrapper/package.xml @@ -0,0 +1,21 @@ + + + + cserialport_wrapper + 0.0.0 + TODO: Package description + chiming2 + TODO: License declaration + + ament_cmake + asio_cmake_module + + ament_lint_auto + ament_lint_common + + rclcpp + + + ament_cmake + + diff --git a/perception/wfly_control/CMakeLists.txt b/perception/wfly_control/CMakeLists.txt new file mode 100644 index 0000000..410f5c5 --- /dev/null +++ b/perception/wfly_control/CMakeLists.txt @@ -0,0 +1,59 @@ +cmake_minimum_required(VERSION 3.8) +project(wfly_control) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set (CMAKE_CXX_STANDARD 20) + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +add_library(wfly_control SHARED + src/wfly_control.cpp + src/wfly_sbus.cpp) + +target_include_directories(wfly_control PRIVATE + $ + $) + +rclcpp_components_register_node(wfly_control + PLUGIN "WflyControl" + EXECUTABLE wfly_control_node) + +ament_target_dependencies(wfly_control + rclcpp + rclcpp_components + operation_interface + cserialport_wrapper +) + +install(TARGETS + wfly_control + DESTINATION lib/${PROJECT_NAME}) + +install( + TARGETS wfly_control + EXPORT export_${PROJECT_NAME} + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/perception/wfly_control/include/wfly_control/wfly_control.hpp b/perception/wfly_control/include/wfly_control/wfly_control.hpp new file mode 100644 index 0000000..db6ac37 --- /dev/null +++ b/perception/wfly_control/include/wfly_control/wfly_control.hpp @@ -0,0 +1,29 @@ +#ifndef WFLY_CONTROL_HPP +#define WFLY_CONTROL_HPP + +#include "rclcpp/rclcpp.hpp" +#include "operation_interface/msg/wfly_control.hpp" +#include + +#include "wfly_control/wfly_sbus.hpp" + +#define PUB_RATE 20 // ms + +class WflyControl +{ +public: + WflyControl(const rclcpp::NodeOptions & options); + ~WflyControl(); + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() const; + +private: + rclcpp::Node::SharedPtr node_; + rclcpp::Publisher::SharedPtr wfly_pub_; + rclcpp::TimerBase::SharedPtr timer_; + + std::unique_ptr sbus_; + + void timer_callback(); +}; + +#endif // WFLY_CONTROL_HPP \ No newline at end of file diff --git a/perception/wfly_control/include/wfly_control/wfly_sbus.hpp b/perception/wfly_control/include/wfly_control/wfly_sbus.hpp new file mode 100644 index 0000000..80c72f6 --- /dev/null +++ b/perception/wfly_control/include/wfly_control/wfly_sbus.hpp @@ -0,0 +1,79 @@ +#ifndef WFLY_SBUS_HPP +#define WFLY_SBUS_HPP + +#include "rclcpp/rclcpp.hpp" +#include +#include + +#include "operation_interface/msg/wfly_control.hpp" +#include "CSerialPort/SerialPort.h" +#include "CSerialPort/SerialPortInfo.h" + +struct WflyData +{ + uint16_t ch1; + uint16_t ch2; + uint16_t ch3; + uint16_t ch4; + uint16_t ch5; + uint16_t ch6; + uint16_t ch7; + uint16_t ch8; +}; + +struct [[gnu::packed]] SbusFrame +{ + uint8_t head : 8; + + // Data packet, 11*16 = 176 bits = 22 bytes + uint16_t ch1 : 11; + uint16_t ch2 : 11; + uint16_t ch3 : 11; + uint16_t ch4 : 11; + uint16_t ch5 : 11; + uint16_t ch6 : 11; + uint16_t ch7 : 11; + uint16_t ch8 : 11; + uint16_t ch9 : 11; + uint16_t ch10 : 11; + uint16_t ch11 : 11; + uint16_t ch12 : 11; + uint16_t ch13 : 11; + uint16_t ch14 : 11; + uint16_t ch15 : 11; + uint16_t ch16 : 11; + + uint8_t falgs : 8; + uint8_t tail : 8; +}; + +class WflySbus +{ +public: + WflySbus(std::string dev_path); + ~WflySbus(); + + operation_interface::msg::WflyControl controller_msg(); + + bool valid() { return first_packet_arrived_; } + +private: + bool first_packet_arrived_ = false; + + std::string dev_path_; + + void process_packet(const uint8_t* packet_buffer); + + bool read_with_timeout(uint8_t* buffer, size_t bytes_to_read, int timeout_ms); + + operation_interface::msg::WflyControl wfly_msg_; + + // Serial port + std::unique_ptr serial_port_; + + // RX thread + std::unique_ptr rx_thread_; + void rx_loop(std::stop_token stop_token); +}; + +#endif // WFLY_SBUS_HPP \ No newline at end of file diff --git a/perception/wfly_control/package.xml b/perception/wfly_control/package.xml new file mode 100644 index 0000000..6beb876 --- /dev/null +++ b/perception/wfly_control/package.xml @@ -0,0 +1,25 @@ + + + + wfly_control + 0.0.0 + TODO: Package description + chiming2 + TODO: License declaration + + ament_cmake + asio_cmake_module + + ament_lint_auto + ament_lint_common + + rclcpp + rclcpp_components + asio + operation_interface + cserialport_wrapper + + + ament_cmake + + diff --git a/perception/wfly_control/src/wfly_control.cpp b/perception/wfly_control/src/wfly_control.cpp new file mode 100644 index 0000000..c2909aa --- /dev/null +++ b/perception/wfly_control/src/wfly_control.cpp @@ -0,0 +1,34 @@ +#include "rclcpp/rclcpp.hpp" +#include "wfly_control/wfly_control.hpp" + +WflyControl::WflyControl(const rclcpp::NodeOptions & options) +{ + node_ = rclcpp::Node::make_shared("dbus_control", options); + std::string port = node_->declare_parameter("sbus_port", "/dev/wfly_receiver"); + sbus_ = std::make_unique(port); + + wfly_pub_ = node_->create_publisher("wfly_control", 10); + timer_ = node_->create_wall_timer(std::chrono::milliseconds(PUB_RATE), std::bind(&WflyControl::timer_callback, this)); + + RCLCPP_INFO(node_->get_logger(), "WflyControl initialized"); +} + +WflyControl::~WflyControl() +{ +} + +rclcpp::node_interfaces::NodeBaseInterface::SharedPtr WflyControl::get_node_base_interface() const +{ + return node_->get_node_base_interface(); +} + +void WflyControl::timer_callback() +{ + if (!sbus_->valid()) return; + auto controller_msg = sbus_->controller_msg(); + wfly_pub_->publish(controller_msg); +} + +#include "rclcpp_components/register_node_macro.hpp" + +RCLCPP_COMPONENTS_REGISTER_NODE(WflyControl) \ No newline at end of file diff --git a/perception/wfly_control/src/wfly_sbus.cpp b/perception/wfly_control/src/wfly_sbus.cpp new file mode 100644 index 0000000..ec86d93 --- /dev/null +++ b/perception/wfly_control/src/wfly_sbus.cpp @@ -0,0 +1,178 @@ +#include "wfly_control/wfly_sbus.hpp" +#include "CSerialPort/SerialPort_global.h" + +#include +#include +#include +#include +#include +#include +#include + +using namespace itas109; + +constexpr uint32_t sbus_baud_rate = 100000; +constexpr Parity sbus_parity = itas109::ParityEven; +constexpr DataBits sbus_data_bits = itas109::DataBits8; +constexpr StopBits sbus_stopbits = itas109::StopTwo; +constexpr FlowControl sbus_flow_control = itas109::FlowHardware; + +WflySbus::WflySbus(std::string dev_path) : dev_path_(dev_path) +{ + serial_port_ = std::make_unique(); + serial_port_->init( + dev_path_.c_str(), + sbus_baud_rate, + sbus_parity, + sbus_data_bits, + sbus_stopbits, + sbus_flow_control + ); + + serial_port_->open(); + + // Start rx thread + rx_thread_ = std::make_unique([this](std::stop_token s) { rx_loop(s); }); +} + +WflySbus::~WflySbus() +{ +} + +operation_interface::msg::WflyControl WflySbus::controller_msg() +{ + return wfly_msg_; +} + +void WflySbus::rx_loop(std::stop_token stop_token) { + uint8_t packet_buffer[25]; // Buffer for the complete packet + + while (!stop_token.stop_requested()) { + + if (!read_with_timeout(&packet_buffer[0], 1, 1000)) { + continue; + } + + if (packet_buffer[0] != 0x0F) { + std::cout << "Skipping invalid header: 0x" << std::hex << static_cast(packet_buffer[0]) << std::endl; + continue; + } + + if (!read_with_timeout(&packet_buffer[1], 24, 1000)) { + std::cerr << "Failed to receive complete packet data" << std::endl; + continue; + } + + process_packet(packet_buffer); + } +} + +bool WflySbus::read_with_timeout(uint8_t* buffer, size_t bytes_to_read, int timeout_ms) { + const auto start_time = std::chrono::steady_clock::now(); + size_t bytes_received = 0; + + while (bytes_received < bytes_to_read) { + // Check for timeout + auto elapsed = std::chrono::duration_cast( + std::chrono::steady_clock::now() - start_time).count(); + if (elapsed > timeout_ms) { + std::cerr << "Timeout after " << elapsed << " ms, received " + << bytes_received << "/" << bytes_to_read << " bytes" << std::endl; + return false; + } + + // Try to read remaining bytes + int result = serial_port_->readData(buffer + bytes_received, bytes_to_read - bytes_received); + + if (result > 0) { + bytes_received += result; + // if (bytes_to_read > 1) { // Only log for multi-byte reads + // std::cout << "Received " << result << " bytes, total: " + // << bytes_received << "/" << bytes_to_read << std::endl; + // } + } else if (result < 0) { + std::cerr << "Error reading from serial port: " << result << std::endl; + return false; // Return on error + } else { + // No data available, wait a bit before retrying + std::this_thread::sleep_for(std::chrono::milliseconds(5)); + } + } + + return true; // Successfully read all bytes +} + +void WflySbus::process_packet(const uint8_t* data) { + if (!first_packet_arrived_) { + first_packet_arrived_ = true; + } + + SbusFrame* sbus_frame = reinterpret_cast(const_cast(data)); + + wfly_msg_.ls_x = (sbus_frame->ch3 - 0x400) / static_cast(0x29e); + wfly_msg_.ls_y = (sbus_frame->ch4 - 0x400) / static_cast(0x29e); + wfly_msg_.rs_x = (sbus_frame->ch2 - 0x400) / static_cast(0x29e); + wfly_msg_.rs_y = (sbus_frame->ch1 - 0x400) / static_cast(0x29e); + switch (sbus_frame->ch5) { + case 0x69e: + wfly_msg_.sa = "down"; + break; + case 0x161: + wfly_msg_.sa = "up"; + break; + default: + wfly_msg_.sa = "up"; + break; + } + switch (sbus_frame->ch6) { + case 0x69e: + wfly_msg_.sb = "down"; + break; + case 0x400: + wfly_msg_.sb = "mid"; + break; + case 0x161: + wfly_msg_.sb = "up"; + break; + default: + wfly_msg_.sb = "up"; + break; + } + switch (sbus_frame->ch7) { + case 0x69e: + wfly_msg_.sc = "down"; + break; + case 0x400: + wfly_msg_.sc = "mid"; + break; + case 0x161: + wfly_msg_.sc = "up"; + break; + default: + wfly_msg_.sc = "up"; + break; + } + switch (sbus_frame->ch8) { + case 0x69e: + wfly_msg_.sd = "down"; + break; + case 0x161: + wfly_msg_.sd = "up"; + break; + default: + wfly_msg_.sd = "up"; + break; + } + + // Print the received data + // std::cout << "Received SBUS data:" << std::endl; + // std::cout << "ls_x: " << wfly_msg_.ls_x << std::endl; + // std::cout << "ls_y: " << wfly_msg_.ls_y << std::endl; + // std::cout << "rs_x: " << wfly_msg_.rs_x << std::endl; + // std::cout << "rs_y: " << wfly_msg_.rs_y << std::endl; + // std::cout << "sa: " << wfly_msg_.sa << std::endl; + // std::cout << "sb: " << wfly_msg_.sb << std::endl; + // std::cout << "sc: " << wfly_msg_.sc << std::endl; + // std::cout << "sd: " << wfly_msg_.sd << std::endl; + // std::cout << "======" << std::endl; +}