def __init__(self, name):
# stuff for grasp planning
rospy.loginfo("Getting a TransformListener...")
self.tf_listener = tf.TransformListener()
rospy.loginfo("Getting a TransformBroadcaster...")
self.tf_broadcaster = tf.TransformBroadcaster()
rospy.loginfo("Initializing a ClusterBoundingBoxFinder...")
self.cbbf = ClusterBoundingBoxFinder(self.tf_listener, self.tf_broadcaster, "base_link")
self.last_clusters = None
rospy.loginfo("Subscribing to '" + RECOGNIZED_OBJECT_ARRAY_TOPIC + "'...")
self.sub = rospy.Subscriber(RECOGNIZED_OBJECT_ARRAY_TOPIC, RecognizedObjectArray, self.objects_callback)
if DEBUG_MODE:
self.to_grasp_object_pose_pub = rospy.Publisher(TO_BE_GRASPED_OBJECT_POSE_TOPIC, PoseStamped)
rospy.loginfo("Connecting to pickup AS '" + PICKUP_AS + "'...")
self.pickup_ac = SimpleActionClient(PICKUP_AS, PickupAction)
self.pickup_ac.wait_for_server()
rospy.loginfo("Connecting to place AS '" + PLACE_AS + "'...")
self.place_ac = SimpleActionClient(PLACE_AS, PlaceAction)
self.place_ac.wait_for_server()
rospy.loginfo("Connecting to grasp generator AS '" + GRASP_GENERATOR_AS + "'...")
self.grasps_ac = SimpleActionClient(GRASP_GENERATOR_AS, GenerateGraspsAction)
self.grasps_ac.wait_for_server()
rospy.loginfo("Connecting to depth throttle server '" + DEPTH_THROTLE_SRV + "'...")
self.depth_service = rospy.ServiceProxy(DEPTH_THROTLE_SRV, Empty)
self.depth_service.wait_for_service()
rospy.loginfo("Getting a PlanningSceneInterface instance...")
self.scene = PlanningSceneInterface()
# blocking action server
rospy.loginfo("Creating Action Server '" + name + "'...")
self.grasp_obj_as = ActionServer(name, ObjectManipulationAction, self.goal_callback, self.cancel_callback, False)
self.as_feedback = ObjectManipulationFeedback()
self.as_result = ObjectManipulationActionResult()
self.current_goal = None
# Take care of left and right arm grasped stuff
self.right_hand_object = None
self.left_hand_object = None
self.current_side = 'right'
rospy.loginfo("Starting '" + OBJECT_MANIPULATION_AS + "' Action Server!")
self.grasp_obj_as.start()
