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')