Skip to content

Commit d8d6aef

Browse files
nice-meenice-mee
authored
Merge pull request #13 from nice-mee/master
`wfly_control` Support for WFLY family remote controllers
2 parents c7000eb + aae62cf commit d8d6aef

File tree

12 files changed

+486
-0
lines changed

12 files changed

+486
-0
lines changed

‎.gitmodules‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -14,3 +14,6 @@
1414
[submodule "decomposition/wheel_leg_rl"]
1515
path = decomposition/wheel_leg_rl
1616
url = https://github.com/Yao-Xinchen/Meta-WL
17+
[submodule "perception/cserialport_wrapper/CSerialPort"]
18+
path = perception/cserialport_wrapper/CSerialPort
19+
url = https://github.com/itas109/CSerialPort.git

‎interfaces/operation_interface/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
1717
"msg/GameInfo.msg"
1818
"msg/PowerState.msg"
1919
"msg/DbusControl.msg"
20+
"msg/WflyControl.msg"
2021
"msg/CustomController.msg"
2122
"msg/RobotState.msg"
2223
)

‎interfaces/operation_interface/msg/WflyControl.msg‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,8 @@
1+
float64 ls_x # stick
2+
float64 ls_y
3+
float64 rs_x
4+
float64 rs_y
5+
string sa # switch
6+
string sb
7+
string sc
8+
string sd

‎perception/cserialport_wrapper/CMakeLists.txt‎

Lines changed: 48 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,48 @@
1+
cmake_minimum_required(VERSION 3.8)
2+
project(cserialport_wrapper)
3+
4+
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
5+
add_compile_options(-Wall -Wextra -Wpedantic)
6+
endif()
7+
8+
set (CMAKE_CXX_STANDARD 20)
9+
10+
# find dependencies
11+
find_package(ament_cmake REQUIRED)
12+
find_package(ament_cmake_auto REQUIRED)
13+
ament_auto_find_build_dependencies()
14+
15+
set(CSerialPortRootPath "${CMAKE_CURRENT_SOURCE_DIR}/CSerialPort")
16+
list(APPEND CSerialPortSourceFiles ${CSerialPortRootPath}/src/SerialPort.cpp ${CSerialPortRootPath}/src/SerialPortBase.cpp ${CSerialPortRootPath}/src/SerialPortInfo.cpp ${CSerialPortRootPath}/src/SerialPortInfoBase.cpp)
17+
list(APPEND CSerialPortSourceFiles ${CSerialPortRootPath}/src/SerialPortInfoUnixBase.cpp ${CSerialPortRootPath}/src/SerialPortUnixBase.cpp)
18+
19+
ament_auto_add_library( ${PROJECT_NAME} SHARED ${CSerialPortSourceFiles})
20+
target_include_directories(${PROJECT_NAME} PUBLIC ${CSerialPortRootPath}/include)
21+
22+
# remove prefix
23+
# set_target_properties( ${PROJECT_NAME} PROPERTIES PREFIX "")
24+
25+
# preprocessor definitions for compiling CSerialPort library
26+
# set_target_properties( ${PROJECT_NAME} PROPERTIES COMPILE_DEFINITIONS BUILDING_LIBCSERIALPORT)
27+
28+
ament_export_include_directories("${CSerialPortRootPath}/include")
29+
install(
30+
DIRECTORY ${CSerialPortRootPath}/include
31+
DESTINATION include
32+
)
33+
34+
target_link_libraries( ${PROJECT_NAME} pthread)
35+
36+
if(BUILD_TESTING)
37+
find_package(ament_lint_auto REQUIRED)
38+
# the following line skips the linter which checks for copyrights
39+
# comment the line when a copyright and license is added to all source files
40+
set(ament_cmake_copyright_FOUND TRUE)
41+
# the following line skips cpplint (only works in a git repo)
42+
# comment the line when this package is in a git repo and when
43+
# a copyright and license is added to all source files
44+
set(ament_cmake_cpplint_FOUND TRUE)
45+
ament_lint_auto_find_test_dependencies()
46+
endif()
47+
48+
ament_auto_package()

