diff options
Diffstat (limited to 'integrationtests/demo_tests/manual_input.py')
-rwxr-xr-x | integrationtests/demo_tests/manual_input.py | 33 |
1 files changed, 31 insertions, 2 deletions
diff --git a/integrationtests/demo_tests/manual_input.py b/integrationtests/demo_tests/manual_input.py index cf139bb1d..9b2471a00 100755 --- a/integrationtests/demo_tests/manual_input.py +++ b/integrationtests/demo_tests/manual_input.py @@ -39,6 +39,7 @@ import sys import rospy from px4.msg import manual_control_setpoint +from px4.msg import offboard_control_mode from mav_msgs.msg import CommandAttitudeThrust from std_msgs.msg import Header @@ -46,13 +47,15 @@ from std_msgs.msg import Header # Manual input control helper # # FIXME: 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. +# the simulator does not instantiate our controller. Probably this whole class will disappear once +# arming works correctly. # class ManualInput: def __init__(self): rospy.init_node('test_node', anonymous=True) self.pubMcsp = rospy.Publisher('px4_multicopter/manual_control_setpoint', manual_control_setpoint, queue_size=10) + self.pubOff = rospy.Publisher('px4_multicopter/offboard_control_mode', offboard_control_mode, queue_size=10) self.pubAtt = rospy.Publisher('iris/command/attitude', CommandAttitudeThrust, queue_size=10) def arm(self): @@ -119,9 +122,35 @@ class ManualInput: rate.sleep() count = count + 1 - def offboard(self): + + def offboard_attctl(self): + self.offboard(False, False, True, True, True, True) + + def offboard_posctl(self): + self.offboard(False, False, True, False, True, True) + + # Trigger offboard and set offboard control mode before + def offboard(self, ignore_thrust=False, ignore_attitude=False, ignore_bodyrate=True, + ignore_position=False, ignore_velocity=True, ignore_acceleration_force=True): rate = rospy.Rate(10) # 10hz + mode = offboard_control_mode() + mode.ignore_thrust = ignore_thrust + mode.ignore_attitude = ignore_attitude + mode.ignore_bodyrate = ignore_bodyrate + mode.ignore_position = ignore_position + mode.ignore_velocity = ignore_velocity + mode.ignore_acceleration_force = ignore_acceleration_force + + count = 0 + while not rospy.is_shutdown() and count < 5: + rospy.loginfo("setting offboard mode") + time = rospy.get_rostime().now() + mode.timestamp = time.secs * 1e6 + time.nsecs / 1000 + self.pubOff.publish(mode) + rate.sleep() + count = count + 1 + # triggers offboard pos = manual_control_setpoint() pos.x = 0 |