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 box target point
# Longitude , latitude and altitude
self.targets = [
[71.9998318945, 19.0009248718, 25.1599967919], \
[71.9998955286, 19.0007046575, 25.1599967919], \
[71.9998955286, 19.0007046575, 22.1599967919] \
]
# Variable to store scanned waypoints
self.scanned_target = [0.0, 0.0, 0.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
# To store the x and y co-ordinates in meters
self.x_lat = 0.0
self.y_long = 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, 1323*0.06*1000*1.1,1082*0.06]
# self.Ki = [0.008*10, 0.008*10, 0.0*0.008]
# self.Kd = [0.3*10000*873, 2202*0.3*10000, 4476*0.3]
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
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)
# -----------------------------------------------------------------------------------------------------------
# Subscribers for gps co-ordinates, and pid_tune GUI, gripper,rangefinder, custom location message
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)
def __init__(self):
# Initializing ros node with name position_control
rospy.init_node('position_controller', log_level=rospy.DEBUG)
# 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 for csv file
self.csv_counter = 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 = 1/300.0
# 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
"""
# 0:takeoff,1:transverse,2:landing,3:takeoff W/P,4:transverse W/P,5:landing W/P
self.states = 0
# List of target buildings
self.buiding_locations = []
# Initial Longitude , latitude and altitude
self.targets = [
[0.0, 0.0, 0.0],
[0.0, 0.0, 0.0],
[0.0, 0.0, 0.0],
]
# Threshold for path cost
self.path_cost_thresh = 500
# Variable to store scanned waypoints
self.scanned_target = [0.0, 0.0, 0.0]
# Marker ids
self.marker_id = 0
# Detection count for marker detection
self.detection_count = 1
# 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
# 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
#variable to store dist
self.waypoint_dist = 0.0
# 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 = [88318, 88318, 1082*0.06]
self.Ki = [0.008*10*0, 0.008*10*0, 0.0*0.008]
self.Kd = [6606000, 6606000, 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.dist_thresh = 120
self.max_error = 0.00038256625782
self.alt_max_err = 5
self.assigned_error_alt = 1
self.assigned_error = 0.000118256625782/3.5
self.throttle = 0
self.base_pwm = 1500
# ----------------------------------------------------------------------------------------------------------
# Allowed errors in long.,and lat.
self.allowed_lon_error = 0.0000047487/6
self.allowed_lat_error = 0.000004517/6
# Checking if we have to scan or Land
self.scan = False
# Variable representing if the box has been grabbed
self.box_grabbed = 0
# Constraining the no of times location_error has been called
self.count = 0
# Time in which PID algorithm runs
self.pid_break_time = 0.060 # in seconds
self.x = 0
# 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)
self.alt_diff = rospy.Publisher("/alt_diff", Float32, 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)