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)