def __init__(self, whicharm = "left_arm", #octomap_filters_service="/create_filter", planner = None, padding = 0.1): """ whicharm: a string, right_arm or left_arm. padding: a float """ self.whicharm = whicharm self.padding = padding #Don't use the object recognition-detection, otherwise it will populate #the collision map self.detector = GenericDetector(detector_service = None, collision_processing = None ) if planner is None: self.planner = PR2MoveArm() else: self.planner = planner self.planner.update_planning_scene() #rospy.loginfo("Waiting for %s", octomap_filters_service) #rospy.wait_for_service(octomap_filters_service) #self.filter_srv = rospy.ServiceProxy(octomap_filters_service,FilterDefine) self.manager = ControllerManagerClient() if self.whicharm.startswith("right"): self.manager.switch_controllers(["r_arm_controller"], ["r_arm_cart_imped_controller"], ) else: self.manager.switch_controllers(["l_arm_controller"], ["l_arm_cart_imped_controller"], ) haptics_service_name = "start_haptic_exploration" rospy.loginfo("Waiting for service %s", haptics_service_name) self.haptics_service = rospy.ServiceProxy(haptics_service_name, Empty) self.move_base = PR2BaseMover(listener = self.planner.tf_listener, use_safety_dist=True) rospy.loginfo("%s is ready", self.__class__.__name__)