Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Roll Pitch Yawrate Thrust (RPYrT) CI integration tests #14796

Merged
merged 1 commit into from
Apr 30, 2020
Merged
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 6 additions & 0 deletions .ci/Jenkinsfile-SITL_tests
Original file line number Diff line number Diff line change
@@ -60,6 +60,12 @@ pipeline {
mission: "",
vehicle: "iris"
],
[
name: "MC_offboard_rpyrt",
test: "mavros_posix_tests_offboard_rpyrt_ctl.test",
mission: "",
vehicle: "iris"
],

[
name: "Rover 1",
6 changes: 6 additions & 0 deletions .ci/Jenkinsfile-SITL_tests_ASan
Original file line number Diff line number Diff line change
@@ -60,6 +60,12 @@ pipeline {
mission: "",
vehicle: "iris"
],
[
name: "MC_offboard_rpyrt",
test: "mavros_posix_tests_offboard_rpyrt_ctl.test",
mission: "",
vehicle: "iris"
],

[
name: "Rover 1",
6 changes: 6 additions & 0 deletions .ci/Jenkinsfile-SITL_tests_coverage
Original file line number Diff line number Diff line change
@@ -34,6 +34,12 @@ pipeline {
mission: "",
vehicle: "iris"
],
[
name: "MC_offboard_rpyrt",
test: "mavros_posix_tests_offboard_rpyrt_ctl.test",
mission: "",
vehicle: "iris"
],

[
name: "Rover 1",
1 change: 1 addition & 0 deletions Makefile
Original file line number Diff line number Diff line change
@@ -379,6 +379,7 @@ tests_mission_coverage:
tests_offboard: rostest
@"$(SRC_DIR)"/test/rostest_px4_run.sh mavros_posix_tests_offboard_attctl.test
@"$(SRC_DIR)"/test/rostest_px4_run.sh mavros_posix_tests_offboard_posctl.test
@"$(SRC_DIR)"/test/rostest_px4_run.sh mavros_posix_tests_offboard_rpyrt_ctl.test

tests_avoidance: rostest
@"$(SRC_DIR)"/test/rostest_avoidance_run.sh mavros_posix_test_avoidance.test
Original file line number Diff line number Diff line change
@@ -0,0 +1,177 @@
#!/usr/bin/env python2
#***************************************************************************
#
# 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 <[email protected]>
#

from __future__ import division

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 six.moves import xrange
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 seperate 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 xrange(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)
14 changes: 12 additions & 2 deletions integrationtests/python_src/px4_it/mavros/mavros_test_common.py
Original file line number Diff line number Diff line change
@@ -10,7 +10,7 @@
from mavros_msgs.srv import CommandBool, ParamGet, SetMode, WaypointClear, \
WaypointPush
from pymavlink import mavutil
from sensor_msgs.msg import NavSatFix
from sensor_msgs.msg import NavSatFix, Imu
from six.moves import xrange


@@ -22,6 +22,7 @@ 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()
@@ -32,7 +33,7 @@ def setUp(self):
key: False
for key in [
'alt', 'ext_state', 'global_pos', 'home_pos', 'local_pos',
'mission_wp', 'state'
'mission_wp', 'state', 'imu'
]
}

@@ -66,6 +67,9 @@ def setUp(self):
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)
@@ -114,6 +118,12 @@ def global_position_callback(self, 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

21 changes: 21 additions & 0 deletions test/mavros_posix_tests_offboard_rpyrt_ctl.test
Original file line number Diff line number Diff line change
@@ -0,0 +1,21 @@
<?xml version="1.0"?>
<launch>
<!-- Posix SITL MAVROS integration tests -->
<!-- Test offboard attitude control -->
<arg name="est" default="ekf2"/>
<arg name="gui" default="false"/>
<arg name="interactive" default="false"/>
<arg name="vehicle" default="iris"/>
<!-- MAVROS, PX4 SITL, Gazebo -->
<include file="$(find px4)/launch/mavros_posix_sitl.launch">
<arg name="est" value="$(arg est)"/>
<arg name="gui" value="$(arg gui)"/>
<arg name="interactive" value="$(arg interactive)"/>
<arg name="respawn_gazebo" value="true"/>
<arg name="respawn_mavros" value="true"/>
<arg name="vehicle" value="$(arg vehicle)"/>
<arg name="verbose" value="true"/>
</include>
<!-- ROStest -->
<test test-name="mavros_offboard_yawrate_test" pkg="px4" type="mavros_offboard_yawrate_test.py" time-limit="300.0"/>
</launch>