small issues fixed

launch/klausen_dampen.launch

@@ -31,6 +31,7 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
 		pkg="oscillation_ctrl"
 		type="ref_signalGen.py"
 		name="refSignal_node"
+		launch-prefix="xterm -e"
 	/>
 	<node
 		pkg="oscillation_ctrl"

launch/mocap_sim.launch

@@ -5,6 +5,7 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
 <launch>
   <arg name="mav_name" default="spiri_mocap"/>
   <arg name="command_input" default="1" />
+  <arg name="model" default="headless_spiri_mocap"/>
 
 	<node
 		pkg="oscillation_ctrl"
@@ -12,12 +13,12 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
 		name="fakeMocap_node"
 		launch-prefix="xterm -e"
 	/>
-	<!--node
+	<node
 		pkg="oscillation_ctrl"
 		type="offb_node"
 		name="offb_node"
 		launch-prefix="xterm -e"
-	/-->
+	/>
 	
   	<!--node pkg="geometric_controller" type="geometric_controller_node" name="geometric_controller" output="screen">
   	  <param name="mav_name" type="string" value="$(arg mav_name)" />
@@ -32,5 +33,5 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
           <param name="Kv_z" value="6.0" />
   	</node-->
 	<!-- PX4 LAUNCH -->
-	<include file="$(find px4)/launch/headless_spiri_mocap.launch"/>
+	<include file="$(find px4)/launch/$(arg model).launch"/>
 </launch>

src/LinkState.py

@@ -22,6 +22,8 @@ class Main:
 
 		self.dt		 = 1.0/rate
 
+		rospy.sleep(5) # Sleep for 1 sec. Need to give time to Gazebo to ru
+
 		# Variables needed for testing start 
 		self.tstart		= rospy.get_time() # Keep track of the start time
 		while self.tstart == 0.0:				 # Need to make sure get_rostime works
@@ -64,14 +66,16 @@ class Main:
 		self.service1 = '/gazebo/get_link_state'
 
 		# need service list to check if models have spawned
-		self.service_list = rosservice.get_service_list()
+#		self.service_list = rosservice.get_service_list()
 
 		# wait for service to exist
-		while self.service1 not in self.service_list:
+		rospy.wait_for_service(self.service1,timeout=10)
-			print "Waiting for models to spawn..."
+
-			self.service_list = rosservice.get_service_list()
+#		while self.service1 not in self.service_list:
-			if rospy.get_time() - self.tstart >= 10.0:
+#			print "Waiting for models to spawn..."
-				break
+#			self.service_list = rosservice.get_service_list()
+#			if rospy.get_time() - self.tstart >= 10.0:
+#				break
 
 		# publisher(s)
 		self.publisher = rospy.Publisher('/status/twoBody_status', tethered_status, queue_size=1)
@@ -143,6 +147,7 @@ class Main:
 					# Determine yaw offset
 					self.yaw_offset = drone_Eul[2]
 						
