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()