mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-07-05 03:21:42 +00:00
8f1f64ffb6
Also update copyright to include "and other WPILib contributors" and clarify license referral language to not be restricted to FIRST teams.
39 lines
1.1 KiB
C++
39 lines
1.1 KiB
C++
// 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 "GazeboDIO.h"
|
|
|
|
#include <string>
|
|
|
|
#include <hal/Value.h>
|
|
#include <hal/simulation/DIOData.h>
|
|
#include <hal/simulation/NotifyListener.h>
|
|
|
|
static void init_callback(const char* name, void* param,
|
|
const struct HAL_Value* value) {
|
|
GazeboDIO* dio = static_cast<GazeboDIO*>(param);
|
|
dio->SetInitialized(value->data.v_boolean);
|
|
if (dio->IsInitialized()) {
|
|
dio->Listen();
|
|
}
|
|
}
|
|
|
|
GazeboDIO::GazeboDIO(int index, HALSimGazebo* halsim) {
|
|
m_index = index;
|
|
m_halsim = halsim;
|
|
m_sub = NULL;
|
|
HALSIM_RegisterDIOInitializedCallback(index, init_callback, this, true);
|
|
}
|
|
|
|
void GazeboDIO::Listen() {
|
|
if (!m_sub)
|
|
m_sub = m_halsim->node.Subscribe<gazebo::msgs::Bool>(
|
|
"~/simulator/dio/" + std::to_string(m_index), &GazeboDIO::Callback,
|
|
this);
|
|
}
|
|
|
|
void GazeboDIO::Callback(const gazebo::msgs::ConstBoolPtr& msg) {
|
|
HALSIM_SetDIOValue(m_index, msg->data());
|
|
}
|