‎perception/cserialport_wrapper/CSerialPort‎

Submodule CSerialPort added at 9495d0c

‎perception/cserialport_wrapper/package.xml‎

Lines changed: 21 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,21 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>cserialport_wrapper</name>
5+
<version>0.0.0</version>
6+
<description>TODO: Package description</description>
7+
<maintainer email="chiming2@illinois.edu">chiming2</maintainer>
8+
<license>TODO: License declaration</license>
9+
10+
<buildtool_depend>ament_cmake</buildtool_depend>
11+
<buildtool_depend>asio_cmake_module</buildtool_depend>
12+
13+
<test_depend>ament_lint_auto</test_depend>
14+
<test_depend>ament_lint_common</test_depend>
15+
16+
<depend>rclcpp</depend>
17+
18+
<export>
19+
<build_type>ament_cmake</build_type>
20+
</export>
21+
</package>

‎perception/wfly_control/CMakeLists.txt‎

Lines changed: 59 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,59 @@
1+
cmake_minimum_required(VERSION 3.8)
2+
project(wfly_control)
3+
4+
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
5+
add_compile_options(-Wall -Wextra -Wpedantic)
6+
endif()
7+
8+
set (CMAKE_CXX_STANDARD 20)
9+
10+
# find dependencies
11+
find_package(ament_cmake REQUIRED)
12+
find_package(ament_cmake_auto REQUIRED)
13+
ament_auto_find_build_dependencies()
14+
15+
add_library(wfly_control SHARED
16+
src/wfly_control.cpp
17+
src/wfly_sbus.cpp)
18+
19+
target_include_directories(wfly_control PRIVATE
20+
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
21+
$<INSTALL_INTERFACE:include>)
22+
23+
rclcpp_components_register_node(wfly_control
24+
PLUGIN "WflyControl"
25+
EXECUTABLE wfly_control_node)
26+
27+
ament_target_dependencies(wfly_control
28+
rclcpp
29+
rclcpp_components
30+
operation_interface
31+
cserialport_wrapper
32+
)
33+
34+
install(TARGETS
35+
wfly_control
36+
DESTINATION lib/${PROJECT_NAME})
37+
38+
install(
39+
TARGETS wfly_control
40+
EXPORT export_${PROJECT_NAME}
41+
LIBRARY DESTINATION lib
42+
ARCHIVE DESTINATION lib
43+
RUNTIME DESTINATION bin
44+
INCLUDES DESTINATION include
45+
)
46+
47+
if(BUILD_TESTING)
48+
find_package(ament_lint_auto REQUIRED)
49+
# the following line skips the linter which checks for copyrights
50+
# comment the line when a copyright and license is added to all source files
51+
set(ament_cmake_copyright_FOUND TRUE)
52+
# the following line skips cpplint (only works in a git repo)
53+
# comment the line when this package is in a git repo and when
54+
# a copyright and license is added to all source files
55+
set(ament_cmake_cpplint_FOUND TRUE)
56+
ament_lint_auto_find_test_dependencies()
57+
endif()
58+
59+
ament_package()

‎perception/wfly_control/include/wfly_control/wfly_control.hpp‎

Lines changed: 29 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
#ifndef WFLY_CONTROL_HPP
2+
#define WFLY_CONTROL_HPP
3+
4+
#include "rclcpp/rclcpp.hpp"
5+
#include "operation_interface/msg/wfly_control.hpp"
6+
#include <memory>
7+
8+
#include "wfly_control/wfly_sbus.hpp"
9+
10+
#define PUB_RATE 20 // ms
11+
12+
class WflyControl
13+
{
14+
public:
15+
WflyControl(const rclcpp::NodeOptions & options);
16+
~WflyControl();
17+
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() const;
18+
19+
private:
20+
rclcpp::Node::SharedPtr node_;
21+
rclcpp::Publisher<operation_interface::msg::WflyControl>::SharedPtr wfly_pub_;
22+
rclcpp::TimerBase::SharedPtr timer_;
23+
24+
std::unique_ptr<WflySbus> sbus_;
25+
26+
void timer_callback();
27+
};
28+
29+
#endif // WFLY_CONTROL_HPP

