Merge branch 'master' of https://git.spirirobotics.com/cesar.alejandro/oscillation_ctrl
This commit is contained in:
commit
624a08af63
40
README.md
40
README.md
|
|
@ -51,16 +51,7 @@ Initilize rosdep:
|
|||
|
||||
## 2) PX4 Environment Development
|
||||
|
||||
### Download PX4 source code
|
||||
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
|
||||
Install [PX4 Firmware](https://docs.px4.io/main/en/dev_setup/dev_env_linux_ubuntu.html#rosgazebo). Then:
|
||||
|
||||
#### 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/worlds/* ~/PX4-Autopilot/Tools/sitl_gazebo/worlds
|
||||
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
|
||||
|
||||
### 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/load_angles # payload angles (and tates) relative to vehicle
|
||||
/status/twoBody_status # no longer used but keeps track of payload and vehicle position
|
||||
/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
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
||||
Loading…
Reference in New Issue