diff --git a/src/.circle_acceleration.py.swp b/src/.circle_acceleration.py.swp new file mode 100644 index 0000000..9259a22 Binary files /dev/null and b/src/.circle_acceleration.py.swp differ 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