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)