mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
rename enableCollision -> allowCollision
This commit is contained in:
parent
fee7352961
commit
f2c0c7bf90
@ -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));
|
||||
}
|
||||
|
||||
|
||||
@ -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);
|
||||
};
|
||||
|
||||
|
||||
|
||||
@ -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());
|
||||
|
||||
Loading…
Reference in New Issue
Block a user