+			
 					# Get tether length based off initial displacement
 					self.tetherL = math.sqrt((drone_P.link_state.pose.position.x - 
 												pload_P.link_state.pose.position.x)**2 +
@@ -164,17 +169,20 @@ class Main:
 			drone_Pz = drone_P.link_state.pose.position.z
 
 			# Get drone orientation
-			drone_q = [drone_P.link_state.pose.orientation.x,drone_P.link_state.pose.orientation.y,
+			#drone_q = [drone_P.link_state.pose.orientation.x,drone_P.link_state.pose.orientation.y,
-								 drone_P.link_state.pose.orientation.z,drone_P.link_state.pose.orientation.w]
+			#					 drone_P.link_state.pose.orientation.z,drone_P.link_state.pose.orientation.w]
 
 			# offset orientation by yaw offset
-			q_offset = quaternion_from_euler(0,0,-self.yaw_offset)
+			#q_offset = quaternion_from_euler(0,0,-self.yaw_offset)
 			#drone_q = quaternion_multiply(drone_q,q_offset)
 
-			drone_P.link_state.pose.orientation.x = drone_q[0]
+			#drone_P.link_state.pose.orientation.x = drone_q[0]
-			drone_P.link_state.pose.orientation.y = drone_q[1]
+			#drone_P.link_state.pose.orientation.y = drone_q[1]
-			drone_P.link_state.pose.orientation.z = drone_q[2]
+			#drone_P.link_state.pose.orientation.z = drone_q[2]
-			drone_P.link_state.pose.orientation.w = drone_q[3]
+			#drone_P.link_state.pose.orientation.w = drone_q[3]
+
+			# Get euler angles again for feedback to user
+			#drone_Eul = self.euler_array(drone_P.link_state.pose.orientation)
 
 			if self.pload == True: # If there is payload, determine the variables
 				# Pload
@@ -213,7 +221,7 @@ class Main:
 			# Print and save results
 			print "\n"
 			rospy.loginfo("")
-			print"Roll:           "+str(round(drone_Eul[0],2)),"\nPitch:         "+str(round(drone_Eul[1],2)),"\nYaw:           "+str(round(drone_Eul[2],4))
+			print"Roll:           "+str(round(drone_Eul[0]*180/3.14,2)),"\nPitch:         "+str(round(drone_Eul[1]*180/3.14,2)),"\nYaw:           "+str(round(drone_Eul[2]*180/3.14,2))
 			print "drone pos.x:    " + str(round(drone_Px,2))
 			print "drone pos.y:    " + str(round(drone_Py,2))
 			print "drone pos.z:    " + str(round(drone_Pz,2))

src/offb_node.cpp

@@ -114,7 +114,7 @@ int main(int argc, char **argv)
     // Populate pose msg
     pose.pose.position.x = 0;
     pose.pose.position.y = 0;
-    pose.pose.position.z = 1.0;
+    pose.pose.position.z = 2.5;
     pose.pose.orientation.x = q_init.x;
     pose.pose.orientation.y = q_init.y;
     pose.pose.orientation.z = q_init.z;

src/ref_signalGen.py

@@ -409,11 +409,13 @@ class Main:
 	def screen_output(self):
 
 		# Feedback to user
-		rospy.loginfo(' Var |   x   |   y   |   z   ')
+		#rospy.loginfo(' Var |   x   |   y   |   z   ')
-		rospy.loginfo('Pos:    %.2f    %.2f    %.2f',self.EPS_F[0],self.EPS_F[1],self.EPS_F[2])
+		#rospy.loginfo('Pos:    %.2f    %.2f    %.2f',self.EPS_F[0],self.EPS_F[1],self.EPS_F[2])
-		rospy.loginfo('Vel:    %.2f    %.2f    %.2f',self.EPS_F[3],self.EPS_F[4],self.EPS_F[5])
+		#rospy.loginfo('Vel:    %.2f    %.2f    %.2f',self.EPS_F[3],self.EPS_F[4],self.EPS_F[5])
-		rospy.loginfo('Acc:    %.2f    %.2f    %.2f',self.EPS_F[6],self.EPS_F[7],self.EPS_F[8])
+		#rospy.loginfo('Acc:    %.2f    %.2f    %.2f',self.EPS_F[6],self.EPS_F[7],self.EPS_F[8])
-		rospy.loginfo('_______________________')
+		#rospy.loginfo('_______________________')
+
+		rospy.loginfo('xd = %.2f',self.xd.x)
 
 	def publisher(self,pub_tim):
 	
@@ -424,12 +426,9 @@ class Main:
 		self.pub_path.publish(self.ref_sig)
 		self.pub_ref.publish(self.ref_sig)
 
+
 		# Give user feedback on published message:
-		rospy.loginfo(' Var |   x   |   y   |   z   ')
+		self.screen_output()
-		rospy.loginfo('Pos:    %.2f    %.2f    %.2f',self.EPS_F[0],self.EPS_F[1],self.EPS_F[2])
-		rospy.loginfo('Vel:    %.2f    %.2f    %.2f',self.EPS_F[3],self.EPS_F[4],self.EPS_F[5])
-		rospy.loginfo('Acc:    %.2f    %.2f    %.2f',self.EPS_F[6],self.EPS_F[7],self.EPS_F[8])
-		rospy.loginfo('_______________________')
 
 		
 if __name__=="__main__":

src/square.py

@@ -41,7 +41,6 @@ class Main:
 	def wait_cb(self,data):
 		self.bool = data
 
-
 	# Publish messages
 	def pub(self,pub_timer):
 		if self.bool == False: 		
@@ -65,17 +64,6 @@ class Main:
 			self.j += 1
 			self.i = self.j // self.buffer
 
-#		self.Point.header.stamp = rospy.Time.now()
-#		self.Point.x = self.xarray[self.i]
-#		self.Point.y = self.yarray[self.i]
-		
-#		rospy.loginfo("Sending [Point x] %d [Point y] %d",
-#									 self.Point.x, self.Point.y) 	
-			
-		# Published desired msgs
-#		self.pub_square.publish(self.Point)
-#		self.i += 1
-
 if __name__=="__main__":
 
 	# Initiate ROS node

src/step.py

@@ -14,9 +14,9 @@ class Main:
 		# variable(s)
 		self.Point 	= Point()
 		# Init x, y, & z coordinates
-		self.Point.x	= 1
+		self.Point.x	= 5
 		self.Point.y	= 0
-		self.Point.z	= 4.0
+		self.Point.z	= 3.5 # need to check what the xd was previously and save the height
 		
 		self.bool = False