Added obstacle avoidance option

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
+

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<bound2)):
+        return float('inf')
+    recentered_intersect = intersect-center1
+    distance = recentered_intersec[0]/vector1[0]
+    #if the distance is negative, the point is behind, so we return infinity to show nothing ahead
+    if distance<0:
+        return float('inf')
+
+    return distance
+
+def calculate_distance_array():
+    distances = np.zeros(21)
+    center = np.array([pose.x,pose.y])
+    first_angle = np.arctan(center_vector[1]/center_vector[0])-10*one_degree
+    for x in range(21):
+        angle = first_angle+x*one_degree
+        vector = np.array([np.cos(angle),np.sin(angle)])
+        line = [0,0]
+        line[0] = vector[1]/vector[0]
+        line[1] = center[1]-line[0]*center[0]
+        distance = float('inf')
+        for line_boundary_set in wall_lines:
+            temp_distance = distance_to_line(line,vector, center,line_boundary_set[0],line_boundary_set[1],line_boundary_set[2])
+            if temp_distance<distance:
+                distance = temp_distance
+        distances[x] = distance
+    return distances
+
+def distance_publisher():
+    pub = rospy.Publisher('obstacle_distances',LaserScan, queue_size = 10)
+    rospy.init_node('obstacle_publisher',anonymous=True)
+    rospy.Subscriber('mavros/local_position/velocity_body', TwistStamped, velocity_callback)
+    rospy.Subscriber('mavros/local_position/pose', PoseStamped, pose_callback)
+    rate = rospy.Rate(10)
+    rospy.sleep(10)
+    while not rospy.is_shutdown():
+        distances = calculate_distance_array()
+        to_publish = LaserScan()
+        to_publish.angle_min = -10*one_degree
+        to_publish.angle_max = 10*one_degree
+        to_publish.angle_increment = one_degree
+        to_publish.time_increment = 0
+        to_publish.scan_time = 0.1 
+        to_publish.range_min = 0
+        to_publish.range_max = float('inf')
+        to_publish.ranges = distances
+        pub.publish(to_publish)
+        rate.sleep
+
+if __name__ == '__main__':
+    one_degree = np.pi/180
+    pose = None
+    wall_lines = []
+    try:
+        distance_publisher()
+    except rospy.ROSInterruptException:
+        pass
+
+
+
+    
+
+
+
+
+
+
+
+
+
+
+