def _get_all_collision_objects(self): collision_object_names = [] planning_scene_request = PlanningSceneComponents() planning_scene_request.components = PlanningSceneComponents.WORLD_OBJECT_NAMES planning_scene = self.plannig_scene_service(planning_scene_request) for collision_object in planning_scene.scene.world.collision_objects: collision_object_names.append(collision_object.id) return collision_object_names
def __init__(self, frame, ns='', init_from_service=True): # ns must be a string if not isinstance(ns, basestring): rospy.logerr('Namespace must be a string!') else: ns += '/' self._fixed_frame = frame # publisher to send objects to MoveIt self._pub = rospy.Publisher(ns + 'collision_object', CollisionObject, queue_size=10) self._attached_pub = rospy.Publisher(ns + 'attached_collision_object', AttachedCollisionObject, queue_size=10) self._scene_pub = rospy.Publisher(ns + 'planning_scene', PlanningScene, queue_size=10) # track the attached and collision objects self._mutex = thread.allocate_lock() # these are updated based what the planning scene actually contains self._attached = list() self._collision = list() # these are updated based on internal state self._objects = dict() self._attached_objects = dict() self._removed = dict() self._attached_removed = dict() self._colors = dict() # get the initial planning scene if init_from_service: rospy.loginfo('Waiting for get_planning_scene') rospy.wait_for_service(ns + 'get_planning_scene') self._service = rospy.ServiceProxy(ns + 'get_planning_scene', GetPlanningScene) try: req = PlanningSceneComponents() req.components = sum([ PlanningSceneComponents.WORLD_OBJECT_NAMES, PlanningSceneComponents.WORLD_OBJECT_GEOMETRY, PlanningSceneComponents.ROBOT_STATE_ATTACHED_OBJECTS ]) scene = self._service(req) self.sceneCb(scene.scene, initial=True) except rospy.ServiceException as e: rospy.logerr( 'Failed to get initial planning scene, results may be wonky: %s', e) # subscribe to planning scene rospy.Subscriber(ns + 'move_group/monitored_planning_scene', PlanningScene, self.sceneCb)
def _get_all_collision_objects(self): rospy.wait_for_service('/get_planning_scene', 10.0) plannig_scene_service = rospy.ServiceProxy('get_planning_scene', GetPlanningScene) collision_object_names = [] planning_scene_request = PlanningSceneComponents() planning_scene_request.components = PlanningSceneComponents.WORLD_OBJECT_NAMES planning_scene = plannig_scene_service(planning_scene_request) for collision_object in planning_scene.scene.world.collision_objects: collision_object_names.append(collision_object.id) return collision_object_names
def _get_scene(self): req = PlanningSceneComponents() req.components = sum([ PlanningSceneComponents.WORLD_OBJECT_NAMES, PlanningSceneComponents.WORLD_OBJECT_GEOMETRY, PlanningSceneComponents.ROBOT_STATE_ATTACHED_OBJECTS ]) try: resp = self._get_planning_scene(req) return resp.scene except rospy.ServiceException as e: raise RuntimeError("Failed to get initial planning scene.")
def __init__(self): self.ignored_submodels = [] self.collision_objects = {} self.collision_objects_updated = {} self.ignored_robot_model = [] self.planning_scene_pub = rospy.Publisher('/planning_scene', PlanningScene, queue_size=10) while self.planning_scene_pub.get_num_connections() < 1: rospy.sleep(0.1) timeout = rospy.get_param('~timeout', 0) if timeout <= 0: timeout = None rospy.loginfo( 'Waiting {} for /get_planning_scene service to be advertised.'. format( '{} seconds'.format(timeout) if timeout else 'indefinitely')) rospy.wait_for_service('/get_planning_scene', timeout) rospy.loginfo( '/get_planning_scene service has been advertised, proceeding.') self.get_planning_scene = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene) self.request = PlanningSceneComponents( components=PlanningSceneComponents.WORLD_OBJECT_NAMES) self.ignored_robot_model = rospy.get_param('~ignore_robot_model', '').split(';') rospy.loginfo('Ignoring robot models of: %s' % self.ignored_robot_model)
def _monitor_func(self, evt): try: planning_scene = self._get_octomap( components=PlanningSceneComponents( PlanningSceneComponents.OCTOMAP)) except rospy.ServiceException as e: rospy.logerr("Error fetching planning scene: {}".format(e)) return # Update the octomap and then check the threshold new_octomap = np.array(planning_scene.scene.world.octomap.octomap.data) free_space_num = np.sum( (1 - (1 / (1 + np.exp(new_octomap)))) # Convert Log-Odds to Probabilities <= OctomapMonitor.OCTOMAP_OCCUPIED_PROBABILITY_THRESHOLD) free_space_perc = free_space_num / len(new_octomap) # Update the trace self.update_trace( OctomapMonitor.OCTOMAP_MONITOR_EVENT_NAME, free_space_perc < OctomapMonitor.FREE_PERC_FAULT_THRESHOLD, {'free_space_perc': free_space_perc}) # Update the pointer to the octomap with self._octomap_lock: self._octomap = new_octomap
def remove_object(name="Object"): co = CollisionObject() co.operation = CollisionObject.REMOVE co.id = name co.header.frame_id = "/base_link" co.header.stamp = rospy.Time.now() while scene._pub_co.get_num_connections() == 0: rospy.sleep(0.01) scene._pub_co.publish(co) while True: rospy.sleep(0.1) result, success = get_planning_scene( PlanningSceneComponents( PlanningSceneComponents.WORLD_OBJECT_NAMES + PlanningSceneComponents.WORLD_OBJECT_GEOMETRY ) ) if not success: continue found = False for object in result.scene.world.collision_objects: if object.id == name: found = True if not found: return
def add_shelf(quality=Shelf.SIMPLE): pose = get_shelf_pose() while scene._pub_co.get_num_connections() == 0: rospy.sleep(0.01) if quality == Shelf.SIMPLE: pose.pose.position.z += HEIGHT / 2 scene.add_box(name="shelf", pose=pose, size=(DEPTH, HEIGHT, WIDTH)) elif quality == Shelf.FULL: scene.add_mesh(name="shelf", pose=pose, filename=kiva_pod) elif quality == Shelf.PADDED: for dx, dy in [(1, 1), (1, -1), (-1, -1), (-1, 1), (0, 0)]: mypose = deepcopy(pose) mypose.pose.position.x += dx * PADDING mypose.pose.position.y += dy * PADDING if (dx, dy) == (0, 0): name = "shelf" else: name = "shelf" + str(dx) + str(dy) scene.add_mesh(name=name, pose=mypose, filename=kiva_pod) elif quality in APC_BINS: add_bin(quality) else: rospy.logwarn("Unsupported quality %s" % quality) while True: rospy.sleep(0.1) result, success = get_planning_scene( PlanningSceneComponents( PlanningSceneComponents.WORLD_OBJECT_NAMES + PlanningSceneComponents.WORLD_OBJECT_GEOMETRY)) if not success: continue for object in result.scene.world.collision_objects: if object.id == "shelf": return
def __init__(self, frame, init_from_service=True): self._fixed_frame = frame # publisher to send objects to MoveIt self._pub = rospy.Publisher('collision_object', CollisionObject, queue_size=10) self._attached_pub = rospy.Publisher('attached_collision_object', AttachedCollisionObject, queue_size=10) self._scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) # track the attached and collision objects self._mutex = thread.allocate_lock() # these are updated based what the planning scene actually contains self._attached = list() self._collision = list() # these are updated based on internal state self._objects = dict() self._attached_objects = dict() self._removed = dict() self._attached_removed = dict() self._colors = dict() # get the initial planning scene if init_from_service: rospy.loginfo('Waiting for get_planning_scene') rospy.wait_for_service('get_planning_scene') self._service = rospy.ServiceProxy('get_planning_scene', GetPlanningScene) try: req = PlanningSceneComponents() req.components = sum([ PlanningSceneComponents.WORLD_OBJECT_NAMES, PlanningSceneComponents.WORLD_OBJECT_GEOMETRY, PlanningSceneComponents.ROBOT_STATE_ATTACHED_OBJECTS]) scene = self._service(req) self.sceneCb(scene.scene, initial=True) except rospy.ServiceException as e: rospy.logerr('Failed to get initial planning scene, results may be wonky: %s', e) # subscribe to planning scene rospy.Subscriber('move_group/monitored_planning_scene', PlanningScene, self.sceneCb)
def __init__(self, frame, ns='', init_from_service=True): # ns must be a string if not isinstance(ns, basestring): rospy.logerr('Namespace must be a string!') ns = '' elif not ns.endswith('/'): ns += '/' self._fixed_frame = frame self._scene_pub = rospy.Publisher(ns + 'planning_scene', PlanningScene, queue_size=10) self._apply_service = rospy.ServiceProxy(ns + 'apply_planning_scene', ApplyPlanningScene) # track the attached and collision objects self._mutex = thread.allocate_lock() # these are updated based what the planning scene actually contains self._attached = list() self._collision = list() # these are updated based on internal state self._objects = dict() self._attached_objects = dict() self._removed = dict() self._attached_removed = dict() self._colors = dict() # get the initial planning scene if init_from_service: rospy.loginfo('Waiting for get_planning_scene') rospy.wait_for_service(ns + 'get_planning_scene') self._service = rospy.ServiceProxy(ns + 'get_planning_scene', GetPlanningScene) try: req = PlanningSceneComponents() req.components = sum([ PlanningSceneComponents.WORLD_OBJECT_NAMES, PlanningSceneComponents.WORLD_OBJECT_GEOMETRY, PlanningSceneComponents.ROBOT_STATE_ATTACHED_OBJECTS]) scene = self._service(req) self.sceneCb(scene.scene, initial=True) except rospy.ServiceException as e: rospy.logerr('Failed to get initial planning scene, results may be wonky: %s', e) # subscribe to planning scene rospy.Subscriber(ns + 'move_group/monitored_planning_scene', PlanningScene, self.sceneCb)
def get_acm(self): req = PlanningSceneComponents( components=(PlanningSceneComponents.ALLOWED_COLLISION_MATRIX | PlanningSceneComponents.SCENE_SETTINGS)) res = self.get_planning_scene(req) acm = res.scene.allowed_collision_matrix return acm
def _get_planning_scene(self): """Get planning scene. Returns: PlanningScene: current scene state """ get_planning_scene = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene) get_planning_scene.wait_for_service(timeout=5.0) req = PlanningSceneComponents() req.components = PlanningSceneComponents.WORLD_OBJECT_NAMES try: resp = get_planning_scene(req) return resp.scene except rospy.ServiceException as ex: rospy.logerr('Failed to get initial planning scene: %s', ex) return PlanningScene()
def __init__(self, frame, init_from_service=True): self._fixed_frame = frame self._scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) self._apply_service = rospy.ServiceProxy('apply_planning_scene', ApplyPlanningScene) # track the attached and collision objects self._mutex = thread.allocate_lock() # these are updated based what the planning scene actually contains self._attached = list() self._collision = list() # these are updated based on internal state self._objects = dict() self._attached_objects = dict() self._removed = dict() self._attached_removed = dict() self._colors = dict() # get the initial planning scene if init_from_service: rospy.loginfo('Waiting for get_planning_scene') rospy.wait_for_service('get_planning_scene') self._service = rospy.ServiceProxy('get_planning_scene', GetPlanningScene) try: req = PlanningSceneComponents() req.components = sum([ PlanningSceneComponents.WORLD_OBJECT_NAMES, PlanningSceneComponents.WORLD_OBJECT_GEOMETRY, PlanningSceneComponents.ROBOT_STATE_ATTACHED_OBJECTS ]) scene = self._service(req) self.sceneCb(scene.scene, initial=True) except rospy.ServiceException as e: rospy.logerr( 'Failed to get initial planning scene, results may be wonky: %s', e) # subscribe to planning scene rospy.Subscriber('move_group/monitored_planning_scene', PlanningScene, self.sceneCb)
def check_fingers_collisions(self, enable=True): """ Disables or enables the collisions check between the fingers and the objects / table @param enable: set to True to enable / False to disable @return True on success """ objects = ["cricket_ball__link", "drill__link", "cafe_table__link"] while self.__pub_planning_scene.get_num_connections() < 1: rospy.loginfo( "waiting for someone to subscribe to the /planning_scene") rospy.sleep(0.1) request = PlanningSceneComponents( components=PlanningSceneComponents.ALLOWED_COLLISION_MATRIX) response = self.__get_planning_scene(request) acm = response.scene.allowed_collision_matrix for object_name in objects: if object_name not in acm.entry_names: # add object to allowed collision matrix acm.entry_names += [object_name] for row in range(len(acm.entry_values)): acm.entry_values[row].enabled += [False] new_row = deepcopy(acm.entry_values[0]) acm.entry_values += {new_row} for index_entry_values, entry_values in enumerate(acm.entry_values): if "H1_F" in acm.entry_names[index_entry_values]: for index_value, _ in enumerate(entry_values.enabled): if acm.entry_names[index_value] in objects: if enable: acm.entry_values[index_entry_values].enabled[ index_value] = False else: acm.entry_values[index_entry_values].enabled[ index_value] = True elif acm.entry_names[index_entry_values] in objects: for index_value, _ in enumerate(entry_values.enabled): if "H1_F" in acm.entry_names[index_value]: if enable: acm.entry_values[index_entry_values].enabled[ index_value] = False else: acm.entry_values[index_entry_values].enabled[ index_value] = True planning_scene_diff = PlanningScene(is_diff=True, allowed_collision_matrix=acm) self.__pub_planning_scene.publish(planning_scene_diff) rospy.sleep(1.0) return True
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 on_enter(self, userdata): self.return_code = None self.scene = None # Retrieve the relevant data try : if (self.given_action_topic is None): self.current_action_topic = userdata.action_topic except Exception as e: Logger.logwarn('Failed to set up the action client for %s - invalid user data parameters\n%s' % (self.current_action_topic, str(e))) self.return_code = 'failed' return try: if (self.client is None): self.client = ProxyActionClient({self.current_action_topic: GetPlanningSceneAction}, self.wait_duration) except Exception as e: Logger.logwarn('Failed to set up the action client for %s\n%s' % (self.current_action_topic, str(e))) self.return_code = 'failed' return try: if not self.client.is_available(self.current_action_topic): self.client.setup_action_client(self.current_action_topic, GetPlanningSceneAction, self.wait_duration) if not self.setup_action_client.is_available(self.current_action_topic): Logger.logerr( 'Action client is not available for %s' % (self.current_action_topic)) self.return_code = 'failed' return except Exception as e: Logger.logwarn('Failed to setup the action client for %s\n%s' % (self.name, str(e))) self.return_code = 'failed' try: # Action Initialization action_goal = GetPlanningSceneGoal() action_goal.components = PlanningSceneComponents() action_goal.components.components = self.components if (self.timeout_duration > rospy.Duration(0.0)): self.timeout_target = rospy.Time.now() + self.timeout_duration else: self.timeout_target = None self.client.send_goal(self.current_action_topic, action_goal) except Exception as e: Logger.logwarn('Failed to send action goal for group - %s\n%s' % (self.name, str(e))) self.return_code = 'failed'
def get_body_names_from_planner(self): rospy.wait_for_service(self.planning_scene_topic, 5) components = PlanningSceneComponents( PlanningSceneComponents.WORLD_OBJECT_NAMES + PlanningSceneComponents.TRANSFORMS) ps_request = moveit_msgs.srv.GetPlanningSceneRequest( components=components) ps_response = self.planning_scene_service_proxy.call(ps_request) body_names = [ co.id for co in ps_response.scene.world.collision_objects ] return body_names
def __init__(self): self.ignored_submodels = [] self.collision_objects = {} self.collision_objects_updated = {} self.planning_scene_pub = rospy.Publisher('/planning_scene', PlanningScene, queue_size=10) while self.planning_scene_pub.get_num_connections() < 1: rospy.sleep(0.1) rospy.wait_for_service('/get_planning_scene', 10.0) self.get_planning_scene = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene) self.request = PlanningSceneComponents( components=PlanningSceneComponents.WORLD_OBJECT_NAMES)
def enable_fingers_collisions(self, gripper_prefix, object_names, enable=True): # type: (str, list, float) -> None """ Disables or enables the collisions check between the fingers and a list of objects When closing the gripper, we should disable fingers collisions. If not, MoveIt! will fail its planning. :param gripper_prefix: either lgripper or rgripper :param object_names: a list of objects :param enable: set to True to enable / False to disable :return: None """ while self.planning_scene_publisher.get_num_connections() < 1: rospy.loginfo("Waiting to subscribe to the /planning_scene") rospy.sleep(0.1) request = PlanningSceneComponents( components=PlanningSceneComponents.ALLOWED_COLLISION_MATRIX) response = self.get_planning_scene(request) acm = response.scene.allowed_collision_matrix for name in object_names: if name not in acm.entry_names: # Add object to allowed collision matrix acm.entry_names += [name] for row in range(len(acm.entry_values)): acm.entry_values[row].enabled += [False] new_row = deepcopy(acm.entry_values[0]) acm.entry_values += {new_row} for index_entry_values, entry_values in enumerate(acm.entry_values): if gripper_prefix in acm.entry_names[index_entry_values]: for index_value, _ in enumerate(entry_values.enabled): if acm.entry_names[index_value] in object_names: if enable: acm.entry_values[index_entry_values].enabled[index_value] = False else: acm.entry_values[index_entry_values].enabled[index_value] = True elif acm.entry_names[index_entry_values] in object_names: for index_value, _ in enumerate(entry_values.enabled): if gripper_prefix in acm.entry_names[index_value]: if enable: acm.entry_values[index_entry_values].enabled[index_value] = False else: acm.entry_values[index_entry_values].enabled[index_value] = True planning_scene_diff = PlanningScene(is_diff=True, allowed_collision_matrix=acm) self.planning_scene_publisher.publish(planning_scene_diff) rospy.sleep(1.0)
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 disallow_scoop_collision(): request = PlanningSceneComponents( components=PlanningSceneComponents.ALLOWED_COLLISION_MATRIX) response = get_planning_scene(request) acm = response.scene.allowed_collision_matrix print acm if not SCOOP in acm.default_entry_names: # add button to allowed collision matrix acm.default_entry_names += [SCOOP] acm.default_entry_values += [False] else: acm.default_entry_values[acm.default_entry_names.index(SCOOP)] = False print acm.default_entry_names, acm.default_entry_values[ acm.default_entry_names.index(SCOOP)], SCOOP in acm.entry_names planning_scene_diff = PlanningScene(is_diff=True, allowed_collision_matrix=acm) pubPlanningScene.publish(planning_scene_diff) rospy.sleep(1.0)
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)
def test_planning_scene(self): ''' Check if publish_simple_scene.py is working ''' rospy.wait_for_service('/get_planning_scene', timeout=20) get_planning_scene = rospy.ServiceProxy("/get_planning_scene", GetPlanningScene) collision_objects = [] # wait for 10 sec time_start = rospy.Time.now() while not collision_objects and (rospy.Time.now() - time_start).to_sec() < 10.0: world = get_planning_scene( PlanningSceneComponents(components=PlanningSceneComponents. WORLD_OBJECT_NAMES)).scene.world collision_objects = world.collision_objects rospy.loginfo("collision_objects = " + str(world.collision_objects)) rospy.sleep(1) self.assertTrue(world.collision_objects != [], world)
def allow_collision(self): # self.pubPlanningScene = rospy.Publisher("planning_scene", PlanningScene) ps = PlanningScene() psc = PlanningSceneComponents() acm = AllowedCollisionMatrix() ace = AllowedCollisionEntry() is_allow = Bool() is_allow.data = False ace.enabled = [False] # get scene components # psc.ALLOWED_COLLISION_MATRIX getScene = self.gps(psc) # make it empty for now # ps = getScene ps.allowed_collision_matrix.entry_names = ["cube"] ps.allowed_collision_matrix.entry_values = [ace] ps.allowed_collision_matrix.default_entry_names = ["cube"] ps.allowed_collision_matrix.default_entry_values = [1] ps.is_diff = 1 # print "gripper name", self.gripper applyScene = self.aps(ps) print ps
def toggleOctomap(boolean, pubPlanningScene, get_planning_scene): ''' Set whether to ignore the point cloud or not. ''' request = PlanningSceneComponents( components=PlanningSceneComponents.ALLOWED_COLLISION_MATRIX) response = get_planning_scene(request) acm = response.scene.allowed_collision_matrix if not '<octomap>' in acm.default_entry_names: acm.default_entry_names += ['<octomap>'] acm.default_entry_values += [boolean] else: index = acm.default_entry_names.index('<octomap>') acm.default_entry_values[index] = boolean planning_scene_diff = PlanningScene(is_diff=True, allowed_collision_matrix=acm) pubPlanningScene.publish(planning_scene_diff) rospy.sleep(1.0)
def update_planning_scene_from_moveit(self): """ Update the current cached scene directly from moveit. Returns true if it the service call succeeds. False implies moveit hasn't started correctly. """ try: rospy.wait_for_service("/get_planning_scene", 5) components = PlanningSceneComponents(PlanningSceneComponents.WORLD_OBJECT_NAMES + PlanningSceneComponents.TRANSFORMS) """ :type ps_response :moveit_msgs.srv.GetPlanningSceneResponse """ ps_request = moveit_msgs.srv.GetPlanningSceneRequest(components=components) ps_response = self.planning_scene_service.call(ps_request) self.body_name_cache = [co.id for co in ps_response.scene.world.collision_objects] rospy.loginfo("body name cache:%s"%(', '.join(self.body_name_cache))) return True except Exception as e: raise e rospy.logerror("Failed to find service /get_planning_scene %s and force use moveit enabled"%(e)) rospy.shutdown() rospy.logwarn("Failed to find service /get_planning_scene %s"%(e)) return False
def __init__(self, frame, ns='', init_from_service=True): ''' Constructor of PlanningSceneInterface class :param frame: The fixed frame in which planning is being done (needs to be part of robot?) :param ns: A namespace to push all topics down into. :param init_from_service: Whether to initialize our list of objects by calling the service NOTE: this requires that said service be in the move_group launch file, which is not the default from the setup assistant. ''' # ns must be a string # if not isinstance(ns, basestring): # rospy.logerr('Namespace must be a string!') # ns = '' # elif not ns.endswith('/'): # ns += '/' self._fixed_frame = frame self._scene_pub = rospy.Publisher(ns + 'planning_scene', PlanningScene, queue_size=10) self._apply_service = rospy.ServiceProxy(ns + \ 'apply_planning_scene', ApplyPlanningScene) # track the attached and collision objects self._mutex = thread.allocate_lock() # these are updated based what the planning scene actually contains self._attached = list() self._collision = list() # these are updated based on internal state self._objects = dict() self._attached_objects = dict() self._removed = dict() self._attached_removed = dict() self._colors = dict() # get the initial planning scene if init_from_service: rospy.loginfo('Waiting for get_planning_scene') rospy.wait_for_service(ns + 'get_planning_scene') self._service = rospy.ServiceProxy(ns + 'get_planning_scene', GetPlanningScene) try: req = PlanningSceneComponents() req.components = sum([ PlanningSceneComponents.WORLD_OBJECT_NAMES, PlanningSceneComponents.WORLD_OBJECT_GEOMETRY, PlanningSceneComponents.ROBOT_STATE_ATTACHED_OBJECTS ]) scene = self._service(req) self.sceneCb(scene.scene, initial=True) except rospy.ServiceException as e: rospy.logerr( 'Failed to get initial planning scene, results may be wonky: %s', e) # subscribe to planning scene rospy.Subscriber(ns + 'move_group/monitored_planning_scene', PlanningScene, self.sceneCb)
def execute(self, goal): try: mAndm_logger.info("===== Planning request received from client ==========") mAndm_logger.info(goal) # re-initialize feedback and result self.feedback = PlanAndExecuteTrajFeedback() self.result = PlanAndExecuteTrajResult() mAndm_logger.info("===== Parameters check: Identifying action type =========") if goal.action_type not in self.possilbe_action_types: mAndm_logger.error("Action parameter: {action_type} does not match with expected values."\ .format(action_type=goal.action_type)) self.result.error_type = 6 self.server.set_aborted(result=self.result) return else: action_type = goal.action_type mAndm_logger.info("- Action is: {action_type}".format(action_type=goal.action_type)) mAndm_logger.info("===== Parameters check: target tag or target pose =========") if action_type == 'move_to' and not goal.tag_name and not (goal.target_pose.position.x or goal.target_pose.position.y or\ goal.target_pose.position.z or goal.target_pose.orientation.x or\ goal.target_pose.orientation.y or goal.target_pose.orientation.z or\ goal.target_pose.orientation.w): # return that we fail mAndm_logger.error("No Tag Name or Pose is provided for action move_to. Quitting path planning.") self.result.error_type = 1 self.server.set_aborted(result=self.result) return elif action_type == 'get_tag_pose' and not goal.tag_name: # return that we fail mAndm_logger.error("No Tag Name is provided for action get_tag_pose. Quitting..") self.result.error_type = 10 self.server.set_aborted(result=self.result) return mAndm_logger.info("- Tag Name is: {tag_name}".format(tag_name=goal.tag_name)) mAndm_logger.info("- Tag Name is: {target_pose}".format(target_pose=goal.target_pose)) # first figure out the target arm mAndm_logger.info("===== Parameters check: Finding target arm =========") if not goal.arm or goal.arm == self.arm: target_arm = self.arm elif goal.arm in self.possible_arm_groups: mAndm_logger.warning("This is a {arm} traj server. We may have a problem!".format(arm=self.arm)) target_arm = goal.arm else: mAndm_logger.warning("Arm parameter:{arm} does not match with expected values:left_arm, right_arm or both_arms. Using both_arms.".format(arm=goal.arm)) #target_arm = "both_arms" self.result.error_type = 7 self.server.set_aborted(result=self.result) return mAndm_logger.info("-- Target arm is {target_arm}".format(target_arm=target_arm)) self.feedback.arm = target_arm mAndm_logger.info("===== Parameters check: Identifying planning mode =========") if goal.planning_mode not in self.possible_planning_modes: mAndm_logger.warning("Planning mode: {planning_mode} does not match with expected values: rough, fine or rough_and_fine. Using rough_and_fine.".format(planning_mode=goal.planning_mode)) planning_mode = 'rough_and_fine' else: planning_mode = goal.planning_mode mAndm_logger.info("Planning mode is: {planning_mode}".format(planning_mode=planning_mode)) ############################# ########## MODES ########### ############################ # ------------ close_gripper # position: 72.65 for module, start with 100, closed 3.2 if action_type == 'close_gripper': self.feedback.executing_grasp = True mAndm_logger.info("===== Closing gripper ======================") # operations on gripper if pickup or drop # open gripper before fine plan if target_arm in ['left_arm','right_arm']: mAndm_logger.debug(target_arm.replace('_arm','')) self.gripper[target_arm.replace('_arm','')].close() time.sleep(2) # so that internal data is updated # feeback if self.gripper[target_arm.replace('_arm','')].gripping(): gripping_successful = True else: gripping_successful = False else: # both arms self.gripper['left'].close() self.gripper['right'].close() time.sleep(2) # so that internal data is updated # feeback if self.gripper['left'].gripping() and self.gripper['right'].gripping(): gripping_successful = True else: gripping_successful = False #mAndm_logger.debug("Position when gripper is closed: {pos}".format(pos=self.gripper['right'].position())) #mAndm_logger.debug("Force when gripper is closed: {pos}".format(pos=self.gripper['right'].force())) #mAndm_logger.debug("Gripping: {pos}".format(pos=self.gripper['right'].gripping())) #mAndm_logger.debug("Missed: {pos}".format(pos=self.gripper['right'].missed())) # send result self.result.grasp_result = gripping_successful mAndm_logger.debug("--Grasping module : {status}".format(status=gripping_successful)) if gripping_successful: self.server.set_succeeded(result=self.result) mAndm_logger.info("===== Closing gripper: successful ======================") else: self.server.set_aborted(result=self.result) mAndm_logger.error("===== Closing gripper: failed - no object in hand =======") # ------------ open_gripper elif action_type == 'open_gripper': self.feedback.executing_grasp = True mAndm_logger.info("===== Opening gripper ======================") # operations on gripper if pickup or drop # open gripper before fine plan if target_arm in ['left_arm','right_arm']: mAndm_logger.debug(target_arm.replace('_arm','')) self.gripper[target_arm.replace('_arm','')].open() else: # both arms self.gripper['left'].open() self.gripper['right'].open() #self.limb['left'].set_joint_velocities({'left_w2':0.3}) #time.sleep(7) #self.limb['left'].set_joint_velocities({'left_w2':0.0}) # feedback on whether grasp is good? self.result.grasp_result = True self.server.set_succeeded(result=self.result) mAndm_logger.info("===== Opening gripper: successful ======================") # ------------ move_to elif action_type == 'move_to': mAndm_logger.info("===== Mode move_to ======================") ######################## ###### rough plan ###### ######################## mAndm_logger.debug("planning_mode:" + str(planning_mode)) if planning_mode in ['rough','rough_and_fine']: #rough plan if not self.planning_traj(goal.tag_name, goal.target_pose, target_arm, 'rough'): return ######################## ###### add module ###### ######################## if self.using_octamap: mAndm_logger.info("===== Adding module ======================") # add module: if goal.tag_name: self.add_module(goal.tag_name) mAndm_logger.info("===== Excluding tag object ======================") request = PlanningSceneComponents(components=PlanningSceneComponents.ALLOWED_COLLISION_MATRIX) response = self.get_planning_scene(request) acm = response.scene.allowed_collision_matrix # add gripper fingers to allowed collision matrix acm.default_entry_names += [goal.tag_name] acm.default_entry_values += [True] planning_scene_diff = PlanningScene(is_diff=True, allowed_collision_matrix=acm) #mAndm_logger.log(4, "acm: {acm}".format(acm=acm)) self._pubPlanningScene.publish(planning_scene_diff) ######################## ###### fine plan ###### ######################## if planning_mode in ['fine','rough_and_fine']: #fine plan if not self.planning_traj(goal.tag_name, goal.target_pose, target_arm, 'fine'): return ########################### ###### remove module ###### ########################### if self.using_octamap: mAndm_logger.info("===== Removing tag object ======================") # remove gripper fingers to allowed collision matrix acm.default_entry_names = [] acm.default_entry_values = [] planning_scene_diff = PlanningScene(is_diff=True, allowed_collision_matrix=acm) #mAndm_logger.log(4, "acm: {acm}".format(acm=acm)) self._pubPlanningScene.publish(planning_scene_diff) self.scene.remove_world_object(goal.tag_name) self.server.set_succeeded(result=self.result) # ------------ move_to_camera_pos elif action_type == 'move_to_camera_pos': mAndm_logger.info("===== Mode move_to_camera_pos ==============") #fine plan if target_arm not in ['left_arm','right_arm']: mAndm_logger.error('move to camera pose only works for left_arm or right_arm.Given {target_arm}'.format(target_arm=target_arm)) self.result.error_type = 8 self.server.set_aborted(result=self.result) return target_pose = self.move_to_camera_poses[target_arm.replace('_arm','')] #mAndm_logger.debug(target_pose) if not self.planning_traj("", target_pose, target_arm, 'rough'): return self.server.set_succeeded(result=self.result) # ------------ get_tag_pose elif action_type == 'get_tag_pose': mAndm_logger.info("===== Mode get_tag_pose ======================") if self.find_target_pose(goal.tag_name, 'rough', target_arm): self.server.set_succeeded(result=self.result) else: self.server.set_aborted(result=self.result) mAndm_logger.info("++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++") mAndm_logger.info("++++++++++++ Motion Planning Execution Done! +++++++++++++++++") mAndm_logger.info("++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n") except: mAndm_logger.error("Unexpected error:") exc_type, exc_value, exc_traceback = sys.exc_info() traceback.print_tb(exc_traceback, limit=1, file=sys.stdout) traceback.print_exception(exc_type, exc_value, exc_traceback, limit=2, file=sys.stdout) self.result.error_type = 99 self.server.set_aborted(result=self.result)
while not rospy.is_shutdown(): pose = PoseStamped() pose.header.frame_id = 'BASE' pose.pose.position.x = 0.3 pose.pose.position.y = -0.35 pose.pose.position.z = 0.5 pose.pose.orientation.w = 1.0 scene_interface.add_box('simple_object_1', pose, (0.2, 0.5, 0.04)) pose.pose.position.x = 0.3 pose.pose.position.y = -0.12 pose.pose.position.z = 0.72 scene_interface.add_box('simple_object_2', pose, (0.2, 0.04, 0.4)) pose.pose.position.x = -0.2 pose.pose.position.y = 0.0 pose.pose.position.z = 0.7 scene_interface.add_box('simple_object_3', pose, (0.04, 1.0, 0.2)) rate.sleep() print len( get_planning_scene( PlanningSceneComponents( components=PlanningSceneComponents.WORLD_OBJECT_NAMES)).scene. world.collision_objects) if get_planning_scene and len( get_planning_scene( PlanningSceneComponents( components=PlanningSceneComponents.WORLD_OBJECT_NAMES)). scene.world.collision_objects) >= 3: break