|
1 | 1 | /* mros2 example |
2 | | - * Copyright (c) 2022 mROS-base |
| 2 | + * Copyright (c) 2023 mROS-base |
3 | 3 | * |
4 | 4 | * Licensed under the Apache License, Version 2.0 (the "License"); |
5 | 5 | * you may not use this file except in compliance with the License. |
|
15 | 15 | */ |
16 | 16 |
|
17 | 17 | #include "mros2.h" |
| 18 | +#include "mros2-platform.h" |
18 | 19 | #include "std_msgs/msg/string.hpp" |
19 | 20 |
|
20 | | -#include "cmsis_os.h" |
21 | | -#include "wifi.h" |
22 | | - |
23 | 21 | #include "M5Unified.h" |
24 | | -/* convert TARGET_NAME to put into message */ |
25 | | -#define quote(x) std::string(q(x)) |
26 | | -#define q(x) #x |
| 22 | + |
27 | 23 |
|
28 | 24 | void userCallback(std_msgs::msg::String *msg) |
29 | 25 | { |
30 | | - printf("subscribed msg: '%s'\r\n", msg->data.c_str()); |
| 26 | + MROS2_INFO("subscribed msg: '%s'", msg->data.c_str()); |
31 | 27 | M5.Lcd.printf("subscribed msg: '%s'\r\n", msg->data.c_str()); |
32 | 28 | } |
33 | 29 |
|
34 | 30 | extern "C" void app_main(void) |
35 | 31 | { |
36 | | - init_wifi(); |
37 | | - osKernelStart(); |
| 32 | + /* connect to the network */ |
| 33 | + if (mros2_platform_network_connect()) |
| 34 | + { |
| 35 | + MROS2_INFO("successfully connect and setup network\r\n---"); |
| 36 | + } |
| 37 | + else |
| 38 | + { |
| 39 | + MROS2_ERROR("failed to connect and setup network! aborting,,,"); |
| 40 | + return; |
| 41 | + } |
| 42 | + |
| 43 | + MROS2_INFO("%s start!", MROS2_PLATFORM_NAME); |
| 44 | + MROS2_INFO("app name: echoback_string"); |
38 | 45 |
|
39 | 46 | auto cfg = M5.config(); |
40 | 47 | M5.begin(cfg); |
41 | 48 | M5.Lcd.setTextSize(2); |
42 | | - M5.Lcd.println("mbed mros2 start!\r\n"); |
| 49 | + M5.Lcd.println(""); |
| 50 | + M5.Lcd.printf("%s start!\r\n", MROS2_PLATFORM_NAME); |
43 | 51 | M5.Lcd.printf("app name: m5stack_sample\r\n"); |
44 | 52 |
|
45 | 53 | mros2::init(0, NULL); |
46 | | - MROS2_DEBUG("mROS 2 initialization is completed\r\n"); |
| 54 | + MROS2_DEBUG("mROS 2 initialization is completed"); |
47 | 55 |
|
48 | 56 | mros2::Node node = mros2::Node::create_node("mros2_node"); |
49 | 57 | mros2::Publisher pub = node.create_publisher<std_msgs::msg::String>("to_linux", 10); |
50 | 58 | mros2::Subscriber sub = node.create_subscription<std_msgs::msg::String>("to_stm", 10, userCallback); |
51 | 59 |
|
52 | 60 | osDelay(100); |
53 | | - MROS2_INFO("ready to pub/sub message\r\n"); |
| 61 | + MROS2_INFO("ready to pub/sub message\r\n---"); |
54 | 62 |
|
55 | 63 | auto count = 0; |
56 | 64 | while (1) |
57 | 65 | { |
58 | 66 | auto msg = std_msgs::msg::String(); |
59 | 67 | msg.data = "Hello from mros2-mbed onto " + quote(TARGET_NAME) + ": " + std::to_string(count++); |
60 | | - printf("publishing msg: '%s'\r\n", msg.data.c_str()); |
| 68 | + MROS2_INFO("publishing msg: '%s'", msg.data.c_str()); |
61 | 69 | pub.publish(msg); |
| 70 | + M5.Lcd.printf("publishing msg: '%s'\r\n", msg.data.c_str()); |
62 | 71 | osDelay(1000); |
63 | 72 | } |
64 | 73 |
|
|
0 commit comments