wfly_control Support for WFLY family remote controllers

.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

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"
 )

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

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()

perception/cserialport_wrapper/package.xml

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>cserialport_wrapper</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="chiming2@illinois.edu">chiming2</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>asio_cmake_module</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <depend>rclcpp</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

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
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+
+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()

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 <memory>
+
+#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<operation_interface::msg::WflyControl>::SharedPtr wfly_pub_;
+    rclcpp::TimerBase::SharedPtr timer_;
+
+    std::unique_ptr<WflySbus> sbus_;
+
+    void timer_callback();
+};
+
+#endif // WFLY_CONTROL_HPP

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 <cstdint>
+#include <operation_interface/msg/detail/wfly_control__struct.hpp>
+
+#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<itas109::CSerialPort> serial_port_;
+
+    // RX thread
+    std::unique_ptr<std::jthread> rx_thread_;
+    void rx_loop(std::stop_token stop_token);
+};
+
+#endif // WFLY_SBUS_HPP

perception/wfly_control/package.xml

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>wfly_control</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="chiming2@illinois.edu">chiming2</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>asio_cmake_module</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>asio</depend>
+  <depend>operation_interface</depend>
+  <depend>cserialport_wrapper</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

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<WflySbus>(port);
+
+    wfly_pub_ = node_->create_publisher<operation_interface::msg::WflyControl>("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)