class MainNode():
def __init__(self):
rospy.init_node('default_offboard', anonymous=True)
self.rate = rospy.Rate(20)
self.mav1 = Mav("mavros")
self._command_sub = rospy.Subscriber("pose_command", String,
self._pose_command_callback)
# wait for FCU connection
self.mav1.wait_for_connection()
print("mavs connected")
self.sm = StateMachine()
self.sm.set_params(self.mav1)
self.sm.set_current_state(self.sm.States.TAKE_OFF)
def loop(self):
# enter the main loop
while (True):
# print "Entered whiled loop"
self.sm.execute()
self.rate.sleep()
def _pose_command_callback(self, topic=String()):
text = topic.data
textarr = np.array(text.split(";"))
arr = textarr.astype(np.float)
print("new target pose: " + str(arr.tolist()))
pose = create_setpoint_message_xyz_yaw(arr[0], arr[1], arr[2], arr[3])
self.mav1.set_target_pose(pose)
self.mav1.max_speed = 1
self.sm.set_next_state(self.sm.States.IDLE)
self.sm.set_current_state(self.sm.States.WAITING_TO_ARRIVE)
pass
class MainNode():
def __init__(self):
rospy.init_node('default_offboard', anonymous=True)
self.rate = rospy.Rate(20)
self.mav1 = Mav("uav1/mavros")
self.mav2 = Mav("uav2/mavros")
self.sm = StateMachine()
self.sm.set_params((self.mav1, self.mav2))
self._command_sub = rospy.Subscriber("pose_command", String,
self._pose_command_callback)
# wait for FCU connection
self.mav1.wait_for_connection()
self.mav2.wait_for_connection()
self.last_request = rospy.Time.now() - rospy.Duration(5.0)
self.sm.set_current_state(self.sm.States.TAKE_OFF)
def loop(self):
# enter the main loop
while (True):
# print "Entered whiled loop"
self.arm_and_set_mode()
self.sm.execute()
self.rate.sleep()
def arm_and_set_mode(self):
if rospy.Time.now() - self.last_request > rospy.Duration(1.0):
if self.mav1.UAV_state.mode != "OFFBOARD":
self.mav1.set_mode(0, 'OFFBOARD')
print("enabling offboard mode")
self.last_request = rospy.Time.now()
if not self.mav1.UAV_state.armed:
if self.mav1.set_arming(True):
print("Vehicle armed")
self.last_request = rospy.Time.now()
if self.mav2.UAV_state.mode != "OFFBOARD":
self.mav2.set_mode(0, 'OFFBOARD')
print("enabling offboard mode")
self.last_request = rospy.Time.now()
if not self.mav2.UAV_state.armed:
if self.mav2.set_arming(True):
print("Vehicle armed")
self.last_request = rospy.Time.now()
def _pose_command_callback(self, topic=String()):
text = topic.data
textarr = np.array(text.split(";"))
arr = textarr.astype(np.float)
print("new target pose: " + str(arr.tolist()))
pose = create_setpoint_message_xyz_yaw(arr[0], arr[1], arr[2], arr[3])
self.mav1.set_target_pose(pose)
self.mav2.set_target_pose(pose)
self.sm.set_next_state(self.sm.States.IDLE)
self.sm.set_current_state(self.sm.States.WAITING_TO_ARRIVE)
pass
