def main(argv):
ros = None
try:
boot_button = Pin(0, Pin.IN)
builtin_led = Pin(BUILTIN_LED, Pin.OUT)
ros = Ros(CONFIG["ros2_web_bridge_ip_addr"])
log_info = Topic(ros, "/hadabot/log/info", "std_msgs/String")
twist_cmd = Topic(ros, "/hadabot/cmd_vel", "geometry_msgs/Twist")
twist_cmd.subscribe(twist_cmd_cb)
cnt = 0
# Loop forever
while(boot_button.value() == 1):
ros.run_once()
if (cnt % 1000) == 0:
log_info.publish(Message(
"Hadabot heartbeat {}".format(cnt * 0.001)))
cnt += 1
time.sleep(0.01)
logger.info("Boot button pushed... exiting")
except KeyboardInterrupt:
logger.info("Keyboard control-c hit... stopping")
except Exception as ex:
logger.error("Some other exception hit {}".format(str(ex)))
if ros is not None:
ros.terminate()
# Blink 5 times
blink_led(5, end_off=True)
raise ex
# Shutdown
ros.terminate()
builtin_led.off()
def main(argv):
ros = None
try:
hadabot_ip_address = CONFIG["network"]["hadabot_ip_address"]
boot_button = Pin(0, Pin.IN)
builtin_led = Pin(BUILTIN_LED, Pin.OUT)
ros = Ros(CONFIG["ros2_web_bridge_ip_addr"])
# Publish out log info
log_info = Topic(ros, "hadabot/log/info", "std_msgs/String")
# Controller
controller = Controller(ros)
# Subscribe to twist topics
# twist_cmd = Topic(ros, "hadabot/cmd_vel", "geometry_msgs/Twist")
# twist_cmd.subscribe(twist_cmd_cb)
# Subscribe to wheel turn callbacks
rw_sub_topic = Topic(
ros, "hadabot/wheel_power_right", "std_msgs/Float32")
rw_sub_topic.subscribe(controller.right_wheel_cb)
lw_sub_topic = Topic(
ros, "hadabot/wheel_power_left", "std_msgs/Float32")
lw_sub_topic.subscribe(controller.left_wheel_cb)
# Loop forever
last_hb_ms = time.ticks_ms()
last_controller_ms = time.ticks_ms()
while(boot_button.value() == 1):
ros.run_once()
cur_ms = time.ticks_ms()
# logger.info("cur ms - {}".format(cur_ms))
# logger.info("last_controller ms - {}".format(last_controller_ms))
# logger.info("last_hb ms - {}".format(last_hb_ms))
# Run the controller
if time.ticks_diff(cur_ms, last_controller_ms) > 100:
controller.run_once()
last_controller_ms = cur_ms
# Publish heartbeat message
if time.ticks_diff(cur_ms, last_hb_ms) > 5000:
# logger.info("Encoder left - {}".format(en_count_left))
# logger.info("Encoder right - {}".format(en_count_right))
log_info.publish(Message(
"Hadabot heartbeat - IP {}".format(hadabot_ip_address)))
last_hb_ms = cur_ms
time.sleep(0.01)
logger.info("Boot button pushed... exiting")
except KeyboardInterrupt:
logger.info("Keyboard control-c hit... stopping")
except Exception as ex:
logger.error("Some other exception hit {}".format(str(ex)))
if ros is not None:
ros.terminate()
# Blink 5 times
blink_led(5, end_off=True)
raise ex
# Shutdown
ros.terminate()
builtin_led.off()