Skip to content

Commit 9047682

Browse files
zhiquanyeozhiquanyeo
authored
[sim] Add XRP-specific plugin (wpilibsuite#5631)
Provides an implementation of a XRP-specific plugin that sends binary messages over UDP (to account for the less performant hardware on the XRP). This plugin leverages the work already done for the WebSocket protocol and does a translation to/from JSON/binary.
1 parent 575348b commit 9047682

15 files changed

Lines changed: 1095 additions & 0 deletions

File tree

‎settings.gradle‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -45,6 +45,7 @@ include 'simulation:halsim_gui'
4545
include 'simulation:halsim_ws_core'
4646
include 'simulation:halsim_ws_client'
4747
include 'simulation:halsim_ws_server'
48+
include 'simulation:halsim_xrp'
4849
include 'cameraserver'
4950
include 'cameraserver:multiCameraServer'
5051
include 'wpilibNewCommands'

‎simulation/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -5,3 +5,4 @@ add_subdirectory(halsim_ds_socket)
55
add_subdirectory(halsim_ws_core)
66
add_subdirectory(halsim_ws_client)
77
add_subdirectory(halsim_ws_server)
8+
add_subdirectory(halsim_xrp)

‎simulation/halsim_ws_core/src/main/native/cpp/WSProvider_HAL.cpp‎

Lines changed: 59 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,59 @@
1+
// Copyright (c) FIRST and other WPILib contributors.
2+
// Open Source Software; you can modify and/or share it under the terms of
3+
// the WPILib BSD license file in the root directory of this project.
4+
5+
#include "WSProvider_HAL.h"
6+
7+
#include <algorithm>
8+
#include <atomic>
9+
#include <string_view>
10+
11+
#include <hal/Extensions.h>
12+
#include <hal/HAL.h>
13+
#include <hal/Ports.h>
14+
#include <hal/simulation/MockHooks.h>
15+
#include <wpi/raw_ostream.h>
16+
17+
namespace wpilibws {
18+
19+
void HALSimWSProviderHAL::Initialize(WSRegisterFunc webRegisterFunc) {
20+
CreateSingleProvider<HALSimWSProviderHAL>("HAL", webRegisterFunc);
21+
}
22+
23+
HALSimWSProviderHAL::~HALSimWSProviderHAL() {
24+
DoCancelCallbacks();
25+
}
26+
27+
void HALSimWSProviderHAL::RegisterCallbacks() {
28+
m_simPeriodicBeforeCbKey = HALSIM_RegisterSimPeriodicBeforeCallback(
29+
[](void* param) {
30+
static_cast<HALSimWSProviderHAL*>(param)->ProcessHalCallback(
31+
{{">sim_periodic_before", true}});
32+
},
33+
this);
34+
35+
m_simPeriodicAfterCbKey = HALSIM_RegisterSimPeriodicAfterCallback(
36+
[](void* param) {
37+
static_cast<HALSimWSProviderHAL*>(param)->ProcessHalCallback(
38+
{{">sim_periodic_after", true}});
39+
},
40+
this);
41+
}
42+
43+
void HALSimWSProviderHAL::CancelCallbacks() {
44+
DoCancelCallbacks();
45+
}
46+
47+
void HALSimWSProviderHAL::DoCancelCallbacks() {
48+
HALSIM_CancelSimPeriodicBeforeCallback(m_simPeriodicBeforeCbKey);
49+
HALSIM_CancelSimPeriodicAfterCallback(m_simPeriodicAfterCbKey);
50+
51+
m_simPeriodicBeforeCbKey = 0;
52+
m_simPeriodicAfterCbKey = 0;
53+
}
54+
55+
void HALSimWSProviderHAL::OnNetValueChanged(const wpi::json& json) {
56+
// no-op. This is all one way
57+
}
58+
59+
} // namespace wpilibws

‎simulation/halsim_ws_core/src/main/native/include/WSProvider_HAL.h‎

Lines changed: 32 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,32 @@
1+
// Copyright (c) FIRST and other WPILib contributors.
2+
// Open Source Software; you can modify and/or share it under the terms of
3+
// the WPILib BSD license file in the root directory of this project.
4+
5+
#pragma once
6+
7+
#include <memory>
8+
9+
#include "WSHalProviders.h"
10+
11+
namespace wpilibws {
12+
13+
class HALSimWSProviderHAL : public HALSimWSHalProvider {
14+
public:
15+
static void Initialize(WSRegisterFunc webRegisterFunc);
16+
17+
using HALSimWSHalProvider::HALSimWSHalProvider;
18+
~HALSimWSProviderHAL() override;
19+
20+
void OnNetValueChanged(const wpi::json& json) override;
21+
22+
protected:
23+
void RegisterCallbacks() override;
24+
void CancelCallbacks() override;
25+
void DoCancelCallbacks();
26+
27+
private:
28+
int32_t m_simPeriodicBeforeCbKey = 0;
29+
int32_t m_simPeriodicAfterCbKey = 0;
30+
};
31+
32+
} // namespace wpilibws

‎simulation/halsim_xrp/CMakeLists.txt‎

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,16 @@
1+
project(halsim_xrp)
2+
3+
include(CompileWarnings)
4+
5+
file(GLOB halsim_xrp_src src/main/native/cpp/*.cpp)
6+
7+
add_library(halsim_xrp SHARED ${halsim_xrp_src})
8+
wpilib_target_warnings(halsim_xrp)
9+
set_target_properties(halsim_xrp PROPERTIES DEBUG_POSTFIX "d")
10+
target_link_libraries(halsim_xrp PUBLIC hal halsim_ws_core)
11+
12+
target_include_directories(halsim_xrp PRIVATE src/main/native/include)
13+
14+
set_property(TARGET halsim_xrp PROPERTY FOLDER "libraries")
15+
16+
install(TARGETS halsim_xrp EXPORT halsim_xrp DESTINATION "${main_lib_dest}")

‎simulation/halsim_xrp/README.md‎

Lines changed: 125 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,125 @@
1+
# HAL XRP Client
2+
3+
This is an extension that provides a client version of the XRP protocol for transmitting robot hardware interface state to an XRP robot over UDP.
4+
5+
## Configuration
6+
7+
The XRP client has a number of configuration options available through environment variables.
8+
9+
``HALSIMXRP_HOST``: The host to connect to. Defaults to localhost.
10+
11+
``HALSIMXRP_PORT``: The port number to connect to. Defaults to 3540.
12+
13+
## XRP Protocol
14+
15+
The WPILib -> XRP protocol is binary-based to save on bandwidth due to hardware limitations of the XRP robot. The messages to/from the XRP follow a the format below:
16+
17+
| 2 bytes | 1 byte | n bytes |
18+
|---------------------|-------------------|-------------------------------------|
19+
| _uint16_t_ sequence | _uint8_t_ control | [&lt;Tagged Data&gt;](#tagged-data) |
20+
21+
### Control Byte
22+
23+
The control byte is used to indicate the current `enabled` state of the WPILib robot code. When this is set to `1`, the robot is enabled, and when it is set to `0` it is disabled.
24+
25+
Messages originating from the XRP have an unspecified value for the control byte.
26+
27+
### Tagged Data
28+
29+
The `Tagged Data` section can contain an arbitrary number of data blocks. Each block has the format below:
30+
31+
| 1 byte | 1 byte | n bytes |
32+
|----------------|-----------------|-----------------|
33+
| _uint8_t_ size | _uint8_t_ tagID | &lt;payload&gt; |
34+
35+
The `size` byte encodes the size of the data block, _excluding_ itself. Thus the smallest block size is 2 bytes, with a size value of 1 (1 size byte, 1 tag byte, 0 payload bytes). Maximum size of the payload is 254 bytes.
36+
37+
38+
Utilizing tagged data blocks allows us to send multiple pieces of data in a single UDP packet. The tags currently implemented for the XRP are as follows:
39+
40+
| Tag | Description |
41+
|------|-------------------------------|
42+
| 0x12 | [XRPMotor](#xrpmotor) |
43+
| 0x13 | [XRPServo](#xrpservo) |
44+
| 0x14 | [DIO](#dio) |
45+
| 0x15 | [AnalogIn](#analogin) |
46+
| 0x16 | [XRPGyro](#xrpgyro) |
47+
| 0x17 | [BuiltInAccel](#builtinaccel) |
48+
| 0x18 | [Encoder](#encoder) |
49+
50+
51+
#### XRPMotor
52+
53+
| Order | Data Type | Description |
54+
|-------|-----------|-------------------|
55+
| 0 | _uint8_t_ | ID |
56+
| 1 | _float_ | Value [-1.0, 1.0] |
57+
58+
IDs:
59+
| ID | Description |
60+
|----|-------------|
61+
| 0 | Left Motor |
62+
| 1 | Right Motor |
63+
| 2 | Motor 3 |
64+
| 3 | Motor 4 |
65+
66+
#### XRPServo
67+
68+
| Order | Data Type | Description |
69+
|-------|-----------|------------------|
70+
| 0 | _uint8_t_ | ID |
71+
| 1 | _float_ | Value [0.0, 1.0] |
72+
73+
IDs:
74+
| ID | Description |
75+
|----|-------------|
76+
| 4 | Servo 1 |
77+
| 5 | Servo 2 |
78+
79+
#### DIO
80+
81+
| Order | Data Type | Description |
82+
|-------|-----------|--------------------|
83+
| 0 | _uint8_t_ | ID |
84+
| 1 | _uint8_t_ | Value (True/False) |
85+
86+
#### AnalogIn
87+
88+
| Order | Data Type | Description |
89+
|-------|-----------|-------------|
90+
| 0 | _uint8_t_ | ID |
91+
| 1 | _float_ | Value |
92+
93+
#### XRPGyro
94+
95+
| Order | Data Type | Description |
96+
|-------|-----------|---------------|
97+
| 0 | _float_ | rate_x (dps) |
98+
| 1 | _float_ | rate_y (dps) |
99+
| 2 | _float_ | rate_z (dps) |
100+
| 3 | _float_ | angle_x (deg) |
101+
| 4 | _float_ | angle_y (deg) |
102+
| 5 | _float_ | angle_z (deg) |
103+
104+
#### BuiltInAccel
105+
106+
| Order | Data Type | Description |
107+
|-------|-----------|-------------|
108+
| 0 | _float_ | accel_x (g) |
109+
| 1 | _float_ | accel_y (g) |
110+
| 2 | _float_ | accel_z (g) |
111+
112+
#### Encoder
113+
114+
| Order | Data Type | Description |
115+
|-------|-----------|-------------|
116+
| 0 | _uint8_t_ | ID |
117+
| 1 | _int32_t_ | Value |
118+
119+
IDs:
120+
| ID | Description |
121+
|----|---------------------|
122+
| 0 | Left Motor Encoder |
123+
| 1 | Right Motor Encoder |
124+
| 2 | Motor 3 Encoder |
125+
| 3 | Motor 4 Encoder |

‎simulation/halsim_xrp/build.gradle‎

Lines changed: 35 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,35 @@
1+
if (!project.hasProperty('onlylinuxathena')) {
2+
3+
description = "XRP Extension"
4+
5+
ext {
6+
includeWpiutil = true
7+
pluginName = 'halsim_xrp'
8+
}
9+
10+
apply plugin: 'google-test-test-suite'
11+
12+
13+
ext {
14+
staticGtestConfigs = [:]
15+
}
16+
17+
staticGtestConfigs["${pluginName}Test"] = []
18+
apply from: "${rootDir}/shared/googletest.gradle"
19+
20+
apply from: "${rootDir}/shared/plugins/setupBuild.gradle"
21+
22+
model {
23+
binaries {
24+
all {
25+
if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio) {
26+
it.buildable = false
27+
return
28+
}
29+
30+
lib project: ':wpinet', library: 'wpinet', linkage: 'shared'
31+
lib project: ":simulation:halsim_ws_core", library: "halsim_ws_core", linkage: "static"
32+
}
33+
}
34+
}
35+
}

‎simulation/halsim_xrp/src/dev/native/cpp/main.cpp‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
// Copyright (c) FIRST and other WPILib contributors.
2+
// Open Source Software; you can modify and/or share it under the terms of
3+
// the WPILib BSD license file in the root directory of this project.
4+
5+
int main() {}

0 commit comments

Comments
 (0)