Andreas Antener
9 years ago
20 changed files with 146 additions and 370 deletions
@ -1,90 +0,0 @@
@@ -1,90 +0,0 @@
|
||||
#!/usr/bin/env python |
||||
#*************************************************************************** |
||||
# |
||||
# Copyright (c) 2015 PX4 Development Team. All rights reserved. |
||||
# |
||||
# Redistribution and use in source and binary forms, with or without |
||||
# modification, are permitted provided that the following conditions |
||||
# are met: |
||||
# |
||||
# 1. Redistributions of source code must retain the above copyright |
||||
# notice, this list of conditions and the following disclaimer. |
||||
# 2. Redistributions in binary form must reproduce the above copyright |
||||
# notice, this list of conditions and the following disclaimer in |
||||
# the documentation and/or other materials provided with the |
||||
# distribution. |
||||
# 3. Neither the name PX4 nor the names of its contributors may be |
||||
# used to endorse or promote products derived from this software |
||||
# without specific prior written permission. |
||||
# |
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
# POSSIBILITY OF SUCH DAMAGE. |
||||
# |
||||
#***************************************************************************/ |
||||
|
||||
# |
||||
# @author Andreas Antener <andreas@uaventure.com> |
||||
# |
||||
PKG = 'px4' |
||||
|
||||
import unittest |
||||
import rospy |
||||
|
||||
from px4.msg import actuator_armed |
||||
from px4.msg import vehicle_control_mode |
||||
from manual_input import ManualInput |
||||
|
||||
# |
||||
# Tests if commander reacts to manual input and sets control flags accordingly |
||||
# |
||||
class ManualInputTest(unittest.TestCase): |
||||
|
||||
def setUp(self): |
||||
self.actuator_status = actuator_armed() |
||||
self.control_mode = vehicle_control_mode() |
||||
|
||||
# |
||||
# General callback functions used in tests |
||||
# |
||||
def actuator_armed_callback(self, data): |
||||
self.actuator_status = data |
||||
|
||||
def vehicle_control_mode_callback(self, data): |
||||
self.control_mode = data |
||||
|
||||
# |
||||
# Test arming |
||||
# |
||||
def test_manual_input(self): |
||||
rospy.init_node('test_node', anonymous=True) |
||||
rospy.Subscriber('actuator_armed', actuator_armed, self.actuator_armed_callback) |
||||
rospy.Subscriber('vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) |
||||
|
||||
man_in = ManualInput() |
||||
|
||||
# Test arming |
||||
man_in.arm() |
||||
self.assertEquals(self.actuator_status.armed, True, "did not arm") |
||||
|
||||
# Test posctl |
||||
man_in.posctl() |
||||
self.assertTrue(self.control_mode.flag_control_position_enabled, "flag_control_position_enabled is not set") |
||||
|
||||
# Test offboard |
||||
man_in.offboard() |
||||
self.assertTrue(self.control_mode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") |
||||
|
||||
|
||||
if __name__ == '__main__': |
||||
import rostest |
||||
rostest.rosrun(PKG, 'direct_manual_input_test', ManualInputTest) |
||||
@ -1,175 +0,0 @@
@@ -1,175 +0,0 @@
|
||||
#!/usr/bin/env python |
||||
#*************************************************************************** |
||||
# |
||||
# Copyright (c) 2015 PX4 Development Team. All rights reserved. |
||||
# |
||||
# Redistribution and use in source and binary forms, with or without |
||||
# modification, are permitted provided that the following conditions |
||||
# are met: |
||||
# |
||||
# 1. Redistributions of source code must retain the above copyright |
||||
# notice, this list of conditions and the following disclaimer. |
||||
# 2. Redistributions in binary form must reproduce the above copyright |
||||
# notice, this list of conditions and the following disclaimer in |
||||
# the documentation and/or other materials provided with the |
||||
# distribution. |
||||
# 3. Neither the name PX4 nor the names of its contributors may be |
||||
# used to endorse or promote products derived from this software |
||||
# without specific prior written permission. |
||||
# |
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS |
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT |
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS |
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE |
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, |
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, |
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS |
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED |
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN |
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
||||
# POSSIBILITY OF SUCH DAMAGE. |
||||
# |
||||
#***************************************************************************/ |
||||
|
||||
# |
||||
# @author Andreas Antener <andreas@uaventure.com> |
||||
# |
||||
PKG = 'px4' |
||||
|
||||
import unittest |
||||
import rospy |
||||
import rosbag |
||||
|
||||
from numpy import linalg |
||||
import numpy as np |
||||
|
||||
from px4.msg import vehicle_local_position |
||||
from px4.msg import vehicle_control_mode |
||||
from px4.msg import position_setpoint_triplet |
||||
from px4.msg import position_setpoint |
||||
from px4.msg import vehicle_local_position_setpoint |
||||
|
||||
from manual_input import ManualInput |
||||
from flight_path_assertion import FlightPathAssertion |
||||
from px4_test_helper import PX4TestHelper |
||||
|
||||
# |
||||
# Tests flying a path in offboard control by directly sending setpoints |
||||
# to the position controller (position_setpoint_triplet). |
||||
# |
||||
# For the test to be successful it needs to stay on the predefined path |
||||
# and reach all setpoints in a certain time. |
||||
# |
||||
class DirectOffboardPosctlTest(unittest.TestCase): |
||||
|
||||
def setUp(self): |
||||
rospy.init_node('test_node', anonymous=True) |
||||
self.helper = PX4TestHelper("direct_offboard_posctl_test") |
||||
self.helper.setUp() |
||||
|
||||
rospy.Subscriber('vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) |
||||
self.sub_vlp = rospy.Subscriber("vehicle_local_position", vehicle_local_position, self.position_callback) |
||||
self.pub_spt = rospy.Publisher('position_setpoint_triplet', position_setpoint_triplet, queue_size=10) |
||||
self.rate = rospy.Rate(10) # 10hz |
||||
self.has_pos = False |
||||
self.local_position = vehicle_local_position() |
||||
self.control_mode = vehicle_control_mode() |
||||
|
||||
def tearDown(self): |
||||
if self.fpa: |
||||
self.fpa.stop() |
||||
|
||||
self.helper.tearDown() |
||||
|
||||
# |
||||
# General callback functions used in tests |
||||
# |
||||
|
||||
def position_callback(self, data): |
||||
self.has_pos = True |
||||
self.local_position = data |
||||
|
||||
def vehicle_control_mode_callback(self, data): |
||||
self.control_mode = data |
||||
|
||||
|
||||
# |
||||
# Helper methods |
||||
# |
||||
def is_at_position(self, x, y, z, offset): |
||||
rospy.logdebug("current position %f, %f, %f" % (self.local_position.x, self.local_position.y, self.local_position.z)) |
||||
desired = np.array((x, y, z)) |
||||
pos = np.array((self.local_position.x, self.local_position.y, self.local_position.z)) |
||||
return linalg.norm(desired - pos) < offset |
||||
|
||||
def reach_position(self, x, y, z, timeout): |
||||
# set a position setpoint |
||||
pos = position_setpoint() |
||||
pos.valid = True |
||||
pos.x = x |
||||
pos.y = y |
||||
pos.z = z |
||||
pos.position_valid = True |
||||
stp = position_setpoint_triplet() |
||||
stp.current = pos |
||||
self.pub_spt.publish(stp) |
||||
self.helper.bag_write('px4/position_setpoint_triplet', stp) |
||||
|
||||
# does it reach the position in X seconds? |
||||
count = 0 |
||||
while count < timeout: |
||||
if self.is_at_position(pos.x, pos.y, pos.z, 0.5): |
||||
break |
||||
count = count + 1 |
||||
self.rate.sleep() |
||||
|
||||
self.assertTrue(count < timeout, "took too long to get to position") |
||||
|
||||
# |
||||
# Test offboard position control |
||||
# |
||||
def test_posctl(self): |
||||
man_in = ManualInput() |
||||
|
||||
# arm and go into offboard |
||||
man_in.arm() |
||||
man_in.offboard() |
||||
self.assertTrue(self.control_mode.flag_armed, "flag_armed is not set") |
||||
self.assertTrue(self.control_mode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") |
||||
self.assertTrue(self.control_mode.flag_control_position_enabled, "flag_control_position_enabled is not set") |
||||
|
||||
# prepare flight path |
||||
positions = ( |
||||
(0, 0, 0), |
||||
(0, 0, -2), |
||||
(2, 2, -2), |
||||
(2, -2, -2), |
||||
(-2, -2, -2), |
||||
(2, 2, -2)) |
||||
|
||||
# flight path assertion |
||||
self.fpa = FlightPathAssertion(positions, 1, 0) |
||||
self.fpa.start() |
||||
|
||||
for i in range(0, len(positions)): |
||||
self.reach_position(positions[i][0], positions[i][1], positions[i][2], 120) |
||||
self.assertFalse(self.fpa.failed, "breached flight path tunnel (%d)" % i) |
||||
|
||||
# does it hold the position for Y seconds? |
||||
count = 0 |
||||
timeout = 50 |
||||
while count < timeout: |
||||
if not self.is_at_position(2, 2, -2, 0.5): |
||||
break |
||||
count = count + 1 |
||||
self.rate.sleep() |
||||
|
||||
self.assertTrue(count == timeout, "position could not be held") |
||||
self.fpa.stop() |
||||
|
||||
|
||||
if __name__ == '__main__': |
||||
import rostest |
||||
rostest.rosrun(PKG, 'direct_offboard_posctl_test', DirectOffboardPosctlTest) |
||||
#unittest.main() |
||||
@ -1,22 +0,0 @@
@@ -1,22 +0,0 @@
|
||||
<launch> |
||||
<arg name="headless" default="true"/> |
||||
<arg name="gui" default="false"/> |
||||
<arg name="enable_logging" default="false"/> |
||||
<arg name="enable_ground_truth" default="true"/> |
||||
<arg name="ns" default="iris"/> |
||||
<arg name="log_file" default="$(arg ns)"/> |
||||
|
||||
<include file="$(find px4)/launch/gazebo_iris_empty_world.launch"> |
||||
<arg name="headless" value="$(arg headless)"/> |
||||
<arg name="gui" value="$(arg gui)"/> |
||||
<arg name="enable_logging" value="$(arg enable_logging)" /> |
||||
<arg name="enable_ground_truth" value="$(arg enable_ground_truth)" /> |
||||
<arg name="log_file" value="$(arg log_file)"/> |
||||
<arg name="ns" value="$(arg ns)"/> |
||||
</include> |
||||
|
||||
<group ns="$(arg ns)"> |
||||
<test test-name="direct_manual_input_test" pkg="px4" type="direct_manual_input_test.py" /> |
||||
<test test-name="direct_offboard_posctl_test" pkg="px4" type="direct_offboard_posctl_test.py" /> |
||||
</group> |
||||
</launch> |
||||
@ -1,25 +0,0 @@
@@ -1,25 +0,0 @@
|
||||
<launch> |
||||
<arg name="headless" default="true"/> |
||||
<arg name="gui" default="false"/> |
||||
<arg name="enable_logging" default="false"/> |
||||
<arg name="enable_ground_truth" default="true"/> |
||||
<arg name="ns" default="iris"/> |
||||
<arg name="log_file" default="$(arg ns)"/> |
||||
|
||||
<include file="$(find px4)/launch/gazebo_iris_empty_world.launch"> |
||||
<arg name="headless" value="$(arg headless)"/> |
||||
<arg name="gui" value="$(arg gui)"/> |
||||
<arg name="enable_logging" value="$(arg enable_logging)" /> |
||||
<arg name="enable_ground_truth" value="$(arg enable_ground_truth)" /> |
||||
<arg name="log_file" value="$(arg log_file)"/> |
||||
<arg name="ns" value="$(arg ns)"/> |
||||
</include> |
||||
<include file="$(find px4)/launch/mavros_sitl.launch"> |
||||
<arg name="ns" value="$(arg ns)"/> |
||||
</include> |
||||
|
||||
<group ns="$(arg ns)"> |
||||
<test test-name="mavros_offboard_posctl_test" pkg="px4" type="mavros_offboard_posctl_test.py" /> |
||||
<test test-name="mavros_offboard_attctl_test" pkg="px4" type="mavros_offboard_attctl_test.py" /> |
||||
</group> |
||||
</launch> |
||||
@ -1,26 +0,0 @@
@@ -1,26 +0,0 @@
|
||||
<launch> |
||||
<arg name="headless" default="true"/> |
||||
<arg name="gui" default="false"/> |
||||
<arg name="ns" default="iris"/> |
||||
|
||||
<node pkg="px4" type="sitl_run.sh" name="simulator" args="posix-configs/SITL/init/rcS none gazebo iris $(find px4)/build_posix_sitl_default"> |
||||
<env name="no_sim" value="1" /> |
||||
</node> |
||||
|
||||
<include file="$(find gazebo_ros)/launch/empty_world.launch"> |
||||
<arg name="headless" value="$(arg headless)"/> |
||||
<arg name="gui" value="$(arg gui)"/> |
||||
<arg name="world_name" value="$(find px4)/Tools/sitl_gazebo/worlds/iris.world" /> |
||||
</include> |
||||
|
||||
<include file="$(find px4)/launch/mavros_sitl.launch"> |
||||
<arg name="ns" value="$(arg ns)"/> |
||||
<arg name="fcu_url" value="udp://:14567@localhost:14557"/> |
||||
<arg name="tgt_component" value="1"/> |
||||
</include> |
||||
|
||||
<group ns="$(arg ns)"> |
||||
<test test-name="mavros_offboard_posctl_test" pkg="px4" type="mavros_offboard_posctl_test.py" time-limit="120.0" /> |
||||
<test test-name="mavros_offboard_attctl_test" pkg="px4" type="mavros_offboard_attctl_test.py" /> |
||||
</group> |
||||
</launch> |
||||
@ -0,0 +1 @@
@@ -0,0 +1 @@
|
||||
TODO: Adopt to new SITL |
||||
@ -0,0 +1,20 @@
@@ -0,0 +1,20 @@
|
||||
#!/bin/bash |
||||
# |
||||
# Setup environment to make PX4 visible to ROS/Gazebo. |
||||
# |
||||
# License: according to LICENSE.md in the root directory of the PX4 Firmware repository |
||||
|
||||
if [ "$#" -lt 1 ] |
||||
then |
||||
echo usage: source setup_gazebo_ros.bash firmware_src_dir |
||||
echo "" |
||||
return 1 |
||||
fi |
||||
|
||||
SRC_DIR=$1 |
||||
|
||||
# setup Gazebo env and update package path |
||||
export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:${SRC_DIR}/Tools/sitl_gazebo/models |
||||
export GAZEBO_PLUGIN_PATH=${SRC_DIR}/Tools/sitl_gazebo/Build/:${GAZEBO_PLUGIN_PATH} |
||||
export LD_LIBRARY_PATH=${LD_LIBRARY_PATH}:${SRC_DIR}/Tools/sitl_gazebo/Build/msgs/ |
||||
export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:${SRC_DIR} |
||||
@ -0,0 +1,21 @@
@@ -0,0 +1,21 @@
|
||||
<launch> |
||||
<!-- MAVROS launch script PX4 (default) --> |
||||
|
||||
<arg name="ns" default="/" /> |
||||
<arg name="fcu_url" default="" /> |
||||
<arg name="gcs_url" default="udp://:14550@:14555" /> |
||||
<arg name="tgt_system" default="1" /> |
||||
<arg name="tgt_component" default="1" /> |
||||
|
||||
<group ns="$(arg ns)"> |
||||
<include file="$(find mavros)/launch/node.launch"> |
||||
<arg name="pluginlists_yaml" value="$(find mavros)/launch/px4_pluginlists.yaml" /> |
||||
<arg name="config_yaml" value="$(find mavros)/launch/px4_config.yaml" /> |
||||
|
||||
<arg name="fcu_url" value="$(arg fcu_url)" /> |
||||
<arg name="gcs_url" value="$(arg gcs_url)" /> |
||||
<arg name="tgt_system" value="$(arg tgt_system)" /> |
||||
<arg name="tgt_component" value="$(arg tgt_component)" /> |
||||
</include> |
||||
</group> |
||||
</launch> |
||||
@ -0,0 +1,22 @@
@@ -0,0 +1,22 @@
|
||||
<launch> |
||||
<!-- MAVROS posix SITL environment launch script --> |
||||
|
||||
<arg name="headless" default="false"/> |
||||
<arg name="gui" default="true"/> |
||||
<arg name="ns" default="/"/> |
||||
<arg name="world" default="iris"/> |
||||
<arg name="build" default="posix_sitl_default"/> |
||||
|
||||
<include file="$(find px4)/launch/posix_sitl.launch"> |
||||
<arg name="headless" value="$(arg headless)"/> |
||||
<arg name="gui" value="$(arg gui)"/> |
||||
<arg name="ns" value="$(arg ns)"/> |
||||
<arg name="world" value="$(arg world)"/> |
||||
<arg name="build" value="$(arg build)"/> |
||||
</include> |
||||
|
||||
<include file="$(find px4)/launch/mavros.launch"> |
||||
<arg name="ns" value="$(arg ns)"/> |
||||
<arg name="fcu_url" value="udp://:14567@localhost:14557"/> |
||||
</include> |
||||
</launch> |
||||
@ -0,0 +1,18 @@
@@ -0,0 +1,18 @@
|
||||
<launch> |
||||
<!-- Posix SITL MAVROS integration tests --> |
||||
|
||||
<arg name="ns" default="/"/> |
||||
<arg name="headless" default="true"/> |
||||
<arg name="gui" default="false"/> |
||||
|
||||
<include file="$(find px4)/launch/mavros_posix_sitl.launch"> |
||||
<arg name="ns" value="$(arg ns)"/> |
||||
<arg name="headless" value="$(arg headless)"/> |
||||
<arg name="gui" value="$(arg gui)"/> |
||||
</include> |
||||
|
||||
<group ns="$(arg ns)"> |
||||
<test test-name="mavros_offboard_posctl_test" pkg="px4" type="mavros_offboard_posctl_test.py" time-limit="120.0" /> |
||||
<test test-name="mavros_offboard_attctl_test" pkg="px4" type="mavros_offboard_attctl_test.py" /> |
||||
</group> |
||||
</launch> |
||||
@ -0,0 +1,19 @@
@@ -0,0 +1,19 @@
|
||||
<launch> |
||||
<!-- Posix SITL environment launch script --> |
||||
|
||||
<arg name="headless" default="false"/> |
||||
<arg name="gui" default="true"/> |
||||
<arg name="ns" default="/"/> |
||||
<arg name="world" default="iris"/> |
||||
<arg name="build" default="posix_sitl_default"/> |
||||
|
||||
<node pkg="px4" type="sitl_run.sh" name="simulator" args="posix-configs/SITL/init/rcS none gazebo iris $(find px4)/build_$(arg build)"> |
||||
<env name="no_sim" value="1" /> |
||||
</node> |
||||
|
||||
<include file="$(find gazebo_ros)/launch/empty_world.launch"> |
||||
<arg name="headless" value="$(arg headless)"/> |
||||
<arg name="gui" value="$(arg gui)"/> |
||||
<arg name="world_name" value="$(find px4)/Tools/sitl_gazebo/worlds/$(arg world).world" /> |
||||
</include> |
||||
</launch> |
||||
Loading…
Reference in new issue