rename enableCollision -> allowCollision

This commit is contained in:
Robert Haschke 2018-04-08 18:36:57 +02:00
parent fee7352961
commit f2c0c7bf90
3 changed files with 25 additions and 25 deletions

View File

@ -115,10 +115,10 @@ int main(int argc, char** argv){
}
{
auto move = std::make_unique<stages::ModifyPlanningScene>("enable object collision");
auto move = std::make_unique<stages::ModifyPlanningScene>("allow object collision");
move->restrictDirection(stages::ModifyPlanningScene::FORWARD);
move->enableCollisions("object", t.getRobotModel()->getJointModelGroup("left_hand")->getLinkModelNamesWithCollisionGeometry(), true);
move->allowCollisions("object", t.getRobotModel()->getJointModelGroup("left_hand")->getLinkModelNamesWithCollisionGeometry(), true);
t.add(std::move(move));
}

View File

@ -86,27 +86,27 @@ public:
attachObjects(Names(objects.cbegin(), objects.cend()), link, false);
}
/// enable / disable collisions for each combination of pairs in first and second lists
void enableCollisions(const Names& first, const Names& second, bool enable_collision);
/// enable / disable all collisions for given object
void enableCollisions(const std::string& object, bool enable_collision) {
enableCollisions(Names({object}), Names(), enable_collision);
/// allow / forbid collisions for each combination of pairs in first and second lists
void allowCollisions(const Names& first, const Names& second, bool allow);
/// allow / forbid all collisions for given object
void allowCollisions(const std::string& object, bool allow) {
allowCollisions(Names({object}), Names(), allow);
}
/// conveniency method accepting arbitrary container types
template <typename T1, typename T2,
typename E1 = typename std::enable_if_t<is_container<T1>::value && std::is_base_of<std::string, typename T1::value_type>::value>,
typename E2 = typename std::enable_if_t<is_container<T2>::value && std::is_base_of<std::string, typename T2::value_type>::value>>
inline void enableCollisions(const T1& first, const T2& second, bool enable_collision) {
enableCollisions(Names(first.cbegin(), first.cend()), Names(second.cbegin(), second.cend()), enable_collision);
inline void allowCollisions(const T1& first, const T2& second, bool enable_collision) {
allowCollisions(Names(first.cbegin(), first.cend()), Names(second.cbegin(), second.cend()), enable_collision);
}
/// conveniency method accepting std::string and an arbitrary container of names
template <typename T, typename E = typename std::enable_if_t<is_container<T>::value && std::is_base_of<std::string, typename T::value_type>::value>>
inline void enableCollisions(const std::string& first, const T& second, bool enable_collision) {
enableCollisions(Names({first}), Names(second.cbegin(), second.cend()), enable_collision);
inline void allowCollisions(const std::string& first, const T& second, bool enable_collision) {
allowCollisions(Names({first}), Names(second.cbegin(), second.cend()), enable_collision);
}
/// conveniency method accepting std::string and JointModelGroup
void enableCollisions(const std::string& first, const moveit::core::JointModelGroup& jmg, bool enable_collision);
void allowCollisions(const std::string& first, const moveit::core::JointModelGroup& jmg, bool allow);
protected:
// list of objects to attach (true) / detach (false) to a given link
@ -116,7 +116,7 @@ protected:
struct CollisionMatrixPairs {
Names first;
Names second;
bool enable;
bool allow;
};
std::list<CollisionMatrixPairs> collision_matrix_edits_;
ApplyCallback callback_;
@ -125,7 +125,7 @@ protected:
// apply stored modifications to scene
InterfaceState apply(const InterfaceState &from, bool invert);
void attachObjects(planning_scene::PlanningScene &scene, const std::pair<std::string, std::pair<Names, bool> >& pair, bool invert);
void enableCollisions(planning_scene::PlanningScene &scene, const CollisionMatrixPairs& pairs, bool invert);
void allowCollisions(planning_scene::PlanningScene &scene, const CollisionMatrixPairs& pairs, bool invert);
};

View File

@ -53,15 +53,15 @@ void ModifyPlanningScene::attachObjects(const Names& objects, const std::string&
o.insert(o.end(), objects.begin(), objects.end());
}
void ModifyPlanningScene::enableCollisions(const Names& first, const Names& second, bool enable_collision) {
collision_matrix_edits_.push_back(CollisionMatrixPairs({first, second, enable_collision}));
void ModifyPlanningScene::allowCollisions(const Names& first, const Names& second, bool allow) {
collision_matrix_edits_.push_back(CollisionMatrixPairs({first, second, allow}));
}
void ModifyPlanningScene::enableCollisions(const std::string &first, const moveit::core::JointModelGroup &jmg, bool enable_collision)
void ModifyPlanningScene::allowCollisions(const std::string &first, const moveit::core::JointModelGroup &jmg, bool allow)
{
const auto& links = jmg.getLinkModelNamesWithCollisionGeometry();
if (!links.empty())
enableCollisions(Names({first}), links, enable_collision);
allowCollisions(Names({first}), links, allow);
}
bool ModifyPlanningScene::computeForward(const InterfaceState &from){
@ -91,21 +91,21 @@ void ModifyPlanningScene::attachObjects(planning_scene::PlanningScene &scene,
}
}
void ModifyPlanningScene::enableCollisions(planning_scene::PlanningScene &scene,
void ModifyPlanningScene::allowCollisions(planning_scene::PlanningScene &scene,
const CollisionMatrixPairs& pairs,
bool invert)
{
collision_detection::AllowedCollisionMatrix& acm = scene.getAllowedCollisionMatrixNonConst();
bool enable = invert ? !pairs.enable : pairs.enable;
bool allow = invert ? !pairs.allow : pairs.allow;
if (pairs.second.empty()) {
for (const auto &name : pairs.first)
acm.setEntry(name, enable);
acm.setEntry(name, allow);
} else
acm.setEntry(pairs.first, pairs.second, enable);
acm.setEntry(pairs.first, pairs.second, allow);
}
// invert indicates, whether to detach instead of attach (and vice versa)
// as well as to disable instead of enable collision (and vice versa)
// as well as to forbid instead of allow collision (and vice versa)
InterfaceState ModifyPlanningScene::apply(const InterfaceState& from, bool invert)
{
planning_scene::PlanningScenePtr scene = from.scene()->diff();
@ -115,9 +115,9 @@ InterfaceState ModifyPlanningScene::apply(const InterfaceState& from, bool inver
for (const auto &pair : attach_objects_)
attachObjects(*scene, pair, invert);
// enable/disable collisions
// allow/forbid collisions
for (const auto &pairs : collision_matrix_edits_)
enableCollisions(*scene, pairs, invert);
allowCollisions(*scene, pairs, invert);
if (callback_)
callback_(scene, properties());