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()
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()
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)
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")
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)
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
def write_grasps(self, *args): GraspingHelper.write_grasps(self.grasps)