Commit e0e7f8c5 authored by Andreas Antener's avatar Andreas Antener Committed by Thomas Gubler
Browse files

- updated manual input to publish directly to px4 and not over joystick topics

- updated tests to work with current setup
parent a54849ee
<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>
......@@ -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
......@@ -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)
......@@ -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">
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment