fix multithread safety

MRPT · Nov 28, 2022 · 8b842b2 · 8b842b2
1 parent b73017d
commit 8b842b2
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 5 deletions.
diff --git a/libselfdriving/include/selfdriving/ptgs/HolonomicBlend.h b/libselfdriving/include/selfdriving/ptgs/HolonomicBlend.h
@@ -18,6 +18,8 @@
 #include <mrpt/nav/tpspace/CParameterizedTrajectoryGenerator.h>
 #include <selfdriving/ptgs/SpeedTrimmablePTG.h>
 
+#include <mutex>
+
 namespace selfdriving::ptg
 {
 /** A PTG for circular-shaped robots with holonomic kinematics.
@@ -96,9 +98,10 @@ class HolonomicBlend : public SpeedTrimmablePTG,
 
     // Compilation of user-given expressions
     mrpt::expr::CRuntimeCompiledExpression m_expr_v, m_expr_w, m_expr_T_ramp;
-    double m_expr_dir = 0;  // Used as symbol "dir" in m_expr_v and m_expr_w
-    double m_expr_target_dir  = 0;  // symbol "target_dir" in expressions
-    double m_expr_target_dist = 0;  // symbol "target_dist" in expressions
+    double     m_expr_dir = 0;  // Used as symbol "dir" in m_expr_v and m_expr_w
+    double     m_expr_target_dir  = 0;  // symbol "target_dir" in expressions
+    double     m_expr_target_dist = 0;  // symbol "target_dist" in expressions
+    std::mutex m_expr_mtx;
 
     /** Evals expr_v */
     double internal_get_v(const double dir) const;

diff --git a/libselfdriving/src/ptgs/HolonomicBlend.cpp b/libselfdriving/src/ptgs/HolonomicBlend.cpp
@@ -21,8 +21,6 @@
 #include <mrpt/system/CTimeLogger.h>
 #include <selfdriving/ptgs/HolonomicBlend.h>
 
-// #include <iostream>  // debug!
-
 using namespace mrpt::nav;
 using namespace selfdriving::ptg;
 using namespace mrpt::system;
@@ -830,16 +828,22 @@ void HolonomicBlend::internal_construct_exprs()
 
 double HolonomicBlend::internal_get_v(const double dir) const
 {
+    auto lck = mrpt::lockHelper(m_expr_mtx);
+
     const_cast<double&>(m_expr_dir) = dir;
     return std::abs(m_expr_v.eval());
 }
 double HolonomicBlend::internal_get_w(const double dir) const
 {
+    auto lck = mrpt::lockHelper(m_expr_mtx);
+
     const_cast<double&>(m_expr_dir) = dir;
     return std::abs(m_expr_w.eval());
 }
 double HolonomicBlend::internal_get_T_ramp(const double dir) const
 {
+    auto lck = mrpt::lockHelper(m_expr_mtx);
+
     const_cast<double&>(m_expr_dir) = dir;
     return m_expr_T_ramp.eval();
 }