def __init__(self):
self.update_rate = 20 # [Hz]
# robot state
self.BHV_RC = 0
self.BHV_WPTNAV = 1
self.bhv = self.BHV_RC
# load behaviours
self.bhv_rc = behaviour_remote_control()
self.bhv_wn = behaviour_wptnav()
# get topic names
topic_rc = rospy.get_param("~remote_control_sub",
'/fmHMI/remote_control')
topic_rc_feedback = rospy.get_param("~remote_control_feedback_pub",
'/fmHMI/remote_control_feedback')
deadman_topic = rospy.get_param("~deadman_pub", "/fmSafe/deadman")
behaviour_topic = rospy.get_param("~automode_pub", "/fmPlan/automode")
# setup deadman publish topic
self.deadman_state = False
self.deadman_msg = BoolStamped()
self.deadman_pub = rospy.Publisher(deadman_topic,
BoolStamped,
queue_size=1)
# setup automode publish topic
self.behaviour_msg = IntStamped()
self.behaviour_pub = rospy.Publisher(behaviour_topic,
IntStamped,
queue_size=1)
# setup subscription topic callbacks
rospy.Subscriber(topic_rc, RemoteControl, self.on_rc_topic)
# sall updater function
self.r = rospy.Rate(self.update_rate)
self.updater()
