MotionPlanningServer.h
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * Copyright (C) 2011-2016, High Performance Humanoid Technologies (H2T), Karlsruhe Institute of Technology (KIT), all rights reserved.
5  *
6  * ArmarX is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License version 2 as
8  * published by the Free Software Foundation.
9  *
10  * ArmarX is distributed in the hope that it will be useful, but
11  * WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program. If not, see <http://www.gnu.org/licenses/>.
17  *
18  * @package RobotComponents
19  * @author Raphael Grimm ( raphael dot grimm at kit dot edu )
20  * @date 2015
21  * @copyright http://www.gnu.org/licenses/gpl.txt
22  * GNU General Public License
23  */
24 #pragma once
25 
26 #include <unordered_map>
27 #include <condition_variable>
28 #include <atomic>
29 #include <mutex>
30 #include <thread>
31 
34 
37 
38 #include <RobotComponents/interface/components/MotionPlanning/MotionPlanningServer.h>
40 
41 
43 
44 namespace armarx
45 {
46  class MotionPlanningTask;
48  class DynamicLibrary;
49  using DynamicLibraryPtr = std::shared_ptr<DynamicLibrary>;
50 
52  /**
53  * @brief A ice handle for a MotionPlanningServerComponent
54  */
56 
57  /**
58  * @brief Properties for a MotionPlanningServerComponent
59  */
62  {
63  public:
64  /**
65  * @brief ctor
66  * @param prefix the used prefix for properties
67  */
70  {
71  defineOptionalProperty<std::string>("RemoteObjectNodes", "", "CSV of RemoteObjectNode names");
72  defineOptionalProperty<bool>("LocalRONStart", true, "start a local remote object node if the CSV RemoteObjectNodes is empty");
73  defineOptionalProperty<float>("LocalRONCorePerc", 0.5, "Local remote object node core count percentage. (has to be >0; 0.5 => 50%");
74  }
75  };
76 
77  /**
78  * @defgroup Component-MotionPlanningServer MotionPlanningServer
79  * @ingroup RobotComponents-Components
80  * @brief Implementation of the slice interface MotionPlanningServerInterface
81  * @see \ref RobotComponents-Tutorials-MotionPlanning
82  * @see \ref RobotComponents-Tutorials-SimoxCSpace
83  * @see \ref RobotComponents-GuiPlugins-SimoxCSpaceVisualizer
84  */
86  virtual public MotionPlanningServerInterface,
87  virtual public armarx::Component
88  {
89  public:
90  /**
91  * @brief ctor
92  */
93  MotionPlanningServer() = default;
94 
95  /**
96  * @brief dtor
97  */
99  {
100 #pragma GCC diagnostic push
101 #pragma GCC diagnostic ignored "-Wterminate"
103  ARMARX_VERBOSE_S << "dtor";
105 #pragma GCC diagnostic pop
106  }
107 
108  // inherited from Component
109  /**
110  * @brief Returns the server's default name.
111  * @return The server's default name.
112  */
113  std::string getDefaultName() const override
114  {
115  return "MotionPlanningServer";
116  }
117  /**
118  * @brief Initializes the server and starts the dispatcher thread
119  */
120  void onInitComponent() override;
121  /**
122  * @brief Connects to the used RemoteObjectNodes
123  */
124  void onConnectComponent() override;
125  /**
126  * @brief noop
127  */
128  void onDisconnectComponent() override {}
129  /**
130  * @brief cleans up and joins the dispatcher
131  */
132  void onExitComponent() override;
133 
134  /**
135  */
136  /**
137  * @see PropertyUser::createPropertyDefinitions()
138  * @return The planning server's PropertyDefinitions
139  */
141  {
143  }
144 
145  //from MotionPlanningServerInterface
146  /**
147  * @brief Returns a task's cspace and all found paths.
148  * @param id The id.
149  * @return The task's cspace and all found paths.
150  */
151  CSpaceAndPaths getTaskCSpaceAndPathsById(Ice::Long id, const Ice::Current& = Ice::emptyCurrent) override;
152 
153  /**
154  * @brief Enqueues a task
155  * @param task The task.
156  * @return The task's proxy.
157  */
158  armarx::ClientSideRemoteHandleControlBlockBasePtr enqueueTask(const MotionPlanningTaskBasePtr& task, const Ice::Current& = Ice::emptyCurrent) override;
159 
160  /**
161  * @brief Returns the number of tasks.
162  * @return The number of tasks.
163  */
164  Ice::Long getNumberOfTasks(const Ice::Current& = Ice::emptyCurrent) const override;
165  /**
166  * @brief Returns the number of queued tasks.
167  * @return The number of queued tasks.
168  */
169  Ice::Long getNumberOfQueuedTasks(const Ice::Current&) const override;
170 
171  /**
172  * @brief Returns information about all tasks.
173  * @return Information about all tasks.
174  */
175  TaskInfoSeq getAllTaskInfo(const Ice::Current& = Ice::emptyCurrent) const override;
176 
177  ClientSideRemoteHandleControlBlockBasePtr getCurrentTaskHandle(const Ice::Current&) override;
178 
179  bool loadLibFromPath(const std::string& path, const Ice::Current& = Ice::emptyCurrent) override;
180  bool loadLibFromPackage(const std::string& package, const std::string& lib, const Ice::Current& = Ice::emptyCurrent) override;
181 
182  protected:
183  bool deleteTaskById(Ice::Long id);
184  /**
185  * @brief the dispatcher task. (executed by the dispatcherThread)
186  */
187  void dispatcherTask();
188 
189  /**
190  * @brief the next task id.
191  */
193  /**
194  * @brief Returns a new task id.
195  * @return A new task id.
196  */
198  {
199  return nextTaskId++;
200  }
201 
202  /**
203  * @brief The used remote object nodes' names
204  */
205  std::vector<std::string> remoteObjectNodeNames;
206  /**
207  * @brief The used remote object nodes
208  */
209  std::vector<armarx::RemoteObjectNodeInterfacePrx> remoteObjectNodes;
210 
211  /**
212  * @brief Whether a local node should be started, if no rons were provided.
213  */
215  /**
216  * @brief Percentage of cores used by the local ron (if started).
217  */
219 
221 
223  {
226  };
227 
228  /**
229  * @brief The type of the map of tasks
230  */
231  using TaskMap = std::unordered_map<Ice::Long, TaskAndRemoteHandle>;
232  /**
233  * @brief The map of tasks.
234  */
236 
237  /**
238  * @brief currentTask stores a handle to the currently dispatched planning task.
239  */
241 
242  /**
243  * @brief Contains the queue of task ids to execute.
244  *
245  * If not empty the first id is the next task to execute
246  * A set is used since the queue lacks an erase function and ids are
247  * added in ascending order.
248  */
249  std::set<Ice::Long> taskQueue;
250 
251  /**
252  * @brief mutex protecting the task queue
253  */
254  mutable std::recursive_mutex queueMutex;
255 
256  //we have to ensure that the waiting thread has only locked the recursive mutex once,
257  //since the condition variable only will use the unlock method on the unique_lock once during the wait.
258  //http://stackoverflow.com/questions/14323340/can-you-combine-stdrecursive-mutex-with-stdcondition-variable
259  //
260  //this is given for dispatcherThread
261  /**
262  * @brief The dispatcher's cv
263  */
264  std::condition_variable_any waitForTaskOrDispatcherKill;
265  /**
266  * @brief The dispatcher's thread
267  */
268  std::thread dispatcherThread;
269  /**
270  * @brief Whether the dispatcher should shut down
271  */
272  std::atomic_bool dispatcherKillRequest {false};
273 
274  std::map<std::string, DynamicLibraryPtr> loadedLibs;
275 
277 
278 
279  private:
280  bool loadLibFromAbsolutePath(const std::string& path);
281  };
282 
283 }
armarx::MotionPlanningServer::dispatcherKillRequest
std::atomic_bool dispatcherKillRequest
Whether the dispatcher should shut down.
Definition: MotionPlanningServer.h:272
armarx::MotionPlanningServer::createPropertyDefinitions
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: MotionPlanningServer.h:140
armarx::MotionPlanningServer::onDisconnectComponent
void onDisconnectComponent() override
noop
Definition: MotionPlanningServer.h:128
armarx::MotionPlanningServer::dispatcherThread
std::thread dispatcherThread
The dispatcher's thread.
Definition: MotionPlanningServer.h:268
armarx::DynamicLibraryPtr
std::shared_ptr< DynamicLibrary > DynamicLibraryPtr
Definition: DynamicLibrary.h:123
armarx::MotionPlanningServer::waitForTaskOrDispatcherKill
std::condition_variable_any waitForTaskOrDispatcherKill
The dispatcher's cv.
Definition: MotionPlanningServer.h:264
armarx::MotionPlanningServer::remoteObjectNodeNames
std::vector< std::string > remoteObjectNodeNames
The used remote object nodes' names.
Definition: MotionPlanningServer.h:205
armarx::MotionPlanningServer::TaskAndRemoteHandle::rh
armarx::RemoteHandleControlBlockPtr rh
Definition: MotionPlanningServer.h:224
armarx::MotionPlanningServer::MotionPlanningServer
MotionPlanningServer()=default
ctor
armarx::MotionPlanningServer::tasks
TaskMap tasks
The map of tasks.
Definition: MotionPlanningServer.h:235
armarx::MotionPlanningServer::onConnectComponent
void onConnectComponent() override
Connects to the used RemoteObjectNodes.
Definition: MotionPlanningServer.cpp:73
armarx::PropertyDefinitionContainer::prefix
std::string prefix
Prefix of the properties such as namespace, domain, component name, etc.
Definition: PropertyDefinitionContainer.h:333
armarx::MotionPlanningServer::currentTask
TaskAndRemoteHandle currentTask
currentTask stores a handle to the currently dispatched planning task.
Definition: MotionPlanningServer.h:240
armarx::MotionPlanningServer::getTaskCSpaceAndPathsById
CSpaceAndPaths getTaskCSpaceAndPathsById(Ice::Long id, const Ice::Current &=Ice::emptyCurrent) override
Returns a task's cspace and all found paths.
Definition: MotionPlanningServer.cpp:371
armarx::MotionPlanningServer::remoteObjectNodes
std::vector< armarx::RemoteObjectNodeInterfacePrx > remoteObjectNodes
The used remote object nodes.
Definition: MotionPlanningServer.h:209
armarx::MotionPlanningServer::loadLibFromPackage
bool loadLibFromPackage(const std::string &package, const std::string &lib, const Ice::Current &=Ice::emptyCurrent) override
Definition: MotionPlanningServer.cpp:444
armarx::MotionPlanningServer
Definition: MotionPlanningServer.h:85
armarx::MotionPlanningServerPropertyDefinitions
Properties for a MotionPlanningServerComponent.
Definition: MotionPlanningServer.h:60
RemoteObjectNode.h
armarx::MotionPlanningServer::getDefaultName
std::string getDefaultName() const override
Returns the server's default name.
Definition: MotionPlanningServer.h:113
armarx::MotionPlanningServer::onInitComponent
void onInitComponent() override
Initializes the server and starts the dispatcher thread.
Definition: MotionPlanningServer.cpp:43
IceInternal::Handle< MotionPlanningTask >
armarx::MotionPlanningServerPropertyDefinitions::MotionPlanningServerPropertyDefinitions
MotionPlanningServerPropertyDefinitions(std::string prefix)
ctor
Definition: MotionPlanningServer.h:68
armarx::MotionPlanningServer::localNodePercent
float localNodePercent
Percentage of cores used by the local ron (if started).
Definition: MotionPlanningServer.h:218
armarx::MotionPlanningServer::onExitComponent
void onExitComponent() override
cleans up and joins the dispatcher
Definition: MotionPlanningServer.cpp:106
armarx::MotionPlanningServer::getNumberOfQueuedTasks
Ice::Long getNumberOfQueuedTasks(const Ice::Current &) const override
Returns the number of queued tasks.
Definition: MotionPlanningServer.cpp:332
armarx::MotionPlanningServer::TaskMap
std::unordered_map< Ice::Long, TaskAndRemoteHandle > TaskMap
The type of the map of tasks.
Definition: MotionPlanningServer.h:231
armarx::MotionPlanningServer::getAllTaskInfo
TaskInfoSeq getAllTaskInfo(const Ice::Current &=Ice::emptyCurrent) const override
Returns information about all tasks.
Definition: MotionPlanningServer.cpp:338
RemoteHandleControlBlock.h
armarx::MotionPlanningServer::localNode
RemoteObjectNodePtr localNode
Definition: MotionPlanningServer.h:220
MotionPlanningTask.h
armarx::VariantType::Long
const VariantTypeId Long
Definition: Variant.h:917
armarx::MotionPlanningServer::enqueueTask
armarx::ClientSideRemoteHandleControlBlockBasePtr enqueueTask(const MotionPlanningTaskBasePtr &task, const Ice::Current &=Ice::emptyCurrent) override
Enqueues a task.
Definition: MotionPlanningServer.cpp:278
armarx::MotionPlanningServer::getCurrentTaskHandle
ClientSideRemoteHandleControlBlockBasePtr getCurrentTaskHandle(const Ice::Current &) override
Definition: MotionPlanningServer.cpp:361
armarx::MotionPlanningServer::~MotionPlanningServer
~MotionPlanningServer() override
dtor
Definition: MotionPlanningServer.h:98
Component.h
armarx::Component
Baseclass for all ArmarX ManagedIceObjects requiring properties.
Definition: Component.h:95
armarx::MotionPlanningServer::startLocalNode
bool startLocalNode
Whether a local node should be started, if no rons were provided.
Definition: MotionPlanningServer.h:214
ExpressionException.h
armarx::Component::getConfigIdentifier
std::string getConfigIdentifier()
Retrieve config identifier for this component as set in constructor.
Definition: Component.cpp:74
armarx::MotionPlanningServer::taskQueue
std::set< Ice::Long > taskQueue
Contains the queue of task ids to execute.
Definition: MotionPlanningServer.h:249
SimoxCSpace.h
armarx::MotionPlanningServer::loadedLibs
std::map< std::string, DynamicLibraryPtr > loadedLibs
Definition: MotionPlanningServer.h:274
armarx::MotionPlanningServer::loadLibFromPath
bool loadLibFromPath(const std::string &path, const Ice::Current &=Ice::emptyCurrent) override
Definition: MotionPlanningServer.cpp:430
armarx::MotionPlanningServer::TaskAndRemoteHandle::task
MotionPlanningTaskPtr task
Definition: MotionPlanningServer.h:225
ARMARX_CHECK_EXPRESSION
#define ARMARX_CHECK_EXPRESSION(expression)
This macro evaluates the expression and if it turns out to be false it will throw an ExpressionExcept...
Definition: ExpressionException.h:73
armarx::MotionPlanningServer::getNumberOfTasks
Ice::Long getNumberOfTasks(const Ice::Current &=Ice::emptyCurrent) const override
Returns the number of tasks.
Definition: MotionPlanningServer.cpp:326
armarx::ComponentPropertyDefinitions
Default component property definition container.
Definition: Component.h:70
armarx::MotionPlanningServer::getNewTaskId
Ice::Long getNewTaskId()
Returns a new task id.
Definition: MotionPlanningServer.h:197
IceUtil::Handle< class PropertyDefinitionContainer >
armarx::MotionPlanningServer::TaskAndRemoteHandle
Definition: MotionPlanningServer.h:222
armarx::MotionPlanningServer::dispatcherTask
void dispatcherTask()
the dispatcher task.
Definition: MotionPlanningServer.cpp:140
armarx::MotionPlanningServer::nextTaskId
std::atomic< Ice::Long > nextTaskId
the next task id.
Definition: MotionPlanningServer.h:192
ARMARX_VERBOSE_S
#define ARMARX_VERBOSE_S
Definition: Logging.h:200
armarx::MotionPlanningServer::cacheCSpace
SimoxCSpacePtr cacheCSpace
Definition: MotionPlanningServer.h:276
armarx::MotionPlanningServer::queueMutex
std::recursive_mutex queueMutex
mutex protecting the task queue
Definition: MotionPlanningServer.h:254
armarx::DynamicLibrary
The DynamicLibrary class provides a mechanism to load libraries at runtime.
Definition: DynamicLibrary.h:49
min
T min(T t1, T t2)
Definition: gdiam.h:42
armarx::PropertyDefinitionsPtr
IceUtil::Handle< class PropertyDefinitionContainer > PropertyDefinitionsPtr
PropertyDefinitions smart pointer type.
Definition: forward_declarations.h:34
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::MotionPlanningServer::deleteTaskById
bool deleteTaskById(Ice::Long id)
Definition: MotionPlanningServer.cpp:256