def __init__(self): rospy.init_node('MarkerLocator') self.rate = rospy.Rate(10) # Hz # needed to convert image self.bridge = CvBridge() # read topic names markerimage_sub_topic = rospy.get_param("~markerimage_sub",'/markerlocator/image_raw') markerpose_pub_topic = rospy.get_param("~markerpose_pub",'/markerlocator/markerpose') # read parameters self.show_image = rospy.get_param("~show_image", False) self.print_debug_messages = rospy.get_param("~print_debug_messages", False) down_scale_factor = rospy.get_param("/image_downscale_factor", 1.0) self.marker_order = rospy.get_param("~marker_order", 0) marker_size = rospy.get_param("~marker_size", 0) / down_scale_factor calib_a_wld = [rospy.get_param("/calibrate_a_world_x", 0), rospy.get_param("/calibrate_a_world_y", 0)] calib_a_img = [rospy.get_param("/calibrate_a_image_x", 0) / down_scale_factor, rospy.get_param("/calibrate_a_image_y", 0) / down_scale_factor] calib_b_wld = [rospy.get_param("/calibrate_b_world_x", 0), rospy.get_param("/calibrate_b_world_y", 0)] calib_b_img = [rospy.get_param("/calibrate_b_image_x", 0) / down_scale_factor, rospy.get_param("/calibrate_b_image_y", 0) / down_scale_factor] calib_c_wld = [rospy.get_param("/calibrate_c_world_x", 0), rospy.get_param("/calibrate_c_world_y", 0)] calib_c_img = [rospy.get_param("/calibrate_c_image_x", 0) / down_scale_factor, rospy.get_param("/calibrate_c_image_y", 0) / down_scale_factor] calib_d_wld = [rospy.get_param("/calibrate_d_world_x", 0), rospy.get_param("/calibrate_d_world_y", 0)] calib_d_img = [rospy.get_param("/calibrate_d_image_x", 0) / down_scale_factor, rospy.get_param("/calibrate_d_image_y", 0) / down_scale_factor] # static parameters scaling_parameter = 1000 # only for (debug) plotting purposes # Calibration of setup, so that the coordinates correspond to real world coordinates. calib_pts_image = [calib_a_img, calib_b_img, calib_c_img, calib_d_img] calib_pts_world = [calib_a_wld, calib_b_wld, calib_c_wld, calib_d_wld] self.perspective_corrector = PerspectiveCorrecter(calib_pts_image, calib_pts_world) if self.print_debug_messages: print 'Calibration points image:', calib_pts_image print 'Calibration points world:', calib_pts_world # instantiate camera driver self.cd = CameraDriver(self.marker_order, marker_size, scaling_parameter) if self.show_image: self.cd.open_image_window() # instantiate markerpose publisher self.markerpose_msg = markerpose() self.markerpose_pub = rospy.Publisher(markerpose_pub_topic, markerpose, queue_size=0) # instantiate image subscribers self.time_prev_image = time() rospy.Subscriber(markerimage_sub_topic, Image, self.on_new_image) # call updater routine self.updater()
def __init__(self): rospy.init_node('uav_navigator') # Initialize variables self.current_markerpose = markerpose() self.yaw_stepsize = 0.1 self.yaw_last = 0 self.uav_yaw = 0 # Initialize subscribers self.markerpose_sub = rospy.Subscriber("/markerlocator/markerpose", markerpose, self.set_markerpose) self.uavpose_sub = rospy.Subscriber("/hummingbird/imu", Imu, self.set_uav_pose) # Initialize publishers self.setpoint_pub = rospy.Publisher("/uav_setpoint", Pose, queue_size=1) # Setup the callback timer, aka. 'main' function rospy.Timer(rospy.Duration(1./100.), self.timer_callback) rospy.spin()
# parameters print_debug_messages = False show_image = True list_of_markers_to_find = [4, 4] get_images_to_flush_cam_buffer = 5 publish_to_ros = True markerpose_ros_topic = '/markerlocator/markerpose' markerlocator_sub_topic = '/iris/camera/image_raw' # global variables stop_flag = False if publish_to_ros: import rospy from markerlocator.msg import markerpose markerpose_msg = markerpose() # define ctrl-c handler def signal_handler(signal, frame): global stop_flag stop_flag = True # install ctrl-c handler signal.signal(signal.SIGINT, signal_handler) class LocatorDriver: """ Purpose: capture images from a ROS Topic and delegate procesing of the
def __init__(self): rospy.init_node('MarkerLocator') self.rate = rospy.Rate(10) # Hz # needed to convert image self.bridge = CvBridge() # read topic names markerimage_sub_topic = rospy.get_param("~markerimage_sub", '/markerlocator/image_raw') markerpose_pub_topic = rospy.get_param("~markerpose_pub", '/markerlocator/markerpose') # read parameters self.show_image = rospy.get_param("~show_image", False) self.print_debug_messages = rospy.get_param("~print_debug_messages", False) down_scale_factor = rospy.get_param("/image_downscale_factor", 1.0) self.marker_order = rospy.get_param("~marker_order", 0) marker_size = rospy.get_param("~marker_size", 0) / down_scale_factor calib_a_wld = [ rospy.get_param("/calibrate_a_world_x", 0), rospy.get_param("/calibrate_a_world_y", 0) ] calib_a_img = [ rospy.get_param("/calibrate_a_image_x", 0) / down_scale_factor, rospy.get_param("/calibrate_a_image_y", 0) / down_scale_factor ] calib_b_wld = [ rospy.get_param("/calibrate_b_world_x", 0), rospy.get_param("/calibrate_b_world_y", 0) ] calib_b_img = [ rospy.get_param("/calibrate_b_image_x", 0) / down_scale_factor, rospy.get_param("/calibrate_b_image_y", 0) / down_scale_factor ] calib_c_wld = [ rospy.get_param("/calibrate_c_world_x", 0), rospy.get_param("/calibrate_c_world_y", 0) ] calib_c_img = [ rospy.get_param("/calibrate_c_image_x", 0) / down_scale_factor, rospy.get_param("/calibrate_c_image_y", 0) / down_scale_factor ] calib_d_wld = [ rospy.get_param("/calibrate_d_world_x", 0), rospy.get_param("/calibrate_d_world_y", 0) ] calib_d_img = [ rospy.get_param("/calibrate_d_image_x", 0) / down_scale_factor, rospy.get_param("/calibrate_d_image_y", 0) / down_scale_factor ] # static parameters scaling_parameter = 1000 # only for (debug) plotting purposes # Calibration of setup, so that the coordinates correspond to real world coordinates. calib_pts_image = [calib_a_img, calib_b_img, calib_c_img, calib_d_img] calib_pts_world = [calib_a_wld, calib_b_wld, calib_c_wld, calib_d_wld] self.perspective_corrector = PerspectiveCorrecter( calib_pts_image, calib_pts_world) if self.print_debug_messages: print 'Calibration points image:', calib_pts_image print 'Calibration points world:', calib_pts_world # instantiate camera driver self.cd = CameraDriver(self.marker_order, marker_size, scaling_parameter) if self.show_image: self.cd.open_image_window() # instantiate markerpose publisher self.markerpose_msg = markerpose() self.markerpose_pub = rospy.Publisher(markerpose_pub_topic, markerpose, queue_size=0) # instantiate image subscribers self.time_prev_image = time() rospy.Subscriber(markerimage_sub_topic, Image, self.on_new_image) # call updater routine self.updater()
# parameters print_debug_messages = False show_image = True list_of_markers_to_find = [5, 6] get_images_to_flush_cam_buffer = 5 publish_to_ros = False markerpose_ros_topic = '/markerlocator/markerpose' # global variables stop_flag = False if publish_to_ros: import rospy from markerlocator.msg import markerpose markerpose_msg = markerpose() # define ctrl-c handler def signal_handler(signal, frame): global stop_flag stop_flag = True # install ctrl-c handler signal.signal(signal.SIGINT, signal_handler) def set_camera_focus(): # Disable autofocus os.system('v4l2-ctl -d 1 -c focus_auto=0')