1
| //// micro-ROS subscriber sample program// for M5Stack//#include #include #include #include #include #include #include #include rcl_subscription_t subscriber;std_msgs__msg__Int32 msg;rclc_executor_t executor;rclc_support_t support;rcl_allocator_t allocator;rcl_node_t node;rcl_timer_t timer;//#define LED_PIN 13#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}}#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}void error_loop(){ M5.Lcd.print("Error!!\n"); //while(1){ // digitalWrite(LED_PIN, !digitalRead(LED_PIN)); // delay(1000); //}}void subscription_callback(const void * msgin){ const std_msgs__msg__Int32 * msg = (const std_msgs__msg__Int32 *)msgin; //digitalWrite(LED_PIN, (msg->data == 0) ? LOW : HIGH); M5.Lcd.printf("Data: %d\n", msg->data); }void setup() { M5.begin(); M5.Power.begin(); M5.Lcd.setTextSize(2); M5.Lcd.print("Hello micro-ROS\n"); M5.Lcd.print("/micro_ros_arduino_subscriber\n"); // for WiFi set_microros_wifi_transports("SSID", "PASS", "192.168.xxx.xxx", 8888); // for USB //set_microros_transports(); //pinMode(LED_PIN, OUTPUT); //digitalWrite(LED_PIN, HIGH); delay(2000); allocator = rcl_get_default_allocator(); //create init_options RCCHECK(rclc_support_init(&support, 0, NULL, &allocator)); // create node RCCHECK(rclc_node_init_default(&node, "micro_ros_arduino_node", "", &support)); // create subscriber RCCHECK(rclc_subscription_init_default( &subscriber, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), "micro_ros_arduino_subscriber")); // create executor RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator)); RCCHECK(rclc_executor_add_subscription(&executor, &subscriber, &msg, &subscription_callback, ON_NEW_DATA));}void loop() { delay(100); RCCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));}
|
