class Node: def __init__(self): rospy.init_node('STATE') self.is_sitl = rospy.get_param('~is_sitl', False) self.is_airsim = rospy.get_param('~is_airsim', False) self.observer = Observer(self.is_sitl, self.is_airsim) self.rate = 1.0 self.pub_diag = rospy.Publisher('/system_diagnostics', Diagnostics, queue_size=1) self.srv_cmd_state = rospy.Service('set_mode', SetMode, self.set_mode_callback) rospy.wait_for_service('/MOVE/clear_costmaps') self.clear_costmaps_srv = rospy.ServiceProxy('/MOVE/clear_costmaps', Empty) rospy.loginfo('Starting state observer...') self.diag = Diagnostics() def run(self): rospy.loginfo('Starting state broadcast') r_time = rospy.Rate(self.rate) while not rospy.is_shutdown(): state, status = self.observer.get_system_info() self.diag.state = state self.diag.status = status self.pub_diag.publish(self.diag) r_time.sleep() def set_mode_callback(self, msg): reply = 'set_mode did not receive response from system...' if msg.cmd == 'set': reply = self.observer.set_system_mode(msg.target_mode) self.clear_costmaps_srv(EmptyRequest()) elif msg.cmd == 'reset': self.observer.system_reset() reply = "system reset" return reply