From 792b791ef84fbb67c92f40bb06fed6e45171eab2 Mon Sep 17 00:00:00 2001
From: cesar <cesar.rodriguez@spirirobotics.com>
Date: Fri, 1 Apr 2022 11:03:22 -0300
Subject: [PATCH] Weekly Improvements

---
 .gitignore                         |  5 ++++-
 config/px4_pluginlists.yaml        |  2 --
 launch/cortex_bridge.launch        | 11 +++++++----
 launch/px4.launch                  |  6 +++---
 src/MoCap_Localization_noTether.py | 19 ++++++++++++++++---
 src/MocapBridge.py                 | 15 ++++++++++++++-
 6 files changed, 44 insertions(+), 14 deletions(-)

diff --git a/.gitignore b/.gitignore
index eae16d7..e14a7e0 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,3 +1,6 @@
 launch/debug.launch
-src/MoCap_*.py
+launch/cortex_bridge.launch
+src/MoCap_Localization_*.py
+src/MocapBridge.py
+src/*_client.py
 *.rviz
diff --git a/config/px4_pluginlists.yaml b/config/px4_pluginlists.yaml
index 5a97f05..cb3d58a 100644
--- a/config/px4_pluginlists.yaml
+++ b/config/px4_pluginlists.yaml
@@ -2,9 +2,7 @@ plugin_blacklist:
 # common
 - safety_area
 # extras
-- tdr_radio
 - image_pub
-- vibration
 - distance_sensor
 - rangefinder
 - wheel_odometry
diff --git a/launch/cortex_bridge.launch b/launch/cortex_bridge.launch
index d2b325e..de74779 100644
--- a/launch/cortex_bridge.launch
+++ b/launch/cortex_bridge.launch
@@ -9,8 +9,8 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
   <arg name="fcu_protocol" default="v2.0" />
   <arg name="respawn_mavros" default="false" />
   <arg name="gazebo_gui" default="false" />
-  <!--arg name="fcu_url" default="udp://:14551@192.168.1.91:14556"/>
-  <arg name="gcs_url" default="udp-b://127.0.0.1:14555@14550" /-->
+  <arg name="fcu_url" default="udp://:14549@192.168.1.91:14554"/>
+  <arg name="gcs_url" default="udp-b://127.0.0.1:14555@14550" />
 
 	<node
 		pkg="oscillation_ctrl"
@@ -26,7 +26,10 @@ Launch file to use klausen oscillaton damping ctrl in Gazebo
 	<include file="$(find mavros)/launch/px4.launch">
 		<arg name="pluginlists_yaml" value="$(find oscillation_ctrl)/config/px4_pluginlists.yaml" />
 		<arg name="config_yaml" value="$(find oscillation_ctrl)/config/px4_config.yaml" />
-		<arg name="fcu_url" value="udp://:14549@192.168.1.91:14554" />
-		<arg name="gcs_url" value="udp-b://127.0.0.1:14555@14550" />
+  		<arg name="fcu_protocol" value="$(arg fcu_protocol)" />
+		<!--arg name="fcu_url" value="udp://:14549@192.168.1.91:14554" />
+		<arg name="gcs_url" value="udp-b://127.0.0.1:14555@14550" /-->
+		<arg name="fcu_url" value="$(arg fcu_url)" />
+		<arg name="gcs_url" value="$(arg gcs_url)" />
 	</include>
 </launch>
diff --git a/launch/px4.launch b/launch/px4.launch
index da9653b..777c00c 100644
--- a/launch/px4.launch
+++ b/launch/px4.launch
@@ -1,7 +1,7 @@
 <launch>
 	<!-- vim: set ft=xml noet : -->
 	<!-- example launch script for PX4 based FCU's -->
-	<arg name="fcu_url" default="udp://:14551@192.168.1.91:14556"/>
+	<arg name="fcu_url" default="udp://:14549@192.168.1.91:14554"/>
 	<arg name="gcs_url" default="udp-b://127.0.0.1:14555@14550" />
 	<arg name="tgt_system" default="1" />
 	<arg name="tgt_component" default="1" />
@@ -10,8 +10,8 @@
 	<arg name="respawn_mavros" default="false" />
 
 	<include file="$(find mavros)/launch/node.launch">
-		<arg name="pluginlists_yaml" value="$(find mavros)/launch/px4_pluginlists.yaml" />
-		<arg name="config_yaml" value="$(find mavros)/launch/px4_config.yaml" />
+		<arg name="pluginlists_yaml" value="$(find oscillation_ctrl)/config/px4_pluginlists.yaml" />
+		<arg name="config_yaml" value="$(find oscillation_ctrl)/config/px4_config.yaml" />
 		<arg name="fcu_url" value="$(arg fcu_url)" />
 		<arg name="gcs_url" value="$(arg gcs_url)" />
 		<arg name="tgt_system" value="$(arg tgt_system)" />
diff --git a/src/MoCap_Localization_noTether.py b/src/MoCap_Localization_noTether.py
index a0fc3f0..1fcd148 100755
--- a/src/MoCap_Localization_noTether.py
+++ b/src/MoCap_Localization_noTether.py
@@ -17,7 +17,7 @@ class Main:
 	def __init__(self):
 
 		# rate(s)
-		rate = 100 # rate for the publisher method, specified in Hz -- 20 Hz
+		rate = 120 # rate for the publisher method, specified in Hz -- 20 Hz
 
 		# Variables needed for testing start 
 		self.tstart		= rospy.get_time() # Keep track of the start time
@@ -99,12 +99,25 @@ class Main:
 															self.drone_pose.pose.orientation.z,
 															self.drone_pose.pose.orientation.w])
 
+		self.q = quaternion_from_euler(self.eul[0],self.eul[1],self.eul[2])
+
+		offset_yaw = math.pi/2 
+		q_offset   = quaternion_from_euler(0,0,-offset_yaw)
+
+		self.q = quaternion_multiply(self.q,q_offset)
+
+		self.eul = euler_from_quaternion([self.q[0],self.q[1],self.q[2],self.q[3]])
+		self.drone_pose.pose.orientation.x = self.q[0]
+		self.drone_pose.pose.orientation.y = self.q[1]
+		self.drone_pose.pose.orientation.z = self.q[2]
+		self.drone_pose.pose.orientation.w = self.q[3]
+
 	def FRD_Transform(self):
 		'''
 		Transforms mocap reading to proper coordinate frame
 		'''
 
-		self.drone_pose = self.buff_pose
+#		self.drone_pose = self.buff_pose
 		self.drone_pose.header.frame_id = "/map"
 
 #		self.drone_pose.pose.position.x = 0
@@ -139,7 +152,7 @@ class Main:
 				
 	def bodyPose_cb(self,msg):
 		try:
-			self.buff_pose = msg
+			self.drone_pose = msg
 							
 		except ValueError:
 			pass
diff --git a/src/MocapBridge.py b/src/MocapBridge.py
index 262a533..39136eb 100755
--- a/src/MocapBridge.py
+++ b/src/MocapBridge.py
@@ -19,7 +19,7 @@ class Main:
 	def __init__(self):
 
 		# rate(s)
-		rate = 50 # rate for the publisher method, specified in Hz -- 20 Hz
+		rate = 30 # rate for the publisher method, specified in Hz -- 20 Hz
 
 		# Variables needed for testing start 
 		self.tstart		= rospy.get_time() # Keep track of the start time
@@ -135,6 +135,18 @@ class Main:
 															pose.orientation.z,
 															pose.orientation.w])
 
+#		self.q = quaternion_from_euler(self.eul[0],self.eul[1],self.eul[2])
+
+#		offset_yaw = math.pi/2 
+#		q_offset   = quaternion_from_euler(0,0,offset_yaw)
+
+#		self.q = quaternion_multiply(self.q,q_offset)
+
+#		self.eul = euler_from_quaternion([self.q[0],self.q[1],self.q[2],self.q[3]])
+#		self.LabDrone_pose.pose.orientation.x = self.q[0]
+#		self.LabDrone_pose.pose.orientation.y = self.q[1]
+#		self.LabDrone_pose.pose.orientation.z = self.q[2]
+
 	def FRD_Transform(self):
 		'''
 		Transforms mocap reading to proper coordinate frame
@@ -172,6 +184,7 @@ class Main:
 
 	def fake_mocap(self):
 		self.LabDrone_pose.header.stamp = rospy.Time.now()
+	#self.LabDrone_pose.header.stamp.secs = self.LabDrone_pose.header.stamp.secs - self.tstart
 		self.LabDrone_pose.pose.position.x = 0.0
 		self.LabDrone_pose.pose.position.y = 0.0
 		self.LabDrone_pose.pose.position.z = 0.5