CollisionCheckerComponent.h
Go to the documentation of this file.
1 /*
2  * This file is part of ArmarX.
3  *
4  * ArmarX is free software; you can redistribute it and/or modify
5  * it under the terms of the GNU General Public License version 2 as
6  * published by the Free Software Foundation.
7  *
8  * ArmarX is distributed in the hope that it will be useful, but
9  * WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
11  * GNU General Public License for more details.
12  *
13  * You should have received a copy of the GNU General Public License
14  * along with this program. If not, see <http://www.gnu.org/licenses/>.
15  *
16  * @package RobotComponents::ArmarXObjects::CollisionCheckerComponent
17  * @author Adrian Knobloch ( adrian dot knobloch at student dot kit dot edu )
18  * @date 2016
19  * @copyright http://www.gnu.org/licenses/gpl-2.0.txt
20  * GNU General Public License
21  */
22 
23 #pragma once
24 
25 
26 #include <mutex>
27 
28 #include <VirtualRobot/Robot.h>
29 #include <VirtualRobot/SceneObject.h>
30 
33 
34 #include <RobotAPI/interface/core/RobotState.h>
35 #include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
37 
38 #include <RobotComponents/interface/components/CollisionCheckerInterface.h>
39 
41 #include <MemoryX/interface/components/WorkingMemoryInterface.h>
42 
43 namespace armarx
44 {
45  /**
46  * @class CollisionCheckerComponentPropertyDefinitions
47  * @brief
48  */
50  {
51  public:
54  {
55  defineRequiredProperty<bool>("UseWorkingMemory",
56  "If true, the WorkingMemory is used to get all "
57  "SceneObjects. Otherwise a RobotStateComponent is used.");
58  defineOptionalProperty<std::string>(
59  "WorkingMemoryName", "WorkingMemory", "The name of the WorkingMemory.");
60  defineOptionalProperty<std::string>(
61  "WorkingMemoryListenerTopicName",
62  "WorkingMemoryUpdates",
63  "The name of the topic to listen for WorkingMemory updates.");
64  defineOptionalProperty<std::string>("RobotStateComponentName",
65  "RobotStateComponent",
66  "The name of the RobotStateComponent.");
67  defineOptionalProperty<int>(
68  "interval", 100, "The interval of time to check for collisions. (in ms)");
69  defineOptionalProperty<std::string>(
70  "CollisionPairs",
71  "[]",
72  "A list of tripels including two SceneObjects or SceneObject lists and a distance "
73  "from which to warn. Example: [{Armar3:(Ring L J0,Ring L J1),Armar3:(Ring R "
74  "J0,Ring R J1),2.0},{Armar3:Index L J0,tableb,3.0}]");
75  defineOptionalProperty<std::string>(
76  "DistanceListenerTopicName",
77  "DistanceListener",
78  "The topic name to get information about the distances.");
79  defineOptionalProperty<std::string>(
80  "CollisionListenerTopicName",
81  "CollisionListener",
82  "The topic name to get information about collisions.");
83  defineOptionalProperty<bool>(
84  "UseDebugDrawer",
85  "false",
86  "If true, the distances are printet using the debug drawer.");
87  defineOptionalProperty<std::string>("DebugDrawerTopicName",
88  "DebugDrawerUpdates",
89  "The name of the debug drawer topic.");
90  }
91  };
92 
93  /**
94  * @defgroup Component-CollisionCheckerComponent CollisionCheckerComponent
95  * @ingroup RobotComponents-Components
96  * A description of the component CollisionCheckerComponent.
97  *
98  * @class CollisionCheckerComponent
99  * @ingroup Component-CollisionCheckerComponent
100  * @brief Brief description of class CollisionCheckerComponent.
101  *
102  * Detailed description of class CollisionCheckerComponent.
103  */
105  virtual public armarx::Component,
106  virtual public armarx::CollisionCheckerInterface
107  {
108  private:
109  typedef struct
110  {
111  VirtualRobot::SceneObjectSetPtr objects1;
112  std::string robotName1;
113  std::vector<std::string> nodeNames1;
114  bool usesNodeSet1;
115  std::string nodeSetName1;
116  VirtualRobot::SceneObjectSetPtr objects2;
117  std::string robotName2;
118  std::vector<std::string> nodeNames2;
119  bool usesNodeSet2;
120  std::string nodeSetName2;
121  double warningDistance;
122  } SceneObjectPair;
123 
124  typedef struct
125  {
127  RobotStateComponentInterfacePrx robotStateComponentPrx;
128  } RobotPair;
129 
130  public:
132 
133  /**
134  * @see armarx::ManagedIceObject::getDefaultName()
135  */
136  std::string
137  getDefaultName() const override
138  {
139  return "CollisionCheckerComponent";
140  }
141 
142  void addCollisionPair(const std::string& robotName1,
143  const std::vector<std::string>& nodeNames1,
144  const std::string& robotName2,
145  const std::vector<std::string>& nodeNames2,
146  double warningDistance,
147  const Ice::Current& = Ice::emptyCurrent) override;
148 
149  void removeCollisionPair(const std::string& robotName1,
150  const std::vector<std::string>& nodeNames1,
151  const std::string& robotName2,
152  const std::vector<std::string>& nodeNames2,
153  const Ice::Current& = Ice::emptyCurrent) override;
154 
155  bool hasCollisionPair(const std::string& robotName1,
156  const std::vector<std::string>& nodeNames1,
157  const std::string& robotName2,
158  const std::vector<std::string>& nodeNames2,
159  const Ice::Current& = Ice::emptyCurrent) const override;
160 
161  void setWarningDistance(const std::string& robotName1,
162  const std::vector<std::string>& nodeNames1,
163  const std::string& robotName2,
164  const std::vector<std::string>& nodeNames2,
165  double warningDistance,
166  const Ice::Current& = Ice::emptyCurrent) override;
167  double getWarningDistance(const std::string& robotName1,
168  const std::vector<std::string>& nodeNames1,
169  const std::string& robotName2,
170  const std::vector<std::string>& nodeNames2,
171  const Ice::Current& = Ice::emptyCurrent) const override;
172 
173  CollisionPairList
174  getAllCollisionPairs(const Ice::Current& = Ice::emptyCurrent) const override;
175 
176  int getInterval(const Ice::Current& = Ice::emptyCurrent) const override;
177  void setInterval(int interval, const Ice::Current& = Ice::emptyCurrent) override;
178 
179  protected:
180  /**
181  * @see armarx::ManagedIceObject::onInitComponent()
182  */
183  void onInitComponent() override;
184 
185  /**
186  * @see armarx::ManagedIceObject::onConnectComponent()
187  */
188  void onConnectComponent() override;
189 
190  /**
191  * @see armarx::ManagedIceObject::onDisconnectComponent()
192  */
193  void onDisconnectComponent() override;
194 
195  /**
196  * @see armarx::ManagedIceObject::onExitComponent()
197  */
198  void onExitComponent() override;
199 
200  /**
201  * @see PropertyUser::createPropertyDefinitions()
202  */
204 
205  private:
206  bool resolveCollisionPair(SceneObjectPair& pair);
207  VirtualRobot::SceneObjectPtr getSceneObject(const std::string& robotName,
208  const std::string& nodeName);
209  bool parseNodeSet(const std::string& setAsString,
210  std::string& robotName,
211  std::vector<std::string>& nodeNames,
212  bool& usesNodeSet,
213  std::string& nodeSetName);
214  void reportDistancesAndCollisions();
215  VirtualRobot::SceneObjectPtr getSceneObjectFromWorkingMemory(const std::string& name);
216  void synchronizeObjectsFromWorkingMemory();
217 
218  virtual void addCollisionPair(const std::string& robotName1,
219  const std::vector<std::string>& nodeNames1,
220  const bool usesNodeSet1,
221  const std::string& nodeSetName1,
222  const std::string& robotName2,
223  const std::vector<std::string>& nodeNames2,
224  const bool usesNodeSet2,
225  const std::string& nodeSetName2,
226  double warningDistance);
227 
228  void reportEntityCreated(const std::string& segmentName,
229  const ::memoryx::EntityBasePtr& entity,
230  const ::Ice::Current& = Ice::emptyCurrent) override;
231  void reportEntityUpdated(const std::string& segmentName,
232  const ::memoryx::EntityBasePtr& entityOld,
233  const ::memoryx::EntityBasePtr& entityNew,
234  const ::Ice::Current& = Ice::emptyCurrent) override;
235  void reportEntityRemoved(const std::string& segmentName,
236  const ::memoryx::EntityBasePtr& entity,
237  const ::Ice::Current& = Ice::emptyCurrent) override;
238  void reportSnapshotLoaded(const std::string& segmentName,
239  const ::Ice::Current& = Ice::emptyCurrent) override;
240  void reportSnapshotCompletelyLoaded(const Ice::Current& c = Ice::emptyCurrent) override;
241  void reportMemoryCleared(const std::string& segmentName,
242  const ::Ice::Current& = Ice::emptyCurrent) override;
243 
244  bool useWorkingMemory;
245 
246  std::string workingMemoryName;
247  memoryx::WorkingMemoryInterfacePrx workingMemoryPrx;
248  memoryx::ObjectInstanceMemorySegmentBasePrx objectInstancesPrx;
249  memoryx::AgentInstancesSegmentBasePrx agentInstancesPrx;
250  memoryx::GridFileManagerPtr fileManager;
251  std::vector<VirtualRobot::SceneObjectPtr> workingMemorySceneObjects;
252  std::map<std::string, armarx::PosePtr> currentPositions;
253  std::mutex wmPositionMutex;
254 
255  std::string robotStateComponentName;
256 
257  std::vector<RobotPair> robots;
258 
259  std::vector<SceneObjectPair> sceneObjectPairs;
260 
261  VirtualRobot::CollisionCheckerPtr collisionChecker;
262 
263  CollisionListenerPrx collisionListenerPrx;
264  DistanceListenerPrx distanceListenerPrx;
265 
266  int interval;
268  mutable std::mutex dataMutex;
269 
270  bool connected;
271  std::shared_mutex connectedMutex;
272 
273  DebugDrawerInterfacePrx debugDrawerTopicPrx;
274  bool useDebugDrawer;
275  };
276 } // namespace armarx
armarx::CollisionCheckerComponent::setWarningDistance
void setWarningDistance(const std::string &robotName1, const std::vector< std::string > &nodeNames1, const std::string &robotName2, const std::vector< std::string > &nodeNames2, double warningDistance, const Ice::Current &=Ice::emptyCurrent) override
Definition: CollisionCheckerComponent.cpp:746
armarx::CollisionCheckerComponent::onInitComponent
void onInitComponent() override
Definition: CollisionCheckerComponent.cpp:57
armarx::CollisionCheckerComponent::getWarningDistance
double getWarningDistance(const std::string &robotName1, const std::vector< std::string > &nodeNames1, const std::string &robotName2, const std::vector< std::string > &nodeNames2, const Ice::Current &=Ice::emptyCurrent) const override
Definition: CollisionCheckerComponent.cpp:768
Pose.h
armarx::CollisionCheckerComponent::getDefaultName
std::string getDefaultName() const override
Definition: CollisionCheckerComponent.h:137
armarx::PropertyDefinitionContainer::prefix
std::string prefix
Prefix of the properties such as namespace, domain, component name, etc.
Definition: PropertyDefinitionContainer.h:345
GridFileManager.h
PeriodicTask.h
c
constexpr T c
Definition: UnscentedKalmanFilterTest.cpp:46
armarx::CollisionCheckerComponent::hasCollisionPair
bool hasCollisionPair(const std::string &robotName1, const std::vector< std::string > &nodeNames1, const std::string &robotName2, const std::vector< std::string > &nodeNames2, const Ice::Current &=Ice::emptyCurrent) const override
Definition: CollisionCheckerComponent.cpp:725
armarx::CollisionCheckerComponentPropertyDefinitions::CollisionCheckerComponentPropertyDefinitions
CollisionCheckerComponentPropertyDefinitions(std::string prefix)
Definition: CollisionCheckerComponent.h:52
armarx::CollisionCheckerComponent::onExitComponent
void onExitComponent() override
Definition: CollisionCheckerComponent.cpp:280
armarx::CollisionCheckerComponent::CollisionCheckerComponent
CollisionCheckerComponent()
Definition: CollisionCheckerComponent.cpp:52
armarx::CollisionCheckerComponent::getInterval
int getInterval(const Ice::Current &=Ice::emptyCurrent) const override
Definition: CollisionCheckerComponent.cpp:806
armarx::CollisionCheckerComponent::createPropertyDefinitions
armarx::PropertyDefinitionsPtr createPropertyDefinitions() override
Definition: CollisionCheckerComponent.cpp:285
armarx::CollisionCheckerComponent::addCollisionPair
void addCollisionPair(const std::string &robotName1, const std::vector< std::string > &nodeNames1, const std::string &robotName2, const std::vector< std::string > &nodeNames2, double warningDistance, const Ice::Current &=Ice::emptyCurrent) override
Definition: CollisionCheckerComponent.cpp:661
armarx::CollisionCheckerComponent::setInterval
void setInterval(int interval, const Ice::Current &=Ice::emptyCurrent) override
Definition: CollisionCheckerComponent.cpp:812
armarx::CollisionCheckerComponentPropertyDefinitions
Definition: CollisionCheckerComponent.h:49
Component.h
armarx::CollisionCheckerComponent::getAllCollisionPairs
CollisionPairList getAllCollisionPairs(const Ice::Current &=Ice::emptyCurrent) const override
Definition: CollisionCheckerComponent.cpp:789
armarx::Component
Baseclass for all ArmarX ManagedIceObjects requiring properties.
Definition: Component.h:91
armarx::CollisionCheckerComponent
Brief description of class CollisionCheckerComponent.
Definition: CollisionCheckerComponent.h:104
armarx::CollisionCheckerComponent::onDisconnectComponent
void onDisconnectComponent() override
Definition: CollisionCheckerComponent.cpp:256
armarx::ComponentPropertyDefinitions
Default component property definition container.
Definition: Component.h:69
memoryx::GridFileManagerPtr
std::shared_ptr< GridFileManager > GridFileManagerPtr
Definition: AbstractEntityWrapper.h:33
IceUtil::Handle< class PropertyDefinitionContainer >
armarx::CollisionCheckerComponent::removeCollisionPair
void removeCollisionPair(const std::string &robotName1, const std::vector< std::string > &nodeNames1, const std::string &robotName2, const std::vector< std::string > &nodeNames2, const Ice::Current &=Ice::emptyCurrent) override
Definition: CollisionCheckerComponent.cpp:704
IceInternal::ProxyHandle<::IceProxy::armarx::RobotStateComponentInterface >
scene3D::SceneObjectPtr
boost::intrusive_ptr< SceneObject > SceneObjectPtr
Definition: PointerDefinitions.h:40
armarx::CollisionCheckerComponent::onConnectComponent
void onConnectComponent() override
Definition: CollisionCheckerComponent.cpp:160
armarx
This file offers overloads of toIce() and fromIce() functions for STL container types.
Definition: ArmarXTimeserver.cpp:27
VirtualRobot::RobotPtr
std::shared_ptr< class Robot > RobotPtr
Definition: Bus.h:19