Skip to content

Commit 3afe1c2

Browse files
Levi ArmstrongLevi-Armstrong
Levi Armstrong
authored and
Levi-Armstrong
committed
Clean up warnings related to setContactDistanceThreshold
1 parent fdfb1f3 commit 3afe1c2

17 files changed

+233
-121
lines changed

‎tesseract/tesseract_collision/include/tesseract_collision/bullet/bullet_cast_bvh_manager.h

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -115,6 +115,10 @@ class BulletCastBVHManager : public ContinuousContactManager
115115

116116
void setCollisionMarginData(CollisionMarginData collision_margin_data) override;
117117

118+
void setDefaultCollisionMarginData(double default_collision_margin) override;
119+
120+
void setPairCollisionMarginData(const std::string& name1, const std::string& name2, double collision_margin) override;
121+
118122
double getContactDistanceThreshold() const override;
119123

120124
const CollisionMarginData& getCollisionMarginData() const override;
@@ -151,6 +155,9 @@ class BulletCastBVHManager : public ContinuousContactManager
151155

152156
/** @brief Filter collision objects before broadphase check */
153157
TesseractOverlapFilterCallback broadphase_overlap_cb_;
158+
159+
/** @brief This function will update internal data when margin data has changed */
160+
void onCollisionMarginDataChanged();
154161
};
155162
} // namespace tesseract_collision_bullet
156163
} // namespace tesseract_collision

‎tesseract/tesseract_collision/include/tesseract_collision/bullet/bullet_cast_simple_manager.h

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -119,6 +119,10 @@ class BulletCastSimpleManager : public ContinuousContactManager
119119

120120
const CollisionMarginData& getCollisionMarginData() const override;
121121

122+
void setDefaultCollisionMarginData(double default_collision_margin) override;
123+
124+
void setPairCollisionMarginData(const std::string& name1, const std::string& name2, double collision_margin) override;
125+
122126
void setIsContactAllowedFn(IsContactAllowedFn fn) override;
123127

124128
IsContactAllowedFn getIsContactAllowedFn() const override;
@@ -148,6 +152,9 @@ class BulletCastSimpleManager : public ContinuousContactManager
148152
* so it can be used to exit collision checking for compound shapes.
149153
*/
150154
ContactTestData contact_test_data_;
155+
156+
/** @brief This function will update internal data when margin data has changed */
157+
void onCollisionMarginDataChanged();
151158
};
152159

153160
} // namespace tesseract_collision_bullet

‎tesseract/tesseract_collision/include/tesseract_collision/bullet/bullet_discrete_bvh_manager.h

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -104,6 +104,10 @@ class BulletDiscreteBVHManager : public DiscreteContactManager
104104

105105
void setCollisionMarginData(CollisionMarginData collision_margin_data) override;
106106

107+
void setDefaultCollisionMarginData(double default_collision_margin) override;
108+
109+
void setPairCollisionMarginData(const std::string& name1, const std::string& name2, double collision_margin) override;
110+
107111
double getContactDistanceThreshold() const override;
108112

109113
const CollisionMarginData& getCollisionMarginData() const override;
@@ -139,6 +143,9 @@ class BulletDiscreteBVHManager : public DiscreteContactManager
139143

140144
/** @brief Filter collision objects before broadphase check */
141145
TesseractOverlapFilterCallback broadphase_overlap_cb_;
146+
147+
/** @brief This function will update internal data when margin data has changed */
148+
void onCollisionMarginDataChanged();
142149
};
143150

144151
} // namespace tesseract_collision_bullet

‎tesseract/tesseract_collision/include/tesseract_collision/bullet/bullet_discrete_simple_manager.h

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -104,6 +104,10 @@ class BulletDiscreteSimpleManager : public DiscreteContactManager
104104

105105
void setCollisionMarginData(CollisionMarginData collision_margin_data) override;
106106

107+
void setDefaultCollisionMarginData(double default_collision_margin) override;
108+
109+
void setPairCollisionMarginData(const std::string& name1, const std::string& name2, double collision_margin) override;
110+
107111
double getContactDistanceThreshold() const override;
108112

