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
if __name__ == '__main__':
rospy.init_node('state_machine', log_level=rospy.INFO)
sm = StateMachine('TOP_LEVEL_SM')
setup_state = SetupState()
nav_state = NavState()
hover_state = HoverState(newPoseStamped(2, 2, 0.4, 0, 0, 0, 'map'))
setup_state.transitions['success'] = 'NAV_STATE'
nav_state.transitions['success'] = 'HOVER_STATE'
sm.add_state(setup_state)
sm.add_state(nav_state)
sm.add_state(hover_state)
rospy.loginfo(rospy.get_name() + ': Starting statemachine...')
try:
sm.setup()
sm.execute()
except ROSInterruptException:
pass
rospy.spin()
