From 187fc9aef9eb8acc1deebb72d83b4ff78cc55057 Mon Sep 17 00:00:00 2001 From: Matas Keras Date: Fri, 29 Jul 2022 12:54:08 -0400 Subject: [PATCH] Added obstacle avoidance option --- src/.circle_acceleration.py.swp | Bin 0 -> 16384 bytes src/obstacle_detection.py | 243 ++++++++++++++++++++++++++++++++ src/obstacle_test.py | 101 +++++++++++++ 3 files changed, 344 insertions(+) create mode 100644 src/.circle_acceleration.py.swp create mode 100755 src/obstacle_detection.py create mode 100755 src/obstacle_test.py diff --git a/src/.circle_acceleration.py.swp b/src/.circle_acceleration.py.swp new file mode 100644 index 0000000000000000000000000000000000000000..9259a2216f384cce50c4556ae0817405ee013215 GIT binary patch literal 16384 zcmeHO&5z_p6|W?aK*$GzNJv~#yA5jhY)|*hPL@Pyy<)SlQbZ=Z+SyGGS;e^B)!k<7 zve#wLw1=+LF9egBAq=vFtk zRv*5w@$ezEUnRo6=Ou|Q*i z#sdGB7D(gU+1H@i?S*zv7k_Wt_xFzC`c(1v`o8DIW%H-8Kx2W%0*wV43p5sJEYMh> zu|Q*i#sZB68VfWQ_%F18J7nwu$n|5L@MHhK%>QrQ&)6S;*ML`m=Ku$I2)GOQ+kK3^ z0DKeJ2A&3N;4JVUa5wPIy^OsL{1JE+_yzC^@N-}f*aofwHt?@|82c?S20Fk`KE&9w zzzBE{c;{}${tWyY_!98$2O0YcFa^fI3h*)Dtq*_>_!;mNa2mLE7uo}F18)M~0=@xs zfj02Qos2yX_`oNCo9_o7;FrMjz%#%#;4{Dp;CA3w?_(?hHUSH`1Gt4E$!oyNz;)mr z;9V3}{sg=Xd>v4L1U?SD4x4Mc|EMgs>>D4vb|jIC^Ii~*ciluedN_o6U_*A&lW>IcM*oMsSg@rA}Nq7S{UOTH%yBX^&irAGD5S zZx1Fs!*m~q5al3RJBnyp5w##L->2k2Ksv#F=hPcn^tMpLRqe&P=KWoTEV1> zTr*4}i4iOW%$P=e;=-A5k}r0|klGv&+P(~PZKZA)i(z44GlDSrd^w7&wiihWKZ{{) z<}Gvz@jiME>G@@*ZN=G4Kh3lnz*lf#onAAJY)K4SjH+E1_R?O}!_epoEekf(J^jGk zG@Trk>{JZ5M6%<1;&KvAbE_S?J5aMQVl z#Mn{G>_BErcoLxMyVmwt^p&iJ8kaPrFd`})FPo_#lB$!EO3w-!;C-xEZUB92e(!QD zgqd)ZzbEYV!scYjV=|}D)%x_??mmgZ5+xhOm|D)e1&3lBB&l8=_#oGlnpXKNCjS;g zT_jjuu$UL-Q#20n5gm8u9Q{s$V zE%(aY(>>zT+KsYc$iIk%T93MzN0}Cyq*U8^&k*S`X;^W?3d0qm4~Lj~1L3)PvD?Mh ze$=cWq^FRe9L~gvqe&KPcVcTTU|CXJ+pyOi75=++qni*6H?vyT+Tw9H9CXE!0)db5jZJwAR0g@x=TFi%mL?M;CkDZCe&>?j#4%BuW$F#U0dY!SVpv`+=BZ^X`O#vDA9}CW8R`ebOP?Izqa8rPYM*B@M8oLF5?Gj)Up7jjkem3# z3DsEjbQI&m33yX(nFOGUR*FoZ%WJ{MK?dMbG0^#vp%@FqkZF>ePHHssFYqIXuWF-i znfUPt(#x>MaVe1yutH~kL}Tp5?QD6(R$0P&oJKN5i{gE@pXgAWq1gN%%|PV_UC z^asA|3%SGNJe@?6>HPn7ScLM)FZvPVSEbs+j z1^6d&_&0$Uf$PBiz+aKa{|<J;Cq0U`#V5$G!|$q&{&|cKx2W%0*wX!M=ii1Hv)A!Vpo~ONlZNv zmnHZsm$n|$CFCTF)6dZZ6lwQWC+ejirgc1=1_ z1?nbGTH2NhXtR2nj`8=G)8^v-_r|VDKL*h-2j6;p-K|oZg|g~`wIi`VfSRrOr_*|6 zM76uFjAR~0nX>g(gprH#CuB5a39G8e+815al)9*GieYg+&U_@Q#f9FvuupEDf8yHt z$4_=uj;uR$G%qkO3W%j9VEzOWNb-y}OnHqA-f^TG3diBL&D)L>y1sOrc9FhmPcE!8 zqtaIu{nM!QzEMXvn%(P})Uvl{@0pa7>F4@5NR!+7ku?&nHf`?TVsJmls`h zc_EMeUQ%MF1wXXDZbp5pEQQjZK&aD9U4f_%2HcB%j~KS<_Z6q?lL{7P`C?JB&PSP~ zt)JOuj$tJH{sZ*Baw*}bUo%akET3q zv&Pn=u9q`Sr;kHu;b0=3GNP>g)OXozvo;*uS3J;awvS-pbT&jw#)eU~)lzIF%+q0> zShGD7nvqlxKj!Y{M!CDG`pTCMUi;+Mo04#rOlw<2i@Vcz7d0jY4>CX%k+x%n iqCH;hp~$q@!_%Df!RWX6NrbYojZp3WSaVyg9rkZMZgH3Z literal 0 HcmV?d00001 diff --git a/src/obstacle_detection.py b/src/obstacle_detection.py new file mode 100755 index 0000000..8a0487f --- /dev/null +++ b/src/obstacle_detection.py @@ -0,0 +1,243 @@ +#!/usr/bin/env python2.7 + + +import numpy as np +import rospy +from geometry_msgs.msg import Point +from geometry_msgs.msg import PoseStamped +from geometry_msgs.msg import TwistStamped +from geometry_msgs.msg import Vector3Stamped +from mavros_msgs.msg import State +from mavros_msgs.srv import SetMode, SetModeRequest, CommandBool +from sensor_msgs.msg import LaserScan + + +def state_callback(data): + global state + state = data + +def pose_callback(data): + global pose + pose = np.array([data.pose.position.x, data.pose.position.y, data.pose.position.z]) + +def velocity_callback(data): + global velocity + velocity = np.array([data.twist.linear.x,data.twist.linear.y,data.twist.linear.z]) + +def obstacle_callback(data): + global weights + weights = np.array(data.ranges) + + +def get_balance(weights): + distance=min(weights) + balance = (-1*distance+30)/27 + if balance<0: + balance=0 + return balance + + + + +def get_min_radius(weights): + length = weights.size + radii_left = np.zeros(length) + radii_right = np.zeros(length) + for x in range(length): + angle = (length-10)*one_degree + radii_left[x],radii_right[x] = get_radius(x,angle) + index_left = np.argmin(radii_left) + index_right = np.argmin(radii_right) + if radii_left[index_left]>radii_right[index_right]: + return index_right, radii_right[index_right], False + else: + return index_left, radii_left[index_left],True + + +def get_radius(weight, angle): + radius_left = (weight*weight-9)/(6+2*weight*np.cos(np.pi/2-angle)) + radius_right = (weight*weight-9)/(6+2*weight*np.cos(np.pi/2+angle)) + return radius_left,radius_right + + +def avoidance(weights): + index, min_radius, go_left = get_min_radius(weights) + angle_velocity=np.arctan(velocity[1]/velocity[0]) + if velocity[0]<0: + angle_velocity+=np.pi + if go_left: + angle_acceleration = angle_velocity+np.pi/2 + else: + angle_acceleration = angle_velocity-np.pi/2 + magnitude_acceleration = 1.2*(velocity[0]*velocity[0]+velocity[1]*velocity[1])/min_radius + acceleration_x = np.cos(angle_acceleration)*magnitude_acceleration + acceleration_y = np.sin(angle_acceleration)*magnitude_acceleration + return np.array([acceleration_x,acceleration_y,0]) + + +def convert_to_cylindrical(point): + #this code converts a point to cylindrical coordinates + recentered = point-start_point + y=np.dot(recentered, unit_vector) + circle_portion = recentered-y*unit_vector + r=np.linalg.norm(circle_portion) + horizontal_position=np.dot(circle_portion, unit_vector_2) + theta=np.arccos(np.linalg.norm(horizontal_position)/r) + if(np.dot(circle_portion,unit_vector_3)<0): + theta=2*np.pi-theta + return y, r, theta, circle_portion + +def get_error_acceleration(circle_position, r, velocity): + #here we take the velocity and displacement in the direction of the second + #and third unit vector, and use them to calculate accelerations + #based off modelling them as a damped spring or based off how far from + #the max speed they are + velocity_unit_2 = np.dot(velocity, unit_vector_2) + velocity_unit_3 = np.dot(velocity, unit_vector_3) + circle_unit_2 = np.dot(circle_position, unit_vector_2) + circle_unit_3 = np.dot(circle_position, unit_vector_3) + + accel_2_1 = -1*(b3*velocity_unit_2+k*circle_unit_2) + accel_2_2 = 2*(9.5-abs(velocity_unit_2))*velocity_unit_2/abs(velocity_unit_2) + + + accel_3_1 = -1*(b*velocity_unit_3+k*circle_unit_3) + accel_3_2 = 2*(9.5-abs(velocity_unit_3))*velocity_unit_3/abs(velocity_unit_3) + error_accel = min(velocity_unit_2*accel_2_1,velocity_unit_2*accel_2_2)*unit_vector_2/velocity_unit_2 + error_accel += min(velocity_unit_3*accel_3_1,velocity_unit_3*accel_3_2)*unit_vector_3/velocity_unit_3 + return error_accel + +def get_forward_acceleration(y, velocity): + #here we calculate the forward acceleration, either based off how far off + #the target speed we are, or based off a damped spring, depending on which + #is less (or if we are close to the finish) + forward_velocity=np.dot(velocity,unit_vector) + forward_acceleration = 2*(9.5-abs(forward_velocity))*forward_velocity/abs(forward_velocity) + forward_acceleration_2 = -1*(b2*forward_velocity+k2*(y-100))/m + return min(forward_acceleration*forward_velocity, forward_acceleration_2*forward_velocity)*unit_vector/forward_velocity + +def guidance_law(): + #here we calculate the acceleration we need in the forward direction, + #and the acceleration we need that's perpendicular to that direction + y,r,theta,circle_position=convert_to_cylindrical(pose) + forward_accel = get_forward_acceleration(y,velocity) + error_accel = get_error_acceleration(circle_position,r,velocity) + #we then add these together to get our acceleration vector + accel_straight = forward_accel+error_accel + #next we deal with the avoidance component + balance = get_balance(weights) + avoidance_accel = avoidance(weights) + #and we add the avoidance and the straight line accelerations together, + #weighted by balance, which is how close we are to something + accel = balance*avoidance_accel+(1-balance)*accel_straight + #vertical component is not affected by avoidance, so we undo the changes + accel[2] = accel[2]/(1-balance) + #then we decrease it if the acceleration is over 2g + + magnitude_accel=np.linalg.norm(accel) + if magnitude_accel>2*G: + accel=accel/magnitude_accel*2*G + accelerations=Vector3Stamped() + accelerations.vector.x = accel[0] + accelerations.vector.y=accel[1] + + accelerations.vector.z=accel[2] + return accelerations + + + +def accel_publisher(): + #first bit of code is to get the drone off the ground + #first we set up publishers and services + pub = rospy.Publisher('mavros/setpoint_accel/accel',Vector3Stamped, queue_size=10) + setpointpub = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=10) + rospy.wait_for_service('mavros/set_mode') + rospy.wait_for_service('mavros/cmd/arming') + arming_srv = rospy.ServiceProxy('mavros/cmd/arming',CommandBool) + set_mode_srv = rospy.ServiceProxy('mavros/set_mode', SetMode) + #next we set the rate + rate = rospy.Rate(20) + #now we wait till we are connected + while not rospy.is_shutdown() and not state.connected: + + rate.sleep() + + rospy.loginfo('connected') + #now we set up our initial position + initial_pose=PoseStamped() + initial_pose.pose.position.x = 0 + initial_pose.pose.position.y = 0 + initial_pose.pose.position.z = 10 + #we publish the initial pose a few times as there needs to be setpoints + #published to allow us to enter offboard mode + for i in range(100): + setpointpub.publish(initial_pose) + rate.sleep() + #now we set up our requests to arm the drone and enter offboard mode + last_request = rospy.get_rostime().secs + offb_set_mode = SetModeRequest() + offb_set_mode.custom_mode="OFFBOARD" + arm = True + #now we keep trying to arm, enter offboard, and publish setpoints until + #we are hovering at 9.5m or higher, with a stable velocity + while not rospy.is_shutdown() and (pose[2]<9.5 or abs( velocity[0])>1 or abs(velocity[1])>1): + if(state.mode != "OFFBOARD" and (rospy.get_rostime().secs-last_request>5)): + rospy.loginfo('setting mode') + res=set_mode_srv(offb_set_mode) + + if(not res.mode_sent): + rospy.loginfo('Mode not sent') + else: + rospy.loginfo('Mode sent') + last_request=rospy.get_rostime().secs + else: + if (not state.armed and (rospy.get_rostime().secs-last_request>3)): + rospy.loginfo('arming') + arming_srv(arm) + last_request=rospy.get_rostime().secs + initial_pose.pose.position.x =pose[0] + initial_pose.pose.position.y=pose[1] + setpointpub.publish(initial_pose) + #here we set our starting point to wherever the navigation control takes + #over control + global start_point + start_point=pose + #Here is where we start doing navigation control + #we get our acceleration from the guidance law, then publish it. + while not rospy.is_shutdown(): + accelerations = guidance_law() + pub.publish(accelerations) + rate.sleep() + +if __name__ == '__main__': + G=9.8 + one_degree=np.pi/180 + pose = None + velocity = None + #describe constants for acceleration + k = 2 + k2 = 2 + m=1.5 + b3 = 0.4*np.sqrt(m*k) + b = 0.4*np.sqrt(m*k) + b2 = 0.4*np.sqrt(m*k2) + #describe the vectors that define our direction of travel + #the first vector is the direction we travel, the other two are two more + #vectors that form an orthonormal basis with the first one + unit_vector = np.array([1,5,0]) + unit_vector_2 = np.array([-5,1,0]) + unit_vector_3 = np.array([0,0,1]) + unit_vector = unit_vector/np.linalg.norm(unit_vector) + unit_vector_2 = unit_vector_2/np.linalg.norm(unit_vector_2) + #here we start creating the subscribers, and launching the drone + state = State(connected=False,armed=False,guided=False,mode="MANUAL",system_status=0) + rospy.init_node('circle_accelerations',anonymous=True) + rospy.Subscriber('mavros/local_position/velocity_body', TwistStamped, velocity_callback) + rospy.Subscriber('mavros/local_position/pose', PoseStamped, pose_callback) + rospy.Subscriber('mavros/state', State, state_callback) + rospy.Subscriber('obstacle_distances', LaserScan, obstacle_callback) + try: + accel_publisher() + except rospy.ROSInterruptException: + pass + diff --git a/src/obstacle_test.py b/src/obstacle_test.py new file mode 100755 index 0000000..73dcdb9 --- /dev/null +++ b/src/obstacle_test.py @@ -0,0 +1,101 @@ +#!/usr/bin/env python2.7 + + +import numpy as np +import rospy +from geometry_msgs.msg import Point +from geometry_msgs.msg import PoseStamped +from geometry_msgs.msg import TwistStamped +from sensor_msgs.msg import LaserScan +from std_msgs.msg import Header + +def velocity_callback(data): + global center_vector + center_vector = (data.twist.linear.x,data.twist.linear.y) + + +def pose_callback(data): + global pose + pose = data.pose.position + +def intersection(line1, line2): + #get y intercept from y = b2-b1/m1-m2 + x = (line2[1]-line1[1])/(line1[0]-line2[0]) + y = x*line1[0]+line1[1] + return np.array([x,y]) + +def distance_to_line(line1,vector1, center1, line2, bound1, bound2): + intersect = intersection(line1,line2) + #if the intersection point is outside where the line actually exists, return inifinity + if(np.all(intersect>bound1 and intercet