109113
const CollisionMarginData& getCollisionMarginData() const override;
@@ -136,6 +140,9 @@ class BulletDiscreteSimpleManager : public DiscreteContactManager
136140
* so it can be used to exit collision checking for compound shapes.
137141
*/
138142
ContactTestData contact_test_data_;
143+
144+
/** @brief This function will update internal data when margin data has changed */
145+
void onCollisionMarginDataChanged();
139146
};
140147

141148
} // namespace tesseract_collision_bullet

‎tesseract/tesseract_collision/include/tesseract_collision/core/continuous_contact_manager.h

Lines changed: 20 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -215,6 +215,26 @@ class ContinuousContactManager
215215
*/
216216
virtual void setCollisionMarginData(CollisionMarginData collision_margin_data) = 0;
217217

218+
/**
219+
* @brief Set the default collision margin
220+
* @param default_collision_margin New default collision margin
221+
*/
222+
virtual void setDefaultCollisionMarginData(double default_collision_margin) = 0;
223+
224+
/**
225+
* @brief Set the margin for a given contact pair
226+
*
227+
* The order of the object names does not matter, that is handled internal to
228+
* the class.
229+
*
230+
* @param obj1 The first object name. Order doesn't matter
231+
* @param obj2 The Second object name. Order doesn't matter
232+
* @param collision_margin contacts with distance < collision_margin are considered in collision
233+
*/
234+
virtual void setPairCollisionMarginData(const std::string& name1,
235+
const std::string& name2,
236+
double collision_margin) = 0;
237+
218238
/**
219239
* @brief Get the contact distance threshold
220240
* @return The contact distance

‎tesseract/tesseract_collision/include/tesseract_collision/core/discrete_contact_manager.h

Lines changed: 20 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -166,6 +166,26 @@ class DiscreteContactManager
166166
*/
167167
virtual void setCollisionMarginData(CollisionMarginData collision_margin_data) = 0;
168168

