28 #include <VirtualRobot/Robot.h>
29 #include <VirtualRobot/SceneObject.h>
34 #include <RobotAPI/interface/core/RobotState.h>
35 #include <RobotAPI/interface/visualization/DebugDrawerInterface.h>
38 #include <RobotComponents/interface/components/CollisionCheckerInterface.h>
41 #include <MemoryX/interface/components/WorkingMemoryInterface.h>
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>(
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",
78 "The topic name to get information about the distances.");
79 defineOptionalProperty<std::string>(
80 "CollisionListenerTopicName",
82 "The topic name to get information about collisions.");
83 defineOptionalProperty<bool>(
86 "If true, the distances are printet using the debug drawer.");
87 defineOptionalProperty<std::string>(
"DebugDrawerTopicName",
89 "The name of the debug drawer topic.");
106 virtual public armarx::CollisionCheckerInterface
111 VirtualRobot::SceneObjectSetPtr objects1;
112 std::string robotName1;
113 std::vector<std::string> nodeNames1;
115 std::string nodeSetName1;
116 VirtualRobot::SceneObjectSetPtr objects2;
117 std::string robotName2;
118 std::vector<std::string> nodeNames2;
120 std::string nodeSetName2;
121 double warningDistance;
139 return "CollisionCheckerComponent";
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;
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;
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;
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;
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;
176 int getInterval(
const Ice::Current& = Ice::emptyCurrent)
const override;
177 void setInterval(
int interval,
const Ice::Current& = Ice::emptyCurrent)
override;
206 bool resolveCollisionPair(SceneObjectPair& pair);
208 const std::string& nodeName);
209 bool parseNodeSet(
const std::string& setAsString,
210 std::string& robotName,
211 std::vector<std::string>& nodeNames,
213 std::string& nodeSetName);
214 void reportDistancesAndCollisions();
216 void synchronizeObjectsFromWorkingMemory();
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);
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;
244 bool useWorkingMemory;
246 std::string workingMemoryName;
247 memoryx::WorkingMemoryInterfacePrx workingMemoryPrx;
248 memoryx::ObjectInstanceMemorySegmentBasePrx objectInstancesPrx;
249 memoryx::AgentInstancesSegmentBasePrx agentInstancesPrx;
251 std::vector<VirtualRobot::SceneObjectPtr> workingMemorySceneObjects;
252 std::map<std::string, armarx::PosePtr> currentPositions;
253 std::mutex wmPositionMutex;
255 std::string robotStateComponentName;
257 std::vector<RobotPair> robots;
259 std::vector<SceneObjectPair> sceneObjectPairs;
261 VirtualRobot::CollisionCheckerPtr collisionChecker;
263 CollisionListenerPrx collisionListenerPrx;
264 DistanceListenerPrx distanceListenerPrx;
268 mutable std::mutex dataMutex;
271 std::shared_mutex connectedMutex;