From 1300570687300df47c674b1da00a705aa42fb838 Mon Sep 17 00:00:00 2001
From: Fabian Reister <fabian.reister@kit.edu>
Date: Fri, 17 Dec 2021 15:31:30 +0100
Subject: [PATCH] introducing mutexes for scene
---
.../components/Navigator/Navigator.cpp | 17 +++++++++++------
source/armarx/navigation/core/types.h | 4 ++++
.../armarx/navigation/global_planning/AStar.cpp | 2 ++
3 files changed, 17 insertions(+), 6 deletions(-)
diff --git a/source/armarx/navigation/components/Navigator/Navigator.cpp b/source/armarx/navigation/components/Navigator/Navigator.cpp
index 74292c5b..9660789c 100644
--- a/source/armarx/navigation/components/Navigator/Navigator.cpp
+++ b/source/armarx/navigation/components/Navigator/Navigator.cpp
@@ -145,10 +145,7 @@ namespace armarx::navigation::components
// initialize scene
ARMARX_TRACE;
- scene.robot = getRobot();
- scene.staticScene = staticScene();
- scene.graph = core::SceneGraph{.subgraphs = graphReader.graphs()};
-
+ updateContext();
executor = server::PlatformUnitExecutor(platformUnit);
@@ -215,9 +212,17 @@ namespace armarx::navigation::components
ARMARX_TRACE;
scene.robot = getRobot();
- scene.staticScene = staticScene();
+
+ {
+ std::lock_guard g{scene.staticSceneMtx};
+ scene.staticScene = staticScene();
+ }
+
// TODO dynamic scene
- scene.graph = core::SceneGraph{.subgraphs = graphReader.graphs()};
+ {
+ std::lock_guard g{scene.graphMtx};
+ scene.graph = core::SceneGraph{.subgraphs = graphReader.graphs()};
+ }
}
std::string
diff --git a/source/armarx/navigation/core/types.h b/source/armarx/navigation/core/types.h
index d065bceb..464966df 100644
--- a/source/armarx/navigation/core/types.h
+++ b/source/armarx/navigation/core/types.h
@@ -78,7 +78,10 @@ namespace armarx::navigation::core
struct Scene
{
+ mutable std::mutex staticSceneMtx;
std::optional<core::StaticScene> staticScene = std::nullopt;
+
+ mutable std::mutex dynamicSceneMtx;
std::optional<core::DynamicScene> dynamicScene = std::nullopt;
// TopologicMapPtr topologicMap;
VirtualRobot::RobotPtr robot;
@@ -86,6 +89,7 @@ namespace armarx::navigation::core
mutable std::mutex platformVelocityMtx;
std::optional<core::Twist> platformVelocity;
+ mutable std::mutex graphMtx;
std::optional<core::SceneGraph> graph;
TimeServerInterface* timeServer;
diff --git a/source/armarx/navigation/global_planning/AStar.cpp b/source/armarx/navigation/global_planning/AStar.cpp
index 9180d28e..baa0409c 100644
--- a/source/armarx/navigation/global_planning/AStar.cpp
+++ b/source/armarx/navigation/global_planning/AStar.cpp
@@ -1,6 +1,7 @@
#include "AStar.h"
#include <algorithm>
+#include <mutex>
#include <optional>
#include <Eigen/Geometry>
@@ -112,6 +113,7 @@ namespace armarx::navigation::glob_plan
{
ARMARX_TRACE;
+ std::lock_guard g{scene.staticSceneMtx};
algorithm::astar::AStarPlanner planner(scene.robot, scene.staticScene->objects, 100.F);
// planner.setRobotColModel("Platform-colmodel");
--
GitLab