Added citadel hill model file

README.md

@@ -97,11 +97,11 @@ Initilize rosdep:
 	source ~/catkin_ws/devel/setup.bash
 	#####Troubleshooting: https://github.com/Jaeyoung-Lim/mavros_controllers
 ### TOOLS/SITL_GAZEBO
-	copy (or add) files in 'oscillation_ctrl/models' and 'oscillation_ctrl/worlds' to 'PX4-Autopilot/Tools/sitl_gazebo/models' and 'PX4-Autopilot/Tools/sitl_gazebo/worlds' respectively
+	copy (or add) files in _oscillation_ctrl/models_ and _oscillation_ctrl/worlds_ to _PX4-Autopilot/Tools/sitl_gazebo/models_ and _PX4-Autopilot/Tools/sitl_gazebo/worlds_ respectively
 ### ROMFS/PX4FMU_COMMON
-	copy (or add) files in 'oscillation_ctrl/airframes' to 'PX4-Autopilot/ROMFS/px4fmu_common/init.d-posix/airframes'
+	copy (or add) files in _oscillation_ctrl/airframes_ to _PX4-Autopilot/ROMFS/px4fmu_common/init.d-posix/airframes_
-	add file name to 'CmakeLists.txt' in same 'airframe' folder (with number)
+	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!)
+	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'
 ### MAVROS
@@ -128,7 +128,7 @@ 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 nede 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 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
@@ -137,13 +137,13 @@ In catkin_ws (or any working directory) add to devel/setup.bash:
 
 
 ## ROS NODES
-- LinkState.py determines payload load angles and their rates (theta and phi), as well as determines tether length and keeps track of variables needed in case of step test.
+- __LinkState.py__ determines payload load angles and their rates (theta and phi), as well as determines tether length and keeps track of variables needed in case of step test.
 ### Publishes to:
 		/status/twoBody_status # localization and angles
 		/status/path_follow    # boolean to run trajectory test
 ### Subscribes to:
 		none
-- ref_signalGen.py takes in desired position (xd) and determines smooth path trajectory.
+- __ref_signalGen.py__ takes in desired position (xd) and determines smooth path trajectory.
 ### Publishes to:
 		/reference/path			# smooth path
 		/reference/flatsetpoint    	# needed to determine thrust
@@ -153,7 +153,7 @@ In catkin_ws (or any working directory) add to devel/setup.bash:
 		/mavros/imu/data
 		/mavros/state
 		/reference/waypoints
-- klausen_control.py determines forces on drone needed based on smooth path and feedback to dampen oscillations. From the forces needed, it publishes attitude commands.
+- __klausen_control.py__ determines forces on drone needed based on smooth path and feedback to dampen oscillations. From the forces needed, it publishes attitude commands.
 ### Publishes to:
 		/command/quaternions		# attitude commands
 		/reference/flatsetpoint    	# needed to determine thrust
@@ -162,8 +162,8 @@ In catkin_ws (or any working directory) add to devel/setup.bash:
 		/reference/path
 		/mavros/imu/data
 		/mavros/local_position/velocity_body
-- node from __mavros_controllers/geometric_controller__ subscribes to _/reference/flatsetpoint_ to determine thrust commands which are published to _command/bodyrate_command_ by default
+- node from _mavros_controllers/geometric_controller_ subscribes to _/reference/flatsetpoint_ to determine thrust commands which are published to _command/bodyrate_command_ by default
-- path_follow.cpp sets the vehicle in OFFBOARD mode (PX4) and takes off to a set height for 25 seconds before starting to publish attitude and thrust commands.
+- __path_follow.cpp__ sets the vehicle in OFFBOARD mode (PX4) and takes off to a set height for 25 seconds before starting to publish attitude and thrust commands.
 ### Publishes to:
 		mavros/setpoint_position/local		# needed to hover @ set height	
 		mavros/setpoint_raw/attitude		# attitude and thrust commands 	

models/citadel_hill/model.config

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<model>
+  <name>Citadel Hills</name>
+  <version>1.0</version>
+  <sdf version="1.6">model.sdf</sdf>
+
+  <author>
+    <name>Riddhi</name>
+    <email>riddhi.dave@pleiadesrobotics.com</email>
+  </author>
+
+  <description>
+    Model for Citadel Hill
+  </description>
+</model>

models/citadel_hill/model.sdf

@@ -0,0 +1,19 @@
+<?xml version="1.0" ?>
+<sdf version='1.6'>
+  <model name='citadel_hill'>
+  <pose>0 0 0 0 0 0</pose>
+  <static>true</static>
+  
+  <link name='base_citadel'>
+      <visual name='base_visual'>
+        <!-- <pose frame=''>300 200 10 1.5 0 3</pose>-->
+        <geometry>
+          <mesh>
+            <scale>1 1 1</scale>
+	    <uri>model://citadel_hill/meshes/citadel.dae</uri>
+          </mesh>
+        </geometry>
+      </visual>
+    </link>
+  </model>
+</sdf>

src/MoCap_Localization_noTether.py

@@ -41,6 +41,7 @@ class Main:
 		### -*-*-*- END -*-*-*- ###
 
 		# initialize variables
+		self.buff_pose  = PoseStamped()
 		self.drone_pose = PoseStamped()
 
 		# Max dot values to prevent 'blowup'
@@ -105,22 +106,19 @@ class Main:
 															pose.orientation.w])
 		return q
 
-#	def FRD_Transform(pose)
+	def FRD_Transform(self)
-#		'''
+		'''
-#		Transforms mocap reading to proper coordinate frame
+		Transforms mocap reading to proper coordinate frame
-#		'''
+		'''
-#		pose.position.x = 
+		self.drone_pose.position.x = self.buff_pose.position.y
-#		pose.position.y =
+		self.drone_pose.position.y = self.buff_pose.position.x
-#		pose.position.z =
+		self.drone_pose.position.z = -self.buff_pose.position.z
 
 #		Keep the w same and change x, y, and z as above.
-#		pose.orientation.x =
+		self.drone_pose.orientation.x = self.buff_pose.orientation.y
-#		pose.orientation.y =
+		self.drone_pose.orientation.y = self.buff_pose.orientation.x
-#		pose.orientation.z =
+		self.drone_pose.orientation.z = -self.buff_pose.orientation.z
-#		pose.orientation.w =
+		self.drone_pose.orientation.w = self.buff_pose.orientation.w
- 
-#		return pose
-
   
 #  def path_follow(self,path_timer):
 #		now = rospy.get_time()
@@ -132,12 +130,13 @@ class Main:
 				
 	def bodyPose_cb(self,msg):
 		try:
-			self.drone_pose = msg
+			self.buff_pose = msg
 							
 		except ValueError:
 			pass
 
 	def publisher(self,pub_timer):
+		self.FRD_Transform()
 		self.pose_pub.publish(self.drone_pose)
 
 if __name__=="__main__":