MotionPlanningTask.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 <atomic>
27 #include <deque>
28 #include <functional>
29 
30 #include <Ice/ObjectAdapter.h>
31 
32 #include <RobotComponents/interface/components/MotionPlanning/Tasks/MotionPlanningTask.h>
33 #include "../MotionPlanningServer.h"
35 
41 
42 namespace armarx
43 {
44  class MotionPlanningTask;
47 
51 
55 
57  virtual public MotionPlanningTaskBase,
58  virtual public MotionPlanningTaskCI
59  {
60  friend class MotionPlanningServer;
61 
62  public:
63  MotionPlanningTask() = default;
64 
65  ~MotionPlanningTask() override = default;
66  /**
67  * @brief Called by the planning server after the task was enqueued.
68  * Override this function to prepare some work (e.g. setting the status from eNew to eQueued)
69  */
70  virtual void postEnqueueing()
71  {
72  setTaskStatus(TaskStatus::eQueued);
73  }
74 
75  virtual void onPlanningDone() {}
76  virtual void onRefiningDone() {}
77 
78  MotionPlanningTaskBasePrx& getProxy()
79  {
80  return selfProxy;
81  }
82 
83  bool setTaskStatus(TaskStatus::Status newTaskStatus, const Ice::Current& = Ice::emptyCurrent) override;
84 
85  TaskStatus::Status getTaskStatus(const Ice::Current& = Ice::emptyCurrent) const override
86  {
87  return taskStatus;
88  }
89 
90  virtual void registerAtIceAdapter(Ice::ObjectAdapterPtr& adapter, const Ice::Identity ident)
91  {
92  if (selfProxy)
93  {
94  throw std::logic_error {"Task already registered as: category: " + selfProxy->ice_getIdentity().category + ", name: " + selfProxy->ice_getIdentity().name};
95  }
96  selfProxy = MotionPlanningTaskBasePrx::uncheckedCast(adapter->add(this, ident));
97  }
98 
99  std::string getTaskName(const Ice::Current& = Ice::emptyCurrent) const override
100  {
101  return taskName;
102  }
103 
104  void addTaskStatusCallback(std::function<void(TaskStatus::Status)> cb)
105  {
106  taskStatusCallbacks.emplace_back(cb);
107  }
108 
109  Ice::Long getPlanningTime(const Ice::Current&) const override
110  {
111  return planningTime;
112  }
113  Ice::Long getRefiningTime(const Ice::Current&) const override
114  {
115  return refiningTime;
116  }
117  Ice::Long getRunningTime(const Ice::Current&) const override
118  {
119  return planningTime + refiningTime;
120  }
121 
122  protected:
123  /**
124  * The planning time in microseconds
125  */
127  /**
128  * The refining time in microseconds
129  */
131 
132  private:
133  /**
134  * @brief A self proxy. (may be required for callbacks)
135  */
136  MotionPlanningTaskBasePrx selfProxy;
137  std::atomic<TaskStatus::Status> taskStatus {TaskStatus::eNew};
138  std::deque<std::function<void(TaskStatus::Status)>> taskStatusCallbacks;
139  };
140 
141 
143  virtual public MotionPlanningTaskWithDefaultMembersBase,
144  virtual public MotionPlanningTask
145  {
146  public:
147  /**
148  * @brief ctor
149  * @param startCfg the start point
150  * @param cspace the planning cspace
151  * @param dcdStep the dcd step size
152  * @param maximalPlanningTimeInSeconds the maximal time in seconds
153  */
155  const VectorXf& startCfg,
156  const VectorXf& goalCfg,
157  const CSpaceBasePtr& cspace,
158  Ice::Float dcdStep,
159  Ice::Long maximalPlanningTimeInSeconds,
160  const std::string& taskName
161  ):
162  MotionPlanningTaskBase(taskName),
163  MotionPlanningTaskWithDefaultMembersBase(taskName, startCfg, goalCfg, cspace, dcdStep, maximalPlanningTimeInSeconds)
164  {
165  }
166 
167 
168  /**
169  * @return The task's start configuration.
170  */
171  VectorXf getStart(const Ice::Current& = Ice::emptyCurrent) const override
172  {
173  return startCfg;
174  }
175 
176  /**
177  * @return The task's start configuration.
178  */
179  VectorXf getGoal(const Ice::Current& = Ice::emptyCurrent) const override
180  {
181  return goalCfg;
182  }
183 
184  /**
185  * @return The task's CSpace .
186  */
187  CSpaceBasePtr getCSpace(const Ice::Current& = Ice::emptyCurrent) const override
188  {
189  return cspace;
190  }
191 
192  /**
193  * @return The task's .
194  */
195  float getDcdStep(const Ice::Current& = Ice::emptyCurrent) const override
196  {
197  return dcdStep;
198  }
199 
200  /**
201  * @return The task's .
202  */
203  Ice::Long getMaximalPlanningTimeInSeconds(const Ice::Current& = Ice::emptyCurrent) const override
204  {
205  return maximalPlanningTimeInSeconds;
206  }
207 
208  protected:
210  };
211 
213  virtual public MotionPlanningTask,
214  virtual public PostprocessingMotionPlanningTaskBase
215  {
216  public:
217  PostprocessingMotionPlanningTask(const MotionPlanningTaskBasePtr& previousStep, const std::string& taskName):
218  MotionPlanningTaskBase(taskName),
219  PostprocessingMotionPlanningTaskBase(taskName, previousStep)
220  {
221  ARMARX_CHECK_EXPRESSION(this->previousStep);
222  }
223 
224  CSpaceBasePtr getCSpace(const Ice::Current& = Ice::emptyCurrent) const override
225  {
226  ARMARX_CHECK_EXPRESSION(previousStep->getCSpace());
227  return previousStep->getCSpace();
228  }
229 
230  void registerAtIceAdapter(Ice::ObjectAdapterPtr& adapter, const Ice::Identity ident) override
231  {
233 
234  auto prev = MotionPlanningTaskPtr::dynamicCast(previousStep);
236 
237  Ice::Identity subIdent;
238  subIdent.name = ManagedIceObject::generateSubObjectName(ident.category + ident.name, "PreviousStep_" + previousStep->ice_id());
239  prev->registerAtIceAdapter(adapter, subIdent);
240  }
241 
242  void postEnqueueing() override
243  {
245  MotionPlanningTaskPtr::dynamicCast(previousStep)->postEnqueueing();
246  }
247 
248  protected:
250  };
251 }
armarx::VariantType::Float
const VariantTypeId Float
Definition: Variant.h:918
armarx::MotionPlanningTask
Definition: MotionPlanningTask.h:56
armarx::MotionPlanningTask::addTaskStatusCallback
void addTaskStatusCallback(std::function< void(TaskStatus::Status)> cb)
Definition: MotionPlanningTask.h:104
armarx::MotionPlanningTaskWithDefaultMembers
Definition: MotionPlanningTask.h:142
armarx::MotionPlanningTaskWithDefaultMembers::getMaximalPlanningTimeInSeconds
Ice::Long getMaximalPlanningTimeInSeconds(const Ice::Current &=Ice::emptyCurrent) const override
Definition: MotionPlanningTask.h:203
armarx::MotionPlanningTask::getProxy
MotionPlanningTaskBasePrx & getProxy()
Definition: MotionPlanningTask.h:78
armarx::MotionPlanningTask::postEnqueueing
virtual void postEnqueueing()
Called by the planning server after the task was enqueued.
Definition: MotionPlanningTask.h:70
RemoteHandle.h
armarx::MotionPlanningTaskWithDefaultMembers::getDcdStep
float getDcdStep(const Ice::Current &=Ice::emptyCurrent) const override
Definition: MotionPlanningTask.h:195
armarx::MotionPlanningTask::getTaskStatus
TaskStatus::Status getTaskStatus(const Ice::Current &=Ice::emptyCurrent) const override
Definition: MotionPlanningTask.h:85
armarx::MotionPlanningServer
Definition: MotionPlanningServer.h:85
armarx::MotionPlanningTask::onRefiningDone
virtual void onRefiningDone()
Definition: MotionPlanningTask.h:76
armarx::PostprocessingMotionPlanningTask::getCSpace
CSpaceBasePtr getCSpace(const Ice::Current &=Ice::emptyCurrent) const override
Definition: MotionPlanningTask.h:224
armarx::MotionPlanningTask::registerAtIceAdapter
virtual void registerAtIceAdapter(Ice::ObjectAdapterPtr &adapter, const Ice::Identity ident)
Definition: MotionPlanningTask.h:90
armarx::MotionPlanningTask::getTaskName
std::string getTaskName(const Ice::Current &=Ice::emptyCurrent) const override
Definition: MotionPlanningTask.h:99
armarx::MotionPlanningTask::refiningTime
Ice::Long refiningTime
The refining time in microseconds.
Definition: MotionPlanningTask.h:130
IceInternal::Handle< MotionPlanningTask >
GfxTL::Identity
void Identity(MatrixXX< N, N, T > *a)
Definition: MatrixXX.h:523
armarx::PostprocessingMotionPlanningTask::PostprocessingMotionPlanningTask
PostprocessingMotionPlanningTask()=default
ManagedIceObject.h
armarx::PostprocessingMotionPlanningTask::postEnqueueing
void postEnqueueing() override
Called by the planning server after the task was enqueued.
Definition: MotionPlanningTask.h:242
armarx::MotionPlanningTaskPtr
IceInternal::Handle< MotionPlanningTask > MotionPlanningTaskPtr
Definition: MotionPlanningServer.h:47
armarx::MotionPlanningTaskWithDefaultMembers::MotionPlanningTaskWithDefaultMembers
MotionPlanningTaskWithDefaultMembers()=default
armarx::ManagedIceObject::generateSubObjectName
static std::string generateSubObjectName(const std::string &superObjectName, const std::string &subObjectName)
Generates a unique name for a sub object from a general name and unique name.
Definition: ManagedIceObject.cpp:117
armarx::VariantType::Long
const VariantTypeId Long
Definition: Variant.h:917
armarx::MotionPlanningTask::getRefiningTime
Ice::Long getRefiningTime(const Ice::Current &) const override
Definition: MotionPlanningTask.h:113
armarx::MotionPlanningTask::getPlanningTime
Ice::Long getPlanningTime(const Ice::Current &) const override
Definition: MotionPlanningTask.h:109
armarx::MotionPlanningTask::~MotionPlanningTask
~MotionPlanningTask() override=default
armarx::MotionPlanningTask::MotionPlanningTask
MotionPlanningTask()=default
armarx::MotionPlanningTaskWithDefaultMembers::MotionPlanningTaskWithDefaultMembers
MotionPlanningTaskWithDefaultMembers(const VectorXf &startCfg, const VectorXf &goalCfg, const CSpaceBasePtr &cspace, Ice::Float dcdStep, Ice::Long maximalPlanningTimeInSeconds, const std::string &taskName)
ctor
Definition: MotionPlanningTask.h:154
ExpressionException.h
armarx::MotionPlanningTaskWithDefaultMembers::getCSpace
CSpaceBasePtr getCSpace(const Ice::Current &=Ice::emptyCurrent) const override
Definition: MotionPlanningTask.h:187
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::MotionPlanningTaskCI
Definition: MotionPlanningTaskControlInterface.h:31
MotionPlanningTaskControlInterface.h
armarx::PostprocessingMotionPlanningTask
Definition: MotionPlanningTask.h:212
armarx::MotionPlanningTaskWithDefaultMembers::getGoal
VectorXf getGoal(const Ice::Current &=Ice::emptyCurrent) const override
Definition: MotionPlanningTask.h:179
armarx::MotionPlanningTask::setTaskStatus
bool setTaskStatus(TaskStatus::Status newTaskStatus, const Ice::Current &=Ice::emptyCurrent) override
Definition: MotionPlanningTask.cpp:30
armarx::MotionPlanningTaskWithDefaultMembers::getStart
VectorXf getStart(const Ice::Current &=Ice::emptyCurrent) const override
Definition: MotionPlanningTask.h:171
armarx::MotionPlanningTask::getRunningTime
Ice::Long getRunningTime(const Ice::Current &) const override
Definition: MotionPlanningTask.h:117
armarx::RemoteHandle
The RemoteHandle class wrapps a ClientSideRemoteHandleControlBlock and can be used just as a Ice prox...
Definition: RemoteHandle.h:45
armarx::PostprocessingMotionPlanningTask::PostprocessingMotionPlanningTask
PostprocessingMotionPlanningTask(const MotionPlanningTaskBasePtr &previousStep, const std::string &taskName)
Definition: MotionPlanningTask.h:217
Logging.h
AMDCallbackCollection.h
armarx::MotionPlanningTask::onPlanningDone
virtual void onPlanningDone()
Definition: MotionPlanningTask.h:75
armarx::MotionPlanningTask::planningTime
Ice::Long planningTime
The planning time in microseconds.
Definition: MotionPlanningTask.h:126
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:28
armarx::PostprocessingMotionPlanningTask::registerAtIceAdapter
void registerAtIceAdapter(Ice::ObjectAdapterPtr &adapter, const Ice::Identity ident) override
Definition: MotionPlanningTask.h:230