Working jinja file generation

README.md

@@ -103,7 +103,7 @@ Initilize rosdep:
 - add file name to _CmakeLists.txt_ in same 'airframe' folder (with number)
 - 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'
+- copy (or add) files from px4_launch directory to '~/PX4-Autopilot/launch'
 ### MAVROS
 - in px4.launch, replace:
 
@@ -127,13 +127,21 @@ In catkin_ws (or any working directory) add to devel/setup.bash:
 		cd $CURRENT_DIR
 
 ## JINJA TETHER FILE
-- spiri_with_tether.sdf.jinja can be altered to create desired tether model
+- _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 
 "make px4_sitl 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)
+- __IMPORTANT:__ in order for jinja file to work, the following needs to be added to the _CMakeLists.txt_ (Ln 288 - may change in future) in the _~/PX4-Autopilot/Tools/sitl_gazebo_ folder:
+
+			COMMAND
+				${PYTHON_EXECUTABLE} ${CMAKE_CURRENT_SOURCE_DIR}/scripts/jinja_gen.py models/spiri_with_tether/spiri_with_tether.sdf} ${CMAKE_CURRENT_SOURCE_DIR}
+			DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/scripts/jinja_gen.py models/spiri_with_tether/spiri_with_tether.sdf
+			VERBATIM
+			)
+- This should be added in the line below VERBATIM in the add_custom_command function, which should be Ln 288)
 
 
 ## ROS NODES

launch/cortex_bridge.launch

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<!--
+Launch file to use klausen oscillaton damping ctrl in Gazebo
+/-->
+<launch>
+  <arg name="mav_name" default="spiri"/>
+  <arg name="command_input" default="1" />
+  <arg name="log_output" default="screen" />
+  <arg name="fcu_protocol" default="v2.0" />
+  <arg name="respawn_mavros" default="false" />
+  <arg name="gazebo_gui" default="false" />
+
+	<node
+		pkg="oscillation_ctrl"
+		type="MoCap_Localization_noTether.py"
+		name="localize_node"
+	/>
+
+	<!-- Cortex bridge launch  -->
+	<include file="$(find cortex_bridge)/launch/cortex_bridge.launch"/>
+
+	<!-- MAVROS launch  -->
+	<include file="$(find mavros)/launch/px4.launch"/>
+</launch>

src/MoCap_Localization_noTether.py

@@ -106,19 +106,19 @@ class Main:
 															pose.orientation.w])
 		return q
 
-	def FRD_Transform(self)
+	def FRD_Transform(self):
 		'''
 		Transforms mocap reading to proper coordinate frame
 		'''
-		self.drone_pose.position.x = self.buff_pose.position.y
-		self.drone_pose.position.y = self.buff_pose.position.x
-		self.drone_pose.position.z = -self.buff_pose.position.z
+		self.drone_pose.pose.position.x = self.buff_pose.pose.position.y
+		self.drone_pose.pose.position.y = self.buff_pose.pose.position.x
+		self.drone_pose.pose.position.z = -self.buff_pose.pose.position.z
 
 #		Keep the w same and change x, y, and z as above.
-		self.drone_pose.orientation.x = self.buff_pose.orientation.y
-		self.drone_pose.orientation.y = self.buff_pose.orientation.x
-		self.drone_pose.orientation.z = -self.buff_pose.orientation.z
-		self.drone_pose.orientation.w = self.buff_pose.orientation.w
+		self.drone_pose.pose.orientation.x = self.buff_pose.pose.orientation.y
+		self.drone_pose.pose.orientation.y = self.buff_pose.pose.orientation.x
+		self.drone_pose.pose.orientation.z = -self.buff_pose.pose.orientation.z
+		self.drone_pose.pose.orientation.w = self.buff_pose.pose.orientation.w
   
 #  def path_follow(self,path_timer):
 #		now = rospy.get_time()