def execute(self, userdata):
"""
Get the modified ACM and publish it in order to update the allowed collisions when moveit plans
@param userdata: Input and output data that can be communicated to other states
@return: - outcomes[-1] ("fail" by default) if an error occurs when modifying the ACM
- outcomes[0] otherwise
"""
# Sanity check during execution
if self.modification_type is None:
rospy.logerr(
"The parameter collision_type can only be {'', 'self-collision', 'object-collision'}"
)
return self.outcomes[-1]
# Call the service allowing to modify an updated ACM with all the interactively added objects
response = self.get_modified_acm(self.modification_type, self.objects,
self.allow_collision)
# If anything went wrong on the service side display an error message and fail the state
if not response.success:
rospy.logerr(
"Could not allow collisions with the provided parameters")
return self.outcomes[-1]
# If everything works fine, we just set the output ACM in a PlanningScene message and send it to Moveit
planning_scene_diff = PlanningScene()
planning_scene_diff.is_diff = True
try:
planning_scene_diff.allowed_collision_matrix = response.acm
self.planning_scene_publisher.publish(planning_scene_diff)
return self.outcomes[0]
except rospy.ROSException as exception:
rospy.logerr(
"Can not change collision state of the scene: {}".format(
exception))
return self.outcomes[-1]
def send_meshes_cb(self, *args):
planning_scene_msg = PlanningScene()
planning_scene_msg.is_diff = self.moveit_planning_scene_msg.is_diff
planning_scene_msg.allowed_collision_matrix = self.moveit_planning_scene_msg.allowed_collision_matrix
planning_scene_msg.name = self.moveit_planning_scene_msg.name
for collision_object in self.moveit_planning_scene_msg.world.collision_objects:
mesh_id = collision_object.id
if self.mesh_list_vars[mesh_id].get() == '1':
planning_scene_msg.world.collision_objects.append(collision_object)
self.refined_planning_scene_publisher.publish(planning_scene_msg)
def remove_object_from_acm(self, object_name):
req = PlanningSceneComponents(
components=(PlanningSceneComponents.ALLOWED_COLLISION_MATRIX
| PlanningSceneComponents.SCENE_SETTINGS))
res = self.get_planning_scene(req)
acm = res.scene.allowed_collision_matrix
planning_scene_diff = PlanningScene()
planning_scene_diff.is_diff = True
acm = remove_from_acm(acm, object_name)
planning_scene_diff.allowed_collision_matrix = acm
self.planning_scene_pub.publish(planning_scene_diff)
def add_object_to_acm(self, object_name):
req = PlanningSceneComponents(
components=(PlanningSceneComponents.ALLOWED_COLLISION_MATRIX
| PlanningSceneComponents.SCENE_SETTINGS))
res = self.get_planning_scene(req)
acm = res.scene.allowed_collision_matrix
planning_scene_diff = PlanningScene(is_diff=True)
acm = add_to_acm(acm, object_name)
planning_scene_diff.allowed_collision_matrix = acm
self.planning_scene_pub.publish(planning_scene_diff)
rospy.sleep(2)
def allow_all(self):
request = PlanningSceneComponents(
components=(PlanningSceneComponents.ALLOWED_COLLISION_MATRIX
| PlanningSceneComponents.SCENE_SETTINGS))
response = self.get_planning_scene(request)
acm = response.scene.allowed_collision_matrix
planning_scene_diff = PlanningScene()
planning_scene_diff.is_diff = True
for object_name in self.object_names:
rospy.logwarn('Adding object {} to allowed collisions...'.format(
object_name))
# acm = acm_allow_grasping(acm, object_name, allow=True)
acm = add_to_acm(acm, object_name)
planning_scene_diff.allowed_collision_matrix = acm
self.planning_scene_publisher.publish(planning_scene_diff)
def remove_object_handler(self, req):
request = PlanningSceneComponents(
components=(PlanningSceneComponents.ALLOWED_COLLISION_MATRIX
| PlanningSceneComponents.SCENE_SETTINGS))
response = self.get_planning_scene(request)
acm = response.scene.allowed_collision_matrix
planning_scene_diff = PlanningScene()
planning_scene_diff.is_diff = True
if not '__link_0' in req.object_name:
req.object_name += '__link_0'
acm = remove_from_acm(acm, req.object_name)
planning_scene_diff.allowed_collision_matrix = acm
self.planning_scene_publisher.publish(planning_scene_diff)
return ChangeCollisionObjectResponse(True)
