Merge branch 'master' of https://git.spirirobotics.com/cesar.alejandro/oscillation_ctrl

README.md

@@ -51,16 +51,7 @@ Initilize rosdep:
 	
 ## 2) PX4 Environment Development
 
-### Download PX4 source code
+Install [PX4 Firmware](https://docs.px4.io/main/en/dev_setup/dev_env_linux_ubuntu.html#rosgazebo). Then:
-	git clone https://github.com/PX4/PX4-Autopilot.git --recursive
-
-### Run ubuntu.sh
-	bash ./PX4-Autopilot/Tools/setup/ubuntu.sh
-	#Restart computer after it is done
-
-### Build ROS and Gazebo - This defaults to Gazebo9
-	wget https://raw.githubusercontent.com/PX4/Devguide/master/build_scripts/ubuntu_sim_ros_melodic.sh
-	bash ubuntu_sim_ros_melodic.sh
 
 #### Download QGroundControl from:
 	https://docs.qgroundcontrol.com/master/en/releases/daily_builds.html
@@ -83,19 +74,19 @@ Initilize rosdep:
 	sudo apt-get install -y xterm
 
 ### Clone oscillation_ctrl
-	cd ~catkin_ws/src
+	cd ~/catkin_ws/src
 	git clone https://git.spirirobotics.com/cesar.alejandro/oscillation_ctrl.git
 
 ### Add files to _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
 
-	cp -R ~catkin_ws/src/oscillation_ctrl/models/* ~/PX4-Autopilot/Tools/sitl_gazebo/models
+	cp -R ~/catkin_ws/src/oscillation_ctrl/models/* ~/PX4-Autopilot/Tools/sitl_gazebo/models
-	cp -R ~catkin_ws/src/oscillation_ctrl/worlds/* ~/PX4-Autopilot/Tools/sitl_gazebo/worlds
+	cp -R ~/catkin_ws/src/oscillation_ctrl/worlds/* ~/PX4-Autopilot/Tools/sitl_gazebo/worlds
 
 ### Add files to _ROMFS/px4mu_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
+	cp -R ~/catkin_ws/src/oscillation_ctrl/airframes/* PX4-Autopilot/ROMFS/px4fmu_common/init.d-posix/airframes
 
 add model names to _CmakeLists.txt_ in same 'airframe' folder (with number... 4000_spiri and 4001_spiri_with_tether)
 
@@ -105,7 +96,7 @@ this should not be a necessary step and will be changed in the future
 
 copy (or add) files from px4_launch directory to '~/PX4-Autopilot/launch'
 
-	cp -R ~catkin_ws/src/oscillation_ctrl/px4_launch/* ~/PX4-Autopilot/launch
+	cp -R ~/catkin_ws/src/oscillation_ctrl/px4_launch/* ~/PX4-Autopilot/launch
 #### Change devel/setup.bash
 In catkin_ws (or any working directory) add to devel/setup.bash:
 
@@ -124,25 +115,23 @@ In catkin_ws (or any working directory) add to devel/setup.bash:
 - 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:
+- __IMPORTANT:__ in order for jinja file to work, the following needs to be added to the _CMakeLists.txt_ in the _~/PX4-Autopilot/Tools/sitl_gazebo_ folder. This creates a custom command to build the tether file whenever a change is done to it. (Currently, it should be added after Ln 290 - may change in future)
-
 
 		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)
 
+- This should be added in the line below VERBATIM in the add_custom_command function, which should be Ln 290). It should be part of the add_custom_command() after the 'else' statement
 
 ## ROS NODES
 ### LinkState.py 
-determines payload load angles and their rates (theta and phi) using Gazebo (needs to be made more robust), as well as determines tether length and keeps track of variables needed in case of step test.
+determines payload load angles and their rates (theta and phi) using Gazebo (needs to be made more robust), as well as determines tether length and keeps track of variables needed in case of a step or square test.
 
 __Publishes to__:
 
-	/status/twoBody_status # localization and angles
+	/status/twoBody_status # no longer used but keeps track of payload and vehicle position
-	/status/load_angles    # payload angles (and tates) relative to vehicle
+	/status/load_angles    # payload angles (and states) relative to vehicle
 	/status/path_follow    # boolean to run trajectory test
 __Subscribes to__:
 
@@ -164,7 +153,6 @@ takes in desired position (xd) and determines smooth path trajectory.
 __Publishes to__:
 
 	/reference/path         # smooth path
-	/reference/flatsetpoint # needed to determine thrust
 
 __Subscribes to__:
 
@@ -180,7 +168,7 @@ determines forces on drone needed based on smooth path and feedback to dampen os
 
 __Publishes to__:
 
-	/command/quaternions # attitude commands
+	/command/att_target # attitude commands
 
 __Subscribes to__:
 
@@ -190,8 +178,6 @@ __Subscribes to__:
 	/mavros/local_position/velocity_body
 	/mavros/imu/data
 
-- 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.
 
@@ -202,7 +188,7 @@ __Publishes to__:
 
 __Subscribes to__:
 
-	/command/quaternions	
+	/command/att_target	
 	/command/bodyrate_command	
 	/mavros/state
 

src/gazebo_setMasses.py

@@ -1,65 +0,0 @@
-#!/usr/bin/env python2.7
-
-### Cesar Rodriguez Jul 2022
-### Sets mass of links in gazebo to desired values
-
-import rospy
-from gazebo_msgs.srv import SetLinkProperties
-
-
-class Main:
-    def __init__(self):
-        self.tstart		= rospy.get_time() # Keep track of the start time
-        while self.tstart == 0.0:				 # Need to make sure get_rostime works
-            self.tstart		= rospy.get_time()
-
-		# Constants
-        self.param_exists = False
-        while self.param_exists == False:
-            if rospy.has_param('drone_mass'):
-                self.drone_m	= rospy.get_param('drone_mass') # wait time
-                self.param_exists = True
-            elif rospy.has_param('sim/drone_mass'):
-                self.drone_m	= rospy.get_param('sim/drone_mass') # wait time
-                self.param_exists = True
-            elif rospy.get_time() - self.tstart >= 3.0:
-                self.drone_m = 1.0
-                rospy.loginfo('DRONE MASS NOT FOUND IN CONFIG FILE')
-                break
-
-        self.param_exists = False
-        while self.param_exists == False:
-            if rospy.has_param('pload_mass'):
-                self.pl_m	= rospy.get_param('pload_mass') # wait time
-                self.param_exists = True
-            elif rospy.has_param('sim/pload_mass'):
-                self.pl_m	= rospy.get_param('sim/pload_mass') # wait time
-                self.param_exists = True
-            elif rospy.get_time() - self.tstart >= 3.0:
-                self.pl_m = 0.0
-                rospy.loginfo('PLOAD MASS NOT FOUND IN CONFIG FILE')
-                break
-
-        self.drone_ids	  = ['spiri_with_tether::spiri::base', 'spiri::base']
-        self.pload_id	  = 'spiri_with_tether::mass::payload'
-        self.service      = '/gazebo/set_link_properties'
-
-        self.set_mass = rospy.ServiceProxy(self.service,SetLinkProperties)
-
-        for name in self.drone_ids:
-            self.set_drone_mass = self.set_mass(link_name=name,mass=self.drone_m)
-            if self.set_drone_mass.success:
-                rospy.loginfo('Set "%s" mass to: %.2f kg', name,self.drone_m)
-                break
-
-        self.set_pload_mass = self.set_mass(link_name=self.pload_id,mass=self.pl_m)
-        if self.set_pload_mass.success:
-            rospy.loginfo('Set "%s" mass to: %.2f kg', self.pload_id,self.pl_m)
-        
-
-if __name__=="__main__":
-
-    # Initiate ROS node
-    rospy.init_node('SetMassNode',anonymous=False)
-    Main() # create class object
-