Skip to content
Snippets Groups Projects
Commit a15a7c10 authored by Christoph Pohl's avatar Christoph Pohl
Browse files

Merge branch 'fix/robot-state-memory-memory-leak' into 'master' 

Fix/robot state memory memory leak

See merge request sw/armarx/robot-api!319
parents 12361909 2f1d4f7c
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
Showing
with 128 additions and 109 deletions
source/RobotAPI/components/armem/server/RobotStateMemory/RobotStateMemory.h
+ 9
− 13
View file @ a15a7c10
......@@ -27,26 +27,24 @@
#include <ArmarXCore/core/Component.h>
#include <RobotAPI/interface/core/RobotLocalization.h>
#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
#include <RobotAPI/libraries/armem/server/plugins/ReadWritePluginUser.h>
#include <RobotAPI/libraries/armem_robot_state/server/common/Visu.h>
#include <RobotAPI/libraries/armem_robot_state/server/description/Segment.h>
#include <RobotAPI/libraries/armem_robot_state/server/localization/Segment.h>
#include <RobotAPI/libraries/armem_robot_state/server/common/Visu.h>
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h>
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h>
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h>
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.h>
#include <RobotAPI/interface/core/RobotLocalization.h>
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/Segment.h>
namespace armarx::plugins
{
class DebugObserverComponentPlugin;
class RobotUnitComponentPlugin;
}
} // namespace armarx::plugins
namespace armarx::armem::server::robot_state
{
......@@ -68,7 +66,6 @@ namespace armarx::armem::server::robot_state
virtual public armarx::GlobalRobotPoseProvider
{
public:
RobotStateMemory();
virtual ~RobotStateMemory() override;
......@@ -77,11 +74,12 @@ namespace armarx::armem::server::robot_state
// GlobalRobotPoseProvider interface
armarx::PoseBasePtr getGlobalRobotPose(Ice::Long timestamp, const std::string& robotName, const ::Ice::Current&) override;
armarx::PoseBasePtr getGlobalRobotPose(Ice::Long timestamp,
const std::string& robotName,
const ::Ice::Current&) override;
protected:
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override;
void onInitComponent() override;
......@@ -91,13 +89,11 @@ namespace armarx::armem::server::robot_state
private:
void startRobotUnitStream();
void stopRobotUnitStream();
private:
armarx::plugins::DebugObserverComponentPlugin* debugObserver = nullptr;
......@@ -129,7 +125,7 @@ namespace armarx::armem::server::robot_state
proprioception::RobotStateWriter writer;
// queue
using Queue = boost::lockfree::spsc_queue<proprioception::RobotUnitData, boost::lockfree::capacity<1024>>;
using Queue = armarx::armem::server::robot_state::proprioception::Queue;
Queue dataBuffer;
bool waitForRobotUnit = false;
......@@ -137,4 +133,4 @@ namespace armarx::armem::server::robot_state
RobotUnit robotUnit;
};
} // namespace armarx::armem::server::robot_state
} // namespace armarx::armem::server::robot_state
source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.cpp
+ 49
− 41
View file @ a15a7c10
......@@ -30,49 +30,43 @@
// ArmarX
#include "ArmarXCore/core/time/Metronome.h"
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
#include "RobotAPI/libraries/armem_robot_state/client/common/constants.h"
#include <RobotAPI/libraries/core/FramedPose.h>
#include <RobotAPI/libraries/aron/core/data/variant/All.h>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/armem/core/ice_conversions.h>
#include <RobotAPI/libraries/armem/server/MemoryToIceAdapter.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Transform.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
#include <RobotAPI/libraries/armem_robot_state/server/localization/Segment.h>
#include <RobotAPI/libraries/aron/core/data/variant/All.h>
#include <RobotAPI/libraries/core/FramedPose.h>
namespace armarx::armem::server::robot_state::proprioception
{
void RobotStateWriter::connect(armarx::plugins::DebugObserverComponentPlugin& debugObserver)
void
RobotStateWriter::connect(armarx::plugins::DebugObserverComponentPlugin& debugObserverPlugin)
{
// Thread-local copy of debug observer helper.
this->debugObserver =
DebugObserverHelper(Logging::tag.tagName, debugObserver.getDebugObserver(), true);
DebugObserverHelper(Logging::tag.tagName, debugObserverPlugin.getDebugObserver(), true);
}
static float toDurationMs(
std::chrono::high_resolution_clock::time_point start,
std::chrono::high_resolution_clock::time_point end)
static float
toDurationMs(std::chrono::high_resolution_clock::time_point start,
std::chrono::high_resolution_clock::time_point end)
{
auto duration = std::chrono::duration_cast<std::chrono::microseconds>(end - start);
return duration.count() / 1000.f;
}
void RobotStateWriter::run(
float pollFrequency,
Queue& dataBuffer,
MemoryToIceAdapter& memory,
localization::Segment& localizationSegment)
void
RobotStateWriter::run(float pollFrequency,
Queue& dataBuffer,
MemoryToIceAdapter& memory,
localization::Segment& localizationSegment)
{
Metronome metronome(Frequency::HertzDouble(pollFrequency));
while (task and not task->isStopped())
{
......@@ -82,7 +76,8 @@ namespace armarx::armem::server::robot_state::proprioception
// }
RobotUnitData robotUnitData;
if (dataBuffer.pop(robotUnitData))
if (const auto status = dataBuffer.wait_pull(robotUnitData);
status == boost::queue_op_status::success)
{
auto start = std::chrono::high_resolution_clock::now();
auto endBuildUpdate = start, endProprioception = start, endLocalization = start;
......@@ -99,47 +94,61 @@ namespace armarx::armem::server::robot_state::proprioception
// Localization
for (const armem::robot_state::Transform& transform : update.localization)
{
localizationSegment.doLocked([&localizationSegment, &transform]()
{
localizationSegment.commitTransform(transform);
});
localizationSegment.doLocked(
[&localizationSegment, &transform]()
{ localizationSegment.commitTransform(transform); });
}
endLocalization = std::chrono::high_resolution_clock::now();
if (not result.allSuccess())
{
ARMARX_WARNING << "Could not commit data to memory. Error message: " << result.allErrorMessages();
ARMARX_WARNING << "Could not commit data to memory. Error message: "
<< result.allErrorMessages();
}
if (debugObserver)
{
auto end = std::chrono::high_resolution_clock::now();
debugObserver->setDebugObserverDatafield("RobotStateWriter | t Commit (ms)", toDurationMs(start, end));
debugObserver->setDebugObserverDatafield("RobotStateWriter | t Commit 1. Build Update (ms)", toDurationMs(start, endBuildUpdate));
debugObserver->setDebugObserverDatafield("RobotStateWriter | t Commit 2. Proprioception (ms)", toDurationMs(endBuildUpdate, endProprioception));
debugObserver->setDebugObserverDatafield("RobotStateWriter | t Commit 3. Localization (ms)", toDurationMs(endProprioception, endLocalization));
debugObserver->setDebugObserverDatafield("RobotStateWriter | t Commit (ms)",
toDurationMs(start, end));
debugObserver->setDebugObserverDatafield(
"RobotStateWriter | t Commit 1. Build Update (ms)",
toDurationMs(start, endBuildUpdate));
debugObserver->setDebugObserverDatafield(
"RobotStateWriter | t Commit 2. Proprioception (ms)",
toDurationMs(endBuildUpdate, endProprioception));
debugObserver->setDebugObserverDatafield(
"RobotStateWriter | t Commit 3. Localization (ms)",
toDurationMs(endProprioception, endLocalization));
}
}
else
{
ARMARX_WARNING << "Queue pull was not successful. "
<< static_cast<std::underlying_type<boost::queue_op_status>::type>(
status);
}
if (debugObserver)
{
debugObserver->sendDebugObserverBatch();
}
metronome.waitForNextTick();
}
}
RobotStateWriter::Update RobotStateWriter::buildUpdate(const RobotUnitData& data)
RobotStateWriter::Update
RobotStateWriter::buildUpdate(const RobotUnitData& data)
{
// Send batch to memory
Update update;
{
armem::EntityUpdate& up = update.proprioception.add();
up.entityID = properties.robotUnitProviderID.withEntityName(properties.robotUnitProviderID.providerSegmentName);
up.entityID = properties.robotUnitProviderID.withEntityName(
properties.robotUnitProviderID.providerSegmentName);
up.timeCreated = data.timestamp;
up.instancesData = { data.proprioception };
up.instancesData = {data.proprioception};
}
// Extract odometry data.
......@@ -147,7 +156,8 @@ namespace armarx::armem::server::robot_state::proprioception
if (data.proprioception->hasElement(platformKey))
{
ARMARX_DEBUG << "Found odometry data.";
auto platformData = aron::data::Dict::DynamicCastAndCheck(data.proprioception->getElement(platformKey));
auto platformData =
aron::data::Dict::DynamicCastAndCheck(data.proprioception->getElement(platformKey));
update.localization.emplace_back(getTransform(platformData, data.timestamp));
}
else
......@@ -156,8 +166,7 @@ namespace armarx::armem::server::robot_state::proprioception
<< "(No element '" << platformKey << "' in proprioception data.)"
<< "\nIf you are using a mobile platform this should not have happened."
<< "\nThis error is only logged once."
<< "\nThese keys exist: " << data.proprioception->getAllKeys()
;
<< "\nThese keys exist: " << data.proprioception->getAllKeys();
noOdometryDataLogged = true;
}
......@@ -166,9 +175,8 @@ namespace armarx::armem::server::robot_state::proprioception
armem::robot_state::Transform
RobotStateWriter::getTransform(
const aron::data::DictPtr& platformData,
const Time& timestamp) const
RobotStateWriter::getTransform(const aron::data::DictPtr& platformData,
const Time& timestamp) const
{
prop::arondto::Platform platform;
platform.fromAron(platformData);
......@@ -176,7 +184,7 @@ namespace armarx::armem::server::robot_state::proprioception
const Eigen::Vector3f& relPose = platform.relativePosition;
Eigen::Affine3f odometryPose = Eigen::Affine3f::Identity();
odometryPose.translation() << relPose.x(), relPose.y(), 0; // TODO set height
odometryPose.translation() << relPose.x(), relPose.y(), 0; // TODO set height
odometryPose.linear() = simox::math::rpy_to_mat3f(0.f, 0.f, relPose.z());
armem::robot_state::Transform transform;
......@@ -189,4 +197,4 @@ namespace armarx::armem::server::robot_state::proprioception
return transform;
}
}
} // namespace armarx::armem::server::robot_state::proprioception
source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotStateWriter.h
+ 2
− 3
View file @ a15a7c10
......@@ -23,7 +23,6 @@
#pragma once
#include <boost/lockfree/spsc_queue.hpp>
#include <optional>
#include "ArmarXCore/util/CPPUtility/TripleBuffer.h"
......@@ -56,9 +55,9 @@ namespace armarx::armem::server::robot_state::proprioception
{
public:
void connect(armarx::plugins::DebugObserverComponentPlugin& debugObserver);
void connect(armarx::plugins::DebugObserverComponentPlugin& debugObserverPlugin);
using Queue = boost::lockfree::spsc_queue<RobotUnitData, boost::lockfree::capacity<1024>>;
using Queue = armarx::armem::server::robot_state::proprioception::Queue;
/// Reads data from `dataQueue` and commits to the memory.
......
source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitData.h
+ 5
− 2
View file @ a15a7c10
......@@ -2,8 +2,10 @@
#include <memory>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
#include <boost/thread/concurrent_queues/sync_queue.hpp>
#include <RobotAPI/libraries/armem/core/Time.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
namespace armarx::armem::server::robot_state::proprioception
{
......@@ -13,5 +15,6 @@ namespace armarx::armem::server::robot_state::proprioception
Time timestamp;
aron::data::DictPtr proprioception;
};
}
using Queue = boost::sync_queue<RobotUnitData>;
} // namespace armarx::armem::server::robot_state::proprioception
source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.cpp
+ 24
− 32
View file @ a15a7c10
#include "RobotUnitReader.h"
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/converters/Armar6Converter.h>
#include <RobotAPI/libraries/armem_robot_state/server/proprioception/aron_conversions.h>
#include <RobotAPI/libraries/armem_robot_state/aron/Proprioception.aron.generated.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
#include <RobotAPI/libraries/aron/core/data/variant/primitive/Long.h>
#include <RobotAPI/libraries/aron/core/data/variant/primitive/String.h>
#include <filesystem>
#include <istream>
#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
#include "ArmarXCore/core/time/Frequency.h"
#include "ArmarXCore/core/time/Metronome.h"
#include "ArmarXCore/core/time/forward_declarations.h"
#include "ArmarXCore/util/CPPUtility/TripleBuffer.h"
#include <ArmarXCore/core/time/CycleUtil.h>
#include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
#include <SimoxUtility/algorithm/string/string_tools.h>
#include <istream>
#include <filesystem>
#include <fstream>
#include <RobotAPI/libraries/RobotUnitDataStreamingReceiver/RobotUnitDataStreamingReceiver.h>
#include <RobotAPI/libraries/aron/core/data/variant/container/Dict.h>
namespace armarx::armem::server::robot_state::proprioception
......@@ -30,16 +18,16 @@ namespace armarx::armem::server::robot_state::proprioception
RobotUnitReader::RobotUnitReader() = default;
void RobotUnitReader::connect(
armarx::plugins::RobotUnitComponentPlugin& robotUnitPlugin,
armarx::plugins::DebugObserverComponentPlugin& debugObserverPlugin,
const std::string& robotTypeName)
void
RobotUnitReader::connect(armarx::plugins::RobotUnitComponentPlugin& robotUnitPlugin,
armarx::plugins::DebugObserverComponentPlugin& debugObserverPlugin,
const std::string& robotTypeName)
{
{
converter = converterRegistry.get(robotTypeName);
ARMARX_CHECK_NOT_NULL(converter)
<< "No converter for robot type '" << robotTypeName << "' available. \n"
<< "Known are: " << converterRegistry.getKeys();
<< "No converter for robot type '" << robotTypeName << "' available. \n"
<< "Known are: " << converterRegistry.getKeys();
config.loggingNames.push_back(properties.sensorPrefix);
receiver = robotUnitPlugin.startDataStreaming(config);
......@@ -47,15 +35,14 @@ namespace armarx::armem::server::robot_state::proprioception
}
{
// Thread-local copy of debug observer helper.
debugObserver =
DebugObserverHelper(Logging::tag.tagName, debugObserverPlugin.getDebugObserver(), true);
debugObserver = DebugObserverHelper(
Logging::tag.tagName, debugObserverPlugin.getDebugObserver(), true);
}
}
void RobotUnitReader::run(
float pollFrequency,
Queue& dataBuffer)
void
RobotUnitReader::run(float pollFrequency, Queue& dataBuffer)
{
Metronome metronome(Frequency::HertzDouble(pollFrequency));
......@@ -63,6 +50,7 @@ namespace armarx::armem::server::robot_state::proprioception
{
if (std::optional<RobotUnitData> commit = fetchAndConvertLatestRobotUnitData())
{
// will lock a mutex
dataBuffer.push(std::move(commit.value()));
}
......@@ -76,7 +64,8 @@ namespace armarx::armem::server::robot_state::proprioception
}
std::optional<RobotUnitData> RobotUnitReader::fetchAndConvertLatestRobotUnitData()
std::optional<RobotUnitData>
RobotUnitReader::fetchAndConvertLatestRobotUnitData()
{
ARMARX_CHECK_NOT_NULL(converter);
......@@ -95,18 +84,21 @@ namespace armarx::armem::server::robot_state::proprioception
auto stop = std::chrono::high_resolution_clock::now();
auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
ARMARX_DEBUG << "RobotUnitReader: The total time needed to convert the data to commit: " << duration;
ARMARX_DEBUG << "RobotUnitReader: The total time needed to convert the data to commit: "
<< duration;
if (debugObserver)
{
debugObserver->setDebugObserverDatafield("RobotUnitReader | t Read+Group [ms]", duration.count() / 1000.f);
debugObserver->setDebugObserverDatafield("RobotUnitReader | t Read+Group [ms]",
duration.count() / 1000.f);
}
return result;
}
std::optional<RobotUnitDataStreaming::TimeStep> RobotUnitReader::fetchLatestData()
std::optional<RobotUnitDataStreaming::TimeStep>
RobotUnitReader::fetchLatestData()
{
std::deque<RobotUnitDataStreaming::TimeStep>& data = receiver->getDataBuffer();
if (debugObserver)
......@@ -126,4 +118,4 @@ namespace armarx::armem::server::robot_state::proprioception
}
}
} // namespace armarx::armem::server::robot_state::proprioception
source/RobotAPI/libraries/armem_robot_state/server/proprioception/RobotUnitReader.h
+ 9
− 18
View file @ a15a7c10
#pragma once
#include <queue>
#include <map>
#include <memory>
#include <optional>
#include <queue>
#include <string>
#include <boost/lockfree/spsc_queue.hpp>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/services/tasks/TaskUtil.h>
#include <ArmarXCore/libraries/DebugObserverHelper/DebugObserverHelper.h>
......@@ -16,15 +14,15 @@
#include <RobotAPI/libraries/RobotAPIComponentPlugins/RobotUnitComponentPlugin.h>
#include "RobotUnitData.h"
#include "converters/ConverterRegistry.h"
#include "converters/ConverterInterface.h"
#include "converters/ConverterRegistry.h"
namespace armarx::plugins
{
class RobotUnitComponentPlugin;
class DebugObserverComponentPlugin;
}
} // namespace armarx::plugins
namespace armarx
{
using RobotUnitDataStreamingReceiverPtr = std::shared_ptr<class RobotUnitDataStreamingReceiver>;
......@@ -35,34 +33,28 @@ namespace armarx::armem::server::robot_state::proprioception
class RobotUnitReader : public armarx::Logging
{
public:
RobotUnitReader();
using Queue = boost::lockfree::spsc_queue<RobotUnitData, boost::lockfree::capacity<1024>>;
using Queue = armarx::armem::server::robot_state::proprioception::Queue;
void connect(
armarx::plugins::RobotUnitComponentPlugin& robotUnitPlugin,
armarx::plugins::DebugObserverComponentPlugin& debugObserverPlugin,
const std::string& robotTypeName);
void connect(armarx::plugins::RobotUnitComponentPlugin& robotUnitPlugin,
armarx::plugins::DebugObserverComponentPlugin& debugObserverPlugin,
const std::string& robotTypeName);
/// Reads data from `handler` and fills `dataQueue`.
void run(float pollFrequency,
Queue& dataBuffer);
void run(float pollFrequency, Queue& dataBuffer);
std::optional<RobotUnitData> fetchAndConvertLatestRobotUnitData();
private:
/// Fetch the latest timestep and clear the robot unit buffer.
std::optional<RobotUnitDataStreaming::TimeStep> fetchLatestData();
public:
struct Properties
{
std::string sensorPrefix = "sens.*";
......@@ -80,7 +72,6 @@ namespace armarx::armem::server::robot_state::proprioception
ConverterInterface* converter = nullptr;
armarx::SimpleRunningTask<>::pointer_type task = nullptr;
};
}
} // namespace armarx::armem::server::robot_state::proprioception
source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.cpp
+ 25
− 0
View file @ a15a7c10
......@@ -25,9 +25,12 @@
#include <VirtualRobot/ManipulationObject.h>
#include <VirtualRobot/SceneObjectSet.h>
#include <VirtualRobot/Visualization/CoinVisualization/CoinVisualizationFactory.h>
#include <VirtualRobot/Visualization/TriMeshModel.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <RobotAPI/components/ArViz/Client/elements/Mesh.h>
namespace armarx::obstacle_avoidance
{
......@@ -151,4 +154,26 @@ namespace armarx::obstacle_avoidance
return std::make_shared<ManipulationObjectSet>(set);
}
void
CollisionModelHelper::visualizeCollisionModel(const VirtualRobot::CollisionModelPtr& model,
viz::Client& arviz)
{
armarx::viz::Mesh mesh(model->getName());
auto faces = model->getTriMeshModel()->faces;
std::vector<armarx::viz::data::Face> viz_faces;
std::transform(
faces.begin(),
faces.end(),
std::back_inserter(viz_faces),
[](const auto& face)
{
return armarx::viz::data::Face(
face.id1, face.id2, face.id3, face.idColor1, face.idColor2, face.idColor3);
});
mesh.vertices(model->getTriMeshModel()->vertices)
.faces(viz_faces)
.pose(model->getGlobalPose());
arviz.commitLayerContaining("CollisionModel", mesh);
}
} // namespace armarx::obstacle_avoidance
\ No newline at end of file
source/RobotAPI/libraries/obstacle_avoidance/CollisionModelHelper.h
+ 5
− 0
View file @ a15a7c10
......@@ -24,6 +24,7 @@
#include <VirtualRobot/VirtualRobot.h>
#include <RobotAPI/components/ArViz/Client/Client.h>
#include <RobotAPI/libraries/ArmarXObjects/ObjectPoseClient.h>
#include <RobotAPI/libraries/ArmarXObjects/forward_declarations.h>
#include <RobotAPI/libraries/armem_vision/OccupancyGridHelper.h>
......@@ -47,10 +48,14 @@ namespace armarx::obstacle_avoidance
static VirtualRobot::SceneObjectSetPtr
asSceneObjects(const armem::vision::OccupancyGrid& occupancyGrid,
const OccupancyGridHelper::Params& params);
static void visualizeCollisionModel(const VirtualRobot::CollisionModelPtr& model,
viz::Client& arviz);
CollisionModelHelper(const objpose::ObjectPoseClient& client);
VirtualRobot::SceneObjectSetPtr fetchSceneObjects();
ManipulationObjectSetPtr fetchManipulationObjects();
private:
objpose::ObjectPoseClient objectPoseClient_;
};
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment