The script used to track the figure-8 with the Clover platform can be found in the following repository:
The code details for SITL are repeated here:
#!/usr/bin/python
# * Simplified complex trajectory tracking in OFFBOARD mode
# *
# * Author: Sean Smith <s.smith@dal.ca>
# *
# * Distributed under MIT License (available at https://opensource.org/licenses/MIT).
# * The above copyright notice and this permission notice shall be included in all
# * copies or substantial portions of the Software.
import rospy
from clover import srv
from std_srvs.srv import Trigger
import math
from geometry_msgs.msg import Point, PoseStamped, TwistStamped
import tf
import numpy as np
# Could plot the stored data in SITL (not hardware) if desired:
import matplotlib.pyplot as plt
from time import sleep
# import all mavros messages and services
from mavros_msgs.msg import *
from mavros_msgs.srv import *
rospy.init_node('Figure8') # Figure8 is the name of the ROS node
# Define the Clover service functions, the only ones used in this application are navigate and land.
get_telemetry = rospy.ServiceProxy('get_telemetry', srv.GetTelemetry)
navigate = rospy.ServiceProxy('navigate', srv.Navigate)
navigate_global = rospy.ServiceProxy('navigate_global', srv.NavigateGlobal)
set_position = rospy.ServiceProxy('set_position', srv.SetPosition)
set_velocity = rospy.ServiceProxy('set_velocity', srv.SetVelocity)
set_attitude = rospy.ServiceProxy('set_attitude', srv.SetAttitude)
set_rates = rospy.ServiceProxy('set_rates', srv.SetRates)
land = rospy.ServiceProxy('land', Trigger)
# Release service is used to allow for complex trajectory publishing i.e it stops the navigate service from publishing setpoints because you dont want two sources of publishing at the same time.
release = rospy.ServiceProxy('simple_offboard/release', Trigger)
# Deine math parameter
PI_2 = math.pi/2
# This class defines a few mavros services that can be used, although the Clover services automatically
# take care of these actions including arming for offboard
# maybe add some more that are usable? confirm they are usable just for more illustration prposes
class fcuModes:
def __init__(self):
pass
def setArm(self):
rospy.wait_for_service('mavros/cmd/arming')
try:
armService = rospy.ServiceProxy('mavros/cmd/arming', mavros_msgs.srv.CommandBool)
armService(True)
except rospy.ServiceException as e:
print ("Service arming call failed: %s")
def setOffboardMode(self):
rospy.wait_for_service('mavros/set_mode')
try:
flightModeService = rospy.ServiceProxy('mavros/set_mode', mavros_msgs.srv.SetMode)
flightModeService(custom_mode='OFFBOARD')
except rospy.ServiceException as e:
print ("service set_mode call failed: %s. Offboard Mode could not be set.")
def setAutoLandMode(self):
rospy.wait_for_service('mavros/set_mode')
try:
flightModeService = rospy.ServiceProxy('mavros/set_mode', mavros_msgs.srv.SetMode)
flightModeService(custom_mode='AUTO.LAND')
except rospy.ServiceException as e:
print ('service set_mode call failed: %s. Autoland Mode could not be set.')
# This class categorizes all of the functions used for complex rajectory tracking
class clover:
def __init__(self, FLIGHT_ALTITUDE, RATE, RADIUS, CYCLE_S, REF_FRAME):
# Publisher which will publish to the topic '/mavros/setpoint_velocity/cmd_vel'.
self.velocity_publisher = rospy.Publisher('/mavros/setpoint_velocity/cmd_vel',TwistStamped, queue_size=10)
# global static variables
self.FLIGHT_ALTITUDE = FLIGHT_ALTITUDE # fgdf
self.RATE = RATE # loop rate hz
self.RADIUS = RADIUS # radius of figure 8 in meters
self.CYCLE_S = CYCLE_S # time to complete one figure 8 cycle in seconds
self.STEPS = int( self.CYCLE_S * self.RATE )
self.FRAME = REF_FRAME # Reference frame for complex trajectory tracking
# Publisher which will publish to the topic '/mavros/setpoint_raw/local'. This has a PositionTarget message type
self.publisher = rospy.Publisher('/mavros/setpoint_raw/local', PositionTarget, queue_size=10)
# Subscribe to drone state. Can use this to subscribe to the drone state with mavros, although the get_telemetry Clover function does this
self.state = rospy.Subscriber('mavros/state', State, self.updateState)
self.current_state = State()
self.rate = rospy.Rate(20)
def updateState(self, msg):
self.current_state = msg
# generate a path following Bernoulli's lemiscate as a parametric equation
# note this is in ENU coordinates (x right, y forward, z up) since mavros will convert to NED.
def navigate_wait(self,x=0, y=0, z=0, yaw=float('nan'), yaw_rate=0, speed=0.5, frame_id='body', tolerance=0.2, auto_arm=False):
res = navigate(x=x,y=y,z=z, yaw=yaw, yaw_rate=yaw_rate, speed=speed, frame_id=frame_id, auto_arm=auto_arm)
if not res.success:
return res
while not rospy.is_shutdown():
telem = get_telemetry(frame_id='navigate_target')
if math.sqrt(telem.x ** 2 + telem.y ** 2 + telem.z ** 2) < tolerance:
return res
rospy.sleep(0.2)
def main(self):
i = 0 # Set the counter
dt = 1.0/self.RATE # Set the sample time step
dadt = math.pi*2 / self.CYCLE_S # first derivative of angle with respect to time
r = self.RADIUS # Set the radius of the figure-8
path = []
# Wait for 5 seconds
rospy.sleep(3)
# Takeoff with Clovers navigate function to the desured altitude. This can be in the body frame, or map reference frame for Motion Capture System. It waits for the CLovers arrival before moving on to complex trajectory tracking.
self.navigate_wait(z=self.FLIGHT_ALTITUDE, frame_id='body',auto_arm=True)
rospy.loginfo('start figure8') # Print a notification to the screen when beginning the figure-8
PI=math.pi
start = get_telemetry() # Use this Clover function to get the current drone state
start_stamp = rospy.get_rostime() # Get the current ROS time
# create random time array with enough elements to complete the entire figure-8 sequence
t = np.arange(0,self.STEPS,1)
# Create arrays for each variable we want to feed information to:
posx = [1]*len(t)
posy = [1]*len(t)
posz = [1]*len(t)
velx = [1]*len(t)
vely = [1]*len(t)
velz = [1]*len(t)
afx = [1]*len(t)
afy = [1]*len(t)
afz = [1]*len(t)
yawc = [1]*len(t)
pitchc = [1]*len(t)
rollc = [1]*len(t)
yaw_ratec = [1]*len(t)
for i in range(0, self.STEPS):
# calculate the parameter 'a' which is an angle sweeping from -pi/2 to 3pi/2
# through the figure-8 curve.
a = (-math.pi/2) + i*(math.pi*2/self.STEPS)
# These are definitions that will make position, velocity, and acceleration calulations easier:
c = math.cos(a)
c2a = math.cos(2.0*a)
c4a = math.cos(4.0*a)
c2am3 = c2a-3.0
c2am3_cubed = c2am3*c2am3*c2am3
s = math.sin(a)
cc = c*c
ss = s*s
sspo = (s*s)+1.0 # sin squared plus one
ssmo = (s*s)-1.0 # sin squared minus one
sspos = sspo*sspo
# For more information on these equations, refer to the GitBook Clover documentation:
# Position
# https:#www.wolframalpha.com/input/?i=%28-r*cos%28a%29*sin%28a%29%29%2F%28%28sin%28a%29%5E2%29%2B1%29
posx[i] = -(r*c*s) / sspo
# https:#www.wolframalpha.com/input/?i=%28r*cos%28a%29%29%2F%28%28sin%28a%29%5E2%29%2B1%29
posy[i] = (r*c) / sspo
posz[i] = self.FLIGHT_ALTITUDE
# Velocity
# https:#www.wolframalpha.com/input/?i=derivative+of+%28-r*cos%28a%29*sin%28a%29%29%2F%28%28sin%28a%29%5E2%29%2B1%29+wrt+a
velx[i] = dadt*r* ( ss*ss + ss + (ssmo*cc) ) / sspos
# https:#www.wolframalpha.com/input/?i=derivative+of+%28r*cos%28a%29%29%2F%28%28sin%28a%29%5E2%29%2B1%29+wrt+a
vely[i] = -dadt*r* s*( ss + 2.0*cc + 1.0 ) / sspos
velz[i] = 0.0
# Acceleration
# https:#www.wolframalpha.com/input/?i=second+derivative+of+%28-r*cos%28a%29*sin%28a%29%29%2F%28%28sin%28a%29%5E2%29%2B1%29+wrt+a
afx[i] = -dadt*dadt*8.0*r*s*c*((3.0*c2a) + 7.0)/ c2am3_cubed
# https:#www.wolframalpha.com/input/?i=second+derivative+of+%28r*cos%28a%29%29%2F%28%28sin%28a%29%5E2%29%2B1%29+wrt+a
afy[i] = dadt*dadt*r*c*((44.0*c2a) + c4a - 21.0) / c2am3_cubed
afz[i] = 0.0
# calculate yaw as direction of velocity vector:
yawc[i] = math.atan2(vely[i], velx[i])
# calculate Pitch and Roll angles, if publishing acceleration isnt possible could send these low level commands
pitchc[i] = math.asin(afx[i]/9.81)
rollc[i] = math.atan2(afy[i], afz[i])
# calculate yaw_rate by dirty differentiating yaw
for i in range(0, self.STEPS):
next = yawc[(i+1)%self.STEPS] # 401%400 = 1 --> used for this reason (when it goes over place 400 or whatever STEPS is)
curr = yawc[i]
# account for wrap around +- PI
if((next-curr) < -math.pi):
next = next + math.pi*2
if((next-curr) > math.pi):
next = next - math.pi*2
yaw_ratec[i] = (next-curr)/dt
# Define object that will be published
target = PositionTarget()
rr = rospy.Rate(self.RATE)
k=0
release() # stop navigate service from publishing before beginning the figure-8 publishing
while not rospy.is_shutdown():
target.header.frame_id = self.FRAME # Define the frame that will be used
target.coordinate_frame = 1 #MAV_FRAME_LOCAL_NED # =1
target.type_mask = 0 # Use everything!
# PositionTarget::IGNORE_VX +
# PositionTarget::IGNORE_VY +
# PositionTarget::IGNORE_VZ +
# PositionTarget::IGNORE_AFX +
# PositionTarget::IGNORE_AFY +
# PositionTarget::IGNORE_AFZ +
# PositionTarget::IGNORE_YAW;
# Gather position for publishing
target.position.x = posx[k]
target.position.y = posy[k]
target.position.z = posz[k]
# Gather velocity for publishing
target.velocity.x = velx[k]
target.velocity.y = vely[k]
target.velocity.z = velz[k]
# Gather acceleration for publishing
target.acceleration_or_force.x = afx[k]
target.acceleration_or_force.y = afy[k]
target.acceleration_or_force.z = afz[k]
# Gather yaw for publishing
target.yaw = yawc[k]
# Gather yaw rate for publishing
target.yaw_rate = yaw_ratec[k]
# Publish to the setpoint topic
self.publisher.publish(target)
# This loop initializes when the figure-8 is complete, therefore it will navigate back to the origin and setpoint publishing will be continued as needed to avoid entering failsafe mode.
k = k+1
if k >= self.STEPS:
self.navigate_wait(z=self.FLIGHT_ALTITUDE, yaw=float('nan'), speed=0.5, frame_id = self.FRAME)
break
rr.sleep()
# Wait for 3 seconds
rospy.sleep(3)
# Perform landing
res = land()
if res.success:
print('Drone is landing')
#res = fcuModes()
#res.setAutoLandMode()
# Debug section (plots all of the published data for analysis), need matplotlib to plot the results for SITL
plt.figure(1)
plt.subplot(211)
plt.plot(t,posx,'r',label='x-pos')
plt.plot(t,posy,'b--',label='y-pos')
plt.plot(t,posz,'g:',label='z-pos')
plt.legend()
plt.grid(True)
plt.ylabel('Position [m]')
plt.subplot(212)
plt.plot(t,velx,'r',label='x-vel')
plt.plot(t,vely,'b--',label='y-vel')
plt.plot(t,velz,'g:',label='z-vel')
plt.legend()
plt.grid(True)
plt.ylabel('Velocity [m/s]')
plt.xlabel('Time [s]')
plt.figure(2)
plt.subplot(211)
plt.plot(t, afx,'r',label='x-acc')
plt.plot(t, afy,'b--',label='y-acc')
plt.plot(t, afz,'g:',label='z-acc')
plt.ylabel('af [m/s^2]')
plt.legend()
plt.grid(True)
plt.subplot(212)
plt.plot(t,yawc,'r',label='yaw')
plt.ylabel('Magnitude')
plt.xlabel('Time [s]')
plt.plot(t,yaw_ratec,'b--',label='yaw_rate')
plt.legend()
plt.grid(True)
plt.show()
# rospy.spin() # press control + C, the node will stop.
if __name__ == '__main__':
try:
# Define the performance parameters here which starts the script
q=clover(FLIGHT_ALTITUDE = 1.0, RATE = 50, RADIUS = 1, CYCLE_S = 18, REF_FRAME = 'aruco_map')
q.main()
except rospy.ROSInterruptException:
pass
The Code Explained
This section will go over the Clover Class defined within the script, where everything before the class is standard function definitions with Simple Offboard and MAVROS.
I will note, the release function defined on line 44 is not typically used, however it is needed for this implementation because we need to release or stop the 'SimpleOffboard' module (via navigate) from publishing while we provide setpoints for the figure-8.
The initialization function:
def __init__(self, FLIGHT_ALTITUDE, RATE, RADIUS, CYCLE_S, REF_FRAME):
# Publisher which will publish to the topic '/mavros/setpoint_velocity/cmd_vel'.
self.velocity_publisher = rospy.Publisher('/mavros/setpoint_velocity/cmd_vel',TwistStamped, queue_size=10)
# global static variables
self.FLIGHT_ALTITUDE = FLIGHT_ALTITUDE # fgdf
self.RATE = RATE # loop rate hz
self.RADIUS = RADIUS # radius of figure 8 in meters
self.CYCLE_S = CYCLE_S # time to complete one figure 8 cycle in seconds
self.STEPS = int( self.CYCLE_S * self.RATE )
self.FRAME = REF_FRAME # Reference frame for complex trajectory tracking
# Publisher which will publish to the topic '/mavros/setpoint_raw/local'. This has a PositionTarget message type
self.publisher = rospy.Publisher('/mavros/setpoint_raw/local', PositionTarget, queue_size=10)
# Subscribe to drone state. Can use this to subscribe to the drone state with mavros, although the get_telemetry Clover function does this (does it?)
self.state = rospy.Subscriber('mavros/state', State, self.updateState)
self.current_state = State()
self.rate = rospy.Rate(20)
initializes all of the important parameters. This includes:
FLIGHT_ALTITUDE: altitude of Clover during flight
RATE: rate the publishing loop is ran at
RADIUS: of the figure-8
CYCLE_S: This is the cycle speed or how long it will take to complete the figure-8. Set this reasonably as lowering the value will require the Clover to track faster.
STEPS: amount of data steps during publishing
REF_FRAME: reference frame used, this will vary depending on setup. aruco_map frame will work in SITL or using markers with the Clover. You will set it to map when using the motion capture system.
The self.publisher defines the topic to publish the setpoints on: /mavros/setpoint_raw/local
The rest of the code is self explanatory and very logical. The function:
def updateState(self, msg):
is a callback function for the state subscriber defined on line ??. This can be used by the user if desired. The function:
is a standard navigate function discusses on the Clover website that waits on the Clovers arrival within some tolerance. The function defined as:
def main(self):
is the main function that commands the Clover. The comments within the code give the reader an understanding on what every command does and its purpose. A summary of the function is listed:
Preliminary definitions are set
The Clover is commanded to takeoff to a hovering point using the navigate_wait function before beginning the figure-8.
A set of arrays are defined for storing data used for publishing setpoints and plotting results.
The first for_loop is used to calculate all of the setpoints for position, velocity, acceleration and yaw using the equations discussed in Complex Trajectory Tracking.
The second for_loop determines the yaw rate for further feedforward publishing.
The rate of the publishing loop is defined with rr = rospy.Rate(self.RATE) and the release() function is used to stop 'simpleoffboard'/navigate from publishing setpoints.
The while not rospy.is_shutdown(): loop publishes all of the determined setpoints until the figure-8 is complete. The variable k is iterated through until the STEPS are complete. In this case the figure-8 publishing stops and the navigate_wait function is used to command the Clover back to the starting position.
The loop is exited and the Clover is commanded to land with res = land()
The plotting section plots all of the logged setpoints for analyzing the published data.
The user can defined the important paramaters in line 331.
Adjusting Code for Hardware Implementation
The script above and on the Github is setup to run within the SITL simulator. However, only a few changes need to be made in order for it to work with hardware and they are listed on the following subsections. The matplotlib library is not installed in the Raspberry Pi and is not needed for hardware testing. Therefore remove the following lines from the code:
import matplotlib.pyplot as plt
Also remove the entire plot section shown:
# Debug section (plots all of the published data for analysis), need matplotlib to plot the results for SITL
plt.figure(1)
plt.subplot(211)
plt.plot(t,posx,'r',label='x-pos')
plt.plot(t,posy,'b--',label='y-pos')
plt.plot(t,posz,'g:',label='z-pos')
plt.legend()
plt.grid(True)
plt.ylabel('Position [m]')
plt.subplot(212)
plt.plot(t,velx,'r',label='x-vel')
plt.plot(t,vely,'b--',label='y-vel')
plt.plot(t,velz,'g:',label='z-vel')
plt.legend()
plt.grid(True)
plt.ylabel('Velocity [m/s]')
plt.xlabel('Time [s]')
plt.figure(2)
plt.subplot(211)
plt.plot(t, afx,'r',label='x-acc')
plt.plot(t, afy,'b--',label='y-acc')
plt.plot(t, afz,'g:',label='z-acc')
plt.ylabel('af [m/s^2]')
plt.legend()
plt.grid(True)
plt.subplot(212)
plt.plot(t,yawc,'r',label='yaw')
plt.ylabel('Magnitude')
plt.xlabel('Time [s]')
plt.plot(t,yaw_ratec,'b--',label='yaw_rate')
plt.legend()
plt.grid(True)
plt.show()
Motion Capture System
This section adjusts the code for use in the motion capture system. The pose is fed back with the motion capture system in the map reference frame. The reference frame is set in line 331 with the important parameters. This is listed as such:
It was originally set to body because when using ArUco markers, the Clover must take off to a certain altitude above the markers before the camera can pick them up and localize itself.
ArUco Marker Localization
Other than removing the matplotlib related functions, nothing needs to be changed as long as an ArUco marker map is being referenced. Single marker references can be easily used as well just follow the Clover website instructions: