File tree Expand file tree Collapse file tree 1 file changed +17
-8
lines changed
rclcpp/include/rclcpp/executors Expand file tree Collapse file tree 1 file changed +17
-8
lines changed Original file line number Diff line number Diff line change @@ -86,48 +86,57 @@ class MultiThreadedExecutor : public rclcpp::Executor
8686 * After the current mutex owner release the lock, a thread that used the high
8787 * priority mechanism will have priority over threads that used the low priority mechanism.
8888 */
89- class MutexTwoPriorities {
89+ class MutexTwoPriorities
90+ {
9091public:
9192 class HpMutex
9293 {
9394public:
94- HpMutex (MutexTwoPriorities & parent) : parent_(parent) {}
95+ explicit HpMutex (MutexTwoPriorities & parent)
96+ : parent_(parent) {}
9597
96- void lock () {
98+ void lock ()
99+ {
97100 parent_.data_ .lock ();
98101 }
99102
100- void unlock () {
103+ void unlock ()
104+ {
101105 parent_.data_ .unlock ();
102106 }
107+
103108private:
104109 MutexTwoPriorities & parent_;
105110 };
106111
107112 class LpMutex
108113 {
109114public:
110- LpMutex (MutexTwoPriorities & parent) : parent_(parent) {}
115+ explicit LpMutex (MutexTwoPriorities & parent)
116+ : parent_(parent) {}
111117
112- void lock () {
118+ void lock ()
119+ {
113120 // low_prio_.lock(); data_.lock();
114121 std::unique_lock<std::mutex> lpg{parent_.low_prio_ };
115122 parent_.data_ .lock ();
116123 lpg.release ();
117124 }
118125
119- void unlock () {
126+ void unlock ()
127+ {
120128 // data_.unlock(); low_prio_.unlock()
121129 std::lock_guard<std::mutex> lpg{parent_.low_prio_ , std::adopt_lock};
122130 parent_.data_ .unlock ();
123131 }
132+
124133private:
125134 MutexTwoPriorities & parent_;
126135 };
127136
128137 HpMutex hp () {return HpMutex{*this };}
129138 LpMutex lp () {return LpMutex{*this };}
130-
139+
131140private:
132141 // Implementation detail: the whole idea here is that only one low priority thread can be
133142 // trying to take the data_ mutex, while all high priority threads are already waiting there.
You can’t perform that action at this time.
0 commit comments