def handle_click(self, event, x, y, flags, params):
if event == cv.CV_EVENT_LBUTTONUP:
#print x, y
self.trajector_location = (x, y)
cloud = rospy.wait_for_message('/camera/rgb/points', PointCloud2)
p = read_points(cloud, uvs=[(x, y)]).next()
ps = PointStamped()
ps.header.stamp = rospy.Time(0)
ps.header.frame_id = cloud.header.frame_id
ps.point = Point(*p[:-1])
p = self.tf.transformPoint('base_footprint', ps)
#print p
#print world_to_bolt(p.point)
self.describe(
DescribePOIRequest(world_to_bolt(p.point), 'point', False))
if self.selected_obj_id:
msg = MoveIt()
msg.target_location = p.point
bolt_box = self.bboxes[self.selected_obj_id]
msg.object_bbox = PolygonMsg([
bolt_to_world(bolt_box.points[0]),
bolt_to_world(bolt_box.points[1])
])
print msg
self.move_it_pub.publish(msg)
self.selected_obj_id = None
for i, p in enumerate(self.polygons):
if p.contains_point(Vec2(x, y)):
self.selected_obj_id = i
# todo: select minimum size rectangle
#break
elif event == cv.CV_EVENT_RBUTTONUP:
# self.trajector_location = None
# self.selected_obj_id = None
# self.object_detector.detect()
self.trajector_location = None
self.selected_obj_id = None
self.meaning = None
self.scene_lmk_to_bbox = {}
res = self.object_detector.detect()
scene_msg = self.construct_scene_msg(res['segmentation_result'],
res['cluster_information'])
self.current_scene = self.construct_abstract_scene(scene_msg)
self.update_scene(scene_msg)
def handle_click(self, event, x, y, flags, params):
if event == cv.CV_EVENT_LBUTTONUP:
#print x, y
self.trajector_location = (x,y)
cloud = rospy.wait_for_message('/camera/rgb/points', PointCloud2)
p = read_points(cloud, uvs=[(x,y)]).next()
ps = PointStamped()
ps.header.stamp = rospy.Time(0)
ps.header.frame_id = cloud.header.frame_id
ps.point = Point(*p[:-1])
p = self.tf.transformPoint('base_footprint', ps)
#print p
#print world_to_bolt(p.point)
self.describe( DescribePOIRequest(world_to_bolt(p.point), 'point', False) )
if self.selected_obj_id:
msg = MoveIt()
msg.target_location = p.point
bolt_box = self.bboxes[self.selected_obj_id]
msg.object_bbox = PolygonMsg( [bolt_to_world(bolt_box.points[0]), bolt_to_world(bolt_box.points[1])] )
print msg
self.move_it_pub.publish(msg)
self.selected_obj_id = None
for i,p in enumerate(self.polygons):
if p.contains_point( Vec2(x,y) ):
self.selected_obj_id = i
# todo: select minimum size rectangle
#break
elif event == cv.CV_EVENT_RBUTTONUP:
# self.trajector_location = None
# self.selected_obj_id = None
# self.object_detector.detect()
self.trajector_location = None
self.selected_obj_id = None
self.meaning = None
self.scene_lmk_to_bbox = {}
res = self.object_detector.detect()
scene_msg = self.construct_scene_msg(res['segmentation_result'], res['cluster_information'])
self.current_scene = self.construct_abstract_scene(scene_msg)
self.update_scene(scene_msg)
from bolt_msgs.msg import MoveIt
from geometry_msgs.msg import Polygon
from geometry_msgs.msg import Point32
def bolt_to_world(location, z=0.0):
return Point32(location.y, -location.x, z)
def world_to_bolt(point):
return Vec2(-point.y, point.x)
if __name__ == '__main__':
rospy.init_node('bolt_to_world_command', anonymous=True)
rospy.sleep(1)
move_it_pub = rospy.Publisher('bolt_move_it', MoveIt)
rospy.sleep(1)
o_min = Vec2(0.04, 0.54)
o_max = Vec2(0.088, 0.598)
location = Vec2(0.16, 0.48)
mit = MoveIt()
mit.target_location = bolt_to_world(location, 0.25)
mit.object_bbox = Polygon([bolt_to_world(o_min), bolt_to_world(o_max)])
move_it_pub.publish(mit)