169+
/**
170+
* @brief Set the default collision margin
171+
* @param default_collision_margin New default collision margin
172+
*/
173+
virtual void setDefaultCollisionMarginData(double default_collision_margin) = 0;
174+
175+
/**
176+
* @brief Set the margin for a given contact pair
177+
*
178+
* The order of the object names does not matter, that is handled internal to
179+
* the class.
180+
*
181+
* @param obj1 The first object name. Order doesn't matter
182+
* @param obj2 The Second object name. Order doesn't matter
183+
* @param collision_margin contacts with distance < collision_margin are considered in collision
184+
*/
185+
virtual void setPairCollisionMarginData(const std::string& name1,
186+
const std::string& name2,
187+
double collision_margin) = 0;
188+
169189
/**
170190
* @brief Get the contact distance threshold
171191
* @return The contact distance

‎tesseract/tesseract_collision/include/tesseract_collision/core/types.h

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -226,7 +226,7 @@ struct CollisionMarginData
226226
* @brief Set the default collision margin
227227
* @param default_collision_margin New default collision margin
228228
*/
229-
void setDefaultCollisionMarginData(const double& default_collision_margin)
229+
void setDefaultCollisionMarginData(double default_collision_margin)
230230
{
231231
default_collision_margin_ = default_collision_margin;
232232
if (default_collision_margin_ > max_collision_margin_)
@@ -243,7 +243,7 @@ struct CollisionMarginData
243243
* @param obj2 The Second object name. Order doesn't matter
244244
* @param collision_margin contacts with distance < collision_margin are considered in collision
245245
*/
246-
void setPairCollisionMarginData(const std::string& obj1, const std::string& obj2, const double& collision_margin)
246+
void setPairCollisionMarginData(const std::string& obj1, const std::string& obj2, double collision_margin)
247247
{
248248
auto key = tesseract_common::makeOrderedLinkPair(obj1, obj2);
249249
lookup_table_[key] = collision_margin;
@@ -263,7 +263,7 @@ struct CollisionMarginData
263263
* @param obj2 The second object name
264264
* @return A Vector2d[Contact Distance Threshold, Coefficient]
265265
*/
266-
const double& getPairCollisionMarginData(const std::string& obj1, const std::string& obj2) const
266+
double getPairCollisionMarginData(const std::string& obj1, const std::string& obj2) const
267267
{
268268
auto key = tesseract_common::makeOrderedLinkPair(obj1, obj2);
269269
const auto it = lookup_table_.find(key);
@@ -283,7 +283,7 @@ struct CollisionMarginData
283283
*
284284
* @return Max contact distance threshold
285285
*/
286-
const double& getMaxCollisionMargin() const { return max_collision_margin_; }
286+
double getMaxCollisionMargin() const { return max_collision_margin_; }
287287

288288
/**
289289
* @brief Increment all margins by input amount. Useful for inflating or reducing margins

‎tesseract/tesseract_collision/include/tesseract_collision/fcl/fcl_discrete_managers.h

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -104,6 +104,10 @@ class FCLDiscreteBVHManager : public DiscreteContactManager
104104

105105
void setCollisionMarginData(CollisionMarginData collision_margin_data) override;
106106

107+
void setDefaultCollisionMarginData(double default_collision_margin) override;
108+
109+
void setPairCollisionMarginData(const std::string& name1, const std::string& name2, double collision_margin) override;
110+
107111
double getContactDistanceThreshold() const override;
108112

109113
const CollisionMarginData& getCollisionMarginData() const override;
@@ -139,6 +143,9 @@ class FCLDiscreteBVHManager : public DiscreteContactManager
139143

140144
/** @brief This is used to store dynamic collision objects to update */
141145
std::vector<CollisionObjectRawPtr> dynamic_update_;
146+
147+
/** @brief This function will update internal data when margin data has changed */
148+
void onCollisionMarginDataChanged();
142149
};
143150

144151
} // namespace tesseract_collision_fcl

‎tesseract/tesseract_collision/src/bullet/bullet_cast_bvh_manager.cpp

Lines changed: 35 additions & 33 deletions
Original file line numberDiff line numberDiff line change
@@ -411,45 +411,26 @@ const std::vector<std::string>& BulletCastBVHManager::getActiveCollisionObjects(
411411
void BulletCastBVHManager::setCollisionMarginData(CollisionMarginData collision_margin_data)
412412
{
413413
contact_test_data_.collision_margin_data = collision_margin_data;
414+
onCollisionMarginDataChanged();
415+
}
414416

415-
for (auto& co : link2cow_)
416-
{
417-
COW::Ptr& cow = co.second;
418-
cow->setContactProcessingThreshold(
419-
static_cast<btScalar>(contact_test_data_.collision_margin_data.getMaxCollisionMargin()));
420-
if (cow->getBroadphaseHandle())
421-
updateBroadphaseAABB(cow, broadphase_, dispatcher_);
422-
}
417+
void BulletCastBVHManager::setDefaultCollisionMarginData(double default_collision_margin)
418+
{
419+
contact_test_data_.collision_margin_data.setDefaultCollisionMarginData(default_collision_margin);
420+
onCollisionMarginDataChanged();
421+
}
423422

424-
for (auto& co : link2castcow_)
425-
{
426-
COW::Ptr& cow = co.second;
427-
cow->setContactProcessingThreshold(
428-
static_cast<btScalar>(contact_test_data_.collision_margin_data.getMaxCollisionMargin()));
429-
if (cow->getBroadphaseHandle())
430-
updateBroadphaseAABB(cow, broadphase_, dispatcher_);
431-
}
423+
void BulletCastBVHManager::setPairCollisionMarginData(const std::string& name1,
424+
const std::string& name2,
425+
double collision_margin)
426+
{
427+
contact_test_data_.collision_margin_data.setPairCollisionMarginData(name1, name2, collision_margin);
428+
onCollisionMarginDataChanged();
432429
}
433430

434431
void BulletCastBVHManager::setContactDistanceThreshold(double contact_distance)
435432
{
436-
contact_test_data_.collision_margin_data = CollisionMarginData(contact_distance);
437-
438-
for (auto& co : link2cow_)
439-
{
440-
COW::Ptr& cow = co.second;
441-
cow->setContactProcessingThreshold(static_cast<btScalar>(contact_distance));
442-
if (cow->getBroadphaseHandle())
443-
updateBroadphaseAABB(cow, broadphase_, dispatcher_);
444-
}
445-
446-
for (auto& co : link2castcow_)
447-
{
448-
COW::Ptr& cow = co.second;
449-
cow->setContactProcessingThreshold(static_cast<btScalar>(contact_distance));
450-
if (cow->getBroadphaseHandle())
451-
updateBroadphaseAABB(cow, broadphase_, dispatcher_);
452-
}
433+
setDefaultCollisionMarginData(contact_distance);
453434
}
454435

455436
double BulletCastBVHManager::getContactDistanceThreshold() const
@@ -507,5 +488,26 @@ void BulletCastBVHManager::addCollisionObject(COW::Ptr cow)
507488
selected_cow->m_collisionFilterMask,
508489
dispatcher_.get()));
509490
}
491+
492+
void BulletCastBVHManager::onCollisionMarginDataChanged()
493+
{
494+
btScalar margin = static_cast<btScalar>(contact_test_data_.collision_margin_data.getMaxCollisionMargin());
495+
for (auto& co : link2cow_)
496+
{
497+
COW::Ptr& cow = co.second;
498+
cow->setContactProcessingThreshold(margin);
499+
if (cow->getBroadphaseHandle())
500+
updateBroadphaseAABB(cow, broadphase_, dispatcher_);
501+
}
502+
503+
for (auto& co : link2castcow_)
504+
{
505+
COW::Ptr& cow = co.second;
506+
cow->setContactProcessingThreshold(margin);
507+
if (cow->getBroadphaseHandle())
508+
updateBroadphaseAABB(cow, broadphase_, dispatcher_);
509+
}
510+
}
511+
510512
} // namespace tesseract_collision_bullet
511513
} // namespace tesseract_collision

