From c2a37cecd5dc2158ccdc87f2a514d8b1111626c0 Mon Sep 17 00:00:00 2001
From: Christian Dreher <c.dreher@kit.edu>
Date: Tue, 30 Jul 2024 18:51:57 +0200
Subject: [PATCH] feat: Add custom battery sensor value
---
.../components/units/RobotUnit/CMakeLists.txt | 1 +
.../RobotUnitModules/RobotUnitModuleUnits.cpp | 11 ++--
.../SensorValues/SensorValueBattery.h | 54 +++++++++++++++++++
.../RobotUnit/Units/DiagnosticsSubUnit.cpp | 30 +++++------
.../RobotUnit/Units/DiagnosticsSubUnit.h | 4 +-
source/RobotAPI/libraries/core/RobotPool.cpp | 4 +-
6 files changed, 78 insertions(+), 26 deletions(-)
create mode 100644 source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBattery.h
diff --git a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
index c3c396371..1daa8bc0f 100755
--- a/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
+++ b/source/RobotAPI/components/units/RobotUnit/CMakeLists.txt
@@ -104,6 +104,7 @@ set(LIB_HEADERS
SensorValues/SensorValueBase.h
SensorValues/SensorValue1DoFActuator.h
SensorValues/SensorValueIMU.h
+ SensorValues/SensorValueBattery.h
SensorValues/SensorValueForceTorque.h
SensorValues/SensorValueHolonomicPlatform.h
SensorValues/SensorValueRTThreadTimings.h
diff --git a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
index 42935c414..09f522529 100644
--- a/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/RobotUnitModules/RobotUnitModuleUnits.cpp
@@ -31,6 +31,7 @@
#include <RobotAPI/components/units/RobotUnit/NJointControllers/NJointHolonomicPlatformVelocityControllerWithRamp.h>
#include <RobotAPI/components/units/RobotUnit/RobotUnit.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValue1DoFActuator.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBattery.h>
#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueHolonomicPlatform.h>
#include <RobotAPI/components/units/RobotUnit/Units/DiagnosticsSubUnit.h>
#include <RobotAPI/components/units/RobotUnit/Units/ForceTorqueSubUnit.h>
@@ -276,26 +277,20 @@ namespace armarx::RobotUnitModule
for (auto i : getIndices(_module<Devices>().getSensorDevices().keys()))
{
const SensorDevicePtr& sensorDevice = _module<Devices>().getSensorDevices().at(i);
- if (sensorDevice->getSensorValue()->isA<SensorValueIMU>())
+ if (sensorDevice->getSensorValue()->isA<SensorValueBattery>())
{
batteryDevs[sensorDevice->getDeviceName()] = i;
}
}
- // Add it.
const std::string configName = getProperty<std::string>("DiagnosticsUnitName");
const std::string confPre = getConfigDomain() + "." + configName + ".";
Ice::PropertiesPtr properties = getIceProperties()->clone();
- //properties->setProperty(confPre + "MinimumLoggingLevel", getProperty<std::string>("MinimumLoggingLevel").getValue());
- // Fill properties.
- properties->setProperty(confPre + "IMUTopicName",
- getProperty<std::string>("IMUTopicName").getValue());
+ // Create and add unit.
IceInternal::Handle<UnitT> unit =
Component::create<UnitT>(properties, configName, getConfigDomain());
unit->setBatteryManagementDevices(batteryDevs);
-
- // Add.
addUnit(std::move(unit));
}
diff --git a/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBattery.h b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBattery.h
new file mode 100644
index 000000000..7cf7c4318
--- /dev/null
+++ b/source/RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBattery.h
@@ -0,0 +1,54 @@
+#pragma once
+
+#include <string>
+
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBase.h>
+
+namespace armarx
+{
+
+ enum class BatteryState : std::uint8_t
+ {
+ discharging = 1,
+ charging = 2,
+ unavailable = 3
+ };
+
+ class SensorValueBattery : virtual public SensorValueBase
+ {
+ public:
+ DETAIL_SensorValueBase_DEFAULT_METHOD_IMPLEMENTATION
+
+ std::uint8_t state;
+ float designEnergy_J;
+ float fullChargeEnergy_J;
+ float energy_J;
+ float energyFromFullCharge_pct;
+ float temperature_degC;
+ float ratedVoltage_V;
+ float voltage_V;
+ float current_A;
+ float power_W;
+ bool hasError;
+
+ static SensorValueInfo<SensorValueBattery>
+ GetClassMemberInfo()
+ {
+ SensorValueInfo<SensorValueBattery> svi;
+ svi.addMemberVariable(&SensorValueBattery::state, "state");
+ svi.addMemberVariable(&SensorValueBattery::designEnergy_J, "designEnergy_J");
+ svi.addMemberVariable(&SensorValueBattery::fullChargeEnergy_J, "fullChargeEnergy_J");
+ svi.addMemberVariable(&SensorValueBattery::energy_J, "energy_J");
+ svi.addMemberVariable(&SensorValueBattery::energyFromFullCharge_pct,
+ "energyFromFullCharge_pct");
+ svi.addMemberVariable(&SensorValueBattery::temperature_degC, "temperature_degC");
+ svi.addMemberVariable(&SensorValueBattery::ratedVoltage_V, "ratedVoltage_V");
+ svi.addMemberVariable(&SensorValueBattery::voltage_V, "voltage_V");
+ svi.addMemberVariable(&SensorValueBattery::current_A, "current_A");
+ svi.addMemberVariable(&SensorValueBattery::power_W, "power_W");
+ svi.addMemberVariable(&SensorValueBattery::hasError, "hasError");
+ return svi;
+ }
+ };
+
+} // namespace armarx
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/DiagnosticsSubUnit.cpp b/source/RobotAPI/components/units/RobotUnit/Units/DiagnosticsSubUnit.cpp
index 766ebda65..2d7391460 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/DiagnosticsSubUnit.cpp
+++ b/source/RobotAPI/components/units/RobotUnit/Units/DiagnosticsSubUnit.cpp
@@ -3,7 +3,7 @@
namespace armarx
{
- BatteryStatus
+ dto::BatteryStatus
DiagnosticsSubUnit::getBatteryStatus(const Ice::Current& current) const
{
static std::string accumulated = "accumulated";
@@ -12,7 +12,7 @@ namespace armarx
if (_batteryManagementDevices.size() == 0)
{
return {.name = accumulated,
- .state = BatteryState::unavailable,
+ .state = dto::BatteryState::unavailable,
.designEnergy_J = -1,
.fullChargeEnergy_J = -1,
.energy_J = -1,
@@ -26,19 +26,19 @@ namespace armarx
.error = ""};
}
- BatteryStatus bs{.name = accumulated,
- .state = BatteryState::discharging,
- .designEnergy_J = -1,
- .fullChargeEnergy_J = -1,
- .energy_J = -1,
- .energyFromFullCharge_pct = 66.7,
- .temperature_degC = -1,
- .ratedVoltage_V = -1,
- .voltage_V = -1,
- .current_A = -1,
- .power_W = -1,
- .hasError = false,
- .error = ""};
+ dto::BatteryStatus bs{.name = accumulated,
+ .state = dto::BatteryState::discharging,
+ .designEnergy_J = -1,
+ .fullChargeEnergy_J = -1,
+ .energy_J = -1,
+ .energyFromFullCharge_pct = 66.7,
+ .temperature_degC = -1,
+ .ratedVoltage_V = -1,
+ .voltage_V = -1,
+ .current_A = -1,
+ .power_W = -1,
+ .hasError = false,
+ .error = ""};
return bs;
}
diff --git a/source/RobotAPI/components/units/RobotUnit/Units/DiagnosticsSubUnit.h b/source/RobotAPI/components/units/RobotUnit/Units/DiagnosticsSubUnit.h
index 1ab001ceb..dd2ff7834 100644
--- a/source/RobotAPI/components/units/RobotUnit/Units/DiagnosticsSubUnit.h
+++ b/source/RobotAPI/components/units/RobotUnit/Units/DiagnosticsSubUnit.h
@@ -3,7 +3,7 @@
#include <ArmarXCore/core/Component.h>
#include <RobotAPI/components/units/ForceTorqueUnit.h>
-#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueForceTorque.h>
+#include <RobotAPI/components/units/RobotUnit/SensorValues/SensorValueBattery.h>
#include <RobotAPI/components/units/RobotUnit/Units/RobotUnitSubUnit.h>
#include <RobotAPI/interface/units/DiagnosticsUnitInterface.h>
#include <RobotAPI/interface/units/LocalizationUnitInterface.h>
@@ -17,7 +17,7 @@ namespace armarx
virtual public Component
{
public:
- BatteryStatus getBatteryStatus(const Ice::Current& current) const override;
+ dto::BatteryStatus getBatteryStatus(const Ice::Current& current) const override;
std::string getDefaultName() const override;
diff --git a/source/RobotAPI/libraries/core/RobotPool.cpp b/source/RobotAPI/libraries/core/RobotPool.cpp
index 2b9444a4d..0b6d58afe 100644
--- a/source/RobotAPI/libraries/core/RobotPool.cpp
+++ b/source/RobotAPI/libraries/core/RobotPool.cpp
@@ -51,10 +51,12 @@ namespace armarx
return r;
}
}
+
+ ARMARX_INFO << "Cloning robot";
auto newRobot = baseRobot->clone(baseRobot->getName(), VirtualRobot::CollisionCheckerPtr(new VirtualRobot::CollisionChecker()));
newRobot->inflateCollisionModel(inflation);
ARMARX_CHECK_EQUAL(newRobot.use_count(), 1);
- ARMARX_DEBUG << "created new robot clone n with inflation " << inflation;
+ ARMARX_INFO << "created new robot clone n with inflation " << inflation;
robots[inflation].push_back(newRobot);
return newRobot;
}
--
GitLab