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->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));
|
t.add(std::move(move));
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -86,27 +86,27 @@ public:
|
|||||||
attachObjects(Names(objects.cbegin(), objects.cend()), link, false);
|
attachObjects(Names(objects.cbegin(), objects.cend()), link, false);
|
||||||
}
|
}
|
||||||
|
|
||||||
/// enable / disable collisions for each combination of pairs in first and second lists
|
/// allow / forbid collisions for each combination of pairs in first and second lists
|
||||||
void enableCollisions(const Names& first, const Names& second, bool enable_collision);
|
void allowCollisions(const Names& first, const Names& second, bool allow);
|
||||||
/// enable / disable all collisions for given object
|
/// allow / forbid all collisions for given object
|
||||||
void enableCollisions(const std::string& object, bool enable_collision) {
|
void allowCollisions(const std::string& object, bool allow) {
|
||||||
enableCollisions(Names({object}), Names(), enable_collision);
|
allowCollisions(Names({object}), Names(), allow);
|
||||||
}
|
}
|
||||||
|
|
||||||
/// conveniency method accepting arbitrary container types
|
/// conveniency method accepting arbitrary container types
|
||||||
template <typename T1, typename T2,
|
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 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>>
|
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) {
|
inline void allowCollisions(const T1& first, const T2& second, bool enable_collision) {
|
||||||
enableCollisions(Names(first.cbegin(), first.cend()), Names(second.cbegin(), second.cend()), 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
|
/// 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>>
|
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) {
|
inline void allowCollisions(const std::string& first, const T& second, bool enable_collision) {
|
||||||
enableCollisions(Names({first}), Names(second.cbegin(), second.cend()), enable_collision);
|
allowCollisions(Names({first}), Names(second.cbegin(), second.cend()), enable_collision);
|
||||||
}
|
}
|
||||||
/// conveniency method accepting std::string and JointModelGroup
|
/// 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:
|
protected:
|
||||||
// list of objects to attach (true) / detach (false) to a given link
|
// list of objects to attach (true) / detach (false) to a given link
|
||||||
@ -116,7 +116,7 @@ protected:
|
|||||||
struct CollisionMatrixPairs {
|
struct CollisionMatrixPairs {
|
||||||
Names first;
|
Names first;
|
||||||
Names second;
|
Names second;
|
||||||
bool enable;
|
bool allow;
|
||||||
};
|
};
|
||||||
std::list<CollisionMatrixPairs> collision_matrix_edits_;
|
std::list<CollisionMatrixPairs> collision_matrix_edits_;
|
||||||
ApplyCallback callback_;
|
ApplyCallback callback_;
|
||||||
@ -125,7 +125,7 @@ protected:
|
|||||||
// apply stored modifications to scene
|
// apply stored modifications to scene
|
||||||
InterfaceState apply(const InterfaceState &from, bool invert);
|
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 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());
|
o.insert(o.end(), objects.begin(), objects.end());
|
||||||
}
|
}
|
||||||
|
|
||||||
void ModifyPlanningScene::enableCollisions(const Names& first, const Names& second, bool enable_collision) {
|
void ModifyPlanningScene::allowCollisions(const Names& first, const Names& second, bool allow) {
|
||||||
collision_matrix_edits_.push_back(CollisionMatrixPairs({first, second, enable_collision}));
|
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();
|
const auto& links = jmg.getLinkModelNamesWithCollisionGeometry();
|
||||||
if (!links.empty())
|
if (!links.empty())
|
||||||
enableCollisions(Names({first}), links, enable_collision);
|
allowCollisions(Names({first}), links, allow);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool ModifyPlanningScene::computeForward(const InterfaceState &from){
|
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,
|
const CollisionMatrixPairs& pairs,
|
||||||
bool invert)
|
bool invert)
|
||||||
{
|
{
|
||||||
collision_detection::AllowedCollisionMatrix& acm = scene.getAllowedCollisionMatrixNonConst();
|
collision_detection::AllowedCollisionMatrix& acm = scene.getAllowedCollisionMatrixNonConst();
|
||||||
bool enable = invert ? !pairs.enable : pairs.enable;
|
bool allow = invert ? !pairs.allow : pairs.allow;
|
||||||
if (pairs.second.empty()) {
|
if (pairs.second.empty()) {
|
||||||
for (const auto &name : pairs.first)
|
for (const auto &name : pairs.first)
|
||||||
acm.setEntry(name, enable);
|
acm.setEntry(name, allow);
|
||||||
} else
|
} 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)
|
// 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)
|
InterfaceState ModifyPlanningScene::apply(const InterfaceState& from, bool invert)
|
||||||
{
|
{
|
||||||
planning_scene::PlanningScenePtr scene = from.scene()->diff();
|
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_)
|
for (const auto &pair : attach_objects_)
|
||||||
attachObjects(*scene, pair, invert);
|
attachObjects(*scene, pair, invert);
|
||||||
|
|
||||||
// enable/disable collisions
|
// allow/forbid collisions
|
||||||
for (const auto &pairs : collision_matrix_edits_)
|
for (const auto &pairs : collision_matrix_edits_)
|
||||||
enableCollisions(*scene, pairs, invert);
|
allowCollisions(*scene, pairs, invert);
|
||||||
|
|
||||||
if (callback_)
|
if (callback_)
|
||||||
callback_(scene, properties());
|
callback_(scene, properties());
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user