‎tesseract/tesseract_collision/src/bullet/bullet_cast_simple_manager.cpp

Lines changed: 25 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -329,25 +329,26 @@ const std::vector<std::string>& BulletCastSimpleManager::getActiveCollisionObjec
329329
void BulletCastSimpleManager::setCollisionMarginData(CollisionMarginData collision_margin_data)
330330
{
331331
contact_test_data_.collision_margin_data = collision_margin_data;
332+
onCollisionMarginDataChanged();
333+
}
332334

333-
for (auto& co : link2cow_)
334-
co.second->setContactProcessingThreshold(
335-
static_cast<btScalar>(contact_test_data_.collision_margin_data.getMaxCollisionMargin()));
335+
void BulletCastSimpleManager::setDefaultCollisionMarginData(double default_collision_margin)
336+
{
337+
contact_test_data_.collision_margin_data.setDefaultCollisionMarginData(default_collision_margin);
338+
onCollisionMarginDataChanged();
339+
}
336340

337-
for (auto& co : link2castcow_)
338-
co.second->setContactProcessingThreshold(
339-
static_cast<btScalar>(contact_test_data_.collision_margin_data.getMaxCollisionMargin()));
341+
void BulletCastSimpleManager::setPairCollisionMarginData(const std::string& name1,
342+
const std::string& name2,
343+
double collision_margin)
344+
{
345+
contact_test_data_.collision_margin_data.setPairCollisionMarginData(name1, name2, collision_margin);
346+
onCollisionMarginDataChanged();
340347
}
341348

342349
void BulletCastSimpleManager::setContactDistanceThreshold(double contact_distance)
343350
{
344-
contact_test_data_.collision_margin_data = CollisionMarginData(contact_distance);
345-
346-
for (auto& co : link2cow_)
347-
co.second->setContactProcessingThreshold(static_cast<btScalar>(contact_distance));
348-
349-
for (auto& co : link2castcow_)
350-
co.second->setContactProcessingThreshold(static_cast<btScalar>(contact_distance));
351+
setDefaultCollisionMarginData(contact_distance);
351352
}
352353

353354
double BulletCastSimpleManager::getContactDistanceThreshold() const
@@ -446,5 +447,16 @@ void BulletCastSimpleManager::addCollisionObject(COW::Ptr cow)
446447
else
447448
cows_.push_back(cow);
448449
}
450+
451+
void BulletCastSimpleManager::onCollisionMarginDataChanged()
452+
{
453+
btScalar margin = static_cast<btScalar>(contact_test_data_.collision_margin_data.getMaxCollisionMargin());
454+
for (auto& co : link2cow_)
455+
co.second->setContactProcessingThreshold(margin);
456+
457+
for (auto& co : link2castcow_)
458+
co.second->setContactProcessingThreshold(margin);
459+
}
460+
449461
} // namespace tesseract_collision_bullet
450462
} // namespace tesseract_collision

0 commit comments

Comments
 (0)