From a0decbdf4368cf214c379bce1f8170813b116f85 Mon Sep 17 00:00:00 2001
From: scorpio1 <matas.keras@spirirobotics.com>
Date: Tue, 26 Jul 2022 11:56:08 -0400
Subject: [PATCH] added comments

---
 src/circle_acceleration.py | 14 ++++++++++++--
 src/circle_waypoints.py    |  5 ++++-
 src/turtle_test.py         |  6 +++++-
 3 files changed, 21 insertions(+), 4 deletions(-)

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)