Exemple #1
0
	def annotate_grasps(self):
		object_id = GraspingHelper.get_name(self.objects)
		gripper = GraspingHelper.get_gripper()
		frame_id = "/reference/" + gripper + "_gripper"
		print("Frame id: " + frame_id)
		self.grasps = []
		keep_going = True
                print "saving time"
                time = rospy.Time.now()
		index = 0
		while keep_going:
			response = "continue"
			while (len(response) > 0 and response not in self.commands.keys()):
				response = raw_input("Press enter to annotate the grasp or type 'save' to end annotation. ")

			grasps = self.commands[response](self.transformer, gripper, frame_id, str(object_id), index, time)
			if response == "save":
				return
			index += 1
			self.grasps.extend(grasps)
			try:
				to_publish_grasps = MoveHelper.set_grasps_at_pose(self.object_poses[object_id], self.grasps, self.transformer)
				self.publish_grasp_markers(to_publish_grasps, object_id)
			except KeyError as e:
				pass
    def markers_callback(self, msg):
        object_poses = dict()

        print("updating objects")
        for object in msg.objects:
            pose = GraspingHelper.getPoseStampedFromPoseWithCovariance(
                object.pose)
            object_poses[str(object.type.key)] = pose

        if rospy.Time.now() - self.time_exited > rospy.Duration(2.0):
            self.pick_and_place(msg.objects, object_poses)
            self.time_exited = rospy.Time.now()
Exemple #3
0
	def object_callback(self, msg):
		self.objects = []
		self.object_poses = dict()
		for object in msg.objects:
			self.objects.append(object.type.key)
			self.object_poses[object.type.key] = GraspingHelper.getPoseStampedFromPoseWithCovariance(object.pose)
		self.broadcast_transforms()

		if not self.is_annotating:
			self.is_annotating = True
			self.current_thread = Thread(None, self.annotate_grasps)
			self.current_thread.start()
Exemple #4
0
    def markers_callback(self, msg):
        object_poses = dict()

        if len(msg.objects) == 0:
            rospy.logerr("No objects identified")
            return

        for object in msg.objects:
            pose = GraspingHelper.getPoseStampedFromPoseWithCovariance(
                object.pose)
            object_poses[str(object.type.key)] = pose

        self.visualize_grasps(object_poses)
Exemple #5
0
	def __init__(self):
		self.broadcaster = TransformBroadcaster()
                topic = "/publish_detections_center/blue_labeled_objects" # node
                #topic = "/ar_objects" # ar tags
		rospy.Subscriber(topic, RecognizedObjectArray, self.object_callback)
		self.markers_publisher = rospy.Publisher("/grasp_markers", Marker, queue_size=10)
		self.object_info = rospy.ServiceProxy('get_object_info', GetObjectInformation)
		self.transformer = TransformListener(True, 
                                                     rospy.Duration(60.0))

		self.is_annotating = False
		self.commands = GraspingHelper.get_available_commands()
		self.commands["save"] = self.write_grasps
    def markers_callback(self, msg):
        object_poses = dict()

        if len(msg.objects) == 0:
            rospy.logerr("No objects identified")
            return

        rospy.loginfo("updating objects")

        for object in msg.objects:
            pose = GraspingHelper.getPoseStampedFromPoseWithCovariance(
                object.pose)
            object_poses[str(object.type.key)] = pose

        if rospy.Time.now() - self.time_exited > rospy.Duration(10.0):
            self.pick_and_place(msg.objects, object_poses)
            self.time_exited = rospy.Time.now()
        rospy.loginfo("updating objects")
Exemple #7
0
    def markers_callback(self, msg):
        object_poses = dict()
        self.objects = ["kinect", "table", "tripod", "boundary1", "boundary2"]
        for object in self.objects:
            self.last_time_seen[object] = rospy.Time.now()
        if len(msg.objects) == 0:
            rospy.logerr("No objects identified")
            return

        for object in msg.objects:
            self.last_time_seen[object.type.key] = rospy.Time.now()
            pose = GraspingHelper.getPoseStampedFromPoseWithCovariance(
                object.pose)
            self.add_object_at_pose(str(object.type.key), pose)
            object_poses[str(object.type.key)] = pose
            self.objects.append(object.type.key)
            marker = MoveHelper.create_pose_marker(pose, object.type.key, 2,
                                                   15, (1, 0, 0, 1))
            self.markers_publisher.publish(marker)
Exemple #8
0
	def markers_callback(self, msg):
		self.object_poses = dict()
		
		for object in msg.objects:
			pose = GraspingHelper.getPoseStampedFromPoseWithCovariance(object.pose)
			self.object_poses[str(object.type.key)] = pose
Exemple #9
0
	def write_grasps(self, *args):
		GraspingHelper.write_grasps(self.grasps)