forked from cesar.alejandro/oscillation_ctrl
Fixing conflicts
This commit is contained in:
commit
f39d7c993d
|
|
@ -9,6 +9,8 @@ src/land_client.py
|
|||
src/MoCap_*.py
|
||||
src/Mocap_*.py
|
||||
src/mocap_*
|
||||
src/segmented_tether.py
|
||||
src/segmented_tether_fast.py
|
||||
msg/Marker.msg
|
||||
msg/Markers.msg
|
||||
*.rviz
|
||||
|
|
|
|||
180
README.md
180
README.md
|
|
@ -2,8 +2,6 @@
|
|||
|
||||
Repo containing oscillation damping controller for tether missions + instructions how to to set up
|
||||
|
||||
WARNING: Currently repo is only for Ubuntu 18.04 and ROS Melodic, work is being made to be upgrade to Ubuntu 20.04 and ROS Noetic
|
||||
|
||||
Cesar Rodriguez
|
||||
|
||||
cesar.rodriguez@spirirobotics.com
|
||||
|
|
@ -12,69 +10,13 @@ February 2022
|
|||
|
||||
Steps to recreate stable PX4 environment + working repo
|
||||
|
||||
### WARNING
|
||||
|
||||
Currently repo is only for Ubuntu 18.04 and ROS Melodic, work is being made to be upgraded to Ubuntu 20.04 and ROS Noetic
|
||||
|
||||
# Setup
|
||||
|
||||
## 1) Installing ROS Melodic
|
||||
|
||||
### Setup sources.list
|
||||
sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
|
||||
### Setup keys
|
||||
sudo apt install curl # if you haven't already installed curl
|
||||
curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | sudo apt-key add -
|
||||
### Installation
|
||||
sudo apt update
|
||||
sudo apt install ros-melodic-desktop-full
|
||||
### Environment setup
|
||||
echo "source /opt/ros/melodic/setup.bash" >> ~/.bashrc
|
||||
### Dependencies
|
||||
sudo apt install python-rosdep python-rosinstall python-rosinstall-generator python-wstool build-essential
|
||||
|
||||
Initilize rosdep:
|
||||
|
||||
sudo apt install python-rosdep
|
||||
sudo rosdep init
|
||||
rosdep update
|
||||
### PX4 Dependencies
|
||||
sudo apt-get install python-catkin-tools python-rosinstall-generator -y
|
||||
wstool init ~/catkin_ws/src
|
||||
### MAVLINK
|
||||
rosinstall_generator --rosdistro melodic mavlink | tee /tmp/mavros.rosinstall
|
||||
### MAVROS
|
||||
rosinstall_generator --upstream mavros | tee -a /tmp/mavros.rosinstall
|
||||
### Create workspace and deps
|
||||
cd ~/catkin_ws
|
||||
wstool merge -t src /tmp/mavros.rosinstall
|
||||
wstool update -t src -j4
|
||||
rosdep install --from-paths src --ignore-src -y
|
||||
### Install geographic datasets
|
||||
cd ~/catkin_ws
|
||||
sudo ./src/mavros/mavros/scripts/install_geographiclib_datasets.sh
|
||||
### Build source
|
||||
cd ~/catkin_ws
|
||||
catkin build
|
||||
|
||||
## 2) PX4 Environment Development
|
||||
Install PX4 firmware and ROS melodic dependencies:
|
||||
|
||||
git clone https://github.com/PX4/PX4-Autopilot.git --recursive
|
||||
git checkout 601c588294973caf105b79a23f7587c6b991bb05
|
||||
bash ./PX4-Autopilot/Tools/setup/ubuntu.sh
|
||||
|
||||
|
||||
Install [PX4 Firmware](https://docs.px4.io/main/en/dev_setup/dev_env_linux_ubuntu.html#rosgazebo). Then:
|
||||
|
||||
#### Download QGroundControl:
|
||||
cd to desired directory to install QGroundControl. For example, Desktop
|
||||
|
||||
cd ~/<desired_directory>
|
||||
wget https://github.com/mavlink/qgroundcontrol/releases/download/v4.2.0/QGroundControl.AppImage
|
||||
chmod +x QGroundControl.AppImage
|
||||
|
||||
#### Build Gazebo Sim
|
||||
cd ~/PX4-Autopilot
|
||||
make px4_sitl gazebo
|
||||
|
||||
## 3) Set up oscillation_ctrl
|
||||
## 1) Set up oscillation_ctrl
|
||||
### Install xterm
|
||||
sudo apt-get update -y
|
||||
sudo apt-get install -y xterm
|
||||
|
|
@ -83,44 +25,64 @@ cd to desired directory to install QGroundControl. For example, Desktop
|
|||
cd ~/catkin_ws/src
|
||||
git clone https://git.spirirobotics.com/cesar.alejandro/oscillation_ctrl.git
|
||||
|
||||
To get the specific version of mavros_msgs for oscillation_ctrl:
|
||||
### Run bash script and build
|
||||
bash /.oscillation_ctrl/px4_setup/ubuntu_sim_ros_melodic.sh
|
||||
This bash script is taken from PX4 Firmware but sets certain versions of dependencies to work with oscillation_ctrl. It also installs common ROS dependencies, ROS Melodic, Gazebo 9, and MAVROS. It should build oscillation_ctrl and add the source path to your .bashrc. If it does not, then run:
|
||||
|
||||
cp -R ~/catkin_ws/src/oscillation_ctrl/px4_setup/rosinstall.txt /tmp/mavros.rosinstall
|
||||
wstool merge -t src /tmp/mavros.rosinstall
|
||||
wstool update -t src -j4
|
||||
rosdep install --from-paths src --ignore-src -y
|
||||
cd ~/catkin_ws
|
||||
catkin build
|
||||
source devel/setup.bash
|
||||
|
||||
#### Now, we finish building the PX4 package
|
||||
If you do not want to source every time, add 'source ~/catkin_ws/devel/setup.bash' to your .bashrc
|
||||
|
||||
## 2) PX4 Environment Development
|
||||
Install PX4 firmware and dependencies:
|
||||
|
||||
git clone https://github.com/PX4/PX4-Autopilot.git --recursive
|
||||
git checkout 601c588294973caf105b79a23f7587c6b991bb05
|
||||
bash ./PX4-Autopilot/Tools/setup/ubuntu.sh
|
||||
|
||||
There will be prompts on the screen, hit 'u' and 'enter' as they come up.
|
||||
|
||||
### Add models, world, and airframe files
|
||||
To add the necessary Spiri Mu files to PX4, run:
|
||||
|
||||
cd ca
|
||||
bash /.oscillation_ctrl/px4_setup/px4_bash.sh
|
||||
|
||||
### Add airframes to cmake targets
|
||||
Add 'spiri' and 'spiri_with_tether' airframe names in _~/PX4-Autopilot/platforms/posix/cmake/sitl_target.cmake_ under set(models...
|
||||
|
||||
__do not add their airframe number! e.i. 4000 or 4001__
|
||||
|
||||
### Download QGroundControl:
|
||||
cd to desired directory to install QGroundControl. For example, Desktop.
|
||||
|
||||
cd ~/<example_Desktop>
|
||||
wget https://github.com/mavlink/qgroundcontrol/releases/download/v4.2.0/QGroundControl.AppImage
|
||||
chmod +x QGroundControl.AppImage
|
||||
|
||||
### Finish Building PX4 package
|
||||
cd ~/PX4-Autopilot
|
||||
DONT_RUN=1 make px4_sitl_default gazebo
|
||||
|
||||
If if fails and you see
|
||||
|
||||
gzerver: symbol lookup error...
|
||||
|
||||
then run:
|
||||
|
||||
sudo apt upgrade libignition-math2
|
||||
|
||||
If all is good, run:
|
||||
|
||||
source Tools/setup_gazebo.bash $(pwd) $(pwd)/build/px4_sitl_default
|
||||
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd)
|
||||
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd)/Tools/sitl_gazebo
|
||||
roslaunch px4 posix_sitl.launch
|
||||
|
||||
If this works, we can move on.
|
||||
PX4 should launch with MAVROS
|
||||
|
||||
### 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
|
||||
|
||||
### 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/18.04/* ~/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)
|
||||
|
||||
add airframe name in _~/PX4-Autopilot/platforms/posix/cmake/sitl_target.cmake_ (no number!)
|
||||
### Add necessary launch files
|
||||
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
|
||||
#### Change devel/setup.bash
|
||||
In catkin_ws (or any working directory) add to devel/setup.bash:
|
||||
|
||||
|
|
@ -144,23 +106,13 @@ Finally, source your setup.bash file
|
|||
- 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_ 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.
|
||||
|
||||
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
|
||||
|
||||
_oscillation_ctrl/px4_setup/CMakeLists.txt has these changes therefore all that is needed is:
|
||||
|
||||
cp -R ~/catkin_ws/src/oscillation_ctrl/px4_setup/CMakeLists.txt ~/PX4-Autopilot/Tools/sitl_gazebo/
|
||||
|
||||
# oscillation_ctrl info
|
||||
# Oscillation_ctrl Info
|
||||
Info pertaining to oscillation_ctrl repo such as what the ROS nodes do, different ROS parameters, etc.
|
||||
|
||||
## 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 a step or square 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__:
|
||||
|
||||
|
|
@ -182,7 +134,7 @@ __Subscribes to__:
|
|||
/reference/waypoints
|
||||
|
||||
### ref_signalGen.py
|
||||
takes in desired position (xd) and determines smooth path trajectory.
|
||||
Takes in desired position (xd) and determines smooth path trajectory.
|
||||
|
||||
__Publishes to__:
|
||||
|
||||
|
|
@ -198,7 +150,7 @@ __Subscribes to__:
|
|||
/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.
|
||||
Determines forces on drone needed based on smooth path and feedback to dampen oscillations. From the forces needed, it publishes attitude commands.
|
||||
|
||||
__Publishes to__:
|
||||
|
||||
|
|
@ -213,9 +165,9 @@ __Subscribes to__:
|
|||
/mavros/imu/data
|
||||
|
||||
### set_ploadmass.py
|
||||
sets the payload mass to be _pload_mass_ value in the _spiri_param.yaml_
|
||||
Sets the payload mass to be _pload_mass_ value in the _spiri_param.yaml_
|
||||
### 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.
|
||||
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__:
|
||||
|
||||
|
|
@ -262,3 +214,19 @@ __pload_mass:__ sets the payload mass in simulation without having to change the
|
|||
|
||||
## Frequent Issues
|
||||
Will populate this section with frequently faces issues
|
||||
|
||||
### No header files error:
|
||||
In oscillation_ctrl/CMakeLists.txt, change:
|
||||
|
||||
add_dependencies(pathFollow_node ${${PROJECT_NAME}EXPORTEDTARGETS} ${catkin_EXPORTED_TARGETS})
|
||||
|
||||
to:
|
||||
|
||||
add_dependencies(pathFollow_node oscillation_ctrl_generate_messages_cpp)
|
||||
|
||||
### Cannot build px4_sitl
|
||||
Normally, this is solved by running:
|
||||
|
||||
make clean
|
||||
|
||||
And redo command you were trying to run. If problem persists, rerun ubuntu_sim_ros_melodic.sh and ubuntu.sh and try again.
|
||||
|
|
@ -1,54 +0,0 @@
|
|||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- MAVROS posix SITL environment launch script -->
|
||||
<!-- launches MAVROS, PX4 SITL, Gazebo environment, and spawns vehicle -->
|
||||
<!-- vehicle pose -->
|
||||
<arg name="x" default="0"/>
|
||||
<arg name="y" default="0"/>
|
||||
<arg name="z" default="0"/>
|
||||
<arg name="R" default="0"/>
|
||||
<arg name="P" default="0"/>
|
||||
<arg name="Y" default="0"/>
|
||||
<!-- vehicle model and world -->
|
||||
<arg name="est" default="ekf2"/>
|
||||
<arg name="vehicle" default="spiri_mocap"/>
|
||||
<arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/citadel_hill_world.world"/>
|
||||
<arg name="sdf" default="$(find mavlink_sitl_gazebo)/models/$(arg vehicle)/$(arg vehicle).sdf"/>
|
||||
|
||||
<!-- gazebo configs -->
|
||||
<arg name="gui" default="false"/>
|
||||
<arg name="debug" default="false"/>
|
||||
<arg name="verbose" default="false"/>
|
||||
<arg name="paused" default="false"/>
|
||||
<arg name="respawn_gazebo" default="false"/>
|
||||
<!-- MAVROS configs -->
|
||||
<arg name="fcu_url" default="udp://:14540@localhost:14557"/>
|
||||
<arg name="respawn_mavros" default="false"/>
|
||||
<!-- PX4 configs -->
|
||||
<arg name="interactive" default="true"/>
|
||||
<!-- PX4 SITL and Gazebo -->
|
||||
<include file="$(find px4)/launch/posix_sitl.launch">
|
||||
<arg name="x" value="$(arg x)"/>
|
||||
<arg name="y" value="$(arg y)"/>
|
||||
<arg name="z" value="$(arg z)"/>
|
||||
<arg name="R" value="$(arg R)"/>
|
||||
<arg name="P" value="$(arg P)"/>
|
||||
<arg name="Y" value="$(arg Y)"/>
|
||||
<arg name="world" value="$(arg world)"/>
|
||||
<arg name="vehicle" value="$(arg vehicle)"/>
|
||||
<arg name="sdf" value="$(arg sdf)"/>
|
||||
<arg name="gui" value="$(arg gui)"/>
|
||||
<arg name="interactive" value="$(arg interactive)"/>
|
||||
<arg name="debug" value="$(arg debug)"/>
|
||||
<arg name="verbose" value="$(arg verbose)"/>
|
||||
<arg name="paused" value="$(arg paused)"/>
|
||||
<arg name="respawn_gazebo" value="$(arg respawn_gazebo)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find mavros)/launch/px4.launch">
|
||||
<!-- GCS link is provided by SITL -->
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="respawn_mavros" value="$(arg respawn_mavros)"/>
|
||||
</include>
|
||||
</launch>
|
||||
|
|
@ -1,54 +0,0 @@
|
|||
<?xml version="1.0"?>
|
||||
<launch>
|
||||
<!-- MAVROS posix SITL environment launch script -->
|
||||
<!-- launches MAVROS, PX4 SITL, Gazebo environment, and spawns vehicle -->
|
||||
<!-- vehicle pose -->
|
||||
<arg name="x" default="0"/>
|
||||
<arg name="y" default="0"/>
|
||||
<arg name="z" default="0"/>
|
||||
<arg name="R" default="0"/>
|
||||
<arg name="P" default="0"/>
|
||||
<arg name="Y" default="0"/>
|
||||
<!-- vehicle model and world -->
|
||||
<arg name="est" default="ekf2"/>
|
||||
<arg name="vehicle" default="spiri_with_tether"/>
|
||||
<arg name="world" default="$(find mavlink_sitl_gazebo)/worlds/citadel_hill_world.world"/>
|
||||
<arg name="sdf" default="$(find mavlink_sitl_gazebo)/models/$(arg vehicle)/$(arg vehicle).sdf"/>
|
||||
|
||||
<!-- gazebo configs -->
|
||||
<arg name="gui" default="false"/>
|
||||
<arg name="debug" default="false"/>
|
||||
<arg name="verbose" default="false"/>
|
||||
<arg name="paused" default="false"/>
|
||||
<arg name="respawn_gazebo" default="false"/>
|
||||
<!-- MAVROS configs -->
|
||||
<arg name="fcu_url" default="udp://:14540@localhost:14557"/>
|
||||
<arg name="respawn_mavros" default="false"/>
|
||||
<!-- PX4 configs -->
|
||||
<arg name="interactive" default="true"/>
|
||||
<!-- PX4 SITL and Gazebo -->
|
||||
<include file="$(find px4)/launch/posix_sitl.launch">
|
||||
<arg name="x" value="$(arg x)"/>
|
||||
<arg name="y" value="$(arg y)"/>
|
||||
<arg name="z" value="$(arg z)"/>
|
||||
<arg name="R" value="$(arg R)"/>
|
||||
<arg name="P" value="$(arg P)"/>
|
||||
<arg name="Y" value="$(arg Y)"/>
|
||||
<arg name="world" value="$(arg world)"/>
|
||||
<arg name="vehicle" value="$(arg vehicle)"/>
|
||||
<arg name="sdf" value="$(arg sdf)"/>
|
||||
<arg name="gui" value="$(arg gui)"/>
|
||||
<arg name="interactive" value="$(arg interactive)"/>
|
||||
<arg name="debug" value="$(arg debug)"/>
|
||||
<arg name="verbose" value="$(arg verbose)"/>
|
||||
<arg name="paused" value="$(arg paused)"/>
|
||||
<arg name="respawn_gazebo" value="$(arg respawn_gazebo)"/>
|
||||
</include>
|
||||
<!-- MAVROS -->
|
||||
<include file="$(find oscillation_ctrl)/launch/px4.launch">
|
||||
<!-- GCS link is provided by SITL -->
|
||||
<arg name="gcs_url" value=""/>
|
||||
<arg name="fcu_url" value="$(arg fcu_url)"/>
|
||||
<arg name="respawn_mavros" value="$(arg respawn_mavros)"/>
|
||||
</include>
|
||||
</launch>
|
||||
|
|
@ -1,88 +0,0 @@
|
|||
<?xml version="1.0"?>
|
||||
<!--
|
||||
Launch file to use klausen oscillaton damping ctrl in Gazebo
|
||||
/-->
|
||||
<launch>
|
||||
<arg name='test' default="none"/>
|
||||
<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" />
|
||||
<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="connection_type" default="wifi"/>
|
||||
|
||||
<group ns="status">
|
||||
<rosparam file="$(find oscillation_ctrl)/config/mocapLab_params.yaml" />
|
||||
<param name="test_type" value="$(arg test)"/>
|
||||
</group>
|
||||
|
||||
<!-- LOCALIZES DRONE & DETERMINES LOAD ANGLES -->
|
||||
<node
|
||||
pkg="oscillation_ctrl"
|
||||
type="Mocap_Bridge.py"
|
||||
name="localize_node"
|
||||
launch-prefix="xterm -e"
|
||||
/>
|
||||
<!-- DETERMINES DESIRED POSITION -->
|
||||
<node
|
||||
pkg="oscillation_ctrl"
|
||||
type="wpoint_tracker.py"
|
||||
name="waypoints_server"
|
||||
/>
|
||||
<!-- CREATES DESIRED TRAJECTORY/ REFERENCE SIGNAL -->
|
||||
<node
|
||||
pkg="oscillation_ctrl"
|
||||
type="ref_signalGen.py"
|
||||
name="refSignal_node"
|
||||
launch-prefix="xterm -e"
|
||||
/>
|
||||
<!-- DETERMINES DESIRED ATTITUDE AND THRUST BASED ON REF. SIG. -->
|
||||
<node
|
||||
pkg="oscillation_ctrl"
|
||||
type="klausen_control.py"
|
||||
name="klausenCtrl_node"
|
||||
launch-prefix="xterm -e"
|
||||
output='screen'
|
||||
/>
|
||||
<!-- PUBLISHES DESIRED COMMANDS -->
|
||||
<node
|
||||
pkg="oscillation_ctrl"
|
||||
type="mocap_pathFollow_node"
|
||||
name="mocap_pathFollow_node"
|
||||
launch-prefix="xterm -e"
|
||||
/>
|
||||
<!-- RUNS DIFFERENT TESTS IF DESIRED -->
|
||||
<group if="$(eval test != 'none')">
|
||||
<node
|
||||
pkg="oscillation_ctrl"
|
||||
type="perform_test.py"
|
||||
name="test_node"
|
||||
launch-prefix="xterm -e"
|
||||
/>
|
||||
</group>
|
||||
|
||||
<!-- RUNS TEST -->
|
||||
<node
|
||||
pkg="oscillation_ctrl"
|
||||
type="mocap_runTest.py"
|
||||
name="mocap_Test"
|
||||
launch-prefix="xterm -e"
|
||||
/>
|
||||
|
||||
<!-- Cortex bridge launch -->
|
||||
<include file="$(find cortex_bridge)/launch/cortex_bridge.launch">
|
||||
<!--param name="local_ip" value="$(param local_ip)" /-->
|
||||
</include>
|
||||
|
||||
<!-- MAVROS launch -->
|
||||
<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_protocol" value="$(arg fcu_protocol)" />
|
||||
<arg name="fcu_url" value="$(arg fcu_url)" />
|
||||
<arg name="gcs_url" value="$(arg gcs_url)" />
|
||||
</include>
|
||||
</launch>
|
||||
|
|
@ -1,64 +0,0 @@
|
|||
<?xml version="1.0"?>
|
||||
<!--
|
||||
Launch file to use klausen oscillaton damping ctrl in Gazebo
|
||||
/-->
|
||||
<launch>
|
||||
<group ns="status">
|
||||
<rosparam file="$(find oscillation_ctrl)/config/mocapGazebo_params.yaml" />
|
||||
</group>
|
||||
<arg name="model" default="headless_spiri"/>
|
||||
<node
|
||||
pkg="oscillation_ctrl"
|
||||
type="MoCap_Localization_fake.py"
|
||||
name="fakeMocap_node"
|
||||
launch-prefix="xterm -e"
|
||||
/>
|
||||
<!-- DETERMINES DESIRED POSITION -->
|
||||
<node
|
||||
pkg="oscillation_ctrl"
|
||||
type="wpoint_tracker.py"
|
||||
name="waypoints_server"
|
||||
/>
|
||||
<!-- CREATES DESIRED TRAJECTORY/ REFERENCE SIGNAL -->
|
||||
<node
|
||||
pkg="oscillation_ctrl"
|
||||
type="ref_signalGen.py"
|
||||
name="refSignal_node"
|
||||
/>
|
||||
<!-- DETERMINES DESIRED ATTITUDE AND THRUST BASED ON REF. SIG. -->
|
||||
<node
|
||||
pkg="oscillation_ctrl"
|
||||
type="klausen_control.py"
|
||||
name="klausenCtrl_node"
|
||||
launch-prefix="xterm -e"
|
||||
/>
|
||||
<!-- PUBLISHES DESIRED COMMANDS -->
|
||||
<node
|
||||
pkg="oscillation_ctrl"
|
||||
type="mocap_pathFollow_node"
|
||||
name="mocap_pathFollow_node"
|
||||
launch-prefix="xterm -e"
|
||||
/>
|
||||
<!-- RUNS TEST -->
|
||||
<node
|
||||
pkg="oscillation_ctrl"
|
||||
type="mocap_runTest.py"
|
||||
name="mocap_Test"
|
||||
launch-prefix="xterm -e"
|
||||
/>
|
||||
<!-- SETS PLOAD MASS -->
|
||||
<node
|
||||
pkg="oscillation_ctrl"
|
||||
type="set_ploadmass.py"
|
||||
name="set_ploadmass"
|
||||
output="screen"
|
||||
/>
|
||||
<!-- PX4 LAUNCH -->
|
||||
<include file="$(find oscillation_ctrl)/launch/$(arg model)_mocap.launch">
|
||||
</include>
|
||||
|
||||
<!-- Tf LAUNCH -->
|
||||
<node pkg="tf" type="static_transform_publisher" name="map_to_pload_imu" args="0 0 0 0 0 0 map pload_imu 10" />
|
||||
<node pkg="tf" type="static_transform_publisher" name="map_to_pload" args="0 0 0 0 0 0 map payload 10" />
|
||||
</launch>
|
||||
|
||||
|
|
@ -1,20 +0,0 @@
|
|||
sudo apt-get install libprotobuf-dev=3.0.0-9.1ubuntu1 libprotoc-dev=3.0.0-9.1ubuntu1 protobuf-compiler=3.0.0-9.1ubuntu1
|
||||
|
||||
commit: 601c588294973caf105b79a23f7587c6b991bb05
|
||||
|
||||
Qground control:
|
||||
wget https://github.com/mavlink/qgroundcontrol/releases/download/v4.2.0/QGroundControl.AppImage
|
||||
chmod +x QGroundControl.AppImage
|
||||
|
||||
mavros/mavlink:
|
||||
cp -R ~/catkin_ws/src/oscillation_ctrl/bash_scripts/rosinstall.txt /tmp/mavros.rosinstall
|
||||
wstool merge -t src /tmp/mavros.rosinstall
|
||||
wstool update -t src -j4
|
||||
rosdep install --from-paths src --ignore-src -y
|
||||
catkin build
|
||||
|
||||
steps:
|
||||
wget https://raw.githubusercontent.com/PX4/Devguide/master/build_scripts/ubuntu_sim_ros_melodic.sh
|
||||
bash ubuntu_sim_ros_melodic.sh
|
||||
git clone https://github.com/PX4/PX4-Autopilot.git --recursive
|
||||
git checkout 601c588294973caf105b79a23f7587c6b991bb05
|
||||
|
|
@ -1,243 +0,0 @@
|
|||
#!/usr/bin/env python2.7
|
||||
|
||||
### Cesar Rodriguez Mar 2022
|
||||
### Script to simulate mocap readings and see how PX4 behaves
|
||||
|
||||
import rospy, tf
|
||||
import rosservice
|
||||
import time
|
||||
import math
|
||||
import random
|
||||
from tf.transformations import *
|
||||
from oscillation_ctrl.msg import TetheredStatus, LoadAngles
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
from gazebo_msgs.srv import GetLinkState
|
||||
from std_msgs.msg import Bool
|
||||
|
||||
class Main:
|
||||
|
||||
def __init__(self):
|
||||
|
||||
# rate(s)
|
||||
pub_rate = 50 # rate for the publisher method, specified in Hz -- 20 Hz
|
||||
loc_rate = 60 # rate we want to localize vehicle -- 60 Hz
|
||||
self.dt = 1.0/loc_rate
|
||||
|
||||
self.user_fback = True
|
||||
|
||||
rospy.sleep(5) # Sleep for 5 sec. Need to give time to Gazebo to run
|
||||
|
||||
# Variables needed for testing start
|
||||
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()
|
||||
|
||||
# initialize variables
|
||||
self.tetherL = 0.0 # Tether length
|
||||
self.has_run = False # Boolean to keep track of first run instance
|
||||
self.phibuf = 0.0 # Need buffers to determine their rates
|
||||
self.thetabuf = 0.0 #
|
||||
self.pload = False # Check if payload exists
|
||||
# Max dot values to prevent 'blowup'
|
||||
self.angledot_max = 2.0
|
||||
self.drone_eul = [0.0,0.0,0.0]
|
||||
|
||||
# variables for message gen
|
||||
#self.buff_pose1 = PoseStamped()
|
||||
self.drone_pose = PoseStamped()
|
||||
self.pload_pose = PoseStamped()
|
||||
self.load_angles = LoadAngles()
|
||||
self.twobody_status = TetheredStatus()
|
||||
self.drone_id = 'spiri_with_tether::spiri::base'
|
||||
self.pload_id = 'spiri_with_tether::mass::payload'
|
||||
|
||||
# service(s)
|
||||
self.service1 = '/gazebo/get_link_state'
|
||||
self.service2 = '/gazebo/set_link_properties'
|
||||
|
||||
|
||||
# need service list to check if models have spawned
|
||||
self.service_list = rosservice.get_service_list()
|
||||
|
||||
# wait for service to exist
|
||||
while self.service1 not in self.service_list:
|
||||
print ("Waiting for models to spawn...")
|
||||
self.service_list = rosservice.get_service_list()
|
||||
if rospy.get_time() - self.tstart >= 10.0:
|
||||
break
|
||||
|
||||
# publisher(s)
|
||||
self.twobody_pub = rospy.Publisher('/status/twoBody_status', TetheredStatus, queue_size=1)
|
||||
self.loadAng_pub = rospy.Publisher('/status/load_angles', LoadAngles, queue_size=1)
|
||||
#self.pub_wd = rospy.Publisher('/status/path_follow', Bool, queue_size=1)
|
||||
|
||||
#self.pub_timer = rospy.Timer(rospy.Duration(1.0/rate), self.link_state)
|
||||
#self.path_timer = rospy.Timer(rospy.Duration(40.0/rate), self.path_follow)
|
||||
|
||||
### Since there is no tether, we can publish directly to mavros
|
||||
self.visionPose_pub = rospy.Publisher('/mavros/vision_pose/pose', PoseStamped, queue_size=1)
|
||||
|
||||
self.loc_timer = rospy.Timer(rospy.Duration(1.0/loc_rate), self.mocap_localize)
|
||||
self.pub_timer = rospy.Timer(rospy.Duration(1.0/pub_rate), self.publisher)
|
||||
|
||||
# subscriber(s)
|
||||
|
||||
def euler_array(self,orientation):
|
||||
"""
|
||||
Takes in pose msg object and outputs array of euler angs:
|
||||
eul[0] = Roll
|
||||
eul[1] = Pitch
|
||||
eul[2] = Yaw
|
||||
"""
|
||||
eul = euler_from_quaternion([orientation.x,
|
||||
orientation.y,
|
||||
orientation.z,
|
||||
orientation.w])
|
||||
return eul
|
||||
|
||||
def mocap_localize(self,loc_timer):
|
||||
"""
|
||||
Uses Gazebo to simulate MoCap
|
||||
"""
|
||||
try:
|
||||
# State which service we are querying
|
||||
get_P = rospy.ServiceProxy(self.service1,GetLinkState)
|
||||
|
||||
# Set reference frame
|
||||
reference = '' # world ref frame
|
||||
|
||||
# Establish links needed --> Spiri base and payload
|
||||
# P = Position vector
|
||||
drone_P = get_P(self.drone_id,reference)
|
||||
|
||||
# Check if payload is part of simulation
|
||||
if not drone_P.success:
|
||||
self.drone_id = 'spiri_mocap::base'
|
||||
drone_P = get_P(self.drone_id,reference) # i.e. no payload
|
||||
|
||||
self.drone_P = drone_P
|
||||
pload_P = get_P(self.pload_id,reference)
|
||||
if pload_P.success: self.pload = True
|
||||
if not self.has_run:
|
||||
if self.pload == True:
|
||||
# Get tether length based off initial displacement
|
||||
self.tetherL = math.sqrt((drone_P.link_state.pose.position.x -
|
||||
pload_P.link_state.pose.position.x)**2 +
|
||||
(drone_P.link_state.pose.position.y -
|
||||
pload_P.link_state.pose.position.y)**2 +
|
||||
(drone_P.link_state.pose.position.z -
|
||||
pload_P.link_state.pose.position.z)**2)
|
||||
rospy.set_param('status/tether_length',self.tetherL)
|
||||
|
||||
else:
|
||||
self.tetherL = 0.0
|
||||
self.has_run = True
|
||||
|
||||
# Need to detemine their location to get angle of deflection
|
||||
# Drone
|
||||
drone_Px = drone_P.link_state.pose.position.x
|
||||
drone_Py = drone_P.link_state.pose.position.y
|
||||
# Get drone orientation
|
||||
|
||||
if self.pload == True: # If there is payload, determine the variables
|
||||
self.twobody_status.pload = True
|
||||
# Pload
|
||||
pload_Px = pload_P.link_state.pose.position.x
|
||||
pload_Py = pload_P.link_state.pose.position.y
|
||||
|
||||
# Determine theta (pitch)
|
||||
x_sep = pload_Px - drone_Px
|
||||
|
||||
if math.fabs(x_sep) >= self.tetherL or x_sep == 0:
|
||||
self.load_angles.theta = 0
|
||||
else:
|
||||
self.load_angles.theta = math.asin(x_sep/self.tetherL)
|
||||
|
||||
# Determine thetadot
|
||||
# self.load_angles.thetadot = min(self.angledot_max,max((self.load_angles.theta - self.thetabuf)/self.dt,-self.angledot_max))
|
||||
self.load_angles.thetadot = (self.load_angles.theta - self.thetabuf)/self.dt
|
||||
self.thetabuf = self.load_angles.theta
|
||||
|
||||
# Determine phi (roll)
|
||||
y_sep = pload_Py - drone_Py
|
||||
|
||||
if math.fabs(y_sep) >= self.tetherL or y_sep == 0:
|
||||
self.load_angles.phi = 0
|
||||
else:
|
||||
self.load_angles.phi = -math.asin(y_sep/self.tetherL)
|
||||
|
||||
# Determine phidot
|
||||
# self.load_angles.phidot = min(self.angledot_max,max((self.load_angles.phi - self.phibuf)/self.dt,-self.angledot_max))
|
||||
self.load_angles.phidot = (self.load_angles.phi - self.phibuf)/self.dt
|
||||
self.phibuf = self.load_angles.phi # Update buffer
|
||||
|
||||
# save pload position
|
||||
self.twobody_status.pload_pos = pload_P.link_state.pose
|
||||
self.pload_pose.pose = self.twobody_status.pload_pos
|
||||
else: # Otherwise, vars = 0
|
||||
x_sep = self.load_angles.phi = self.load_angles.phidot = self.load_angles.theta = self.load_angles.thetadot = 0
|
||||
|
||||
# Populate message
|
||||
#self.status.drone_pos = drone_P.link_state.pose
|
||||
self.drone_pose.pose = drone_P.link_state.pose
|
||||
self.twobody_status.drone_pos = drone_P.link_state.pose
|
||||
|
||||
except rospy.ServiceException as e:
|
||||
rospy.loginfo("Get Link State call failed: {0}".format(e))
|
||||
|
||||
def add_noise(self):
|
||||
# self.drone_pose.pose.position.x = self.drone_pose.pose.position.x
|
||||
# self.drone_pose.pose.position.y = self.drone_pose.pose.position.y
|
||||
# self.drone_pose.pose.position.z = self.drone_pose.pose.position.z
|
||||
self.drone_pose.pose.orientation.x = self.drone_pose.pose.orientation.x + random.uniform(0,0.004)
|
||||
self.drone_pose.pose.orientation.y = self.drone_pose.pose.orientation.y + random.uniform(0,0.004)
|
||||
self.drone_pose.pose.orientation.z = self.drone_pose.pose.orientation.z + random.uniform(0,0.004)
|
||||
self.drone_pose.pose.orientation.w = self.drone_pose.pose.orientation.w + random.uniform(0,0.004)
|
||||
|
||||
def publisher(self,pub_timer):
|
||||
# add noise to signal
|
||||
self.add_noise()
|
||||
# fill out necesssary fields
|
||||
self.drone_pose.header.frame_id = "/map"
|
||||
self.drone_pose.header.stamp = rospy.Time.now()
|
||||
# load angles
|
||||
self.load_angles.header.stamp = rospy.Time.now()
|
||||
# twobody status
|
||||
self.twobody_status.header.frame_id = "/map"
|
||||
self.twobody_status.header.stamp = rospy.Time.now()
|
||||
# publish
|
||||
self.visionPose_pub.publish(self.drone_pose) # publish pose to mavros
|
||||
self.loadAng_pub.publish(self.load_angles) # publish load angles to controller
|
||||
self.twobody_pub.publish(self.twobody_status) # actual pose. Redundant but nice to have
|
||||
# get euler array for user feedback
|
||||
self.drone_eul = self.euler_array(self.drone_pose.pose.orientation)
|
||||
self.user_feedback()
|
||||
|
||||
def user_feedback(self):
|
||||
if self.user_fback:
|
||||
print ("\n")
|
||||
rospy.loginfo("")
|
||||
print ("drone pos.x: " + str(round(self.drone_pose.pose.position.x,2)))
|
||||
print ("drone pos.y: " + str(round(self.drone_pose.pose.position.y,2)))
|
||||
print ("drone pos.z: " + str(round(self.drone_pose.pose.position.z,2)))
|
||||
print ("Roll: " + str(round(self.drone_eul[0]*180/3.14,2)))
|
||||
print ("Pitch: " + str(round(self.drone_eul[1]*180/3.14,2)))
|
||||
print ("Yaw: " + str(round(self.drone_eul[2]*180/3.14,2)))
|
||||
if self.pload:
|
||||
print("Tether length: " + str(round(self.tetherL,2)))
|
||||
print("Theta: " + str(round(self.load_angles.theta*180/3.14,2)))
|
||||
print("Phi: " + str(round(self.load_angles.phi*180/3.14,2)))
|
||||
else:
|
||||
rospy.loginfo_once(self.tetherL)
|
||||
|
||||
if __name__=="__main__":
|
||||
|
||||
# Initiate ROS node
|
||||
rospy.init_node('MoCap_node',anonymous=False)
|
||||
try:
|
||||
Main() # create class object
|
||||
rospy.spin() # loop until shutdown signal
|
||||
|
||||
except rospy.ROSInterruptException:
|
||||
pass
|
||||
|
||||
|
|
@ -1,221 +0,0 @@
|
|||
/**
|
||||
* @file path_follow.cpp
|
||||
* @brief Offboard path trajectory tracking
|
||||
*/
|
||||
|
||||
#include <ros/ros.h>
|
||||
#include <sensor_msgs/NavSatFix.h>
|
||||
#include <oscillation_ctrl/RefSignal.h>
|
||||
#include <oscillation_ctrl/EulerAngles.h>
|
||||
#include <oscillation_ctrl/WaypointTrack.h>
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <geometry_msgs/Quaternion.h>
|
||||
#include <mavros_msgs/CommandBool.h>
|
||||
#include <mavros_msgs/SetMode.h>
|
||||
#include <mavros_msgs/State.h>
|
||||
#include <mavros_msgs/CommandTOL.h>
|
||||
#include <mavros_msgs/AttitudeTarget.h>
|
||||
#include <tf2/LinearMath/Quaternion.h>
|
||||
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
|
||||
#include <iostream>
|
||||
#include <cmath> // for std::abs
|
||||
|
||||
/********* CALLBACK FUNCTIONS **********************/
|
||||
|
||||
// Callback function which will save the current state of the autopilot.
|
||||
// Allows to check connection, arming, and Offboard tags*/
|
||||
mavros_msgs::State current_state;
|
||||
bool land = false;
|
||||
void state_cb(const mavros_msgs::State::ConstPtr& msg){
|
||||
current_state = *msg;
|
||||
}
|
||||
|
||||
// Cb to determine attitude target
|
||||
mavros_msgs::AttitudeTarget att_targ;
|
||||
void att_targ_cb(const mavros_msgs::AttitudeTarget::ConstPtr& msg){
|
||||
att_targ = *msg;
|
||||
}
|
||||
|
||||
// Cb to recieve pose information
|
||||
geometry_msgs::PoseStamped buff_pose;
|
||||
geometry_msgs::Quaternion q_init;
|
||||
geometry_msgs::PoseStamped mavPose;
|
||||
bool gps_read = false;
|
||||
double current_altitude;
|
||||
void mavPose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg){
|
||||
mavPose = *msg;
|
||||
current_altitude = mavPose.pose.position.z;
|
||||
while(gps_read == false){
|
||||
q_init = mavPose.pose.orientation;
|
||||
if(q_init.x == 0.0 && q_init.w == 0.0){
|
||||
ROS_INFO("Waiting...");
|
||||
} else {
|
||||
buff_pose.pose.orientation.x = 0.0;
|
||||
buff_pose.pose.orientation.y = 0.0;
|
||||
buff_pose.pose.orientation.z = 0.0;
|
||||
buff_pose.pose.orientation.w = 1.0;
|
||||
gps_read = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "path_follow");
|
||||
ros::NodeHandle nh;
|
||||
|
||||
/********************** SUBSCRIBERS **********************/
|
||||
// Get current state
|
||||
ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
|
||||
("mavros/state", 10, state_cb);
|
||||
|
||||
// Get attitude target from klausen control
|
||||
ros::Subscriber att_target_sub = nh.subscribe<mavros_msgs::AttitudeTarget>
|
||||
("command/att_target",10,att_targ_cb);
|
||||
|
||||
// Pose subscriber
|
||||
ros::Subscriber mavPose_sub = nh.subscribe<geometry_msgs::PoseStamped>
|
||||
("mavros/local_position/pose",10,mavPose_cb);
|
||||
|
||||
/********************** PUBLISHERS **********************/
|
||||
// Initiate publisher to publish commanded local position
|
||||
ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
|
||||
("mavros/setpoint_position/local", 10);
|
||||
|
||||
// Publish attitude and thrust commands
|
||||
ros::Publisher att_targ_pub = nh.advertise<mavros_msgs::AttitudeTarget>
|
||||
("mavros/setpoint_raw/attitude",10);
|
||||
|
||||
// Service Clients
|
||||
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
|
||||
("mavros/cmd/arming");
|
||||
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
|
||||
("mavros/set_mode");
|
||||
ros::ServiceClient takeoff_cl = nh.serviceClient<mavros_msgs::CommandTOL>
|
||||
("mavros/cmd/takeoff");
|
||||
ros::ServiceClient waypoint_cl = nh.serviceClient<oscillation_ctrl::WaypointTrack>
|
||||
("status/waypoint_tracker");
|
||||
|
||||
//the setpoint publishing rate MUST be faster than 2Hz... PX4 timeout = 500 ms
|
||||
ros::Rate rate_wait(20.0);
|
||||
|
||||
// wait for FCU connection
|
||||
while(ros::ok() && !current_state.connected){
|
||||
ros::spinOnce();
|
||||
ROS_INFO("Waiting for FCU connection");
|
||||
rate_wait.sleep();
|
||||
}
|
||||
|
||||
if (current_state.connected){
|
||||
ROS_INFO("Connected to FCU");
|
||||
} else {
|
||||
ROS_INFO("Never Connected");
|
||||
}
|
||||
|
||||
/*********** Initiate variables ************************/
|
||||
//the setpoint publishing rate MUST be faster than 2Hz... PX4 timeout = 500 ms
|
||||
ros::Rate rate_pub(25.0);
|
||||
|
||||
// Desired mode is offboard
|
||||
mavros_msgs::SetMode offb_set_mode;
|
||||
offb_set_mode.request.custom_mode = "OFFBOARD";
|
||||
|
||||
// Arm UAV
|
||||
mavros_msgs::CommandBool arm_cmd;
|
||||
arm_cmd.request.value = true;
|
||||
|
||||
// Take off command
|
||||
bool takeoff = false, att_cmds = false;
|
||||
bool oscillation_damp = true;
|
||||
|
||||
// Keep track of time since requests
|
||||
ros::Time tkoff_req = ros::Time::now();
|
||||
ros::Time last_request = ros::Time::now();
|
||||
|
||||
//send a few setpoints before starting
|
||||
for(int i = 100; ros::ok() && i > 0; --i){
|
||||
local_pos_pub.publish(buff_pose);
|
||||
ros::spinOnce();
|
||||
ROS_INFO("Publishing position setpoints");
|
||||
rate_wait.sleep();
|
||||
}
|
||||
|
||||
// Retrieve desired waypoints
|
||||
oscillation_ctrl::WaypointTrack wpoint_srv;
|
||||
wpoint_srv.request.query = false;
|
||||
if (waypoint_cl.call(wpoint_srv)){
|
||||
ROS_INFO("Waypoints received");
|
||||
// populate desired waypoints - this is only for original hover as
|
||||
// a change of waypoints is handled by ref_signal.py
|
||||
buff_pose.pose.position.x = wpoint_srv.response.xd.x;
|
||||
buff_pose.pose.position.y = wpoint_srv.response.xd.y;
|
||||
buff_pose.pose.position.z = wpoint_srv.response.xd.z;
|
||||
} else {
|
||||
ROS_INFO("NO waypoints received");
|
||||
}
|
||||
double alt_des = wpoint_srv.response.xd.z; // Desired height
|
||||
while(ros::ok()){
|
||||
if(current_state.mode == "AUTO.LAND"){
|
||||
land = true;
|
||||
while(land == true){
|
||||
ROS_INFO("Des Altitude: LANDING");
|
||||
}
|
||||
} else {
|
||||
if(current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0))){
|
||||
if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent){
|
||||
} else {
|
||||
ROS_INFO("Could not enter offboard mode");
|
||||
}
|
||||
//last_request = ros::Time::now();
|
||||
} else {
|
||||
if( !current_state.armed && (ros::Time::now() - last_request > ros::Duration(8.0))){
|
||||
if( arming_client.call(arm_cmd) && arm_cmd.response.success){
|
||||
ROS_INFO("Vehicle armed");
|
||||
}
|
||||
last_request = ros::Time::now();
|
||||
}
|
||||
}
|
||||
if(current_state.mode == "OFFBOARD" && current_state.armed){
|
||||
ROS_INFO_ONCE("Spiri is taking off");
|
||||
if(!takeoff){
|
||||
tkoff_req = ros::Time::now();
|
||||
takeoff = true;
|
||||
}
|
||||
}
|
||||
if (waypoint_cl.call(wpoint_srv)){
|
||||
// check if waypoints have changed
|
||||
buff_pose.pose.position.x = wpoint_srv.response.xd.x;
|
||||
buff_pose.pose.position.y = wpoint_srv.response.xd.y;
|
||||
buff_pose.pose.position.z = wpoint_srv.response.xd.z;
|
||||
alt_des = wpoint_srv.response.xd.z;
|
||||
}
|
||||
// Check if we want to use oscillation controller
|
||||
if (ros::param::has("/status/use_ctrl")){
|
||||
ros::param::get("/status/use_ctrl", oscillation_damp);
|
||||
if(oscillation_damp){
|
||||
ROS_INFO("USING ATTITUDE CTRL");
|
||||
att_targ.header.stamp = ros::Time::now();
|
||||
att_targ_pub.publish(att_targ);
|
||||
} else {
|
||||
// Check if waypoints have changed
|
||||
if (waypoint_cl.call(wpoint_srv))
|
||||
{
|
||||
// populate desired waypoints
|
||||
buff_pose.pose.position.x = wpoint_srv.response.xd.x;
|
||||
buff_pose.pose.position.y = wpoint_srv.response.xd.y;
|
||||
buff_pose.pose.position.z = wpoint_srv.response.xd.z;
|
||||
}
|
||||
local_pos_pub.publish(buff_pose);
|
||||
ROS_INFO("USING POSITION CTRL");
|
||||
}
|
||||
}
|
||||
ROS_INFO("Des Altitude: %.2f", alt_des);
|
||||
ROS_INFO("Cur Altitude: %.2f", current_altitude);
|
||||
ROS_INFO("---------------------------");
|
||||
ros::spinOnce();
|
||||
rate_pub.sleep();
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -1,104 +0,0 @@
|
|||
#! /usr/bin/env python2.7
|
||||
# Cesar Rodriguez Aug 2022
|
||||
# Sets attitude mode as well as new waypoints
|
||||
|
||||
import rospy
|
||||
import time
|
||||
|
||||
from oscillation_ctrl.srv import WaypointTrack, WaypointTrackResponse
|
||||
from geometry_msgs.msg import Point
|
||||
|
||||
class Main:
|
||||
def __init__(self):
|
||||
|
||||
# initialize variables
|
||||
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()
|
||||
|
||||
# set up client
|
||||
self.get_xd = rospy.ServiceProxy('/status/waypoint_tracker',WaypointTrack)
|
||||
|
||||
# Set up desired waypoints for test
|
||||
self.xd1 = Point()
|
||||
self.xd1.x = 0.0
|
||||
self.xd1.y = -0.5
|
||||
self.xd1.z = 1.75
|
||||
|
||||
self.xd2 = Point()
|
||||
self.xd2.x = 0.0
|
||||
self.xd2.y = 2.0
|
||||
self.xd2.z = 1.75
|
||||
|
||||
# Determine if we want to run test with or without controller
|
||||
################# CHANGE THIS TO CHANGE TYPE Of TEST ###############################
|
||||
|
||||
self.change_mode = True # True = Change to oscillation damping mode after wait time
|
||||
self.multiple_setpoins = True # True - will send multiple setpoints
|
||||
|
||||
#####################################################################################
|
||||
if self.change_mode: self.loginfo_string = 'Attitude mode in...'
|
||||
else: self.loginfo_string = 'Staying in Position mode.'
|
||||
|
||||
self.get_wait_time() # get wait time
|
||||
self.run_test() # runs the test
|
||||
|
||||
def get_wait_time(self):
|
||||
""" Determine desired wait time based on ros params"""
|
||||
self.param_exists = False
|
||||
while self.param_exists == False:
|
||||
rospy.loginfo_once('Getting wait time')
|
||||
if rospy.has_param('status/wait_time'):
|
||||
self.wait_time = rospy.get_param('status/wait_time') # Tether length
|
||||
self.param_exists = True
|
||||
rospy.loginfo('Wait time: %.2f',self.wait_time)
|
||||
elif rospy.get_time() - self.tstart >= 30:
|
||||
break
|
||||
|
||||
def run_test(self):
|
||||
""" Waits desired amount before setting UAV to appropriate mode, and then sets up desired waypoints"""
|
||||
run_test = False
|
||||
use_ctrl = False
|
||||
waypoint_sent = False
|
||||
while not run_test:
|
||||
time_left = self.wait_time - (rospy.get_time() - self.tstart)
|
||||
if not rospy.get_time() - self.tstart > self.wait_time:
|
||||
rospy.loginfo(self.loginfo_string + ' %.2f',time_left)
|
||||
elif rospy.get_time() - self.tstart >= self.wait_time and not use_ctrl:
|
||||
rospy.set_param('status/use_ctrl',self.change_mode)
|
||||
self.t_param = rospy.get_time()
|
||||
use_ctrl = True
|
||||
if use_ctrl:
|
||||
time_until_test1 = 25.0 - rospy.get_time() + self.t_param
|
||||
time_until_test2 = 30.0 - rospy.get_time() + self.t_param
|
||||
if time_until_test1 >= 0.0 and not waypoint_sent:
|
||||
rospy.loginfo('In %.2f\nSending waypoints: x = %.2f y = %.2f z = %.2f',time_until_test1,self.xd1.x,self.xd1.y,self.xd1.z)
|
||||
elif not waypoint_sent and time_until_test1 < 0.0:
|
||||
waypoint_sent = True
|
||||
self.set_waypoint(self.xd1)
|
||||
|
||||
if time_until_test2 >= 0.0 and waypoint_sent:
|
||||
rospy.loginfo('In %.2f\nSending waypoints: x = %.2f y = %.2f z = %.2f',time_until_test2,self.xd2.x,self.xd2.y,self.xd2.z)
|
||||
elif waypoint_sent and time_until_test2 < 0.0:
|
||||
self.set_waypoint(self.xd2)
|
||||
run_test = True
|
||||
break
|
||||
|
||||
def set_waypoint(self,xd):
|
||||
""" Set waypoints for oscillation controller """
|
||||
rospy.wait_for_service('/status/waypoint_tracker')
|
||||
try:
|
||||
self.get_xd(True,xd)
|
||||
except ValueError:
|
||||
pass
|
||||
|
||||
if __name__=="__main__":
|
||||
|
||||
# Initiate ROS node
|
||||
rospy.init_node('MoCap_node',anonymous=False)
|
||||
try:
|
||||
Main() # create class object
|
||||
rospy.spin() # loop until shutdown signal
|
||||
|
||||
except rospy.ROSInterruptException:
|
||||
pass
|
||||
Loading…
Reference in New Issue