diff --git a/.github/workflows/mavros_tests.yml b/.github/workflows/mavros_tests.yml deleted file mode 100644 index b8df2541d8..0000000000 --- a/.github/workflows/mavros_tests.yml +++ /dev/null @@ -1,73 +0,0 @@ -name: MAVROS Tests - -on: - push: - branches: - - 'main' - paths-ignore: - - 'docs/**' - pull_request: - branches: - - '**' - paths-ignore: - - 'docs/**' - -concurrency: - group: ${{ github.workflow }}-${{ github.ref }} - cancel-in-progress: true - -jobs: - test: - name: "MAVROS ${{ matrix.config.name }}" - runs-on: [runs-on,runner=4cpu-linux-x64,image=ubuntu24-full-x64,"run-id=${{ github.run_id }}",spot=false,extras=s3-cache] - permissions: - contents: read - container: - image: px4io/px4-dev-ros-noetic:2021-09-08 - env: - PX4_SBOM_DISABLE: 1 - strategy: - fail-fast: false - matrix: - config: - - {name: "Mission", test_file: "mavros_posix_test_mission.test", params: "mission:=MC_mission_box vehicle:=iris"} - - {name: "Offboard", test_file: "mavros_posix_tests_offboard_posctl.test", params: "vehicle:=iris"} - steps: - - uses: runs-on/action@v2 - - uses: actions/checkout@v6 - with: - fetch-depth: 1 - - name: Configure Git Safe Directory - run: git config --system --add safe.directory '*' - - - name: Setup - Install Python Test Dependencies - run: pip3 install -r Tools/setup/requirements.txt - - - uses: ./.github/actions/setup-ccache - id: ccache - with: - cache-key-prefix: ccache-sitl-gazebo-classic - max-size: 350M - - - uses: ./.github/actions/build-gazebo-sitl - - - name: Test - MAVROS ${{ matrix.config.name }} - run: | - ./test/rostest_px4_run.sh \ - ${{ matrix.config.test_file }} \ - ${{ matrix.config.params }} - timeout-minutes: 10 - - - uses: ./.github/actions/save-ccache - if: always() - with: - cache-primary-key: ${{ steps.ccache.outputs.cache-primary-key }} - - - name: Upload - Failed Test Logs - if: failure() - uses: actions/upload-artifact@v7 - with: - name: failed-mavros-${{ matrix.config.name }}-logs.zip - path: | - logs/**/**/**/*.log - logs/**/**/**/*.ulg diff --git a/integrationtests/python_src/px4_it/mavros/__init__.py b/integrationtests/python_src/px4_it/mavros/__init__.py deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/integrationtests/python_src/px4_it/mavros/mavros_offboard_attctl_test.py b/integrationtests/python_src/px4_it/mavros/mavros_offboard_attctl_test.py deleted file mode 100755 index 4432e466e7..0000000000 --- a/integrationtests/python_src/px4_it/mavros/mavros_offboard_attctl_test.py +++ /dev/null @@ -1,154 +0,0 @@ -#!/usr/bin/env python3 -#*************************************************************************** -# -# 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 -# - -PKG = 'px4' - -import rospy -from geometry_msgs.msg import Quaternion, Vector3 -from mavros_msgs.msg import AttitudeTarget -from mavros_test_common import MavrosTestCommon -from pymavlink import mavutil -from std_msgs.msg import Header -from threading import Thread -from tf.transformations import quaternion_from_euler - - -class MavrosOffboardAttctlTest(MavrosTestCommon): - """ - Tests flying in offboard control by sending attitude and thrust setpoints - via MAVROS. - - For the test to be successful it needs to cross a certain boundary in time. - """ - - def setUp(self): - super(MavrosOffboardAttctlTest, self).setUp() - - self.att = AttitudeTarget() - - self.att_setpoint_pub = rospy.Publisher( - 'mavros/setpoint_raw/attitude', AttitudeTarget, queue_size=1) - - # send setpoints in separate thread to better prevent failsafe - self.att_thread = Thread(target=self.send_att, args=()) - self.att_thread.daemon = True - self.att_thread.start() - - def tearDown(self): - super(MavrosOffboardAttctlTest, self).tearDown() - - # - # Helper methods - # - def send_att(self): - rate = rospy.Rate(10) # Hz - self.att.body_rate = Vector3() - self.att.header = Header() - self.att.header.frame_id = "base_footprint" - self.att.orientation = Quaternion(*quaternion_from_euler(-0.25, 0.15, - 0)) - self.att.thrust = 0.7 - self.att.type_mask = 7 # ignore body rate - - while not rospy.is_shutdown(): - self.att.header.stamp = rospy.Time.now() - self.att_setpoint_pub.publish(self.att) - try: # prevent garbage in console output when thread is killed - rate.sleep() - except rospy.ROSInterruptException: - pass - - # - # Test method - # - def test_attctl(self): - """Test offboard attitude control""" - # boundary to cross - boundary_x = 200 - boundary_y = 100 - boundary_z = 20 - - # make sure the simulation is ready to start the mission - self.wait_for_topics(60) - self.wait_for_landed_state(mavutil.mavlink.MAV_LANDED_STATE_ON_GROUND, - 10, -1) - - self.log_topic_vars() - self.set_mode("OFFBOARD", 5) - self.set_arm(True, 5) - - rospy.loginfo("run mission") - rospy.loginfo("attempting to cross boundary | x: {0}, y: {1}, z: {2}". - format(boundary_x, boundary_y, boundary_z)) - # does it cross expected boundaries in 'timeout' seconds? - timeout = 90 # (int) seconds - loop_freq = 2 # Hz - rate = rospy.Rate(loop_freq) - crossed = False - for i in range(timeout * loop_freq): - if (self.local_position.pose.position.x > boundary_x and - self.local_position.pose.position.y > boundary_y and - self.local_position.pose.position.z > boundary_z): - rospy.loginfo("boundary crossed | seconds: {0} of {1}".format( - i / loop_freq, timeout)) - crossed = True - break - - try: - rate.sleep() - except rospy.ROSException as e: - self.fail(e) - - self.assertTrue(crossed, ( - "took too long to cross boundaries | current position x: {0:.2f}, y: {1:.2f}, z: {2:.2f} | timeout(seconds): {3}". - format(self.local_position.pose.position.x, - self.local_position.pose.position.y, - self.local_position.pose.position.z, timeout))) - - self.set_mode("AUTO.LAND", 5) - self.wait_for_landed_state(mavutil.mavlink.MAV_LANDED_STATE_ON_GROUND, - 90, 0) - self.set_arm(False, 5) - - -if __name__ == '__main__': - import rostest - rospy.init_node('test_node', anonymous=True) - - rostest.rosrun(PKG, 'mavros_offboard_attctl_test', - MavrosOffboardAttctlTest) diff --git a/integrationtests/python_src/px4_it/mavros/mavros_offboard_posctl_test.py b/integrationtests/python_src/px4_it/mavros/mavros_offboard_posctl_test.py deleted file mode 100755 index 8e88bf608b..0000000000 --- a/integrationtests/python_src/px4_it/mavros/mavros_offboard_posctl_test.py +++ /dev/null @@ -1,188 +0,0 @@ -#!/usr/bin/env python3 -#*************************************************************************** -# -# 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 -# - -PKG = 'px4' - -import rospy -import math -import numpy as np -from geometry_msgs.msg import PoseStamped, Quaternion -from mavros_msgs.msg import ParamValue -from mavros_test_common import MavrosTestCommon -from pymavlink import mavutil -from std_msgs.msg import Header -from threading import Thread -from tf.transformations import quaternion_from_euler - - -class MavrosOffboardPosctlTest(MavrosTestCommon): - """ - Tests flying a path in offboard control by sending position setpoints - via MAVROS. - - For the test to be successful it needs to reach all setpoints in a certain time. - - FIXME: add flight path assertion (needs transformation from ROS frame to NED) - """ - - def setUp(self): - super(MavrosOffboardPosctlTest, self).setUp() - - self.pos = PoseStamped() - self.radius = 1 - - self.pos_setpoint_pub = rospy.Publisher( - 'mavros/setpoint_position/local', PoseStamped, queue_size=1) - - # send setpoints in separate thread to better prevent failsafe - self.pos_thread = Thread(target=self.send_pos, args=()) - self.pos_thread.daemon = True - self.pos_thread.start() - - def tearDown(self): - super(MavrosOffboardPosctlTest, self).tearDown() - - # - # Helper methods - # - def send_pos(self): - rate = rospy.Rate(10) # Hz - self.pos.header = Header() - self.pos.header.frame_id = "base_footprint" - - while not rospy.is_shutdown(): - self.pos.header.stamp = rospy.Time.now() - self.pos_setpoint_pub.publish(self.pos) - try: # prevent garbage in console output when thread is killed - rate.sleep() - except rospy.ROSInterruptException: - pass - - def is_at_position(self, x, y, z, offset): - """offset: meters""" - rospy.logdebug( - "current position | x:{0:.2f}, y:{1:.2f}, z:{2:.2f}".format( - self.local_position.pose.position.x, self.local_position.pose. - position.y, self.local_position.pose.position.z)) - - desired = np.array((x, y, z)) - pos = np.array((self.local_position.pose.position.x, - self.local_position.pose.position.y, - self.local_position.pose.position.z)) - return np.linalg.norm(desired - pos) < offset - - def reach_position(self, x, y, z, timeout): - """timeout(int): seconds""" - # set a position setpoint - self.pos.pose.position.x = x - self.pos.pose.position.y = y - self.pos.pose.position.z = z - rospy.loginfo( - "attempting to reach position | x: {0}, y: {1}, z: {2} | current position x: {3:.2f}, y: {4:.2f}, z: {5:.2f}". - format(x, y, z, self.local_position.pose.position.x, - self.local_position.pose.position.y, - self.local_position.pose.position.z)) - - # For demo purposes we will lock yaw/heading to north. - yaw_degrees = 0 # North - yaw = math.radians(yaw_degrees) - quaternion = quaternion_from_euler(0, 0, yaw) - self.pos.pose.orientation = Quaternion(*quaternion) - - # does it reach the position in 'timeout' seconds? - loop_freq = 2 # Hz - rate = rospy.Rate(loop_freq) - reached = False - for i in range(timeout * loop_freq): - if self.is_at_position(self.pos.pose.position.x, - self.pos.pose.position.y, - self.pos.pose.position.z, self.radius): - rospy.loginfo("position reached | seconds: {0} of {1}".format( - i / loop_freq, timeout)) - reached = True - break - - try: - rate.sleep() - except rospy.ROSException as e: - self.fail(e) - - self.assertTrue(reached, ( - "took too long to get to position | current position x: {0:.2f}, y: {1:.2f}, z: {2:.2f} | timeout(seconds): {3}". - format(self.local_position.pose.position.x, - self.local_position.pose.position.y, - self.local_position.pose.position.z, timeout))) - - # - # Test method - # - def test_posctl(self): - """Test offboard position control""" - - # make sure the simulation is ready to start the mission - self.wait_for_topics(60) - self.wait_for_landed_state(mavutil.mavlink.MAV_LANDED_STATE_ON_GROUND, - 10, -1) - - self.log_topic_vars() - # exempting failsafe from lost RC to allow offboard - rcl_except = ParamValue(1<<2, 0.0) - self.set_param("COM_RCL_EXCEPT", rcl_except, 5) - self.set_mode("OFFBOARD", 5) - self.set_arm(True, 5) - - rospy.loginfo("run mission") - positions = ((0, 0, 0), (50, 50, 20), (50, -50, 20), (-50, -50, 20), - (0, 0, 20)) - - for i in range(len(positions)): - self.reach_position(positions[i][0], positions[i][1], - positions[i][2], 30) - - self.set_mode("AUTO.LAND", 5) - self.wait_for_landed_state(mavutil.mavlink.MAV_LANDED_STATE_ON_GROUND, - 45, 0) - self.set_arm(False, 5) - - -if __name__ == '__main__': - import rostest - rospy.init_node('test_node', anonymous=True) - - rostest.rosrun(PKG, 'mavros_offboard_posctl_test', - MavrosOffboardPosctlTest) diff --git a/integrationtests/python_src/px4_it/mavros/mavros_offboard_yawrate_test.py b/integrationtests/python_src/px4_it/mavros/mavros_offboard_yawrate_test.py deleted file mode 100755 index 88b646a19b..0000000000 --- a/integrationtests/python_src/px4_it/mavros/mavros_offboard_yawrate_test.py +++ /dev/null @@ -1,174 +0,0 @@ -#!/usr/bin/env python3 -#*************************************************************************** -# -# Copyright (c) 2020 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 Pedro Roque -# - -PKG = 'px4' - -import rospy -from geometry_msgs.msg import Quaternion, Vector3 -from mavros_msgs.msg import AttitudeTarget -from mavros_test_common import MavrosTestCommon -from pymavlink import mavutil -from std_msgs.msg import Header -from threading import Thread -from tf.transformations import quaternion_from_euler - - -class MavrosOffboardYawrateTest(MavrosTestCommon): - """ - Tests flying in offboard control by sending a Roll Pitch Yawrate Thrust (RPYrT) - as attitude setpoint. - - For the test to be successful it needs to achieve a desired yawrate and height. - """ - - def setUp(self): - super(MavrosOffboardYawrateTest, self).setUp() - - self.att = AttitudeTarget() - - self.att_setpoint_pub = rospy.Publisher( - 'mavros/setpoint_raw/attitude', AttitudeTarget, queue_size=1) - - # send setpoints in separate thread to better prevent failsafe - self.att_thread = Thread(target=self.send_att, args=()) - self.att_thread.daemon = True - self.att_thread.start() - - # desired yawrate target - self.des_yawrate = 0.1 - self.yawrate_tol = 0.02 - - def tearDown(self): - super(MavrosOffboardYawrateTest, self).tearDown() - - # - # Helper methods - # - def send_att(self): - rate = rospy.Rate(10) # Hz - self.att.body_rate = Vector3() - self.att.header = Header() - self.att.header.frame_id = "base_footprint" - self.att.orientation = self.local_position.pose.orientation - - self.att.body_rate.x = 0 - self.att.body_rate.y = 0 - self.att.body_rate.z = self.des_yawrate - - self.att.thrust = 0.59 - - self.att.type_mask = 3 # ignore roll and pitch rate - - while not rospy.is_shutdown(): - self.att.header.stamp = rospy.Time.now() - self.att_setpoint_pub.publish(self.att) - try: # prevent garbage in console output when thread is killed - rate.sleep() - except rospy.ROSInterruptException: - pass - - # - # Test method - # - def test_attctl(self): - """Test offboard yawrate control""" - - # boundary to cross - # Stay leveled, go up, and test yawrate - boundary_x = 5 - boundary_y = 5 - boundary_z = 10 - - - # make sure the simulation is ready to start the mission - self.wait_for_topics(60) - self.wait_for_landed_state(mavutil.mavlink.MAV_LANDED_STATE_ON_GROUND, - 10, -1) - - self.log_topic_vars() - self.set_arm(True, 5) - self.set_mode("OFFBOARD", 5) - rospy.loginfo("run mission") - rospy.loginfo("attempting to cross boundary | z: {2} , stay within x: {0} y: {1} \n and achieve {3} yawrate". - format(boundary_x, boundary_y, boundary_z, self.des_yawrate)) - - # does it cross expected boundaries in 'timeout' seconds? - timeout = 90 # (int) seconds - loop_freq = 2 # Hz - rate = rospy.Rate(loop_freq) - crossed = False - for i in range(timeout * loop_freq): - if (self.local_position.pose.position.x < boundary_x and - self.local_position.pose.position.x > -boundary_x and - self.local_position.pose.position.y < boundary_y and - self.local_position.pose.position.y > -boundary_y and - self.local_position.pose.position.z > boundary_z and - abs(self.imu_data.angular_velocity.z - self.des_yawrate) < self.yawrate_tol): - rospy.loginfo("Test successful. Final altitude and yawrate achieved") - crossed = True - break - - try: - rate.sleep() - except rospy.ROSException as e: - self.fail(e) - - self.assertTrue(crossed, ( - "took too long to finish test | current position x: {0:.2f}, y: {1:.2f}, z: {2:.2f} \n " \ - " | current att qx: {3:.2f}, qy: {4:.2f}, qz: {5:.2f} qw: {6:.2f}, yr: {7:.2f}| timeout(seconds): {8}". - format(self.local_position.pose.position.x, - self.local_position.pose.position.y, - self.local_position.pose.position.z, - self.imu_data.orientation.x, - self.imu_data.orientation.y, - self.imu_data.orientation.z, - self.imu_data.orientation.w, - self.imu_data.angular_velocity.z, - timeout))) - - self.set_mode("AUTO.LAND", 5) - self.wait_for_landed_state(mavutil.mavlink.MAV_LANDED_STATE_ON_GROUND, - 90, 0) - self.set_arm(False, 5) - - -if __name__ == '__main__': - import rostest - rospy.init_node('test_node', anonymous=True) - - rostest.rosrun(PKG, 'mavros_offboard_yawrate_test', - MavrosOffboardYawrateTest) diff --git a/integrationtests/python_src/px4_it/mavros/mavros_test_common.py b/integrationtests/python_src/px4_it/mavros/mavros_test_common.py deleted file mode 100644 index ee5a95d438..0000000000 --- a/integrationtests/python_src/px4_it/mavros/mavros_test_common.py +++ /dev/null @@ -1,458 +0,0 @@ -#!/usr/bin/env python3 - -import unittest -import rospy -import math -from geometry_msgs.msg import PoseStamped -from mavros_msgs.msg import Altitude, ExtendedState, HomePosition, ParamValue, State, \ - WaypointList -from mavros_msgs.srv import CommandBool, ParamGet, ParamSet, SetMode, SetModeRequest, WaypointClear, \ - WaypointPush -from pymavlink import mavutil -from sensor_msgs.msg import NavSatFix, Imu - - -class MavrosTestCommon(unittest.TestCase): - def __init__(self, *args): - super(MavrosTestCommon, self).__init__(*args) - - def setUp(self): - self.altitude = Altitude() - self.extended_state = ExtendedState() - self.global_position = NavSatFix() - self.imu_data = Imu() - self.home_position = HomePosition() - self.local_position = PoseStamped() - self.mission_wp = WaypointList() - self.state = State() - self.mav_type = None - - self.sub_topics_ready = { - key: False - for key in [ - 'alt', 'ext_state', 'global_pos', 'home_pos', 'local_pos', - 'mission_wp', 'state', 'imu' - ] - } - - # ROS services - service_timeout = 30 - rospy.loginfo("waiting for ROS services") - try: - rospy.wait_for_service('mavros/param/get', service_timeout) - rospy.wait_for_service('mavros/param/set', service_timeout) - rospy.wait_for_service('mavros/cmd/arming', service_timeout) - rospy.wait_for_service('mavros/mission/push', service_timeout) - rospy.wait_for_service('mavros/mission/clear', service_timeout) - rospy.wait_for_service('mavros/set_mode', service_timeout) - rospy.loginfo("ROS services are up") - except rospy.ROSException: - self.fail("failed to connect to services") - self.get_param_srv = rospy.ServiceProxy('mavros/param/get', ParamGet) - self.set_param_srv = rospy.ServiceProxy('mavros/param/set', ParamSet) - self.set_arming_srv = rospy.ServiceProxy('mavros/cmd/arming', - CommandBool) - self.set_mode_srv = rospy.ServiceProxy('mavros/set_mode', SetMode) - self.wp_clear_srv = rospy.ServiceProxy('mavros/mission/clear', - WaypointClear) - self.wp_push_srv = rospy.ServiceProxy('mavros/mission/push', - WaypointPush) - - # ROS subscribers - self.alt_sub = rospy.Subscriber('mavros/altitude', Altitude, - self.altitude_callback) - self.ext_state_sub = rospy.Subscriber('mavros/extended_state', - ExtendedState, - self.extended_state_callback) - self.global_pos_sub = rospy.Subscriber('mavros/global_position/global', - NavSatFix, - self.global_position_callback) - self.imu_data_sub = rospy.Subscriber('mavros/imu/data', - Imu, - self.imu_data_callback) - self.home_pos_sub = rospy.Subscriber('mavros/home_position/home', - HomePosition, - self.home_position_callback) - self.local_pos_sub = rospy.Subscriber('mavros/local_position/pose', - PoseStamped, - self.local_position_callback) - self.mission_wp_sub = rospy.Subscriber( - 'mavros/mission/waypoints', WaypointList, self.mission_wp_callback) - self.state_sub = rospy.Subscriber('mavros/state', State, - self.state_callback) - - def tearDown(self): - self.log_topic_vars() - - # - # Callback functions - # - def altitude_callback(self, data): - self.altitude = data - - # amsl has been observed to be nan while other fields are valid - if not self.sub_topics_ready['alt'] and not math.isnan(data.amsl): - self.sub_topics_ready['alt'] = True - - def extended_state_callback(self, data): - if self.extended_state.vtol_state != data.vtol_state: - rospy.loginfo("VTOL state changed from {0} to {1}".format( - mavutil.mavlink.enums['MAV_VTOL_STATE'] - [self.extended_state.vtol_state].name, mavutil.mavlink.enums[ - 'MAV_VTOL_STATE'][data.vtol_state].name)) - - if self.extended_state.landed_state != data.landed_state: - rospy.loginfo("landed state changed from {0} to {1}".format( - mavutil.mavlink.enums['MAV_LANDED_STATE'] - [self.extended_state.landed_state].name, mavutil.mavlink.enums[ - 'MAV_LANDED_STATE'][data.landed_state].name)) - - self.extended_state = data - - if not self.sub_topics_ready['ext_state']: - self.sub_topics_ready['ext_state'] = True - - def global_position_callback(self, data): - self.global_position = data - - if not self.sub_topics_ready['global_pos']: - self.sub_topics_ready['global_pos'] = True - - def imu_data_callback(self, data): - self.imu_data = data - - if not self.sub_topics_ready['imu']: - self.sub_topics_ready['imu'] = True - - def home_position_callback(self, data): - self.home_position = data - - if not self.sub_topics_ready['home_pos']: - self.sub_topics_ready['home_pos'] = True - - def local_position_callback(self, data): - self.local_position = data - - if not self.sub_topics_ready['local_pos']: - self.sub_topics_ready['local_pos'] = True - - def mission_wp_callback(self, data): - if self.mission_wp.current_seq != data.current_seq: - rospy.loginfo("current mission waypoint sequence updated: {0}". - format(data.current_seq)) - - self.mission_wp = data - - if not self.sub_topics_ready['mission_wp']: - self.sub_topics_ready['mission_wp'] = True - - def state_callback(self, data): - if self.state.armed != data.armed: - rospy.loginfo("armed state changed from {0} to {1}".format( - self.state.armed, data.armed)) - - if self.state.connected != data.connected: - rospy.loginfo("connected changed from {0} to {1}".format( - self.state.connected, data.connected)) - - if self.state.mode != data.mode: - rospy.loginfo("mode changed from {0} to {1}".format( - self.state.mode, data.mode)) - - if self.state.system_status != data.system_status: - rospy.loginfo("system_status changed from {0} to {1}".format( - mavutil.mavlink.enums['MAV_STATE'][ - self.state.system_status].name, mavutil.mavlink.enums[ - 'MAV_STATE'][data.system_status].name)) - - self.state = data - - # mavros publishes a disconnected state message on init - if not self.sub_topics_ready['state'] and data.connected: - self.sub_topics_ready['state'] = True - - # - # Helper methods - # - def set_arm(self, arm, timeout): - """arm: True to arm or False to disarm, timeout(int): seconds""" - rospy.loginfo("setting FCU arm: {0}".format(arm)) - old_arm = self.state.armed - loop_freq = 1 # Hz - rate = rospy.Rate(loop_freq) - arm_set = False - for i in range(timeout * loop_freq): - if self.state.armed == arm: - arm_set = True - rospy.loginfo("set arm success | seconds: {0} of {1}".format( - i / loop_freq, timeout)) - break - else: - try: - res = self.set_arming_srv(arm) - if not res.success: - rospy.logerr("failed to send arm command") - except rospy.ServiceException as e: - rospy.logerr(e) - - try: - rate.sleep() - except rospy.ROSException as e: - self.fail(e) - - self.assertTrue(arm_set, ( - "failed to set arm | new arm: {0}, old arm: {1} | timeout(seconds): {2}". - format(arm, old_arm, timeout))) - - def set_mode(self, mode, timeout): - """mode: PX4 mode string, timeout(int): seconds""" - rospy.loginfo("setting FCU mode: {0}".format(mode)) - old_mode = self.state.mode - loop_freq = 1 # Hz - rate = rospy.Rate(loop_freq) - mode_set = False - for i in range(timeout * loop_freq): - if self.state.mode == mode: - mode_set = True - rospy.loginfo("set mode success | seconds: {0} of {1}".format( - i / loop_freq, timeout)) - break - else: - try: - res = self.set_mode_srv(0, mode) # 0 is custom mode - if not res.mode_sent: - rospy.logerr("failed to send mode command") - except rospy.ServiceException as e: - rospy.logerr(e) - - try: - rate.sleep() - except rospy.ROSException as e: - self.fail(e) - - self.assertTrue(mode_set, ( - "failed to set mode | new mode: {0}, old mode: {1} | timeout(seconds): {2}". - format(mode, old_mode, timeout))) - - def set_param(self, param_id, param_value, timeout): - """param: PX4 param string, ParamValue, timeout(int): seconds""" - if param_value.integer != 0: - value = param_value.integer - else: - value = param_value.real - rospy.loginfo("setting PX4 parameter: {0} with value {1}". - format(param_id, value)) - loop_freq = 1 # Hz - rate = rospy.Rate(loop_freq) - param_set = False - for i in range(timeout * loop_freq): - try: - res = self.set_param_srv(param_id, param_value) - if res.success: - rospy.loginfo("param {0} set to {1} | seconds: {2} of {3}". - format(param_id, value, i / loop_freq, timeout)) - break - except rospy.ServiceException as e: - rospy.logerr(e) - - try: - rate.sleep() - except rospy.ROSException as e: - self.fail(e) - - self.assertTrue(res.success, ( - "failed to set param | param_id: {0}, param_value: {1} | timeout(seconds): {2}". - format(param_id, value, timeout))) - - def wait_for_topics(self, timeout): - """wait for simulation to be ready, make sure we're getting topic info - from all topics by checking dictionary of flag values set in callbacks, - timeout(int): seconds""" - rospy.loginfo("waiting for subscribed topics to be ready") - loop_freq = 1 # Hz - rate = rospy.Rate(loop_freq) - simulation_ready = False - for i in range(timeout * loop_freq): - if all(value for value in self.sub_topics_ready.values()): - simulation_ready = True - rospy.loginfo("simulation topics ready | seconds: {0} of {1}". - format(i / loop_freq, timeout)) - break - - try: - rate.sleep() - except rospy.ROSException as e: - self.fail(e) - - self.assertTrue(simulation_ready, ( - "failed to hear from all subscribed simulation topics | topic ready flags: {0} | timeout(seconds): {1}". - format(self.sub_topics_ready, timeout))) - - def wait_for_landed_state(self, desired_landed_state, timeout, index): - rospy.loginfo("waiting for landed state | state: {0}, index: {1}". - format(mavutil.mavlink.enums['MAV_LANDED_STATE'][ - desired_landed_state].name, index)) - loop_freq = 10 # Hz - rate = rospy.Rate(loop_freq) - landed_state_confirmed = False - for i in range(timeout * loop_freq): - if self.extended_state.landed_state == desired_landed_state: - landed_state_confirmed = True - rospy.loginfo("landed state confirmed | seconds: {0} of {1}". - format(i / loop_freq, timeout)) - break - - try: - rate.sleep() - except rospy.ROSException as e: - self.fail(e) - - self.assertTrue(landed_state_confirmed, ( - "landed state not detected | desired: {0}, current: {1} | index: {2}, timeout(seconds): {3}". - format(mavutil.mavlink.enums['MAV_LANDED_STATE'][ - desired_landed_state].name, mavutil.mavlink.enums[ - 'MAV_LANDED_STATE'][self.extended_state.landed_state].name, - index, timeout))) - - def wait_for_vtol_state(self, transition, timeout, index): - """Wait for VTOL transition, timeout(int): seconds""" - rospy.loginfo( - "waiting for VTOL transition | transition: {0}, index: {1}".format( - mavutil.mavlink.enums['MAV_VTOL_STATE'][ - transition].name, index)) - loop_freq = 10 # Hz - rate = rospy.Rate(loop_freq) - transitioned = False - for i in range(timeout * loop_freq): - if transition == self.extended_state.vtol_state: - rospy.loginfo("transitioned | seconds: {0} of {1}".format( - i / loop_freq, timeout)) - transitioned = True - break - - try: - rate.sleep() - except rospy.ROSException as e: - self.fail(e) - - self.assertTrue(transitioned, ( - "transition not detected | desired: {0}, current: {1} | index: {2} timeout(seconds): {3}". - format(mavutil.mavlink.enums['MAV_VTOL_STATE'][transition].name, - mavutil.mavlink.enums['MAV_VTOL_STATE'][ - self.extended_state.vtol_state].name, index, timeout))) - - def clear_wps(self, timeout): - """timeout(int): seconds""" - loop_freq = 1 # Hz - rate = rospy.Rate(loop_freq) - wps_cleared = False - for i in range(timeout * loop_freq): - if not self.mission_wp.waypoints: - wps_cleared = True - rospy.loginfo("clear waypoints success | seconds: {0} of {1}". - format(i / loop_freq, timeout)) - break - else: - try: - res = self.wp_clear_srv() - if not res.success: - rospy.logerr("failed to send waypoint clear command") - except rospy.ServiceException as e: - rospy.logerr(e) - - try: - rate.sleep() - except rospy.ROSException as e: - self.fail(e) - - self.assertTrue(wps_cleared, ( - "failed to clear waypoints | timeout(seconds): {0}".format(timeout) - )) - - def send_wps(self, waypoints, timeout): - """waypoints, timeout(int): seconds""" - rospy.loginfo("sending mission waypoints") - if self.mission_wp.waypoints: - rospy.loginfo("FCU already has mission waypoints") - - loop_freq = 1 # Hz - rate = rospy.Rate(loop_freq) - wps_sent = False - wps_verified = False - for i in range(timeout * loop_freq): - if not wps_sent: - try: - res = self.wp_push_srv(start_index=0, waypoints=waypoints) - wps_sent = res.success - if wps_sent: - rospy.loginfo("waypoints successfully transferred") - except rospy.ServiceException as e: - rospy.logerr(e) - else: - if len(waypoints) == len(self.mission_wp.waypoints): - rospy.loginfo("number of waypoints transferred: {0}". - format(len(waypoints))) - wps_verified = True - - if wps_sent and wps_verified: - rospy.loginfo("send waypoints success | seconds: {0} of {1}". - format(i / loop_freq, timeout)) - break - - try: - rate.sleep() - except rospy.ROSException as e: - self.fail(e) - - self.assertTrue(( - wps_sent and wps_verified - ), "mission could not be transferred and verified | timeout(seconds): {0}". - format(timeout)) - - def wait_for_mav_type(self, timeout): - """Wait for MAV_TYPE parameter, timeout(int): seconds""" - rospy.loginfo("waiting for MAV_TYPE") - loop_freq = 1 # Hz - rate = rospy.Rate(loop_freq) - res = False - for i in range(timeout * loop_freq): - try: - res = self.get_param_srv('MAV_TYPE') - if res.success: - self.mav_type = res.value.integer - rospy.loginfo( - "MAV_TYPE received | type: {0} | seconds: {1} of {2}". - format(mavutil.mavlink.enums['MAV_TYPE'][self.mav_type] - .name, i / loop_freq, timeout)) - break - except rospy.ServiceException as e: - rospy.logerr(e) - - try: - rate.sleep() - except rospy.ROSException as e: - self.fail(e) - - self.assertTrue(res.success, ( - "MAV_TYPE param get failed | timeout(seconds): {0}".format(timeout) - )) - - def log_topic_vars(self): - """log the state of topic variables""" - rospy.loginfo("========================") - rospy.loginfo("===== topic values =====") - rospy.loginfo("========================") - rospy.loginfo("altitude:\n{}".format(self.altitude)) - rospy.loginfo("========================") - rospy.loginfo("extended_state:\n{}".format(self.extended_state)) - rospy.loginfo("========================") - rospy.loginfo("global_position:\n{}".format(self.global_position)) - rospy.loginfo("========================") - rospy.loginfo("home_position:\n{}".format(self.home_position)) - rospy.loginfo("========================") - rospy.loginfo("local_position:\n{}".format(self.local_position)) - rospy.loginfo("========================") - rospy.loginfo("mission_wp:\n{}".format(self.mission_wp)) - rospy.loginfo("========================") - rospy.loginfo("state:\n{}".format(self.state)) - rospy.loginfo("========================") diff --git a/integrationtests/python_src/px4_it/mavros/mission_test.py b/integrationtests/python_src/px4_it/mavros/mission_test.py deleted file mode 100755 index c4134cff00..0000000000 --- a/integrationtests/python_src/px4_it/mavros/mission_test.py +++ /dev/null @@ -1,358 +0,0 @@ -#!/usr/bin/env python3 -#*************************************************************************** -# -# Copyright (c) 2015-2016 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 -# - -PKG = 'px4' - -import rospy -import glob -import json -import math -import os -import numpy as np -from pyulog import ULog -import sys -from mavros import mavlink -from mavros_msgs.msg import Mavlink, Waypoint, WaypointReached -from mavros_test_common import MavrosTestCommon -from pymavlink import mavutil -from threading import Thread - - -def get_last_log(): - try: - log_path = os.environ['PX4_LOG_DIR'] - except KeyError: - try: - log_path = os.path.join(os.environ['ROS_HOME'], 'log') - except KeyError: - log_path = os.path.join(os.environ['HOME'], '.ros/log') - last_log_dir = sorted(glob.glob(os.path.join(log_path, '*')))[-1] - last_log = sorted(glob.glob(os.path.join(last_log_dir, '*.ulg')))[-1] - return last_log - - -def analyze_estimator_attitude(log_file): - """Compute attitude estimator error metrics from a ULog file.""" - ulog = ULog(log_file) - - att = ulog.get_dataset('vehicle_attitude').data - att_gt = ulog.get_dataset('vehicle_attitude_groundtruth').data - - def quat_to_euler(q0, q1, q2, q3): - """Quaternion (w,x,y,z) to (roll, pitch, yaw) via ZYX Tait-Bryan.""" - roll = np.arctan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1**2 + q2**2)) - sinp = 2 * (q0 * q2 - q3 * q1) - pitch = np.where(np.abs(sinp) >= 1, - np.copysign(np.pi / 2, sinp), np.arcsin(sinp)) - yaw = np.arctan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2**2 + q3**2)) - return roll, pitch, yaw - - roll, pitch, yaw = quat_to_euler( - att['q[0]'], att['q[1]'], att['q[2]'], att['q[3]']) - roll_gt, pitch_gt, yaw_gt = quat_to_euler( - att_gt['q[0]'], att_gt['q[1]'], att_gt['q[2]'], att_gt['q[3]']) - - # interpolate groundtruth onto attitude timestamps - ts = att['timestamp'] - ts_gt = att_gt['timestamp'] - roll_gt = np.interp(ts, ts_gt, roll_gt) - pitch_gt = np.interp(ts, ts_gt, pitch_gt) - yaw_gt = np.interp(ts, ts_gt, yaw_gt) - - wrap = lambda x: np.arcsin(np.sin(x)) - e_roll = wrap(roll - roll_gt) - e_pitch = wrap(pitch - pitch_gt) - e_yaw = wrap(yaw - yaw_gt) - - return { - 'roll_error_mean': np.rad2deg(np.mean(e_roll)), - 'pitch_error_mean': np.rad2deg(np.mean(e_pitch)), - 'yaw_error_mean': np.rad2deg(np.mean(e_yaw)), - 'roll_error_std': np.rad2deg(np.std(e_roll)), - 'pitch_error_std': np.rad2deg(np.std(e_pitch)), - 'yaw_error_std': np.rad2deg(np.std(e_yaw)), - } - - -def read_mission(mission_filename): - wps = [] - with open(mission_filename, 'r') as f: - for waypoint in read_plan_file(f): - wps.append(waypoint) - rospy.logdebug(waypoint) - - # set first item to current - if wps: - wps[0].is_current = True - - return wps - - -def read_plan_file(f): - d = json.load(f) - if 'mission' in d: - d = d['mission'] - - if 'items' in d: - for wp in d['items']: - yield Waypoint( - is_current=False, - frame=int(wp['frame']), - command=int(wp['command']), - param1=float('nan' - if wp['params'][0] is None else wp['params'][0]), - param2=float('nan' - if wp['params'][1] is None else wp['params'][1]), - param3=float('nan' - if wp['params'][2] is None else wp['params'][2]), - param4=float('nan' - if wp['params'][3] is None else wp['params'][3]), - x_lat=float(wp['params'][4]), - y_long=float(wp['params'][5]), - z_alt=float(wp['params'][6]), - autocontinue=bool(wp['autoContinue'])) - else: - raise IOError("no mission items") - - -class MavrosMissionTest(MavrosTestCommon): - """ - Run a mission - """ - - def setUp(self): - super(self.__class__, self).setUp() - - self.mission_item_reached = -1 # first mission item is 0 - self.mission_name = "" - - self.mavlink_pub = rospy.Publisher('mavlink/to', Mavlink, queue_size=1) - self.mission_item_reached_sub = rospy.Subscriber( - 'mavros/mission/reached', WaypointReached, - self.mission_item_reached_callback) - - # need to simulate heartbeat to prevent datalink loss detection - self.hb_mav_msg = mavutil.mavlink.MAVLink_heartbeat_message( - mavutil.mavlink.MAV_TYPE_GCS, 0, 0, 0, 0, 0) - self.hb_mav_msg.pack(mavutil.mavlink.MAVLink('', 2, 1)) - self.hb_ros_msg = mavlink.convert_to_rosmsg(self.hb_mav_msg) - self.hb_thread = Thread(target=self.send_heartbeat, args=()) - self.hb_thread.daemon = True - self.hb_thread.start() - - def tearDown(self): - super(MavrosMissionTest, self).tearDown() - - # - # Helper methods - # - def send_heartbeat(self): - rate = rospy.Rate(2) # Hz - while not rospy.is_shutdown(): - self.mavlink_pub.publish(self.hb_ros_msg) - try: # prevent garbage in console output when thread is killed - rate.sleep() - except rospy.ROSInterruptException: - pass - - def mission_item_reached_callback(self, data): - if self.mission_item_reached != data.wp_seq: - rospy.loginfo("mission item reached: {0}".format(data.wp_seq)) - self.mission_item_reached = data.wp_seq - - def distance_to_wp(self, lat, lon, alt): - """alt(amsl): meters""" - R = 6371000 # metres - rlat1 = math.radians(lat) - rlat2 = math.radians(self.global_position.latitude) - - rlat_d = math.radians(self.global_position.latitude - lat) - rlon_d = math.radians(self.global_position.longitude - lon) - - a = (math.sin(rlat_d / 2) * math.sin(rlat_d / 2) + math.cos(rlat1) * - math.cos(rlat2) * math.sin(rlon_d / 2) * math.sin(rlon_d / 2)) - c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a)) - - d = R * c - alt_d = abs(alt - self.altitude.amsl) - - rospy.logdebug("d: {0}, alt_d: {1}".format(d, alt_d)) - return d, alt_d - - def reach_position(self, lat, lon, alt, timeout, index): - """alt(amsl): meters, timeout(int): seconds""" - rospy.loginfo( - "trying to reach waypoint | lat: {0:.9f}, lon: {1:.9f}, alt: {2:.2f}, index: {3}". - format(lat, lon, alt, index)) - best_pos_xy_d = None - best_pos_z_d = None - reached = False - mission_length = len(self.mission_wp.waypoints) - - # does it reach the position in 'timeout' seconds? - loop_freq = 2 # Hz - rate = rospy.Rate(loop_freq) - for i in range(timeout * loop_freq): - pos_xy_d, pos_z_d = self.distance_to_wp(lat, lon, alt) - - # remember best distances - if not best_pos_xy_d or best_pos_xy_d > pos_xy_d: - best_pos_xy_d = pos_xy_d - if not best_pos_z_d or best_pos_z_d > pos_z_d: - best_pos_z_d = pos_z_d - - # FCU advanced to the next mission item, or finished mission - reached = ( - # advanced to next wp - (index < self.mission_wp.current_seq) - # end of mission - or (index == (mission_length - 1) and - self.mission_item_reached == index)) - - if reached: - rospy.loginfo( - "position reached | pos_xy_d: {0:.2f}, pos_z_d: {1:.2f}, index: {2} | seconds: {3} of {4}". - format(pos_xy_d, pos_z_d, index, i / loop_freq, timeout)) - break - elif i == 0 or ((i / loop_freq) % 10) == 0: - # log distance first iteration and every 10 sec - rospy.loginfo( - "current distance to waypoint | pos_xy_d: {0:.2f}, pos_z_d: {1:.2f}, index: {2}". - format(pos_xy_d, pos_z_d, index)) - - try: - rate.sleep() - except rospy.ROSException as e: - self.fail(e) - - self.assertTrue( - reached, - "position not reached | lat: {0:.9f}, lon: {1:.9f}, alt: {2:.2f}, current pos_xy_d: {3:.2f}, current pos_z_d: {4:.2f}, best pos_xy_d: {5:.2f}, best pos_z_d: {6:.2f}, index: {7} | timeout(seconds): {8}". - format(lat, lon, alt, pos_xy_d, pos_z_d, best_pos_xy_d, - best_pos_z_d, index, timeout)) - - # - # Test method - # - def test_mission(self): - """Test mission""" - if len(sys.argv) < 2: - self.fail("usage: mission_test.py mission_file") - return - - self.mission_name = sys.argv[1] - mission_file = os.path.dirname( - os.path.realpath(__file__)) + "/missions/" + sys.argv[1] - - rospy.loginfo("reading mission {0}".format(mission_file)) - try: - wps = read_mission(mission_file) - except IOError as e: - self.fail(e) - - # make sure the simulation is ready to start the mission - self.wait_for_topics(60) - self.wait_for_landed_state(mavutil.mavlink.MAV_LANDED_STATE_ON_GROUND, - 10, -1) - self.wait_for_mav_type(10) - - # push waypoints to FCU and start mission - self.send_wps(wps, 30) - self.log_topic_vars() - self.set_mode("AUTO.MISSION", 5) - self.set_arm(True, 5) - - rospy.loginfo("run mission {0}".format(self.mission_name)) - for index, waypoint in enumerate(wps): - # only check position for waypoints where this makes sense - if (waypoint.frame == Waypoint.FRAME_GLOBAL_REL_ALT or - waypoint.frame == Waypoint.FRAME_GLOBAL): - alt = waypoint.z_alt - if waypoint.frame == Waypoint.FRAME_GLOBAL_REL_ALT: - alt += self.altitude.amsl - self.altitude.relative - - self.reach_position(waypoint.x_lat, waypoint.y_long, alt, 60, - index) - - # check if VTOL transition happens if applicable - if (waypoint.command == mavutil.mavlink.MAV_CMD_NAV_VTOL_TAKEOFF or - waypoint.command == mavutil.mavlink.MAV_CMD_NAV_VTOL_LAND - or waypoint.command == - mavutil.mavlink.MAV_CMD_DO_VTOL_TRANSITION): - transition = waypoint.param1 # used by MAV_CMD_DO_VTOL_TRANSITION - if waypoint.command == mavutil.mavlink.MAV_CMD_NAV_VTOL_TAKEOFF: # VTOL takeoff implies transition to FW - transition = mavutil.mavlink.MAV_VTOL_STATE_FW - if waypoint.command == mavutil.mavlink.MAV_CMD_NAV_VTOL_LAND: # VTOL land implies transition to MC - transition = mavutil.mavlink.MAV_VTOL_STATE_MC - - self.wait_for_vtol_state(transition, 60, index) - - # after reaching position, wait for landing detection if applicable - if (waypoint.command == mavutil.mavlink.MAV_CMD_NAV_VTOL_LAND or - waypoint.command == mavutil.mavlink.MAV_CMD_NAV_LAND): - self.wait_for_landed_state( - mavutil.mavlink.MAV_LANDED_STATE_ON_GROUND, 120, index) - - self.set_arm(False, 5) - self.clear_wps(5) - - rospy.loginfo("mission done, calculating performance metrics") - last_log = get_last_log() - rospy.loginfo("log file {0}".format(last_log)) - res = analyze_estimator_attitude(last_log) - - # enforce performance - self.assertTrue(abs(res['roll_error_mean']) < 5.0, str(res)) - self.assertTrue(abs(res['pitch_error_mean']) < 5.0, str(res)) - self.assertTrue(abs(res['yaw_error_mean']) < 5.0, str(res)) - - self.assertTrue(res['roll_error_std'] < 5.0, str(res)) - self.assertTrue(res['pitch_error_std'] < 5.0, str(res)) - - # TODO: fix by excluding initial heading init and reset preflight - self.assertTrue(res['yaw_error_std'] < 15.0, str(res)) - - -if __name__ == '__main__': - import rostest - rospy.init_node('test_node', anonymous=True) - - name = "mavros_mission_test" - if len(sys.argv) > 1: - name += "-%s" % sys.argv[1] - rostest.rosrun(PKG, name, MavrosMissionTest) diff --git a/integrationtests/python_src/px4_it/mavros/missions/FW_mission_1.plan b/integrationtests/python_src/px4_it/mavros/missions/FW_mission_1.plan deleted file mode 100644 index 03986f692e..0000000000 --- a/integrationtests/python_src/px4_it/mavros/missions/FW_mission_1.plan +++ /dev/null @@ -1,123 +0,0 @@ -{ - "fileType": "Plan", - "geoFence": { - "circles": [ - ], - "polygons": [ - ], - "version": 2 - }, - "groundStation": "QGroundControl", - "mission": { - "cruiseSpeed": 15, - "firmwareType": 12, - "hoverSpeed": 5, - "items": [ - { - "AMSLAltAboveTerrain": 50, - "Altitude": 50, - "AltitudeMode": 0, - "autoContinue": true, - "command": 22, - "doJumpId": 1, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.397757116360204, - 8.54802567938998, - 50 - ], - "type": "SimpleItem" - }, - { - "AMSLAltAboveTerrain": 50, - "Altitude": 50, - "AltitudeMode": 0, - "autoContinue": true, - "command": 16, - "doJumpId": 2, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.395834920202326, - 8.548141558196932, - 50 - ], - "type": "SimpleItem" - }, - { - "autoContinue": true, - "command": 189, - "doJumpId": 3, - "frame": 2, - "params": [ - 0, - 0, - 0, - 0, - 0, - 0, - 0 - ], - "type": "SimpleItem" - }, - { - "AMSLAltAboveTerrain": 25, - "Altitude": 25, - "AltitudeMode": 0, - "autoContinue": true, - "command": 31, - "doJumpId": 4, - "frame": 3, - "params": [ - 1, - 50, - 0, - 1, - 47.39580995751514, - 8.545613891100459, - 25 - ], - "type": "SimpleItem" - }, - { - "AMSLAltAboveTerrain": 0, - "Altitude": 0, - "AltitudeMode": 0, - "autoContinue": true, - "command": 21, - "doJumpId": 5, - "frame": 3, - "params": [ - 25, - 0, - 0, - null, - 47.39757125904106, - 8.545589014690734, - 0 - ], - "type": "SimpleItem" - } - ], - "plannedHomePosition": [ - 47.397742, - 8.545594, - 489 - ], - "vehicleType": 1, - "version": 2 - }, - "rallyPoints": { - "points": [ - ], - "version": 2 - }, - "version": 1 -} diff --git a/integrationtests/python_src/px4_it/mavros/missions/MC_mission_box.plan b/integrationtests/python_src/px4_it/mavros/missions/MC_mission_box.plan deleted file mode 100644 index f1b04ffd63..0000000000 --- a/integrationtests/python_src/px4_it/mavros/missions/MC_mission_box.plan +++ /dev/null @@ -1,145 +0,0 @@ -{ - "fileType": "Plan", - "geoFence": { - "circles": [ - ], - "polygons": [ - ], - "version": 2 - }, - "groundStation": "QGroundControl", - "mission": { - "cruiseSpeed": 15, - "firmwareType": 12, - "hoverSpeed": 5, - "items": [ - { - "AMSLAltAboveTerrain": null, - "Altitude": 5, - "AltitudeMode": 0, - "autoContinue": true, - "command": 22, - "doJumpId": 1, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.39773941040039, - 8.5455904006958, - 5 - ], - "type": "SimpleItem" - }, - { - "AMSLAltAboveTerrain": null, - "Altitude": 10, - "AltitudeMode": 0, - "autoContinue": true, - "command": 16, - "doJumpId": 2, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.39787292480469, - 8.54559326171875, - 10 - ], - "type": "SimpleItem" - }, - { - "AMSLAltAboveTerrain": null, - "Altitude": 10, - "AltitudeMode": 0, - "autoContinue": true, - "command": 16, - "doJumpId": 3, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.39787292480469, - 8.545341491699219, - 10 - ], - "type": "SimpleItem" - }, - { - "AMSLAltAboveTerrain": null, - "Altitude": 10, - "AltitudeMode": 0, - "autoContinue": true, - "command": 16, - "doJumpId": 4, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.39773941040039, - 8.545336723327637, - 10 - ], - "type": "SimpleItem" - }, - { - "AMSLAltAboveTerrain": null, - "Altitude": 10, - "AltitudeMode": 0, - "autoContinue": true, - "command": 16, - "doJumpId": 5, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.39773941040039, - 8.5455904006958, - 10 - ], - "type": "SimpleItem" - }, - { - "AMSLAltAboveTerrain": null, - "Altitude": 0, - "AltitudeMode": 0, - "autoContinue": true, - "command": 21, - "doJumpId": 6, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.39773941040039, - 8.545591354370117, - 0 - ], - "type": "SimpleItem" - } - ], - "plannedHomePosition": [ - 47.397742, - 8.545594, - 488 - ], - "vehicleType": 2, - "version": 2 - }, - "rallyPoints": { - "points": [ - ], - "version": 2 - }, - "version": 1 -} diff --git a/integrationtests/python_src/px4_it/mavros/missions/MC_safe_landing.plan b/integrationtests/python_src/px4_it/mavros/missions/MC_safe_landing.plan deleted file mode 100644 index f2593118ba..0000000000 --- a/integrationtests/python_src/px4_it/mavros/missions/MC_safe_landing.plan +++ /dev/null @@ -1,107 +0,0 @@ -{ - "fileType": "Plan", - "geoFence": { - "circles": [ - ], - "polygons": [ - ], - "version": 2 - }, - "groundStation": "QGroundControl", - "mission": { - "cruiseSpeed": 15, - "firmwareType": 12, - "hoverSpeed": 5, - "items": [ - { - "AMSLAltAboveTerrain": null, - "Altitude": 18, - "AltitudeMode": 1, - "autoContinue": true, - "command": 22, - "doJumpId": 1, - "frame": 3, - "params": [ - 15, - 0, - 0, - null, - 47.3977394, - 8.5455942, - 18 - ], - "type": "SimpleItem" - }, - { - "AMSLAltAboveTerrain": null, - "Altitude": 18, - "AltitudeMode": 1, - "autoContinue": true, - "command": 16, - "doJumpId": 2, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.3977394, - 8.5456982, - 18 - ], - "type": "SimpleItem" - }, - { - "AMSLAltAboveTerrain": null, - "Altitude": 18, - "AltitudeMode": 1, - "autoContinue": true, - "command": 16, - "doJumpId": 3, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.3977432, - 8.545805, - 18 - ], - "type": "SimpleItem" - }, - { - "AMSLAltAboveTerrain": null, - "Altitude": 0, - "AltitudeMode": 1, - "autoContinue": true, - "command": 21, - "doJumpId": 4, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.3977432, - 8.545804, - 0 - ], - "type": "SimpleItem" - } - ], - "plannedHomePosition": [ - 47.3977419, - 8.5455941, - 489 - ], - "vehicleType": 2, - "version": 2 - }, - "rallyPoints": { - "points": [ - ], - "version": 2 - }, - "version": 1 -} diff --git a/integrationtests/python_src/px4_it/mavros/missions/VTOL_mission_1.plan b/integrationtests/python_src/px4_it/mavros/missions/VTOL_mission_1.plan deleted file mode 100644 index 8068b43127..0000000000 --- a/integrationtests/python_src/px4_it/mavros/missions/VTOL_mission_1.plan +++ /dev/null @@ -1,107 +0,0 @@ -{ - "fileType": "Plan", - "geoFence": { - "circles": [ - ], - "polygons": [ - ], - "version": 2 - }, - "groundStation": "QGroundControl", - "mission": { - "cruiseSpeed": 15, - "firmwareType": 12, - "hoverSpeed": 5, - "items": [ - { - "AMSLAltAboveTerrain": 15, - "Altitude": 15, - "AltitudeMode": 0, - "autoContinue": true, - "command": 84, - "doJumpId": 1, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.39773615102264, - 8.547064163497595, - 15 - ], - "type": "SimpleItem" - }, - { - "AMSLAltAboveTerrain": null, - "Altitude": 30, - "AltitudeMode": 0, - "autoContinue": true, - "command": 31, - "doJumpId": 2, - "frame": 3, - "params": [ - 0, - 50, - 0, - 1, - 47.396840504834636, - 8.548309212214804, - 30 - ], - "type": "SimpleItem" - }, - { - "AMSLAltAboveTerrain": null, - "Altitude": 15, - "AltitudeMode": 0, - "autoContinue": true, - "command": 16, - "doJumpId": 3, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.39617583578575, - 8.546774195044463, - 15 - ], - "type": "SimpleItem" - }, - { - "AMSLAltAboveTerrain": 0, - "Altitude": 0, - "AltitudeMode": 0, - "autoContinue": true, - "command": 85, - "doJumpId": 4, - "frame": 3, - "params": [ - 0, - 0, - 0, - null, - 47.39706525003061, - 8.545322355693173, - 0 - ], - "type": "SimpleItem" - } - ], - "plannedHomePosition": [ - 47.397742, - 8.545594, - 489 - ], - "vehicleType": 20, - "version": 2 - }, - "rallyPoints": { - "points": [ - ], - "version": 2 - }, - "version": 1 -} diff --git a/launch/mavros_posix_sitl.launch b/launch/mavros_posix_sitl.launch deleted file mode 100644 index 31b93fa15d..0000000000 --- a/launch/mavros_posix_sitl.launch +++ /dev/null @@ -1,54 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/launch/pub_test.launch b/launch/pub_test.launch deleted file mode 100644 index 769a51aa9c..0000000000 --- a/launch/pub_test.launch +++ /dev/null @@ -1,13 +0,0 @@ - - - - - - - - - - - - - diff --git a/platforms/posix/CMakeLists.txt b/platforms/posix/CMakeLists.txt index 594fa95c99..2c1dd89112 100644 --- a/platforms/posix/CMakeLists.txt +++ b/platforms/posix/CMakeLists.txt @@ -181,7 +181,6 @@ elseif("${PX4_BOARD}" MATCHES "sitl") # px4 dirs install( DIRECTORY - ${PROJECT_SOURCE_DIR}/integrationtests ${PROJECT_SOURCE_DIR}/launch ${CMAKE_RUNTIME_OUTPUT_DIRECTORY} DESTINATION diff --git a/test/mavros_posix_test_mission.test b/test/mavros_posix_test_mission.test deleted file mode 100644 index 4d946f1b43..0000000000 --- a/test/mavros_posix_test_mission.test +++ /dev/null @@ -1,22 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - diff --git a/test/mavros_posix_test_safe_landing.test b/test/mavros_posix_test_safe_landing.test deleted file mode 100644 index 909a5f86d2..0000000000 --- a/test/mavros_posix_test_safe_landing.test +++ /dev/null @@ -1,51 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/test/mavros_posix_tests_iris_opt_flow.test b/test/mavros_posix_tests_iris_opt_flow.test deleted file mode 100644 index c7d3f71b14..0000000000 --- a/test/mavros_posix_tests_iris_opt_flow.test +++ /dev/null @@ -1,21 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - diff --git a/test/mavros_posix_tests_missions.test b/test/mavros_posix_tests_missions.test deleted file mode 100644 index 16b6d62557..0000000000 --- a/test/mavros_posix_tests_missions.test +++ /dev/null @@ -1,26 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/test/mavros_posix_tests_offboard_attctl.test b/test/mavros_posix_tests_offboard_attctl.test deleted file mode 100644 index 58fc2df6f0..0000000000 --- a/test/mavros_posix_tests_offboard_attctl.test +++ /dev/null @@ -1,21 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - diff --git a/test/mavros_posix_tests_offboard_posctl.test b/test/mavros_posix_tests_offboard_posctl.test deleted file mode 100644 index 7fbb35737e..0000000000 --- a/test/mavros_posix_tests_offboard_posctl.test +++ /dev/null @@ -1,21 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - diff --git a/test/mavros_posix_tests_offboard_rpyrt_ctl.test b/test/mavros_posix_tests_offboard_rpyrt_ctl.test deleted file mode 100644 index 6d26412d26..0000000000 --- a/test/mavros_posix_tests_offboard_rpyrt_ctl.test +++ /dev/null @@ -1,21 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - diff --git a/test/rostest_px4_run.sh b/test/rostest_px4_run.sh deleted file mode 100755 index e66e833e61..0000000000 --- a/test/rostest_px4_run.sh +++ /dev/null @@ -1,14 +0,0 @@ -#!/usr/bin/env bash - -DIR=$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd ) -PX4_SRC_DIR=${DIR}/.. - -source /opt/ros/${ROS_DISTRO:-kinetic}/setup.bash -source ${PX4_SRC_DIR}/Tools/simulation/gazebo-classic/setup_gazebo.bash ${PX4_SRC_DIR} ${PX4_SRC_DIR}/build/px4_sitl_default - -export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:${PX4_SRC_DIR}:${PX4_SRC_DIR}/Tools/simulation/gazebo-classic/sitl_gazebo-classic - -export ROS_LOG_DIR="$HOME/.ros/ros_logs" -mkdir -p "$ROS_LOG_DIR" - -rostest px4 "$@"