|
|
|
@ -65,12 +65,12 @@ class MavrosOffboardAttctlTest(unittest.TestCase):
@@ -65,12 +65,12 @@ class MavrosOffboardAttctlTest(unittest.TestCase):
|
|
|
|
|
|
|
|
|
|
def setUp(self): |
|
|
|
|
rospy.init_node('test_node', anonymous=True) |
|
|
|
|
rospy.wait_for_service('mavros/cmd/arming', 30) |
|
|
|
|
rospy.Subscriber('px4_multicopter/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) |
|
|
|
|
rospy.Subscriber("mavros/position/local", PoseStamped, self.position_callback) |
|
|
|
|
self.pubAtt = rospy.Publisher('mavros/setpoint/attitude', PoseStamped, queue_size=10) |
|
|
|
|
self.pubThr = rospy.Publisher('mavros/setpoint/att_throttle', Float64, queue_size=10) |
|
|
|
|
self.cmdArm = rospy.ServiceProxy("mavros/cmd/arming", CommandBool) |
|
|
|
|
rospy.wait_for_service('iris/mavros/cmd/arming', 30) |
|
|
|
|
rospy.Subscriber('iris/vehicle_control_mode', vehicle_control_mode, self.vehicle_control_mode_callback) |
|
|
|
|
rospy.Subscriber("iris/mavros/position/local", PoseStamped, self.position_callback) |
|
|
|
|
self.pubAtt = rospy.Publisher('iris/mavros/setpoint/attitude', PoseStamped, queue_size=10) |
|
|
|
|
self.pubThr = rospy.Publisher('iris/mavros/setpoint/att_throttle', Float64, queue_size=10) |
|
|
|
|
self.cmdArm = rospy.ServiceProxy("iris/mavros/cmd/arming", CommandBool) |
|
|
|
|
self.rate = rospy.Rate(10) # 10hz |
|
|
|
|
self.rateSec = rospy.Rate(1) |
|
|
|
|
self.hasPos = False |
|
|
|
|