diff options
-rw-r--r-- | integrationtests/demo_tests/demo_tests.launch | 9 | ||||
-rwxr-xr-x | integrationtests/demo_tests/manual_input.py | 95 | ||||
-rwxr-xr-x | integrationtests/demo_tests/offboard_posctl_test.py (renamed from integrationtests/demo_tests/posctl_test.py) | 10 | ||||
-rw-r--r-- | launch/gazebo_ardrone_empty_world.launch | 2 |
4 files changed, 84 insertions, 32 deletions
diff --git a/integrationtests/demo_tests/demo_tests.launch b/integrationtests/demo_tests/demo_tests.launch index b5377659f..34281b781 100644 --- a/integrationtests/demo_tests/demo_tests.launch +++ b/integrationtests/demo_tests/demo_tests.launch @@ -1,8 +1,8 @@ <launch> - <arg name="headless" default="true"/> - <arg name="gui" default="false"/> + <arg name="headless" default="false"/> + <arg name="gui" default="true"/> <arg name="enable_logging" default="false"/> - <arg name="enable_ground_truth" default="true"/> + <arg name="enable_ground_truth" default="false"/> <arg name="log_file" default="iris"/> <include file="$(find px4)/launch/gazebo_iris_empty_world.launch"> @@ -12,7 +12,8 @@ <arg name="enable_ground_truth" value="$(arg enable_ground_truth)" /> <arg name="log_file" value="$(arg log_file)"/> </include> + <include file="$(find px4)/launch/mavros_sitl.launch" /> <test test-name="arm" pkg="px4" type="arm_test.py" /> - <test test-name="posctl" pkg="px4" type="posctl_test.py" /> + <test test-name="offboard_posctl" pkg="px4" type="offboard_posctl_test.py" /> </launch> diff --git a/integrationtests/demo_tests/manual_input.py b/integrationtests/demo_tests/manual_input.py index 4bf508509..eb9144bbc 100755 --- a/integrationtests/demo_tests/manual_input.py +++ b/integrationtests/demo_tests/manual_input.py @@ -2,43 +2,59 @@ import sys import rospy -from sensor_msgs.msg import Joy +from px4.msg import manual_control_setpoint +from mav_msgs.msg import CommandAttitudeThrust from std_msgs.msg import Header - # -# Manual input control helper, fakes joystick input -# > needs to correspond to default mapping in manual_input node +# Manual input control helper +# +# Note: this is not the way to do it. ATM it fakes input to iris/command/attitude because else +# the simulator does not instantiate our controller. # class ManualInput: def __init__(self): rospy.init_node('test_node', anonymous=True) - self.joyPx4 = rospy.Publisher('px4_multicopter/joy', Joy, queue_size=10) - self.joyIris = rospy.Publisher('iris/joy', Joy, queue_size=10) + self.pubMcsp = rospy.Publisher('px4_multicopter/manual_control_setpoint', manual_control_setpoint, queue_size=10) + self.pubAtt = rospy.Publisher('iris/command/attitude', CommandAttitudeThrust, queue_size=10) def arm(self): rate = rospy.Rate(10) # 10hz - msg = Joy() - msg.header = Header() - msg.buttons = [0, 0, 0, 0, 0] - msg.axes = [-0.0, -0.0, 1.0, -0.0, -0.0, 0.0, 0.0] + att = CommandAttitudeThrust() + att.header = Header() + + pos = manual_control_setpoint() + pos.x = 0 + pos.z = 0 + pos.y = 0 + pos.r = 0 + pos.mode_switch = 3 + pos.return_switch = 3 + pos.posctl_switch = 3 + pos.loiter_switch = 3 + pos.acro_switch = 0 + pos.offboard_switch = 3 + count = 0 while not rospy.is_shutdown() and count < 10: rospy.loginfo("zeroing") - self.joyPx4.publish(msg) - self.joyIris.publish(msg) + time = rospy.get_rostime().now() + pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 + self.pubMcsp.publish(pos) + # Fake input to iris commander + self.pubAtt.publish(att) rate.sleep() count = count + 1 - msg.buttons = [0, 0, 0, 0, 0] - msg.axes = [-1.0, -0.0, 1.0, -0.0, -0.0, 0.0, 0.0] + pos.r = 1 count = 0 while not rospy.is_shutdown() and count < 10: rospy.loginfo("arming") - self.joyPx4.publish(msg) - self.joyIris.publish(msg) + time = rospy.get_rostime().now() + pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 + self.pubMcsp.publish(pos) rate.sleep() count = count + 1 @@ -46,14 +62,49 @@ class ManualInput: rate = rospy.Rate(10) # 10hz # triggers posctl - msg = Joy() - msg.header = Header() - msg.buttons = [0, 0, 1, 0, 0] - msg.axes = [-0.0, -0.0, 1.0, -0.0, -0.0, 0.0, 0.0] + pos = manual_control_setpoint() + pos.x = 0 + pos.z = 0 + pos.y = 0 + pos.r = 0 + pos.mode_switch = 2 + pos.return_switch = 3 + pos.posctl_switch = 1 + pos.loiter_switch = 3 + pos.acro_switch = 0 + pos.offboard_switch = 3 + + count = 0 + while not rospy.is_shutdown() and count < 10: + rospy.loginfo("triggering posctl") + time = rospy.get_rostime().now() + pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 + self.pubMcsp.publish(pos) + rate.sleep() + count = count + 1 + + def offboard(self): + rate = rospy.Rate(10) # 10hz + + # triggers posctl + pos = manual_control_setpoint() + pos.x = 0 + pos.z = 0 + pos.y = 0 + pos.r = 0 + pos.mode_switch = 3 + pos.return_switch = 3 + pos.posctl_switch = 3 + pos.loiter_switch = 3 + pos.acro_switch = 0 + pos.offboard_switch = 1 + count = 0 while not rospy.is_shutdown() and count < 10: rospy.loginfo("triggering posctl") - self.joyPx4.publish(msg) - self.joyIris.publish(msg) + time = rospy.get_rostime().now() + pos.timestamp = time.secs * 1e6 + time.nsecs / 1000 + self.pubMcsp.publish(pos) rate.sleep() count = count + 1 + diff --git a/integrationtests/demo_tests/posctl_test.py b/integrationtests/demo_tests/offboard_posctl_test.py index 1cbf09cf7..0d5608245 100755 --- a/integrationtests/demo_tests/posctl_test.py +++ b/integrationtests/demo_tests/offboard_posctl_test.py @@ -16,7 +16,7 @@ from std_msgs.msg import Header from manual_input import ManualInput -class PosctlTest(unittest.TestCase): +class OffboardPosctlTest(unittest.TestCase): # # General callback functions used in tests @@ -37,7 +37,7 @@ class PosctlTest(unittest.TestCase): return self.localPosition.z > (z - offset) and self.localPosition.z < (z + offset) # - # Test POSCTL + # Test offboard POSCTL # def test_posctl(self): rospy.init_node('test_node', anonymous=True) @@ -48,9 +48,9 @@ class PosctlTest(unittest.TestCase): manIn = ManualInput() - # arm and go into POSCTL + # arm and go into offboard manIn.arm() - manIn.posctl() + manIn.offboard() self.assertTrue(self.controlMode.flag_armed, "flag_armed is not set") self.assertTrue(self.controlMode.flag_control_offboard_enabled, "flag_control_offboard_enabled is not set") self.assertTrue(self.controlMode.flag_control_position_enabled, "flag_control_position_enabled is not set") @@ -93,4 +93,4 @@ class PosctlTest(unittest.TestCase): if __name__ == '__main__': import rostest - rostest.rosrun(PKG, 'posctl_test', PosctlTest) + rostest.rosrun(PKG, 'posctl_test', OffboardPosctlTest) diff --git a/launch/gazebo_ardrone_empty_world.launch b/launch/gazebo_ardrone_empty_world.launch index 9d19fe5f9..22bfb0c79 100644 --- a/launch/gazebo_ardrone_empty_world.launch +++ b/launch/gazebo_ardrone_empty_world.launch @@ -3,7 +3,7 @@ <arg name="headless" default="false"/> <arg name="gui" default="true"/> <arg name="enable_logging" default="false"/> - <arg name="enable_ground_truth" default="true"/> + <arg name="enable_ground_truth" default="false"/> <arg name="log_file" default="ardrone"/> <include file="$(find rotors_gazebo)/launch/ardrone_empty_world_with_joy.launch"> |