Skip to content
This repository was archived by the owner on Jan 15, 2021. It is now read-only.

Commit bcfc3a4

Browse files
pjreinigerpjreiniger
committed
Adding ability for C++ robots to load yaml configs
1 parent b0f3c49 commit bcfc3a4

File tree

23 files changed

+924
-21
lines changed

23 files changed

+924
-21
lines changed

‎.gitmodules

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
[submodule "3rdParty/yaml-cpp"]
2+
path = 3rdParty/yaml-cpp
3+
url = https://github.com/jbeder/yaml-cpp.git

‎3rdParty/build.gradle

Lines changed: 26 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,26 @@
1+
apply plugin: "cpp"
2+
apply plugin: 'visual-studio'
3+
apply plugin: 'edu.wpi.first.NativeUtils'
4+
5+
apply from: "${rootDir}/common/config.gradle"
6+
7+
8+
model {
9+
components {
10+
yamlcpp(NativeLibrarySpec) {
11+
sources {
12+
cpp {
13+
source {
14+
srcDirs = ['yaml-cpp/src']
15+
includes = ['**/*.cpp']
16+
}
17+
exportedHeaders {
18+
srcDirs = ["yaml-cpp/include"]
19+
}
20+
}
21+
}
22+
}
23+
24+
}
25+
26+
}

‎3rdParty/yaml-cpp

Submodule yaml-cpp added at 562aefc

‎appveyor.yml

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,16 @@ platform:
55
- x86
66
- x64
77

8+
clone_script:
9+
- cmd: >-
10+
git clone -q --branch=%APPVEYOR_REPO_BRANCH% https://github.com/%APPVEYOR_REPO_NAME%.git %APPVEYOR_BUILD_FOLDER%
11+
12+
cd %APPVEYOR_BUILD_FOLDER%
13+
14+
git checkout -qf %APPVEYOR_REPO_COMMIT%
15+
16+
git submodule update --init --recursive
17+
818
build_script:
919
- ps: >-
1020
mkdir build

‎common/gtest.gradle

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,13 @@
1+
model {
2+
dependencyConfigs {
3+
googletest(DependencyConfig) {
4+
groupId = 'edu.wpi.first.thirdparty.frc2019'
5+
artifactId = 'googletest'
6+
headerClassifier = 'headers'
7+
ext = 'zip'
8+
version = '1.8.0-4-4e4df22'
9+
sharedConfigs = [:]
10+
staticConfigs = project.staticGtestConfigs
11+
}
12+
}
13+
}

‎settings.gradle

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@ include 'snobot_sim_gui'
1919
include 'snobot_sim_joysticks'
2020
include 'sim_extension_navx'
2121
include 'snobot_sim_example_robot'
22+
include '3rdParty'
2223

2324

2425
rootProject.name = 'CppSimulator'

‎snobot_sim/src/main/native/cpp/SnobotSim/Config/V1/SimulatorConfigReaderV1.cpp

