diff --git a/src/circle_acceleration.py b/src/circle_acceleration.py index b3e7083..d98cbd8 100755 --- a/src/circle_acceleration.py +++ b/src/circle_acceleration.py @@ -47,10 +47,13 @@ def calculate_acceleration_vector(velocity,pose): acceleration_z = abs(velocity.z-1)/(1-velocity.z) else: acceleration_z = -1*(np.sign(velocity.z)) + #the tether factor was added because with the tether added, the drone + #would just drop out of the sky. return acceleration_x, acceleration_y, tether_factor*acceleration_z def accel_publisher(): + #we start off by setting up the publishers and services we will need pub = rospy.Publisher('mavros/setpoint_accel/accel',Vector3Stamped, queue_size=10) @@ -60,19 +63,24 @@ def accel_publisher(): arming_srv = rospy.ServiceProxy('mavros/cmd/arming',CommandBool) set_mode_srv = rospy.ServiceProxy('mavros/set_mode', SetMode) - + #now we wait for the drone to connect rate = rospy.Rate(20) while not rospy.is_shutdown() and not state.connected: rate.sleep() rospy.loginfo('connected') initial_pose=PoseStamped() + #next we publish some positions, as the drone needs to have recieved some + #set point commands before it can arm and enter offboard mode initial_pose.pose.position.x = 0 initial_pose.pose.position.y = 0 initial_pose.pose.position.z = 5 for i in range(100): setpointpub.publish(initial_pose) rate.sleep() + #next we arm and enter offboard mode, and then use setpoint commands until + #we are hovering above 4.5m, and our velocity in the x and y directions are + #below 1m/s last_request = rospy.get_rostime().secs offb_set_mode = SetModeRequest() offb_set_mode.custom_mode="OFFBOARD" @@ -96,7 +104,9 @@ def accel_publisher(): initial_pose.pose.position.x =pose.x initial_pose.pose.position.y=pose.y setpointpub.publish(initial_pose) - + #finally, we take control with the acceleration module, which gets the + #accelerations from the calculate_acceleration_vector function, and then + #publishes them to the acceleration topic while not rospy.is_shutdown(): x_accel, y_accel, z_accel=calculate_acceleration_vector(velocity,pose) to_publish=Vector3Stamped() diff --git a/src/circle_waypoints.py b/src/circle_waypoints.py index 30b6327..084dabf 100755 --- a/src/circle_waypoints.py +++ b/src/circle_waypoints.py @@ -5,13 +5,16 @@ from std_msgs.msg import String from geometry_msgs.msg import Point import numpy as np def circle_points(): - + #this code publishes waypoints in the shape of a circle + #the first part initializes the code, and then waits for 48 seconds to + #let the drone get into a hovering position pub = rospy.Publisher('reference/waypoints',Point, queue_size=10) rospy.init_node('circle_points', anonymous=True) rospy.loginfo('init_node') rate=rospy.Rate(10) rospy.sleep(48.) now = rospy.get_rostime().secs + #now we publish the waypoints while not rospy.is_shutdown(): x_pos = np.cos((rospy.get_rostime().secs-now)*np.pi/30)-1 y_pos = np.sin((rospy.get_rostime().secs-now)*np.pi/30) diff --git a/src/turtle_test.py b/src/turtle_test.py index 8f30475..58356fb 100755 --- a/src/turtle_test.py +++ b/src/turtle_test.py @@ -3,18 +3,22 @@ import rospy from turtlesim.srv import * from geometry_msgs.msg import PoseStamped +#callback that takes a pose, and calls the teleport absolute service to +#set the turtles position based on the pose positions def callback(data): teleport_turtle(data.pose.position.x/100+6,data.pose.position.y/100+6,0) - +#code that initializes the subscriber def turtle_follower(): rospy.wait_for_service('turtle1/teleport_absolute') rospy.Subscriber("mavros/local_position/pose", PoseStamped, callback) rospy.spin() if __name__ == '__main__': + #here we set up the service, then call turtle_follower to set up the + #subscriber rospy.wait_for_service('turtle1/teleport_absolute') try: teleport_turtle = rospy.ServiceProxy('turtle1/teleport_absolute',TeleportAbsolute)