Merging branches

CMakeLists.txt

@@ -13,6 +13,7 @@ find_package(catkin REQUIRED COMPONENTS
   message_generation
   message_runtime
   roscpp
+  controller_msgs
 )
 
 ## System dependencies are found with CMake's conventions
@@ -75,6 +76,8 @@ generate_messages(
   DEPENDENCIES
   std_msgs
   geometry_msgs
+  sensor_msgs
+  controller_msgs
 )
 
 ################################################

README.md

@@ -104,7 +104,7 @@ Initilize rosdep:
 ### ROMFS/PX4FMU_COMMON
 - copy (or add) files in _oscillation_ctrl/airframes_ to _PX4-Autopilot/ROMFS/px4fmu_common/init.d-posix/airframes_
 		cp -R ~catkin_ws/src/oscillation_ctrl/airframes/* PX4-Autopilot/ROMFS/px4fmu_common/init.d-posix/airframes
-- add file name to _CmakeLists.txt_ in same 'airframe' folder (with number)
+- add model names to _CmakeLists.txt_ in same 'airframe' folder (with number... 4000_spiri and 4001_spiri_with_tether)
 - add airframe name in _~/PX4-Autopilot/platforms/posix/cmake/sitl_target.cmake_ (no number!)
 ### LAUNCH FILES
 - copy (or add) files from px4_launch directory to '~/PX4-Autopilot/launch'
@@ -122,9 +122,8 @@ In catkin_ws (or any working directory) add to devel/setup.bash:
 
 ## JINJA TETHER FILE
 - _spiri_with_tether.sdf.jinja_ can be altered to create desired tether model
-- changes need to be made in px4 directory and will only take effect and 
+- changes need to be made in px4 directory and will only take effect after running: "make px4_sitl gazebo"
-"make px4_sitl gazebo"
+	- can do "DONT_RUN=1 make px4_sitl gazebo" to avoid starting px4 shell and gazebo
-	- can do DONT_RUN=1 make px4_sitl gazebo to avoid starting px4 shell and gazebo
 - First two elements can be changed to tweak tether parameters
 	- number_elements: number of segments tether will be composed of
 	- tl: segment length (should be no shorter than 0.3 meters)
@@ -176,4 +175,20 @@ In catkin_ws (or any working directory) add to devel/setup.bash:
 		/command/bodyrate_command	
 		/mavros/state
 
+## Launching simulation
+- To launch a simulation, run the following command:
+		roslaunch oscillation_ctrl klausen_dampen.launch
+- This simulation is set to have a Spiri Mu hover at an alitude of 5 m.
+- The launch file itself has two usable arguments:
+- __model:__
+		spiri_with_tether	   # default			
+		spiri	   
+		headless_spiri_with_tether # headless mode: launches with no Gazebo GUI
+		headless_spiri 
+-__test:__
+		none		# default
+		step		# step input - default is 5 m
+		square		# square trajectory
+- To run the simulation with a tethered payload headless mode and perform a step test:
+		roslaunch oscillation_ctrl klausen_dampen.launch model:=headless_spiri_with_tether test:=step
 

launch/klausen_dampen.launch

@@ -56,6 +56,7 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
 			pkg="oscillation_ctrl"
 			type="perform_test.py"
 			name="test_node"
+			launch-prefix="xterm -e"
 		/>
 	</group>
   	<node pkg="geometric_controller" type="geometric_controller_node" name="geometric_controller" output="screen">

package.xml

@@ -21,16 +21,30 @@
   <build_depend>roscpp</build_depend>
   <exec_depend>roscpp</exec_depend>
 
-  <build_depend>geometry_msgs</build_depend>
+  <build_depend>rospy</build_depend> 
+  <exec_depend>rospy</exec_depend>
+
+  <build_depend>geometry_msgs</build_depend> 
   <exec_depend>geometry_msgs</exec_depend>
 
+  <build_depend>sensor_msgs</build_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+
   <build_depend>std_msgs</build_depend>
   <exec_depend>std_msgs</exec_depend>
 
+  <build_depend>controller_msgs</build_depend> 
+  <exec_depend>controller_msgs</exec_depend>
+
   <build_depend>message_generation</build_depend>
   <build_depend>message_runtime</build_depend>
   <exec_depend>message_runtime</exec_depend>
 
+  <exec_depend>python-numpy</exec_depend>
+  <exec_depend>python-scipy</exec_depend>
+  <exec_depend>python-pymavlink</exec_depend>
+
+
   <!-- The export tag contains other, unspecified, tags -->
   <export>
     <!-- Other tools can request additional information be placed here -->

src/LinkState.py

@@ -205,7 +205,7 @@ class Main:
 
 	def user_feedback(self,gui_timer):
 		# Print and save results
-		print "\n"
+		#print "\n"
 		rospy.loginfo("")
 		print"Roll:           "+str(round(self.drone_Eul[0]*180/3.14,2)),"\nPitch:         "+str(round(self.drone_Eul[1]*180/3.14,2)),"\nYaw:           "+str(round(self.drone_Eul[2]*180/3.14,2))
 		print "drone pos.x:    " + str(round(self.drone_Px,2))

src/klausen_control.py

@@ -52,7 +52,7 @@ class Main:
 		self.EulerAng	 = [0,0,0] # Will find the euler angles, and then convert to q
 
 		# Drone var
-		self.has_run		= 0		# Bool to keep track of first run instance
+		self.has_run	 = 0		# Bool to keep track of first run instance
 
 		# Col1 = theta, theta dot; Col2 = phi, phidot for self.PHI
 		self.PHI 				=	np.array([[0,0],[0,0]]) 

src/path_follow.cpp

@@ -229,7 +229,7 @@ int main(int argc, char **argv)
 		attitude.thrust = thrust.thrust;
 		
 		// Determine which messages to send
-		if(ros::Time::now() - tkoff_req > ros::Duration(30.0) && takeoff){
+		if(ros::Time::now() - tkoff_req > ros::Duration(22.0) && takeoff){
 			attitude.orientation = q_des;
 			attitude.header.stamp = ros::Time::now();
 			att_targ.publish(attitude);

src/perform_test.py

@@ -1,12 +1,12 @@
 #!/usr/bin/env python2.7
 
-
 ### Cesar Rodriguez June 2021
-### Script to generate set points which form a square with 2m side lengths
+### Runs either a step test or a square trajectory to assess controller performance
 import rospy,  tf
 import time
 from geometry_msgs.msg import Point
 from std_msgs.msg import Bool
+from oscillation_ctrl.srv import WaypointTrack
 
 class Main:
 	# class method used to initialize variables once when the program starts
@@ -18,18 +18,23 @@ class Main:
 
 		# variable(s)
 		self.Point	= Point() # Point which will actually be published
-		self.xd			= Point() # buffer point to determine current location
+		self.xd		= Point() # buffer point to determine current location
 
 		# Init x, y, & z coordinates
 		self.Point.x	= 0
 		self.Point.y	= 0
 		self.Point.z	= 3.5
 
+		# Test parameters
+		# Square
 		self.xarray = [1,2,2,2,1,0,0]
 		self.yarray = [0,0,1,2,2,2,1]
 		self.i = 0
 		self.j = 0
 		self.buffer = 10
+
+		# Step
+		self.step_size = 5.0 # meters
 		self.bool = False
 
 		self.param_exists = False # check in case param does not exist
@@ -37,10 +42,12 @@ class Main:
 			if rospy.has_param('status/test_type'):
 				self.test = rospy.get_param('status/test_type')
 				self.param_exists = True
-
 			elif rospy.get_time() - self.tstart >= 3.0 or rospy.get_param('status/test_type') == 'none':
 				self.test = 'none'
 				break
+		
+		# get waypoints
+		self.get_xd = rospy.ServiceProxy('/status/waypoint_tracker',WaypointTrack)		
 
 		# subscriber(s), with specified topic, message type, and class callback method
 		rospy.Subscriber('/status/path_follow',Bool, self.wait_cb) 
@@ -57,7 +64,7 @@ class Main:
 	def wait_cb(self,data):
 		self.bool = data
 
-	def waypoints_srv_cb(self):
+	def waypoints_client(self):
 		rospy.wait_for_service('/status/waypoint_tracker')
 		try:
 			resp = self.get_xd(False,self.xd)
@@ -81,7 +88,7 @@ class Main:
 
 	def step(self):
 		# Published desired msgs
-		self.Point.x = 5 # STEP (meters)
+		self.Point.x = self.step_size # STEP (meters)
 
 	def user_feedback(self):
 		rospy.loginfo("Sending [Point x] %d [Point y] %d",
@@ -97,6 +104,7 @@ class Main:
 	def pub(self,pub_timer):
 		if self.bool == False: 		
 			rospy.loginfo('Waiting...')
+			self.waypoints_client() # get waypoints while waiting
 		elif self.bool == False and self.test == 'none':
 			rospy.loginfo('NO TEST WILL BE PERFORMED')
 		else:

src/ref_signalGen.py

@@ -91,7 +91,7 @@ class Main:
 		self.EPS_I 		= np.zeros(9)		 # Epsilon shapefilter
 		
 		# Constants for smooth path generation
-		self.w_tune		= 3.5 # 3.13 works well? #########################################################################
+		self.w_tune		= 3.13 # 3.13 works well? #########################################################################
 		self.epsilon	= 0.7 								# Damping ratio
 		
 		# need exception if we do not have tether:
@@ -108,7 +108,7 @@ class Main:
 		
 		# For saturation:
 		self.jmax = [3, 3, 1]
-		self.amax = [1.5, 1.5, 1]
+		self.amax = [2, 2, 1]
 		self.vmax = [3, 3, 1]
 		self.max  =	[0, 3, 1.5, 3]  #[0, 3, 1.5, 3]
 		# create the array: [vmax; amax; jmax]

src/waypoint_determine.py

@@ -0,0 +1,83 @@
+#!/usr/bin/env python2.7
+
+### Cesar Rodriguez March 2022
+### Reads waypoints from .yaml file and keeps track of them
+
+import rospy,  tf
+import rosservice
+import time
+import math
+from oscillation_ctrl.srv import WaypointTrack, WaypointTrackRequest
+from sensor_msgs.msg import NavSatFix
+
+class Waypoint:
+	def __init__(self,latitude,longitude):
+		self.lat = latitude
+		self.lon = longitude
+
+class Main:
+	def __init__(self):
+		self.request 	   = WaypointTrackRequest()
+		self.request.query = True # set to true to set new waypoints
+		self.get_xd 	   = WaypointTrackRequest() # need to get desired height
+		self.has_run 	   = False
+		self.waypoints 	   = []
+		self.R			   = 6371e3 # Earth radius in meters
+		self.global_home   = NavSatFix()
+		
+		# service(s)
+		self.waypoint_cl = rospy.ServiceProxy('/status/waypoint_tracker',WaypointTrack)
+
+		# subscribers
+		rospy.Subscriber('/mavros/global_position/global', NavSatFix, self.globalPos_cb)
+
+	def globalPos_cb(self,msg):
+		if not self.has_run: # Will not run after global home has been set
+			if msg.latitude != 0.0 and msg.longitude != 0.0:
+				self.gl_home = Waypoint(msg.latitude,msg.longitude) # Save global home
+				self.has_run = True 
+
+	def run(self):
+		self.wp_input()
+		self.determine_xd(self.wp)
+		self.set_waypoint()
+
+	def wp_input(self):
+		# testing:
+		self.lat =  input('Enter lat: ')
+		self.lon = input('Enter lon: ')
+		
+		self.lat =  44.6480876
+		self.lon = -63.5782662		
+		self.wp = Waypoint(self.lat,self.lon)
+
+	def determine_xd(self,wp):
+		# https://en.wikipedia.org/wiki/Equirectangular_projection
+		self.request.xd.x = self.R*(wp.lon - self.gl_home.lon)*(math.pi/180)*math.cos((wp.lat + self.gl_home.lat)*(math.pi/360))
+		self.request.xd.y = self.R*(wp.lat - self.gl_home.lat)*(math.pi/180)
+		"""
+		360 in xd.x because (wp.lat + self.gl_home.lat)/2
+		"""
+
+	def set_waypoint(self):
+		rospy.wait_for_service('/status/waypoint_tracker')
+		try:
+			resp = self.waypoint_cl(self.get_xd) # get xd.z
+			self.request.xd.z = resp.xd.z
+			self.waypoint_cl(self.request) 
+			rospy.loginfo(self.waypoint_cl(self.get_xd)) # want to return value to give feedback to user?
+		except rospy.ServiceException as e:
+			print('Service call failed: %s'%e)
+			
+
+if __name__=="__main__":
+
+	# Initiate ROS node
+	rospy.init_node('waypoint_client',anonymous=True)
+	try:
+		service = Main() # create class object
+		service.run()
+		rospy.spin() 	 # loop until shutdown signal
+
+	except rospy.ROSInterruptException:
+	        pass