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__)