Lines changed: 292 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,292 @@
1+
2+
#include "SnobotSim/Config/SimulatorConfigReaderV1.h"
3+
4+
#include <iostream>
5+
6+
#include <filesystem>
7+
8+
#include "SnobotSim/Config/SimulatorConfigV1.h"
9+
#include "SnobotSim/GetSensorActuatorHelper.h"
10+
#include "SnobotSim/Logging/SnobotLogger.h"
11+
#include "SnobotSim/ModuleWrapper/Factories/FactoryContainer.h"
12+
#include "SnobotSim/MotorSim/GravityLoadDcMotorSim.h"
13+
#include "SnobotSim/MotorSim/RotationalLoadDcMotorSim.h"
14+
#include "SnobotSim/MotorSim/SimpleMotorSimulator.h"
15+
#include "SnobotSim/MotorSim/StaticLoadDcMotorSim.h"
16+
#include "SnobotSim/SensorActuatorRegistry.h"
17+
#include "yaml-cpp/yaml.h"
18+
19+
void ParseMap(const YAML::Node& aNode, std::map<int, std::string>& aMap)
20+
{
21+
for (YAML::const_iterator it = aNode.begin(); it != aNode.end(); ++it)
22+
{
23+
aMap[it->first.as<int>()] = it->second.as<std::string>();
24+
}
25+
}
26+
27+
template <typename T>
28+
void ParseVector(const YAML::Node& aNode, std::vector<T>& aVector)
29+
{
30+
for (YAML::const_iterator it = aNode.begin(); it != aNode.end(); ++it)
31+
{
32+
T value;
33+
(*it) >> value;
34+
aVector.push_back(value);
35+
}
36+
}
37+
38+
void LoadBasicConfig(const YAML::Node& aNode, BasicModuleConfig& aOutput)
39+
{
40+
aOutput.mHandle = aNode["mHandle"].as<int>();
41+
aOutput.mName = aNode["mName"].as<std::string>();
42+
aOutput.mType = aNode["mType"].as<std::string>();
43+
}
44+
45+
const YAML::Node& operator>>(const YAML::Node& aNode, EncoderConfig& aOutput)
46+
{
47+
LoadBasicConfig(aNode, aOutput);
48+
aOutput.mConnectedSpeedControllerHandle = aNode["mConnectedSpeedControllerHandle"].as<int>();
49+
return aNode;
50+
}
51+
52+
const YAML::Node& operator>>(const YAML::Node& aNode, PwmConfig& aOutput)
53+
{
54+
LoadBasicConfig(aNode, aOutput);
55+
56+
if (aNode["mMotorSimConfig"])
57+
{
58+
const YAML::Node& motorSimConfig = aNode["mMotorSimConfig"];
59+
const std::string& motorSimConfigTag = motorSimConfig.Tag();
60+
61+
if (motorSimConfigTag == "tag:yaml.org,2002:com.snobot.simulator.motor_sim.SimpleMotorSimulationConfig")
62+
{
63+
aOutput.mMotorSimConfigType = PwmConfig::Simple;
64+
aOutput.mMotorSimConfig.mSimple.mMaxSpeed = motorSimConfig["mMaxSpeed"].as<double>();
65+
}
66+
else if (motorSimConfigTag == "tag:yaml.org,2002:com.snobot.simulator.motor_sim.StaticLoadMotorSimulationConfig")
67+
{
68+
aOutput.mMotorSimConfigType = PwmConfig::Static;
69+
aOutput.mMotorSimConfig.mStatic.mLoad = motorSimConfig["mLoad"].as<double>();
70+
aOutput.mMotorSimConfig.mStatic.mConversionFactor = motorSimConfig["mConversionFactor"].as<double>();
71+
}
72+
else if (motorSimConfigTag == "tag:yaml.org,2002:com.snobot.simulator.motor_sim.GravityLoadMotorSimulationConfig")
73+
{
74+
aOutput.mMotorSimConfigType = PwmConfig::Gravity;
75+
aOutput.mMotorSimConfig.mGravity.mLoad = motorSimConfig["mLoad"].as<double>();
76+
}
77+
else if (motorSimConfigTag == "tag:yaml.org,2002:com.snobot.simulator.motor_sim.RotationalLoadMotorSimulationConfig")
78+
{
79+
aOutput.mMotorSimConfigType = PwmConfig::Rotational;
80+
aOutput.mMotorSimConfig.mRotational.mArmCenterOfMass = motorSimConfig["mArmCenterOfMass"].as<double>();
81+
aOutput.mMotorSimConfig.mRotational.mArmMass = motorSimConfig["mArmMass"].as<double>();
82+
aOutput.mMotorSimConfig.mRotational.mConstantAssistTorque = motorSimConfig["mConstantAssistTorque"].as<double>();
83+
aOutput.mMotorSimConfig.mRotational.mOverCenterAssistTorque = motorSimConfig["mOverCenterAssistTorque"].as<double>();
84+
}
85+
}
86+
else
87+
{
88+
SNOBOT_LOG(SnobotLogging::LOG_LEVEL_CRITICAL, "Thing is null");
89+
}
90+
91+
return aNode;
92+
}
93+
94+
const YAML::Node& operator>>(const YAML::Node& aNode, BasicModuleConfig& aOutput)
95+
{
96+
LoadBasicConfig(aNode, aOutput);
97+
return aNode;
98+
}
99+
100+
const YAML::Node& operator>>(const YAML::Node& configNode, SimulatorConfigV1& config)
101+
{
102+
ParseMap(configNode["mDefaultI2CWrappers"], config.mDefaultI2CWrappers);
103+
ParseMap(configNode["mDefaultSpiWrappers"], config.mDefaultSpiWrappers);
104+
ParseVector(configNode["mAccelerometers"], config.mAccelerometers);
105+
ParseVector(configNode["mAnalogIn"], config.mAnalogIn);
106+
ParseVector(configNode["mAnalogOut"], config.mAnalogOut);
107+
ParseVector(configNode["mDigitalIO"], config.mDigitalIO);
108+
ParseVector(configNode["mGyros"], config.mGyros);
109+
ParseVector(configNode["mRelays"], config.mRelays);
110+
ParseVector(configNode["mSolenoids"], config.mSolenoids);
111+
ParseVector(configNode["mEncoders"], config.mEncoders);
112+
ParseVector(configNode["mPwm"], config.mPwm);
113+
114+
return configNode;
115+
}
116+
117+
SimulatorConfigReaderV1::SimulatorConfigReaderV1()
118+
{
119+
}
120+
121+
SimulatorConfigReaderV1::~SimulatorConfigReaderV1()
122+
{
123+
}
124+
125+
template <typename FactoryType, typename WrapperType>
126+
void CreateBasicComponent(std::shared_ptr<FactoryType> aFactory, const std::map<int, WrapperType>& wrapperMap, const BasicModuleConfig& aConfig)
127+
{
128+
aFactory->Create(aConfig.mHandle, aConfig.mType);
129+
auto findIter = wrapperMap.find(aConfig.mHandle);
130+
if (findIter != wrapperMap.end())
131+
{
132+
findIter->second->SetName(aConfig.mName);
133+
}
134+
else
135+
{
136+
SNOBOT_LOG(SnobotLogging::LOG_LEVEL_CRITICAL, "Could not set name for handle " << aConfig.mHandle);
137+
}
138+
}
139+
140+
template <typename FactoryType, typename WrapperType>
141+
void CreateBasicComponents(std::shared_ptr<FactoryType> aFactory, const std::map<int, WrapperType>& wrapperMap, const std::vector<BasicModuleConfig>& aConfigs)
142+
{
143+
for (auto it : aConfigs)
144+
{
145+
CreateBasicComponent(aFactory, wrapperMap, it);
146+
}
147+
}
148+
149+
void CreatePwmComponents(std::shared_ptr<SpeedControllerFactory> aFactory, const std::map<int, std::shared_ptr<ISpeedControllerWrapper>>& wrapperMap, const std::vector<PwmConfig>& aConfigs)
150+
{
151+
for (auto it : aConfigs)
152+
{
153+
CreateBasicComponent(aFactory, wrapperMap, it);
154+
155+
std::shared_ptr<ISpeedControllerWrapper> speedController = GetSensorActuatorHelper::GetISpeedControllerWrapper(it.mHandle);
156+
if (!speedController)
157+
{
158+
SNOBOT_LOG(SnobotLogging::LOG_LEVEL_CRITICAL, "Invalid Speed Controller " << it.mHandle);
159+
continue;
160+
}
161+
162+
DcMotorModelConfig::FactoryParams factoryParams(
163+
it.mMotorModelConfig.mFactoryParams.mMotorType,
164+
it.mMotorModelConfig.mFactoryParams.mNumMotors,
165+
it.mMotorModelConfig.mFactoryParams.mGearReduction,
166+
it.mMotorModelConfig.mFactoryParams.mGearboxEfficiency);
167+
168+
DcMotorModelConfig motorModelConfig(
169+
factoryParams,
170+
it.mMotorModelConfig.mMotorParams.mNominalVoltage,
171+
it.mMotorModelConfig.mMotorParams.mFreeSpeedRpm,
172+
it.mMotorModelConfig.mMotorParams.mFreeCurrent,
173+
it.mMotorModelConfig.mMotorParams.mStallTorque,
174+
it.mMotorModelConfig.mMotorParams.mStallCurrent,
175+
it.mMotorModelConfig.mMotorParams.mMotorInertia,
176+
it.mMotorModelConfig.mFactoryParams.mHasBrake,
177+
it.mMotorModelConfig.mFactoryParams.mInverted);
178+
179+
DcMotorModel motorModel(motorModelConfig);
180+
181+
switch (it.mMotorSimConfigType)
182+
{
183+
case PwmConfig::Simple:
184+
{
185+
speedController->SetMotorSimulator(std::shared_ptr<IMotorSimulator>(new SimpleMotorSimulator(
186+
it.mMotorSimConfig.mSimple.mMaxSpeed)));
187+
break;
188+
}
189+
case PwmConfig::Static:
190+
{
191+
speedController->SetMotorSimulator(std::shared_ptr<IMotorSimulator>(new StaticLoadDcMotorSim(
192+
motorModel,
193+
it.mMotorSimConfig.mStatic.mLoad,
194+
it.mMotorSimConfig.mStatic.mConversionFactor)));
195+
break;
196+
}
197+
case PwmConfig::Gravity:
198+
{
199+
speedController->SetMotorSimulator(std::shared_ptr<IMotorSimulator>(new GravityLoadDcMotorSim(
200+
motorModel,
201+
it.mMotorSimConfig.mGravity.mLoad)));
202+
break;
203+
}
204+
case PwmConfig::Rotational:
205+
{
206+
speedController->SetMotorSimulator(std::shared_ptr<IMotorSimulator>(new RotationalLoadDcMotorSim(
207+
motorModel,
208+
speedController,
209+
it.mMotorSimConfig.mRotational.mArmCenterOfMass,
210+
it.mMotorSimConfig.mRotational.mArmMass,
211+
it.mMotorSimConfig.mRotational.mConstantAssistTorque,
212+
it.mMotorSimConfig.mRotational.mOverCenterAssistTorque)));
213+
break;
214+
}
215+
case PwmConfig::None:
216+
default:
217+
break;
218+
}
219+
}
220+
}
221+
222+
void CreateEncoderComponents(std::shared_ptr<EncoderFactory> aFactory, const std::map<int, std::shared_ptr<IEncoderWrapper>>& wrapperMap, const std::vector<EncoderConfig>& aConfigs)
223+
{
224+
for (auto it : aConfigs)
225+
{
226+
CreateBasicComponent(aFactory, wrapperMap, it);
227+
if (it.mHandle != -1 && it.mConnectedSpeedControllerHandle != -1)
228+
{
229+
std::shared_ptr<IEncoderWrapper> encoder = GetSensorActuatorHelper::GetIEncoderWrapper(it.mHandle);
230+
std::shared_ptr<ISpeedControllerWrapper> speedController = GetSensorActuatorHelper::GetISpeedControllerWrapper(it.mConnectedSpeedControllerHandle);
231+
if (encoder && speedController)
232+
{
233+
encoder->SetSpeedController(speedController);
234+
}
235+
else
236+
{
237+
SNOBOT_LOG(SnobotLogging::LOG_LEVEL_CRITICAL, "Necessary components not set (" << encoder << ", " << speedController << ")");
238+
}
239+
}
240+
}
241+
}
242+
243+
void SetupSimulator(const SimulatorConfigV1& aConfig)
244+
{
245+
for (auto it : aConfig.mDefaultI2CWrappers)
246+
{
247+
FactoryContainer::Get().GetI2CWrapperFactory()->RegisterDefaultWrapperType(it.first, it.second);
248+
FactoryContainer::Get().GetI2CWrapperFactory()->GetI2CWrapper(it.first);
249+
}
250+
for (auto it : aConfig.mDefaultSpiWrappers)
251+
{
252+
FactoryContainer::Get().GetSpiWrapperFactory()->RegisterDefaultWrapperType(it.first, it.second);
253+
FactoryContainer::Get().GetSpiWrapperFactory()->GetSpiWrapper(it.first);
254+
}
255+
256+
CreateBasicComponents(FactoryContainer::Get().GetAccelerometerFactory(), SensorActuatorRegistry::Get().GetIAccelerometerWrapperMap(), aConfig.mAccelerometers);
257+
CreateBasicComponents(FactoryContainer::Get().GetAnalogInFactory(), SensorActuatorRegistry::Get().GetIAnalogInWrapperMap(), aConfig.mAnalogIn);
258+
CreateBasicComponents(FactoryContainer::Get().GetAnalogOutFactory(), SensorActuatorRegistry::Get().GetIAnalogOutWrapperMap(), aConfig.mAnalogOut);
259+
CreateBasicComponents(FactoryContainer::Get().GetDigitalIoFactory(), SensorActuatorRegistry::Get().GetIDigitalIoWrapperMap(), aConfig.mDigitalIO);
260+
CreateBasicComponents(FactoryContainer::Get().GetGyroFactory(), SensorActuatorRegistry::Get().GetIGyroWrapperMap(), aConfig.mGyros);
261+
CreateBasicComponents(FactoryContainer::Get().GetRelayFactory(), SensorActuatorRegistry::Get().GetIRelayWrapperMap(), aConfig.mRelays);
262+
CreateBasicComponents(FactoryContainer::Get().GetSolenoidFactory(), SensorActuatorRegistry::Get().GetISolenoidWrapperMap(), aConfig.mSolenoids);
263+
264+
CreatePwmComponents(FactoryContainer::Get().GetSpeedControllerFactory(), SensorActuatorRegistry::Get().GetISpeedControllerWrapperMap(), aConfig.mPwm);
265+
CreateEncoderComponents(FactoryContainer::Get().GetEncoderFactory(), SensorActuatorRegistry::Get().GetIEncoderWrapperMap(), aConfig.mEncoders);
266+
}
267+
268+
bool SimulatorConfigReaderV1::LoadConfig(const std::string& aConfigFile)
269+
{
270+
namespace fs = std::experimental::filesystem;
271+
fs::path configFile = aConfigFile;
272+
273+
bool success = true;
274+
SNOBOT_LOG(SnobotLogging::LOG_LEVEL_INFO, "Loading config file '" << fs::canonical(configFile) << "'");
275+
276+
try
277+
{
278+
YAML::Node configNode = YAML::LoadFile(aConfigFile);
279+
280+
SimulatorConfigV1 config;
281+
configNode >> config;
282+
283+
SetupSimulator(config);
284+
}
285+
catch (std::exception& ex)
286+
{
287+
SNOBOT_LOG(SnobotLogging::LOG_LEVEL_CRITICAL, "Could not parse config file... " << ex.what());
288+
success = false;
289+
}
290+
291+
return success;
292+
}

0 commit comments

Comments
 (0)