Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[hal, wpilib] Add OpMode support (#7744)

Browse Source 
User code:
- OpModeRobot used as the robot base class
- LinearOpMode and PeriodicOpMode are provided opmode base classes
- In Java, annotations can be used to automatically register opmode classes

Additional user code functionality:
- OpMode (string) is available in addition to the overall
auto/teleop/test robot mode
- OpMode does not indicate enable (enable/disable is still separate)
- The HAL API uses integer UIDs; these are exposed at the user API level
as well for faster checks
- User code creates opmodes on startup (these have name, category,
description, etc).

DS:
- DS will present opmode selection lists for auto and teleop for
match/practice. During a match, the DS will automatically activate the
selected opmode in the corresponding match period.
- For testing, an overall mode is selected (e.g. teleop/auto/test) and a
single opmode is selected

Future work:
- Command framework support/integration
- Python annotation support
- Unit tests (needs race-free DS sim updates)
- Porting of examples

Co-authored-by: Joseph Eng <91924258+KangarooKoala@users.noreply.github.com>
This commit is contained in:
Peter Johnson
2025-12-12 21:25:57 -07:00
committed by GitHub
parent 2a41b80e00
commit dacded37e5
163 changed files with 7454 additions and 2175 deletions
Download Patch File Download Diff File Expand all files Collapse all files

162
hal/src/main/native/cpp/DashboardOpMode.cpp Normal file
View File

@@ -0,0 +1,162 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "wpi/hal/DashboardOpMode.hpp"
#include <atomic>
#include <string>
#include <vector>
#include <fmt/format.h>
#include "wpi/hal/DriverStationTypes.h"
#include "wpi/nt/NetworkTableInstance.hpp"
#include "wpi/nt/NetworkTableListener.hpp"
#include "wpi/nt/StringArrayTopic.hpp"
#include "wpi/nt/StringTopic.hpp"
#include "wpi/nt/ntcore_cpp.hpp"
#include "wpi/util/StringMap.hpp"
#include "wpi/util/mutex.hpp"
#include "wpi/util/string.h"
using namespace wpi;
namespace {
class DashboardOpModeSender {
public:
void Start(nt::NetworkTableInstance inst, std::string_view tableName) {
m_typeTopic = inst.GetStringTopic(fmt::format("{}/.type", tableName));
m_optionsTopic =
inst.GetStringArrayTopic(fmt::format("{}/options", tableName));
m_activeTopic = inst.GetStringTopic(fmt::format("{}/active", tableName));
m_selectedSub = inst.GetStringTopic(fmt::format("{}/selected", tableName))
.Subscribe("");
m_selectedListener = nt::NetworkTableListener::CreateListener(
m_selectedSub, NT_EVENT_VALUE_ALL | NT_EVENT_IMMEDIATE,
[this](const nt::Event& event) {
if (auto data = event.GetValueEventData()) {
if (data->value.IsString()) {
m_activePub.Set(data->value.GetString());
}
}
});
}
void Enable() {
m_typePub = m_typeTopic.Publish();
m_typePub.Set("String Chooser");
m_optionsPub = m_optionsTopic.Publish();
m_optionsPub.Set(m_options);
m_activePub = m_activeTopic.Publish();
m_activePub.Set("");
}
void SetOptions(std::span<const HAL_OpModeOption> options,
HAL_RobotMode mode) {
m_optionMap.clear();
m_options.clear();
for (auto&& option : options) {
if (HAL_OpMode_GetRobotMode(option.id) == mode) {
auto name = util::to_string_view(&option.name);
m_optionMap[name] = option.id;
m_options.emplace_back(name);
}
}
if (m_optionsPub) {
m_optionsPub.Set(m_options);
}
}
int64_t GetSelected() const {
auto it = m_optionMap.find(m_selectedSub.Get());
if (it == m_optionMap.end()) {
return 0;
}
return it->second;
}
private:
nt::StringTopic m_typeTopic;
nt::StringPublisher m_typePub;
nt::StringArrayTopic m_optionsTopic;
nt::StringArrayPublisher m_optionsPub;
nt::StringTopic m_activeTopic;
nt::StringPublisher m_activePub;
nt::StringSubscriber m_selectedSub;
nt::NetworkTableListener m_selectedListener;
util::StringMap<int64_t> m_optionMap;
std::vector<std::string> m_options;
};
struct DashboardOpModeInstance {
void Start(nt::NetworkTableInstance inst) {
autoOpModes.Start(inst, "/SmartDashboard/Auto OpMode");
teleopOpModes.Start(inst, "/SmartDashboard/Teleop OpMode");
testOpModes.Start(inst, "/SmartDashboard/Test OpMode");
}
util::mutex mutex;
DashboardOpModeSender autoOpModes;
DashboardOpModeSender teleopOpModes;
DashboardOpModeSender testOpModes;
};
} // namespace
static DashboardOpModeInstance* gInstance;
static std::atomic_flag gStarted{};
static std::atomic_flag gEnabled{};
void hal::InitializeDashboardOpMode() {
static DashboardOpModeInstance inst;
gInstance = &inst;
}
void hal::SetDashboardOpModeOptions(std::span<const HAL_OpModeOption> options) {
std::scoped_lock lock{gInstance->mutex};
gInstance->autoOpModes.SetOptions(options, HAL_ROBOTMODE_AUTONOMOUS);
gInstance->teleopOpModes.SetOptions(options, HAL_ROBOTMODE_TELEOPERATED);
gInstance->testOpModes.SetOptions(options, HAL_ROBOTMODE_TEST);
}
void hal::StartDashboardOpMode() {
if (gStarted.test_and_set()) {
return;
}
std::scoped_lock lock{gInstance->mutex};
gInstance->Start(nt::NetworkTableInstance::GetDefault());
}
void hal::EnableDashboardOpMode() {
if (gEnabled.test_and_set()) {
return;
}
StartDashboardOpMode();
std::scoped_lock lock{gInstance->mutex};
gInstance->autoOpModes.Enable();
gInstance->teleopOpModes.Enable();
gInstance->testOpModes.Enable();
}
int64_t hal::GetDashboardSelectedOpMode(HAL_RobotMode robotMode) {
if (!gEnabled.test()) {
return 0;
}
std::scoped_lock lock{gInstance->mutex};
switch (robotMode) {
case HAL_ROBOTMODE_AUTONOMOUS:
return gInstance->autoOpModes.GetSelected();
case HAL_ROBOTMODE_TELEOPERATED:
return gInstance->teleopOpModes.GetSelected();
case HAL_ROBOTMODE_TEST:
return gInstance->testOpModes.GetSelected();
default:
return 0;
}
}

105
hal/src/main/native/cpp/jni/DriverStationJNI.cpp
View File

@@ -5,6 +5,8 @@
#include <jni.h>
#include <cassert>
#include <utility>
#include <vector>
#include <fmt/format.h>
@@ -61,68 +63,77 @@ Java_org_wpilib_hardware_hal_DriverStationJNI_observeUserProgramStarting
/*
* Class: org_wpilib_hardware_hal_DriverStationJNI
* Method: observeUserProgramDisabled
* Signature: ()V
* Method: observeUserProgram
* Signature: (J)V
*/
JNIEXPORT void JNICALL
Java_org_wpilib_hardware_hal_DriverStationJNI_observeUserProgramDisabled
(JNIEnv*, jclass)
Java_org_wpilib_hardware_hal_DriverStationJNI_observeUserProgram
(JNIEnv*, jclass, jlong word)
{
HAL_ObserveUserProgramDisabled();
}
/*
* Class: org_wpilib_hardware_hal_DriverStationJNI
* Method: observeUserProgramAutonomous
* Signature: ()V
*/
JNIEXPORT void JNICALL
Java_org_wpilib_hardware_hal_DriverStationJNI_observeUserProgramAutonomous
(JNIEnv*, jclass)
{
HAL_ObserveUserProgramAutonomous();
}
/*
* Class: org_wpilib_hardware_hal_DriverStationJNI
* Method: observeUserProgramTeleop
* Signature: ()V
*/
JNIEXPORT void JNICALL
Java_org_wpilib_hardware_hal_DriverStationJNI_observeUserProgramTeleop
(JNIEnv*, jclass)
{
HAL_ObserveUserProgramTeleop();
}
/*
* Class: org_wpilib_hardware_hal_DriverStationJNI
* Method: observeUserProgramTest
* Signature: ()V
*/
JNIEXPORT void JNICALL
Java_org_wpilib_hardware_hal_DriverStationJNI_observeUserProgramTest
(JNIEnv*, jclass)
{
HAL_ObserveUserProgramTest();
HAL_ObserveUserProgram({.value = word});
}
/*
* Class: org_wpilib_hardware_hal_DriverStationJNI
* Method: nativeGetControlWord
* Signature: ()I
* Signature: ()J
*/
JNIEXPORT jint JNICALL
JNIEXPORT jlong JNICALL
Java_org_wpilib_hardware_hal_DriverStationJNI_nativeGetControlWord
(JNIEnv*, jclass)
{
static_assert(sizeof(HAL_ControlWord) == sizeof(jint),
static_assert(sizeof(HAL_ControlWord) == sizeof(jlong),
"Java int must match the size of control word");
HAL_ControlWord controlWord;
HAL_GetControlWord(&controlWord);
jint retVal = 0;
std::memcpy(&retVal, &controlWord, sizeof(HAL_ControlWord));
return retVal;
return controlWord.value;
}
/*
* Class: org_wpilib_hardware_hal_DriverStationJNI
* Method: nativeGetUncachedControlWord
* Signature: ()J
*/
JNIEXPORT jlong JNICALL
Java_org_wpilib_hardware_hal_DriverStationJNI_nativeGetUncachedControlWord
(JNIEnv*, jclass)
{
static_assert(sizeof(HAL_ControlWord) == sizeof(jlong),
"Java int must match the size of control word");
HAL_ControlWord controlWord;
HAL_GetUncachedControlWord(&controlWord);
return controlWord.value;
}
/*
* Class: org_wpilib_hardware_hal_DriverStationJNI
* Method: setOpModeOptions
* Signature: ([Ljava/lang/Object;)V
*/
JNIEXPORT void JNICALL
Java_org_wpilib_hardware_hal_DriverStationJNI_setOpModeOptions
(JNIEnv* env, jclass, jobjectArray options)
{
std::vector<HAL_OpModeOption> coptions;
if (options != nullptr) {
jsize length = env->GetArrayLength(options);
coptions.reserve(length);
for (jsize i = 0; i < length; i++) {
JLocal<jobject> option{env, env->GetObjectArrayElement(options, i)};
if (!option) {
ThrowIllegalArgumentException(env, "Null OpModeOption passed in array");
return;
}
auto coption = CreateOpModeOptionFromJava(env, option);
if (coption.id == 0) {
// exception thrown
return;
}
coptions.emplace_back(std::move(coption));
}
}
int32_t status = HAL_SetOpModeOptions(coptions.data(), coptions.size());
CheckStatusForceThrow(env, status);
}
/*

67
hal/src/main/native/cpp/jni/HALUtil.cpp
View File

@@ -53,6 +53,7 @@ static JClass matchInfoDataCls;
static JClass canReceiveMessageCls;
static JClass canStreamMessageCls;
static JClass halValueCls;
static JClass opModeOptionCls;
static JClass revPHVersionCls;
static JClass canStreamOverflowExCls;
@@ -63,6 +64,7 @@ static const JClassInit classes[] = {
{"org/wpilib/hardware/hal/MatchInfoData", &matchInfoDataCls},
{"org/wpilib/hardware/hal/can/CANReceiveMessage", &canReceiveMessageCls},
{"org/wpilib/hardware/hal/can/CANStreamMessage", &canStreamMessageCls},
{"org/wpilib/hardware/hal/OpModeOption", &opModeOptionCls},
{"org/wpilib/hardware/hal/HALValue", &halValueCls},
{"org/wpilib/hardware/hal/REVPHVersion", &revPHVersionCls},
{"org/wpilib/hardware/hal/can/CANStreamOverflowException",
@@ -188,6 +190,71 @@ void ThrowBoundaryException(JNIEnv* env, double value, double lower,
env->Throw(static_cast<jthrowable>(ex));
}
jobject CreateOpModeOption(JNIEnv* env, const HAL_OpModeOption& option) {
static jmethodID constructor = env->GetMethodID(
opModeOptionCls, "<init>",
"(JLjava/lang/String;L/java/lang/String;Ljava/lang/String;II)V");
JLocal<jstring> name{
env, MakeJString(env, wpi::util::to_string_view(&option.name))};
JLocal<jstring> group{
env, MakeJString(env, wpi::util::to_string_view(&option.group))};
JLocal<jstring> desc{
env, MakeJString(env, wpi::util::to_string_view(&option.description))};
return env->NewObject(opModeOptionCls, constructor,
static_cast<jlong>(option.id), name.obj(), group.obj(),
desc.obj(), static_cast<jint>(option.textColor),
static_cast<jint>(option.backgroundColor));
}
jobjectArray CreateOpModeOptionArray(
JNIEnv* env, std::span<const HAL_OpModeOption> options) {
jobjectArray arr =
env->NewObjectArray(options.size(), opModeOptionCls, nullptr);
if (!arr) {
return nullptr;
}
size_t i = 0;
for (auto& option : options) {
JLocal<jobject> elem{env, CreateOpModeOption(env, option)};
env->SetObjectArrayElement(arr, i++, elem);
}
return arr;
}
HAL_OpModeOption CreateOpModeOptionFromJava(JNIEnv* env, jobject option) {
static jfieldID idField = env->GetFieldID(opModeOptionCls, "id", "J");
static jfieldID nameField =
env->GetFieldID(opModeOptionCls, "name", "Ljava/lang/String;");
static jfieldID groupField =
env->GetFieldID(opModeOptionCls, "group", "Ljava/lang/String;");
static jfieldID descriptionField =
env->GetFieldID(opModeOptionCls, "description", "Ljava/lang/String;");
static jfieldID textColorField =
env->GetFieldID(opModeOptionCls, "textColor", "I");
static jfieldID backgroundColorField =
env->GetFieldID(opModeOptionCls, "backgroundColor", "I");
if (!idField || !nameField || !groupField || !descriptionField ||
!textColorField || !backgroundColorField) {
ThrowIllegalArgumentException(env, "Missing field in OpModeOption");
return {0, {}, {}, {}, 0, 0};
}
int64_t id = env->GetLongField(option, idField);
JLocal<jstring> name{
env, static_cast<jstring>(env->GetObjectField(option, nameField))};
JLocal<jstring> group{
env, static_cast<jstring>(env->GetObjectField(option, groupField))};
JLocal<jstring> description{
env, static_cast<jstring>(env->GetObjectField(option, descriptionField))};
int32_t textColor = env->GetIntField(option, textColorField);
int32_t backgroundColor = env->GetIntField(option, backgroundColorField);
return {id,
wpi::util::alloc_wpi_string(JStringRef{env, name}),
wpi::util::alloc_wpi_string(JStringRef{env, group}),
wpi::util::alloc_wpi_string(JStringRef{env, description}),
textColor,
backgroundColor};
}
jobject CreateREVPHVersion(JNIEnv* env, uint32_t firmwareMajor,
uint32_t firmwareMinor, uint32_t firmwareFix,
uint32_t hardwareMinor, uint32_t hardwareMajor,

7
hal/src/main/native/cpp/jni/HALUtil.h
View File

@@ -7,9 +7,11 @@
#include <jni.h>
#include <stdint.h>
#include <span>
#include <string_view>
struct HAL_MatchInfo;
struct HAL_OpModeOption;
struct HAL_Value;
namespace wpi::hal {
@@ -49,6 +51,11 @@ void ThrowIndexOutOfBoundsException(JNIEnv* env, std::string_view msg);
void ThrowBoundaryException(JNIEnv* env, double value, double lower,
double upper);
jobject CreateOpModeOption(JNIEnv* env, const HAL_OpModeOption& option);
jobjectArray CreateOpModeOptionArray(JNIEnv* env,
std::span<const HAL_OpModeOption> options);
HAL_OpModeOption CreateOpModeOptionFromJava(JNIEnv* env, jobject option);
jobject CreateREVPHVersion(JNIEnv* env, uint32_t firmwareMajor,
uint32_t firmwareMinor, uint32_t firmwareFix,
uint32_t hardwareMinor, uint32_t hardwareMajor,

178
hal/src/main/native/cpp/jni/simulation/DriverStationDataJNI.cpp
View File

@@ -4,7 +4,10 @@
#include <jni.h>
#include "../HALUtil.h"
#include "CallbackStore.h"
#include "OpModeOptionsCallbackStore.h"
#include "SimulatorJNI.h"
#include "org_wpilib_hardware_hal_simulation_DriverStationDataJNI.h"
#include "wpi/hal/simulation/DriverStationData.h"
#include "wpi/hal/simulation/MockHooks.h"
@@ -69,103 +72,53 @@ Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_setEnabled
/*
* Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI
* Method: registerAutonomousCallback
* Method: registerRobotModeCallback
* Signature: (Ljava/lang/Object;Z)I
*/
JNIEXPORT jint JNICALL
Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_registerAutonomousCallback
Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_registerRobotModeCallback
(JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
{
return sim::AllocateCallbackNoIndex(
env, callback, initialNotify,
&HALSIM_RegisterDriverStationAutonomousCallback);
&HALSIM_RegisterDriverStationRobotModeCallback);
}
/*
* Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI
* Method: cancelAutonomousCallback
* Method: cancelRobotModeCallback
* Signature: (I)V
*/
JNIEXPORT void JNICALL
Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_cancelAutonomousCallback
(JNIEnv* env, jclass, jint handle)
{
return sim::FreeCallbackNoIndex(
env, handle, &HALSIM_CancelDriverStationAutonomousCallback);
}
/*
* Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI
* Method: getAutonomous
* Signature: ()Z
*/
JNIEXPORT jboolean JNICALL
Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_getAutonomous
(JNIEnv*, jclass)
{
return HALSIM_GetDriverStationAutonomous();
}
/*
* Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI
* Method: setAutonomous
* Signature: (Z)V
*/
JNIEXPORT void JNICALL
Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_setAutonomous
(JNIEnv*, jclass, jboolean value)
{
HALSIM_SetDriverStationAutonomous(value);
}
/*
* Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI
* Method: registerTestCallback
* Signature: (Ljava/lang/Object;Z)I
*/
JNIEXPORT jint JNICALL
Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_registerTestCallback
(JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
{
return sim::AllocateCallbackNoIndex(
env, callback, initialNotify, &HALSIM_RegisterDriverStationTestCallback);
}
/*
* Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI
* Method: cancelTestCallback
* Signature: (I)V
*/
JNIEXPORT void JNICALL
Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_cancelTestCallback
Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_cancelRobotModeCallback
(JNIEnv* env, jclass, jint handle)
{
return sim::FreeCallbackNoIndex(env, handle,
&HALSIM_CancelDriverStationTestCallback);
&HALSIM_CancelDriverStationRobotModeCallback);
}
/*
* Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI
* Method: getTest
* Signature: ()Z
* Method: nativeGetRobotMode
* Signature: ()I
*/
JNIEXPORT jboolean JNICALL
Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_getTest
JNIEXPORT jint JNICALL
Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_nativeGetRobotMode
(JNIEnv*, jclass)
{
return HALSIM_GetDriverStationTest();
return HALSIM_GetDriverStationRobotMode();
}
/*
* Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI
* Method: setTest
* Signature: (Z)V
* Method: nativeSetRobotMode
* Signature: (I)V
*/
JNIEXPORT void JNICALL
Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_setTest
(JNIEnv*, jclass, jboolean value)
Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_nativeSetRobotMode
(JNIEnv*, jclass, jint value)
{
HALSIM_SetDriverStationTest(value);
HALSIM_SetDriverStationRobotMode(static_cast<HAL_RobotMode>(value));
}
/*
@@ -423,6 +376,99 @@ Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_setMatchTime
HALSIM_SetDriverStationMatchTime(value);
}
/*
* Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI
* Method: registerOpModeCallback
* Signature: (Ljava/lang/Object;Z)I
*/
JNIEXPORT jint JNICALL
Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_registerOpModeCallback
(JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
{
return sim::AllocateCallbackNoIndex(
env, callback, initialNotify,
&HALSIM_RegisterDriverStationOpModeCallback);
}
/*
* Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI
* Method: cancelOpModeCallback
* Signature: (I)V
*/
JNIEXPORT void JNICALL
Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_cancelOpModeCallback
(JNIEnv* env, jclass, jint handle)
{
return sim::FreeCallbackNoIndex(env, handle,
&HALSIM_CancelDriverStationOpModeCallback);
}
/*
* Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI
* Method: getOpMode
* Signature: ()J
*/
JNIEXPORT jlong JNICALL
Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_getOpMode
(JNIEnv* env, jclass)
{
return HALSIM_GetDriverStationOpMode();
}
/*
* Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI
* Method: setOpMode
* Signature: (J)V
*/
JNIEXPORT void JNICALL
Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_setOpMode
(JNIEnv* env, jclass, jlong value)
{
HALSIM_SetDriverStationOpMode(value);
}
/*
* Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI
* Method: registerOpModeOptionsCallback
* Signature: (Ljava/lang/Object;Z)I
*/
JNIEXPORT jint JNICALL
Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_registerOpModeOptionsCallback
(JNIEnv* env, jclass, jobject callback, jboolean initialNotify)
{
return sim::AllocateOpModeOptionsCallback(
env, callback, initialNotify, &HALSIM_RegisterOpModeOptionsCallback);
}
/*
* Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI
* Method: cancelOpModeOptionsCallback
* Signature: (I)V
*/
JNIEXPORT void JNICALL
Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_cancelOpModeOptionsCallback
(JNIEnv* env, jclass, jint handle)
{
sim::FreeOpModeOptionsCallback(env, handle,
&HALSIM_CancelOpModeOptionsCallback);
}
/*
* Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI
* Method: getOpModeOptions
* Signature: ()[Ljava/lang/Object;
*/
JNIEXPORT jobjectArray JNICALL
Java_org_wpilib_hardware_hal_simulation_DriverStationDataJNI_getOpModeOptions
(JNIEnv* env, jclass)
{
int32_t count;
HAL_OpModeOption* options = HALSIM_GetOpModeOptions(&count);
auto rv = CreateOpModeOptionArray(env, {options, options + count});
HALSIM_FreeOpModeOptionsArray(options, count);
return rv;
}
/*
* Class: org_wpilib_hardware_hal_simulation_DriverStationDataJNI
* Method: setJoystickAxes

121
hal/src/main/native/cpp/jni/simulation/OpModeOptionsCallbackStore.cpp Normal file
View File

@@ -0,0 +1,121 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "OpModeOptionsCallbackStore.h"
#include <jni.h>
#include <cstdio>
#include <memory>
#include "../HALUtil.h"
#include "SimulatorJNI.h"
#include "wpi/hal/Types.h"
#include "wpi/hal/handles/UnlimitedHandleResource.h"
#include "wpi/util/jni_util.hpp"
using namespace wpi::hal;
using namespace wpi::hal::sim;
using namespace wpi::util::java;
static UnlimitedHandleResource<SIM_JniHandle, OpModeOptionsCallbackStore,
HAL_HandleEnum::SimulationJni>* callbackHandles;
namespace wpi::hal::sim {
void InitializeOpModeOptionsStore() {
static UnlimitedHandleResource<SIM_JniHandle, OpModeOptionsCallbackStore,
HAL_HandleEnum::SimulationJni>
cb;
callbackHandles = &cb;
}
} // namespace wpi::hal::sim
void OpModeOptionsCallbackStore::create(JNIEnv* env, jobject obj) {
m_call = JGlobal<jobject>(env, obj);
}
void OpModeOptionsCallbackStore::performCallback(
const char* name, const HAL_OpModeOption* opmodes, int32_t count) {
JNIEnv* env;
JavaVM* vm = sim::GetJVM();
bool didAttachThread = false;
int tryGetEnv = vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6);
if (tryGetEnv == JNI_EDETACHED) {
// Thread not attached
didAttachThread = true;
if (vm->AttachCurrentThread(reinterpret_cast<void**>(&env), nullptr) != 0) {
// Failed to attach, log and return
std::puts("Failed to attach");
std::fflush(stdout);
return;
}
} else if (tryGetEnv == JNI_EVERSION) {
std::puts("Invalid JVM Version requested");
std::fflush(stdout);
}
JLocal<jobjectArray> toCallbackArr{
env, CreateOpModeOptionArray(env, {opmodes, opmodes + count})};
env->CallVoidMethod(m_call, sim::GetBiConsumerCallback(),
MakeJString(env, name), toCallbackArr.obj());
if (env->ExceptionCheck()) {
env->ExceptionDescribe();
}
if (didAttachThread) {
vm->DetachCurrentThread();
}
}
void OpModeOptionsCallbackStore::free(JNIEnv* env) {
m_call.free(env);
}
SIM_JniHandle sim::AllocateOpModeOptionsCallback(
JNIEnv* env, jobject callback, jboolean initialNotify,
RegisterOpModeOptionsCallbackFunc createCallback) {
auto callbackStore = std::make_shared<OpModeOptionsCallbackStore>();
auto handle = callbackHandles->Allocate(callbackStore);
if (handle == HAL_kInvalidHandle) {
return -1;
}
uintptr_t handleAsPtr = static_cast<uintptr_t>(handle);
void* handleAsVoidPtr = reinterpret_cast<void*>(handleAsPtr);
callbackStore->create(env, callback);
auto callbackFunc = [](const char* name, void* param,
const HAL_OpModeOption* opmodes, int32_t count) {
uintptr_t handleTmp = reinterpret_cast<uintptr_t>(param);
SIM_JniHandle handle = static_cast<SIM_JniHandle>(handleTmp);
auto data = callbackHandles->Get(handle);
if (!data) {
return;
}
data->performCallback(name, opmodes, count);
};
auto id = createCallback(callbackFunc, handleAsVoidPtr, initialNotify);
callbackStore->setCallbackId(id);
return handle;
}
void sim::FreeOpModeOptionsCallback(
JNIEnv* env, SIM_JniHandle handle,
FreeOpModeOptionsCallbackFunc freeCallback) {
auto callback = callbackHandles->Free(handle);
if (callback == nullptr) {
return;
}
freeCallback(callback->getCallbackId());
callback->free(env);
}

40
hal/src/main/native/cpp/jni/simulation/OpModeOptionsCallbackStore.h Normal file
View File

@@ -0,0 +1,40 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <jni.h>
#include "SimulatorJNI.h"
#include "wpi/hal/Types.h"
#include "wpi/hal/simulation/DriverStationData.h"
#include "wpi/util/jni_util.hpp"
namespace wpi::hal::sim {
class OpModeOptionsCallbackStore {
public:
void create(JNIEnv* env, jobject obj);
void performCallback(const char* name, const HAL_OpModeOption* opmodes,
int32_t count);
void free(JNIEnv* env);
void setCallbackId(int32_t id) { callbackId = id; }
int32_t getCallbackId() { return callbackId; }
private:
wpi::util::java::JGlobal<jobject> m_call;
int32_t callbackId;
};
void InitializeOpModeOptionsStore();
using RegisterOpModeOptionsCallbackFunc = int32_t (*)(
HAL_OpModeOptionsCallback callback, void* param, HAL_Bool initialNotify);
using FreeOpModeOptionsCallbackFunc = void (*)(int32_t uid);
SIM_JniHandle AllocateOpModeOptionsCallback(
JNIEnv* env, jobject callback, jboolean initialNotify,
RegisterOpModeOptionsCallbackFunc createCallback);
void FreeOpModeOptionsCallback(JNIEnv* env, SIM_JniHandle handle,
FreeOpModeOptionsCallbackFunc freeCallback);
} // namespace wpi::hal::sim

100
hal/src/main/native/cpp/jni/simulation/SimulatorJNI.cpp
View File

@@ -7,6 +7,7 @@
#include "BufferCallbackStore.h"
#include "CallbackStore.h"
#include "ConstBufferCallbackStore.h"
#include "OpModeOptionsCallbackStore.h"
#include "SimDeviceDataJNI.h"
#include "org_wpilib_hardware_hal_simulation_SimulatorJNI.h"
#include "wpi/hal/HAL.h"
@@ -20,9 +21,35 @@ static JavaVM* jvm = nullptr;
static JClass notifyCallbackCls;
static JClass bufferCallbackCls;
static JClass constBufferCallbackCls;
static JClass biConsumerCls;
static jmethodID notifyCallbackCallback;
static jmethodID bufferCallbackCallback;
static jmethodID constBufferCallbackCallback;
static jmethodID biConsumerCallback;
static const JClassInit classes[] = {
{"org/wpilib/hardware/hal/simulation/NotifyCallback", &notifyCallbackCls},
{"org/wpilib/hardware/hal/simulation/BufferCallback", &bufferCallbackCls},
{"org/wpilib/hardware/hal/simulation/ConstBufferCallback",
&constBufferCallbackCls},
{"java/util/function/BiConsumer", &biConsumerCls},
};
static const struct JMethodInit {
JClass* cls;
const char* name;
const char* sig;
jmethodID* method;
} methods[] = {
{&notifyCallbackCls, "callbackNative", "(Ljava/lang/String;IJD)V",
&notifyCallbackCallback},
{&bufferCallbackCls, "callback", "(Ljava/lang/String;[BI)V",
&bufferCallbackCallback},
{&constBufferCallbackCls, "callback", "(Ljava/lang/String;[BI)V",
&constBufferCallbackCallback},
{&biConsumerCls, "accept", "(Ljava/lang/Object;Ljava/lang/Object;)V",
&biConsumerCallback},
};
namespace wpi::hal::sim {
jint SimOnLoad(JavaVM* vm, void* reserved) {
@@ -33,45 +60,24 @@ jint SimOnLoad(JavaVM* vm, void* reserved) {
return JNI_ERR;
}
notifyCallbackCls =
JClass(env, "org/wpilib/hardware/hal/simulation/NotifyCallback");
if (!notifyCallbackCls) {
return JNI_ERR;
for (auto& c : classes) {
*c.cls = JClass(env, c.name);
if (!*c.cls) {
return JNI_ERR;
}
}
notifyCallbackCallback = env->GetMethodID(notifyCallbackCls, "callbackNative",
"(Ljava/lang/String;IJD)V");
if (!notifyCallbackCallback) {
return JNI_ERR;
}
bufferCallbackCls =
JClass(env, "org/wpilib/hardware/hal/simulation/BufferCallback");
if (!bufferCallbackCls) {
return JNI_ERR;
}
bufferCallbackCallback = env->GetMethodID(bufferCallbackCls, "callback",
"(Ljava/lang/String;[BI)V");
if (!bufferCallbackCallback) {
return JNI_ERR;
}
constBufferCallbackCls =
JClass(env, "org/wpilib/hardware/hal/simulation/ConstBufferCallback");
if (!constBufferCallbackCls) {
return JNI_ERR;
}
constBufferCallbackCallback = env->GetMethodID(
constBufferCallbackCls, "callback", "(Ljava/lang/String;[BI)V");
if (!constBufferCallbackCallback) {
return JNI_ERR;
for (auto& m : methods) {
*m.method = env->GetMethodID(*m.cls, m.name, m.sig);
if (!*m.method) {
return JNI_ERR;
}
}
InitializeStore();
InitializeBufferStore();
InitializeConstBufferStore();
InitializeOpModeOptionsStore();
if (!InitializeSimDeviceDataJNI(env)) {
return JNI_ERR;
}
@@ -88,6 +94,7 @@ void SimOnUnload(JavaVM* vm, void* reserved) {
notifyCallbackCls.free(env);
bufferCallbackCls.free(env);
constBufferCallbackCls.free(env);
biConsumerCls.free(env);
FreeSimDeviceDataJNI(env);
jvm = nullptr;
}
@@ -108,6 +115,9 @@ jmethodID GetConstBufferCallback() {
return constBufferCallbackCallback;
}
jmethodID GetBiConsumerCallback() {
return biConsumerCallback;
}
} // namespace wpi::hal::sim
extern "C" {
@@ -159,6 +169,32 @@ Java_org_wpilib_hardware_hal_simulation_SimulatorJNI_getProgramStarted
return HALSIM_GetProgramStarted();
}
/*
* Class: org_wpilib_hardware_hal_simulation_SimulatorJNI
* Method: setProgramState
* Signature: (J)V
*/
JNIEXPORT void JNICALL
Java_org_wpilib_hardware_hal_simulation_SimulatorJNI_setProgramState
(JNIEnv*, jclass, jlong word)
{
HALSIM_SetProgramState({word});
}
/*
* Class: org_wpilib_hardware_hal_simulation_SimulatorJNI
* Method: nativeGetProgramState
* Signature: ()J
*/
JNIEXPORT jlong JNICALL
Java_org_wpilib_hardware_hal_simulation_SimulatorJNI_nativeGetProgramState
(JNIEnv*, jclass)
{
HAL_ControlWord word;
HALSIM_GetProgramState(&word);
return word.value;
}
/*
* Class: org_wpilib_hardware_hal_simulation_SimulatorJNI
* Method: restartTiming

1
hal/src/main/native/cpp/jni/simulation/SimulatorJNI.h
View File

@@ -15,4 +15,5 @@ JavaVM* GetJVM();
jmethodID GetNotifyCallback();
jmethodID GetBufferCallback();
jmethodID GetConstBufferCallback();
jmethodID GetBiConsumerCallback();
} // namespace wpi::hal::sim

16
hal/src/main/native/cpp/proto/ControlDataProto.cpp
View File

@@ -11,8 +11,7 @@ static_assert(sizeof(mrc::ControlFlags) == sizeof(uint32_t));
namespace {
constexpr uint32_t EnabledMask = 0x1;
constexpr uint32_t AutoMask = 0x2;
constexpr uint32_t TestMask = 0x4;
constexpr uint32_t RobotModeMask = 0x6;
constexpr uint32_t EStopMask = 0x8;
constexpr uint32_t FmsConnectedMask = 0x10;
constexpr uint32_t DsConnectedMask = 0x20;
@@ -20,8 +19,7 @@ constexpr uint32_t WatchdogActiveMask = 0x40;
constexpr uint32_t AllianceMask = 0x1F80;
constexpr uint32_t EnabledShift = 0;
constexpr uint32_t AutoShift = 1;
constexpr uint32_t TestShift = 2;
constexpr uint32_t RobotModeShift = 1;
constexpr uint32_t EStopShift = 3;
constexpr uint32_t FmsConnectedShift = 4;
constexpr uint32_t DsConnectedShift = 5;
@@ -33,8 +31,7 @@ constexpr uint32_t AllianceShift = 7;
constexpr uint32_t FromControlWord(mrc::ControlFlags Word) {
uint32_t Ret = 0;
WORD_TO_INT(Enabled);
WORD_TO_INT(Auto);
WORD_TO_INT(Test);
WORD_TO_INT(RobotMode);
WORD_TO_INT(EStop);
WORD_TO_INT(FmsConnected);
WORD_TO_INT(DsConnected);
@@ -50,8 +47,7 @@ constexpr uint32_t FromControlWord(mrc::ControlFlags Word) {
constexpr mrc::ControlFlags ToControlWord(uint32_t Word) {
mrc::ControlFlags Ret = {};
INT_TO_WORD(Enabled);
INT_TO_WORD(Auto);
INT_TO_WORD(Test);
INT_TO_WORD(RobotMode);
INT_TO_WORD(EStop);
INT_TO_WORD(FmsConnected);
INT_TO_WORD(DsConnected);
@@ -67,10 +63,10 @@ std::optional<mrc::ControlData> wpi::util::Protobuf<mrc::ControlData>::Unpack(
wpi::util::UnpackCallback<mrc::Joystick, MRC_MAX_NUM_JOYSTICKS> JoystickCb;
mrc_proto_ProtobufControlData Msg{
.ControlWord = 0,
.MatchTime = 0,
.Joysticks = JoystickCb.Callback(),
.CurrentOpMode = 0,
.ControlWord = 0,
};
if (!Stream.Decode(Msg)) {
@@ -99,10 +95,10 @@ bool wpi::util::Protobuf<mrc::ControlData>::Pack(
wpi::util::PackCallback Joysticks{Sticks};
mrc_proto_ProtobufControlData Msg{
.ControlWord = FromControlWord(Value.ControlWord),
.MatchTime = Value.MatchTime,
.Joysticks = Joysticks.Callback(),
.CurrentOpMode = Value.CurrentOpMode.ToValue(),
.ControlWord = FromControlWord(Value.ControlWord),
};
return Stream.Encode(Msg);

30
hal/src/main/native/cpp/proto/OpModeProto.cpp
View File

@@ -12,36 +12,52 @@
std::optional<mrc::OpMode> wpi::util::Protobuf<mrc::OpMode>::Unpack(
InputStream& Stream) {
wpi::util::UnpackCallback<std::string> NameCb;
wpi::util::UnpackCallback<std::string> GroupCb;
wpi::util::UnpackCallback<std::string> DescriptionCb;
mrc_proto_ProtobufOpMode Msg;
Msg.Name = NameCb.Callback();
Msg.Group = GroupCb.Callback();
Msg.Description = DescriptionCb.Callback();
if (!Stream.Decode(Msg)) {
return {};
}
auto Name = NameCb.Items();
auto Group = GroupCb.Items();
auto Description = DescriptionCb.Items();
if (Name.empty()) {
if (Name.empty() || Group.empty() || Description.empty()) {
return {};
}
mrc::OpMode OutputData;
OutputData.MoveName(std::move(Name[0]));
OutputData.Hash = mrc::OpModeHash::FromValue(Msg.Hash);
return OutputData;
return mrc::OpMode{
mrc::OpModeHash::FromValue(Msg.Hash),
std::move(Name[0]),
std::move(Group[0]),
std::move(Description[0]),
Msg.TextColor,
Msg.BackgroundColor,
};
}
bool wpi::util::Protobuf<mrc::OpMode>::Pack(OutputStream& Stream,
const mrc::OpMode& Value) {
std::string_view EventNameStr = Value.GetName();
wpi::util::PackCallback EventName{&EventNameStr};
std::string_view EventGroupStr = Value.GetGroup();
wpi::util::PackCallback EventGroup{&EventGroupStr};
std::string_view EventDescriptionStr = Value.GetDescription();
wpi::util::PackCallback EventDescription{&EventDescriptionStr};
mrc_proto_ProtobufOpMode Msg{
.Hash = Value.Hash.ToValue(),
.Name = EventName.Callback(),
.Group = EventGroup.Callback(),
.Description = EventDescription.Callback(),
.TextColor = Value.GetTextColor(),
.BackgroundColor = Value.GetBackgroundColor(),
};
return Stream.Encode(Msg);