From aecfd6e0c371e7d8d111a00cecca342e7b9cb27c Mon Sep 17 00:00:00 2001 From: scorpio1 Date: Fri, 22 Jul 2022 14:55:02 -0400 Subject: [PATCH] added comments to straight_line.py --- src/straight_line.py | 62 +++++++++++++++++++++++++------------------- 1 file changed, 36 insertions(+), 26 deletions(-) diff --git a/src/straight_line.py b/src/straight_line.py index 069338e..29dc16e 100755 --- a/src/straight_line.py +++ b/src/straight_line.py @@ -59,38 +59,62 @@ def get_forward_acceleration(y, velocity): forward_acceleration_2 = -1*(b2*forward_velocity+k2*(y-300))/m return min(forward_acceleration, forward_acceleration_2)*unit_vector - +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 = forward_accel+error_accel + #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] + #we add 2 to the vertical acceleration because otherwise it sometimes + #just falls + accelerations.vector.z=accel[2]+2 + return to_publish 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 = 12 + 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 - - while not rospy.is_shutdown() and (pose[2]<9.5 or velocity[0]>1 or velocity[1]>1): + #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) @@ -108,29 +132,15 @@ def accel_publisher(): 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(): - #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 = forward_accel+error_accel - #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 - - to_publish=Vector3Stamped() - to_publish.vector.x = accel[0] - to_publish.vector.y=accel[1] - #we add 2 to the vertical acceleration because otherwise it sometimes - #just falls - to_publish.vector.z=accel[2]+2 - pub.publish(to_publish) + accelerations = guidance_law() + pub.publish(accelerations) rate.sleep() if __name__ == '__main__':