def select_place(self, pt, click):
if self.place_selected: return
self.label.setPlainText("Select a place")
items = self.scene.collidingItems(pt.viz)
for iit in self.ignored_items: # TODO test it
if iit in items: items.remove(iit)
pointed_place = pt.get_pointed_place()
if len(items) == 0 and (pointed_place is not None):
# TODO how to keep some minimal spacing between places?
skip = False
for pl in self.viz_places:
if dist(pl.pos, pointed_place) < 150:
rospy.logwarn(pt.id + ": place near to x=" + str(pointed_place[0]) + ", y=" + str(pointed_place[1]) + " already exists")
skip = True
break
if skip: return
self.label.setPlainText("Place selected at x=" + str(round(pointed_place[0], 2)) + ", y=" + str(round(pointed_place[1], 2)))
sp = scene_place(self.scene, pointed_place, self.size(), self.calib)
self.state_manager.select_place(sp.get_pose())
self.viz_places.append(sp)
self.place_selected = True
self.selected_at = rospy.Time.now()
return sp
def select_polygon(self, pt, click):
if self.polygon_selected: return
lp = len(self.viz_polygons)
if lp==1: self.label.setPlainText("Select a pick polygon")
elif lp==2: self.label.setPlainText("Select a place polygon")
self.viz_polygons[-1].set_pos(pt.pos)
pointed_place = pt.get_pointed_place()
if pointed_place is not None:
if len(self.viz_polygons[-1].points) > 0:
d = dist(self.viz_polygons[-1].points[-1], pointed_place)
if d < 60: return
if self.viz_polygons[-1].add_point(pointed_place):
self.state_manager.select_polygon(self.viz_polygons[-1].msg())
self.label.setPlainText("Polygon selected")
self.polygon_selected = True
self.selected_at = rospy.Time.now()