def __init__(self):
rospy.init_node('marker_detect') # Initialise rosnode
self.img = np.empty([])
self.theta = 0
self.vertical_distance = 0
img_width = 400
hfov_rad = 1.3962634
self.focal_length = (img_width / 2) / math.tan(hfov_rad / 2)
self.curr_marker_id = ""
self.marker_data = MarkerData()
# This will contain your image frame from camera
self.bridge = CvBridge()
# Subscribing to the camera topic
self.image_sub = rospy.Subscriber("/edrone/camera/image_raw", Image,
self.image_callback)
rospy.Subscriber("/edrone/curr_marker_id", String,
self.marker_id_callback)
rospack = rospkg.RosPack()
filepath = rospack.get_path('vitarana_drone') + '/data/cascade.xml'
self.logo_cascade = cv2.CascadeClassifier(filepath)
self.marker_data_pub = rospy.Publisher("/edrone/marker_data",
MarkerData,
queue_size=1)
# rospy.Subscriber("/edrone/vertical_distance", Float64, self.vertical_distance_callback)
rospy.Subscriber("/edrone/yaw", Float64, self.theta_callback)
rospy.Subscriber('/edrone/range_finder_bottom', LaserScan,
self.range_finder_callback)
rospy.spin()
def init_props(self):
self.img = np.empty([])
self.marker_data_msg = MarkerData()
self.err_rp_m = [float('nan'), float('nan')]
self.err_xy_m = [float('nan'), float('nan')]
self.focal_length = 1e-7 # safeguard from division by zero error
self.rel_altitude = 0.0
self.gps_location = [0.0, 0.0, 0.0]
self.xyz = [0.0, 0.0, 0.0]
self.orientation_euler = [0.0, 0.0, 0.0]
self.zero_error = (0, 0) #(0.11, 0.2)
def __init__(self):
# Initializing ros node with name position_control
rospy.init_node('position_controller')
# Creating an instance of NavSatFix message and edrone_cmd message
self.location = NavSatFix()
self.drone_cmd = edrone_cmd()
# Created a flag for changing the setpoints
self.targets_achieved = 0
# Counter Check if obstacle was deteceted
self.obstacle_count = 0
# Gripper check vaiable
self.gripper_data = False
# A multiplication factor to increase number of waypoints proportional to distance between final and initial
self.stride = 0.05
# Hardcoded initial target point
"""
Building 1: lat: 18.9990965928, long: 72.0000664814, alt: 10.75
Building 2: lat: 18.9990965925, long: 71.9999050292, alt: 22.2
Building 3: lat: 18.9993675932, long: 72.0000569892, alt: 10.7
"""
self.buiding_locations = [
[72.0000664814, 18.9990965928, 10.75],
[71.9999050292, 18.9990965925, 22.20],
[72.0000569892, 18.9993675932, 10.70]
]
# Initial Longitude , latitude and altitude
self.targets = [
[71.9998195486, 18.999241138, 20],
[72.0000664814, 18.9990965928, 20],
[72.0000664814, 18.9990965928, 20]
]
# Variable to store scanned waypoints
self.scanned_target = [0.0, 0.0, 0.0]
# Marker ids
self.marker_id = 0
# To store bottom range finder values
self.range_finder_bottom = 0
# Variable for top range finder sensor data
# [1] right, [2] back, [3] left, [0,4] front w.r.t eyantra logo
self.range_finder_top_list = [0.0, 0.0, 0.0, 0.0, 0.0]
# Weights for left right sensor values
self.weights_lr = [1.3, -1.3]
# Counter for number of landings made
self.bottom_count = 0
# Offset altitude
self.offset_alt = 3.00
# Safety distances
self.safe_dist_lat = 21.555/105292.0089353767
self.safe_dist_long = 6/110692.0702932625
# Marker data object
self.marker_data = MarkerData()
# To store the x and y co-ordinates in meters
self.err_x_m = 0.0
self.err_y_m = 0.0
# Number of waypoints initialized to -1
self.n = -1
# List to store waypoints Points calculated
self.points = []
# Check if height attained
self.start_to_check_for_obstacles = False
# Kp, Ki and Kd found out experimentally using PID tune
self.Kp = [0.06*1000*156*2*3*1.025, 0.06*1000*156*2*3*1.025, 1082*0.06]
self.Ki = [0.008*10*1.05, 0.008*10*1.05, 0.0*0.008]
self.Kd = [0.3*10000*873*3*1.025, 0.3*10000*873*3*1.025, 4476*0.3]
# Output, Error ,Cummulative Error and Previous Error of the PID equation in the form [Long, Lat, Alt]
self.error = [0.0, 0.0, 0.0]
self.ouput = [0.0, 0.0, 0.0]
self.cummulative_error = [0.0, 0.0, 0.0]
self.previous_error = [0.0, 0.0, 0.0]
self.max_cummulative_error = [1e-3, 1e-3, 100]
self.throttle = 0
self.base_pwm = 1500
# ----------------------------------------------------------------------------------------------------------
# Allowed errors in long.,and lat.
self.allowed_lon_error = 0.0000047487/4
self.allowed_lat_error = 0.000004517/4
# Checking if we have to scan or Land
self.scan = False
# Time in which PID algorithm runs
self.pid_break_time = 0.060 # in seconds
# Publishing servo-control messaages and altitude,longitude,latitude and zero error on errors /drone_command, /alt_error, /long_error, /lat_error, and current marker id
self.drone_pub = rospy.Publisher(
'/drone_command', edrone_cmd, queue_size=1)
self.alt_error = rospy.Publisher('/alt_error', Float32, queue_size=1)
self.long_error = rospy.Publisher('/long_error', Float32, queue_size=1)
self.lat_error = rospy.Publisher('/lat_error', Float32, queue_size=1)
self.zero_error = rospy.Publisher('/zero_error', Float32, queue_size=1)
self.curr_m_id = rospy.Publisher(
'/edrone/curr_marker_id', Int32, queue_size=1)
# -----------------------------------------------------------------------------------------------------------
# Subscribers for gps co-ordinates, and pid_tune GUI, gripper,rangefinder, custom location message and x,y errors in meters
rospy.Subscriber('/edrone/gps', NavSatFix, self.gps_callback)
rospy.Subscriber('/pid_tuning_altitude',
PidTune, self.altitude_set_pid)
rospy.Subscriber('/pid_tuning_roll', PidTune, self.long_set_pid)
rospy.Subscriber('/pid_tuning_pitch', PidTune, self.lat_set_pid)
rospy.Subscriber('/edrone/location_custom',
location_custom, self.scanQR)
rospy.Subscriber('/edrone/range_finder_bottom',
LaserScan, self.range_bottom)
rospy.Subscriber('/edrone/range_finder_top', LaserScan, self.range_top)
rospy.Subscriber('/edrone/gripper_check',
String, self.gripper_callback)
rospy.Subscriber('/edrone/err_x_m', Float32, self.handle_x_m_err)
rospy.Subscriber('/edrone/err_y_m', Float32, self.handle_y_m_err)