Add ability to move CollisionObjects (#567)

This commit is contained in:
Captain Yoshi 2024-05-12 05:03:47 -04:00 committed by GitHub
parent 819e560ed4
commit ad19ea5479
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
2 changed files with 14 additions and 1 deletions

View File

@ -81,6 +81,8 @@ public:
void addObject(const moveit_msgs::CollisionObject& collision_object);
/// Remove an object from the planning scene
void removeObject(const std::string& object_name);
/// Move an object from the planning scene
void moveObject(const moveit_msgs::CollisionObject& collision_object);
/// conviency methods accepting a single object name
inline void attachObject(const std::string& object, const std::string& link);

View File

@ -72,6 +72,15 @@ void ModifyPlanningScene::removeObject(const std::string& object_name) {
collision_objects_.push_back(obj);
}
void ModifyPlanningScene::moveObject(const moveit_msgs::CollisionObject& collision_object) {
if (collision_object.operation != moveit_msgs::CollisionObject::MOVE) {
ROS_ERROR_STREAM_NAMED("ModifyPlanningScene", name() << ": moveObject is called with object's operation not set "
"to MOVE -- ignoring the object");
return;
}
collision_objects_.push_back(collision_object);
}
void ModifyPlanningScene::allowCollisions(const Names& first, const Names& second, bool allow) {
collision_matrix_edits_.push_back(CollisionMatrixPairs({ first, second, allow }));
}
@ -128,7 +137,7 @@ std::pair<InterfaceState, SubTrajectory> ModifyPlanningScene::apply(const Interf
InterfaceState state(scene);
SubTrajectory traj;
try {
// add/remove objects
// add/remove/move objects
for (auto& collision_object : collision_objects_)
processCollisionObject(*scene, collision_object, invert);
@ -157,6 +166,8 @@ void ModifyPlanningScene::processCollisionObject(planning_scene::PlanningScene&
const_cast<moveit_msgs::CollisionObject&>(object).operation = moveit_msgs::CollisionObject::REMOVE;
else if (op == moveit_msgs::CollisionObject::REMOVE)
throw std::runtime_error("cannot apply removeObject() backwards");
else if (op == moveit_msgs::CollisionObject::MOVE)
throw std::runtime_error("cannot apply moveObject() backwards");
}
scene.processCollisionObjectMsg(object);