moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
filter_job.h
Go to the documentation of this file.
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2013, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34
35/* Author: Suat Gedikli */
36
37#pragma once
38
40
41namespace mesh_filter
42{
43MOVEIT_CLASS_FORWARD(Job); // Defines JobPtr, ConstPtr, WeakPtr... etc
44
49class Job
50{
51public:
52 Job() : done_(false)
53 {
54 }
55 virtual ~Job() = default;
56
57 inline void wait() const;
58 virtual void execute() = 0;
59 inline void cancel();
60 inline bool isDone() const;
61
62protected:
63 bool done_;
64 mutable std::condition_variable condition_;
65 mutable std::mutex mutex_;
66};
67
68void Job::wait() const
69{
70 std::unique_lock<std::mutex> lock(mutex_);
71 while (!done_)
72 condition_.wait(lock);
73}
74
76{
77 std::unique_lock<std::mutex> lock(mutex_);
78 done_ = true;
79 condition_.notify_all();
80}
81
82bool Job::isDone() const
83{
84 return done_;
85}
86
87template <typename ReturnType>
88class FilterJob : public Job
89{
90public:
91 FilterJob(const std::function<ReturnType()>& exec) : Job(), exec_(exec)
92 {
93 }
94 void execute() override;
95 const ReturnType& getResult() const;
96
97private:
98 std::function<ReturnType()> exec_;
99 ReturnType result_;
100};
101
102template <typename ReturnType>
104{
105 std::unique_lock<std::mutex> lock(mutex_);
106 if (!done_) // not canceled !
107 result_ = exec_();
108
109 done_ = true;
110 condition_.notify_all();
111}
112
113template <typename ReturnType>
114const ReturnType& FilterJob<ReturnType>::getResult() const
115{
116 wait();
117 return result_;
118}
119
120template <>
121class FilterJob<void> : public Job
122{
123public:
124 FilterJob(const std::function<void()>& exec) : Job(), exec_(exec)
125 {
126 }
127 void execute() override
128 {
129 std::unique_lock<std::mutex> lock(mutex_);
130 if (!done_) // not canceled !
131 exec_();
132
133 done_ = true;
134 condition_.notify_all();
135 }
136
137private:
138 std::function<void()> exec_;
139};
140} // namespace mesh_filter
#define MOVEIT_CLASS_FORWARD(C)
FilterJob(const std::function< void()> &exec)
Definition filter_job.h:124
void execute() override
Definition filter_job.h:103
FilterJob(const std::function< ReturnType()> &exec)
Definition filter_job.h:91
const ReturnType & getResult() const
Definition filter_job.h:114
This class is used to execute functions within the thread that holds the OpenGL context.
Definition filter_job.h:50
std::condition_variable condition_
Definition filter_job.h:64
virtual ~Job()=default
void wait() const
Definition filter_job.h:68
std::mutex mutex_
Definition filter_job.h:65
virtual void execute()=0
bool isDone() const
Definition filter_job.h:82
std::mutex mutex_