‎perception/wfly_control/include/wfly_control/wfly_sbus.hpp‎

Lines changed: 79 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,79 @@
1+
#ifndef WFLY_SBUS_HPP
2+
#define WFLY_SBUS_HPP
3+
4+
#include "rclcpp/rclcpp.hpp"
5+
#include <cstdint>
6+
#include <operation_interface/msg/detail/wfly_control__struct.hpp>
7+
8+
#include "operation_interface/msg/wfly_control.hpp"
9+
#include "CSerialPort/SerialPort.h"
10+
#include "CSerialPort/SerialPortInfo.h"
11+
12+
struct WflyData
13+
{
14+
uint16_t ch1;
15+
uint16_t ch2;
16+
uint16_t ch3;
17+
uint16_t ch4;
18+
uint16_t ch5;
19+
uint16_t ch6;
20+
uint16_t ch7;
21+
uint16_t ch8;
22+
};
23+
24+
struct [[gnu::packed]] SbusFrame
25+
{
26+
uint8_t head : 8;
27+
28+
// Data packet, 11*16 = 176 bits = 22 bytes
29+
uint16_t ch1 : 11;
30+
uint16_t ch2 : 11;
31+
uint16_t ch3 : 11;
32+
uint16_t ch4 : 11;
33+
uint16_t ch5 : 11;
34+
uint16_t ch6 : 11;
35+
uint16_t ch7 : 11;
36+
uint16_t ch8 : 11;
37+
uint16_t ch9 : 11;
38+
uint16_t ch10 : 11;
39+
uint16_t ch11 : 11;
40+
uint16_t ch12 : 11;
41+
uint16_t ch13 : 11;
42+
uint16_t ch14 : 11;
43+
uint16_t ch15 : 11;
44+
uint16_t ch16 : 11;
45+
46+
uint8_t falgs : 8;
47+
uint8_t tail : 8;
48+
};
49+
50+
class WflySbus
51+
{
52+
public:
53+
WflySbus(std::string dev_path);
54+
~WflySbus();
55+
56+
operation_interface::msg::WflyControl controller_msg();
57+
58+
bool valid() { return first_packet_arrived_; }
59+
60+
private:
61+
bool first_packet_arrived_ = false;
62+
63+
std::string dev_path_;
64+
65+
void process_packet(const uint8_t* packet_buffer);
66+
67+
bool read_with_timeout(uint8_t* buffer, size_t bytes_to_read, int timeout_ms);
68+
69+
operation_interface::msg::WflyControl wfly_msg_;
70+
71+
// Serial port
72+
std::unique_ptr<itas109::CSerialPort> serial_port_;
73+
74+
// RX thread
75+
std::unique_ptr<std::jthread> rx_thread_;
76+
void rx_loop(std::stop_token stop_token);
77+
};
78+
79+
#endif // WFLY_SBUS_HPP

‎perception/wfly_control/package.xml‎

Lines changed: 25 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,25 @@
1+
<?xml version="1.0"?>
2+
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
3+
<package format="3">
4+
<name>wfly_control</name>
5+
<version>0.0.0</version>
6+
<description>TODO: Package description</description>
7+
<maintainer email="chiming2@illinois.edu">chiming2</maintainer>
8+
<license>TODO: License declaration</license>
9+
10+
<buildtool_depend>ament_cmake</buildtool_depend>
11+
<buildtool_depend>asio_cmake_module</buildtool_depend>
12+
13+
<test_depend>ament_lint_auto</test_depend>
14+
<test_depend>ament_lint_common</test_depend>
15+
16+
<depend>rclcpp</depend>
17+
<depend>rclcpp_components</depend>
18+
<depend>asio</depend>
19+
<depend>operation_interface</depend>
20+
<depend>cserialport_wrapper</depend>
21+
22+
<export>
23+
<build_type>ament_cmake</build_type>
24+
</export>
25+
</package>

0 commit comments

Comments
 (0)