diff --git a/.clang-format b/.clang-format new file mode 100644 index 00000000..e608f4ed --- /dev/null +++ b/.clang-format @@ -0,0 +1,190 @@ +# Source: https://github.com/arduino/tooling-project-assets/tree/main/other/clang-format-configuration +--- +AccessModifierOffset: -2 +AlignAfterOpenBracket: Align +AlignArrayOfStructures: None +AlignConsecutiveAssignments: None +AlignConsecutiveBitFields: None +AlignConsecutiveDeclarations: None +AlignConsecutiveMacros: None +AlignEscapedNewlines: DontAlign +AlignOperands: Align +AlignTrailingComments: true +AllowAllArgumentsOnNextLine: true +AllowAllConstructorInitializersOnNextLine: true +AllowAllParametersOfDeclarationOnNextLine: true +AllowShortBlocksOnASingleLine: Always +AllowShortCaseLabelsOnASingleLine: true +AllowShortEnumsOnASingleLine: true +AllowShortFunctionsOnASingleLine: Empty +AllowShortIfStatementsOnASingleLine: AllIfsAndElse +AllowShortLambdasOnASingleLine: Empty +AllowShortLoopsOnASingleLine: true +AlwaysBreakAfterDefinitionReturnType: None +AlwaysBreakAfterReturnType: None +AlwaysBreakBeforeMultilineStrings: false +AlwaysBreakTemplateDeclarations: No +AttributeMacros: + - __capability +BasedOnStyle: LLVM +BinPackArguments: true +BinPackParameters: true +BitFieldColonSpacing: Both +BraceWrapping: + AfterCaseLabel: false + AfterClass: false + AfterControlStatement: Never + AfterEnum: false + AfterFunction: false + AfterNamespace: false + AfterObjCDeclaration: false + AfterStruct: false + AfterUnion: false + AfterExternBlock: false + BeforeCatch: false + BeforeElse: false + BeforeLambdaBody: false + BeforeWhile: false + IndentBraces: false + SplitEmptyFunction: true + SplitEmptyRecord: true + SplitEmptyNamespace: true +BreakAfterJavaFieldAnnotations: false +BreakBeforeBinaryOperators: NonAssignment +BreakBeforeBraces: Attach +BreakBeforeConceptDeclarations: false +BreakBeforeInheritanceComma: false +BreakBeforeTernaryOperators: true +BreakConstructorInitializers: BeforeColon +BreakConstructorInitializersBeforeComma: false +BreakInheritanceList: BeforeColon +BreakStringLiterals: false +ColumnLimit: 0 +CommentPragmas: '' +CompactNamespaces: false +ConstructorInitializerAllOnOneLineOrOnePerLine: false +ConstructorInitializerIndentWidth: 2 +ContinuationIndentWidth: 2 +Cpp11BracedListStyle: false +DeriveLineEnding: true +DerivePointerAlignment: true +DisableFormat: false +EmptyLineAfterAccessModifier: Leave +EmptyLineBeforeAccessModifier: Leave +ExperimentalAutoDetectBinPacking: false +FixNamespaceComments: false +ForEachMacros: + - foreach + - Q_FOREACH + - BOOST_FOREACH +IfMacros: + - KJ_IF_MAYBE +IncludeBlocks: Preserve +IncludeCategories: + - Regex: '^"(llvm|llvm-c|clang|clang-c)/' + Priority: 2 + SortPriority: 0 + CaseSensitive: false + - Regex: '^(<|"(gtest|gmock|isl|json)/)' + Priority: 3 + SortPriority: 0 + CaseSensitive: false + - Regex: '.*' + Priority: 1 + SortPriority: 0 + CaseSensitive: false +IncludeIsMainRegex: '' +IncludeIsMainSourceRegex: '' +IndentAccessModifiers: false +IndentCaseBlocks: true +IndentCaseLabels: true +IndentExternBlock: Indent +IndentGotoLabels: false +IndentPPDirectives: None +IndentRequires: true +IndentWidth: 2 +IndentWrappedFunctionNames: false +InsertTrailingCommas: None +JavaScriptQuotes: Leave +JavaScriptWrapImports: true +KeepEmptyLinesAtTheStartOfBlocks: true +LambdaBodyIndentation: Signature +Language: Cpp +MacroBlockBegin: '' +MacroBlockEnd: '' +MaxEmptyLinesToKeep: 100000 +NamespaceIndentation: None +ObjCBinPackProtocolList: Auto +ObjCBlockIndentWidth: 2 +ObjCBreakBeforeNestedBlockParam: true +ObjCSpaceAfterProperty: false +ObjCSpaceBeforeProtocolList: true +PPIndentWidth: -1 +PackConstructorInitializers: BinPack +PenaltyBreakAssignment: 1 +PenaltyBreakBeforeFirstCallParameter: 1 +PenaltyBreakComment: 1 +PenaltyBreakFirstLessLess: 1 +PenaltyBreakOpenParenthesis: 1 +PenaltyBreakString: 1 +PenaltyBreakTemplateDeclaration: 1 +PenaltyExcessCharacter: 1 +PenaltyIndentedWhitespace: 1 +PenaltyReturnTypeOnItsOwnLine: 1 +PointerAlignment: Right +QualifierAlignment: Leave +ReferenceAlignment: Pointer +ReflowComments: false +RemoveBracesLLVM: false +SeparateDefinitionBlocks: Leave +ShortNamespaceLines: 0 +SortIncludes: Never +SortJavaStaticImport: Before +SortUsingDeclarations: false +SpaceAfterCStyleCast: false +SpaceAfterLogicalNot: false +SpaceAfterTemplateKeyword: false +SpaceAroundPointerQualifiers: Default +SpaceBeforeAssignmentOperators: true +SpaceBeforeCaseColon: false +SpaceBeforeCpp11BracedList: false +SpaceBeforeCtorInitializerColon: true +SpaceBeforeInheritanceColon: true +SpaceBeforeParens: ControlStatements +SpaceBeforeParensOptions: + AfterControlStatements: true + AfterForeachMacros: true + AfterFunctionDefinitionName: false + AfterFunctionDeclarationName: false + AfterIfMacros: true + AfterOverloadedOperator: false + BeforeNonEmptyParentheses: false +SpaceBeforeRangeBasedForLoopColon: true +SpaceBeforeSquareBrackets: false +SpaceInEmptyBlock: false +SpaceInEmptyParentheses: false +SpacesBeforeTrailingComments: 2 +SpacesInAngles: Leave +SpacesInCStyleCastParentheses: false +SpacesInConditionalStatement: false +SpacesInContainerLiterals: false +SpacesInLineCommentPrefix: + Minimum: 0 + Maximum: -1 +SpacesInParentheses: false +SpacesInSquareBrackets: false +Standard: Auto +StatementAttributeLikeMacros: + - Q_EMIT +StatementMacros: + - Q_UNUSED + - QT_REQUIRE_VERSION +TabWidth: 2 +UseCRLF: false +UseTab: Never +WhitespaceSensitiveMacros: + - STRINGIZE + - PP_STRINGIZE + - BOOST_PP_STRINGIZE + - NS_SWIFT_NAME + - CF_SWIFT_NAME diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 787a9b4f..20a55a24 100755 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -22,6 +22,12 @@ jobs: path: repo - name: Install environment uses: ./repo/.github/actions/platformio-env + - name: Install clang-format + run: apt install -y clang-format + - name: Check formatting + run: | + cd repo + bash ci/check_formatting.sh - name: Build shell: bash run: | diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 00000000..35572481 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,5 @@ +{ + "editor.formatOnSave": true, + "C_Cpp.clang_format_style": "file:${workspaceFolder}/.clang-format", + "files.insertFinalNewline": true +} diff --git a/ci/check_formatting.sh b/ci/check_formatting.sh new file mode 100644 index 00000000..8999cfee --- /dev/null +++ b/ci/check_formatting.sh @@ -0,0 +1,7 @@ +#!/bin/bash + +find . -regex '.*\.\(cpp\|hpp\|c\|h\)' -exec clang-format -dry-run -Werror -style=file:".clang-format" -i {} + +formatting_violated=$? +if [ $formatting_violated -eq 1 ]; then + exit 1 +fi diff --git a/ci/src/main.cpp b/ci/src/main.cpp index 4d4909ff..9a6f474c 100644 --- a/ci/src/main.cpp +++ b/ci/src/main.cpp @@ -18,10 +18,18 @@ // Test custom transports #if defined(MICRO_ROS_TRANSPORT_ARDUINO_CUSTOM) -bool platformio_transport_open(struct uxrCustomTransport * transport) {return false;}; -bool platformio_transport_close(struct uxrCustomTransport * transport) {return false;}; -size_t platformio_transport_write(struct uxrCustomTransport* transport, const uint8_t * buf, size_t len, uint8_t * err) {return 0;}; -size_t platformio_transport_read(struct uxrCustomTransport* transport, uint8_t* buf, size_t len, int timeout, uint8_t* err) {return 0;}; +bool platformio_transport_open(struct uxrCustomTransport* transport) { + return false; +}; +bool platformio_transport_close(struct uxrCustomTransport* transport) { + return false; +}; +size_t platformio_transport_write(struct uxrCustomTransport* transport, const uint8_t* buf, size_t len, uint8_t* err) { + return 0; +}; +size_t platformio_transport_read(struct uxrCustomTransport* transport, uint8_t* buf, size_t len, int timeout, uint8_t* err) { + return 0; +}; #endif // Test extra packages @@ -40,19 +48,26 @@ 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)){}} +#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(){ - while(1){ +void error_loop() { + while (1) { digitalWrite(LED_PIN, !digitalRead(LED_PIN)); delay(100); } } -void timer_callback(rcl_timer_t * timer, int64_t last_call_time) -{ +void timer_callback(rcl_timer_t* timer, int64_t last_call_time) { RCLC_UNUSED(last_call_time); if (timer != NULL) { RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL)); @@ -77,7 +92,7 @@ void setup() { size_t agent_port = 8888; char ssid[] = "WIFI_SSID"; - char psk[]= "WIFI_PSK"; + char psk[] = "WIFI_PSK"; set_microros_wifi_transports(ssid, psk, agent_ip, agent_port); #elif defined(MICRO_ROS_TRANSPORT_ARDUINO_ETHERNET) @@ -95,8 +110,7 @@ void setup() { platformio_transport_open, platformio_transport_close, platformio_transport_write, - platformio_transport_read - ); + platformio_transport_read); #else #error "No transport defined" #endif @@ -137,4 +151,4 @@ void setup() { void loop() { RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100))); -} \ No newline at end of file +} diff --git a/examples/ethernet_pubsub/src/main.cpp b/examples/ethernet_pubsub/src/main.cpp index 419b5773..b981979f 100644 --- a/examples/ethernet_pubsub/src/main.cpp +++ b/examples/ethernet_pubsub/src/main.cpp @@ -1,5 +1,5 @@ #define ETH_PHY_POWER 5 -#define ETH_PHY_MDC 23 +#define ETH_PHY_MDC 23 #define ETH_PHY_MDIO 18 #define ETH_PHY_TYPE ETH_PHY_LAN8720 #define ETH_PHY_ADDR 0 @@ -25,7 +25,7 @@ const char* kNodeName = "eth_pubsub_node"; const char* kPublisherTopic = "micro_ros_response"; const char* kSubscriberTopic = "micro_ros_name"; const int kExecutorTimeout = 100; // ms -const size_t kDomainId = 8; // ROS domain ID +const size_t kDomainId = 8; // ROS domain ID // ROS entities rclc_executor_t executor; @@ -65,16 +65,16 @@ void OnEthernetEvent(arduino_event_id_t event, void* event_info) { case ARDUINO_EVENT_ETH_CONNECTED: Serial.println("[ETH] Connected"); break; - + case ARDUINO_EVENT_ETH_DISCONNECTED: Serial.println("[ETH] Disconnected"); break; - + case ARDUINO_EVENT_ETH_GOT_IP: Serial.print("[ETH] Got IP: "); Serial.println(*(IPAddress*)event_info); break; - + default: break; } @@ -92,9 +92,8 @@ void setup() { // Initialize micro-ROS transport with unified callback set_microros_ethernet_transports( - kClientIP, kGateway, kNetmask, kAgentIP, kAgentPort, - kHostname, OnEthernetEvent - ); + kClientIP, kGateway, kNetmask, kAgentIP, kAgentPort, + kHostname, OnEthernetEvent); delay(2000); connection_state = ConnectionState::kWaitingForAgent; @@ -152,7 +151,7 @@ void SubscriptionCallback(const void* msgin) { // Create response message: "Hello !" snprintf(response_buffer, sizeof(response_buffer), "Hello %s!", msg->data.data); response_msg.data.size = strlen(response_buffer); - + // Publish response PublishResponse(); } @@ -195,16 +194,18 @@ bool CreateEntities() { } // Create publisher - if (rclc_publisher_init_default(&publisher, &node, - ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, String), - kPublisherTopic) != RCL_RET_OK) { + if (rclc_publisher_init_default(&publisher, &node, + ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, String), + kPublisherTopic) + != RCL_RET_OK) { return false; } // Create subscriber if (rclc_subscription_init_default(&subscriber, &node, - ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, String), - kSubscriberTopic) != RCL_RET_OK) { + ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, String), + kSubscriberTopic) + != RCL_RET_OK) { return false; } @@ -215,7 +216,8 @@ bool CreateEntities() { // Add subscriber to executor if (rclc_executor_add_subscription(&executor, &subscriber, &received_msg, - &SubscriptionCallback, ON_NEW_DATA) != RCL_RET_OK) { + &SubscriptionCallback, ON_NEW_DATA) + != RCL_RET_OK) { return false; } @@ -223,13 +225,13 @@ bool CreateEntities() { } void DestroyEntities() { - rmw_context_t* rmw_context = rcl_context_get_rmw_context(&support.context); - (void)rmw_uros_set_context_entity_destroy_session_timeout(rmw_context, 0); - - rcl_ret_t rc = RCL_RET_OK; - rc = rcl_subscription_fini(&subscriber, &node); - rc = rcl_publisher_fini(&publisher, &node); - rclc_executor_fini(&executor); - rc = rcl_node_fini(&node); - rclc_support_fini(&support); -} \ No newline at end of file + rmw_context_t* rmw_context = rcl_context_get_rmw_context(&support.context); + (void)rmw_uros_set_context_entity_destroy_session_timeout(rmw_context, 0); + + rcl_ret_t rc = RCL_RET_OK; + rc = rcl_subscription_fini(&subscriber, &node); + rc = rcl_publisher_fini(&publisher, &node); + rclc_executor_fini(&executor); + rc = rcl_node_fini(&node); + rclc_support_fini(&support); +} diff --git a/examples/micro-ros_publisher/src/Main.cpp b/examples/micro-ros_publisher/src/main.cpp similarity index 84% rename from examples/micro-ros_publisher/src/Main.cpp rename to examples/micro-ros_publisher/src/main.cpp index 0bd58e91..6d12cad5 100644 --- a/examples/micro-ros_publisher/src/Main.cpp +++ b/examples/micro-ros_publisher/src/main.cpp @@ -20,17 +20,25 @@ rcl_allocator_t allocator; rcl_node_t node; rcl_timer_t timer; -#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)){}} +#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)) {} \ + } // Error handle loop void error_loop() { - while(1) { + while (1) { delay(100); } } -void timer_callback(rcl_timer_t * timer, int64_t last_call_time) { +void timer_callback(rcl_timer_t* timer, int64_t last_call_time) { RCLC_UNUSED(last_call_time); if (timer != NULL) { RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL)); diff --git a/platform_code/arduino/clock_gettime.cpp b/platform_code/arduino/clock_gettime.cpp index 2422498e..2e4afc28 100644 --- a/platform_code/arduino/clock_gettime.cpp +++ b/platform_code/arduino/clock_gettime.cpp @@ -5,21 +5,20 @@ #if !defined(_POSIX_TIMERS) || !_POSIX_TIMERS -extern "C" int clock_gettime(clockid_t unused, struct timespec *tp) -{ - (void)unused; - static uint32_t rollover = 0; - static uint32_t last_measure = 0; +extern "C" int clock_gettime(clockid_t unused, struct timespec *tp) { + (void)unused; + static uint32_t rollover = 0; + static uint32_t last_measure = 0; - uint32_t m = micros(); - rollover += (m < last_measure) ? 1 : 0; + uint32_t m = micros(); + rollover += (m < last_measure) ? 1 : 0; - uint64_t real_us = (uint64_t) (m + rollover * micro_rollover_useconds); - tp->tv_sec = real_us / 1000000; - tp->tv_nsec = (real_us % 1000000) * 1000; - last_measure = m; + uint64_t real_us = (uint64_t)(m + rollover * micro_rollover_useconds); + tp->tv_sec = real_us / 1000000; + tp->tv_nsec = (real_us % 1000000) * 1000; + last_measure = m; - return 0; + return 0; } #endif // ifndef _POSIX_TIMERS diff --git a/platform_code/arduino/ethernet/micro_ros_transport.cpp b/platform_code/arduino/ethernet/micro_ros_transport.cpp index 5368fe54..68abb146 100644 --- a/platform_code/arduino/ethernet/micro_ros_transport.cpp +++ b/platform_code/arduino/ethernet/micro_ros_transport.cpp @@ -8,68 +8,63 @@ #include #include -void* transport_args; +void *transport_args; extern "C" { -static WiFiUDP udp_client; + static WiFiUDP udp_client; -bool platformio_transport_open(struct uxrCustomTransport * transport) -{ + bool platformio_transport_open(struct uxrCustomTransport *transport) { transport_args = transport->args; - struct micro_ros_agent_locator * locator = (struct micro_ros_agent_locator *) transport->args; + struct micro_ros_agent_locator *locator = (struct micro_ros_agent_locator *)transport->args; bool success = udp_client.begin(locator->port); return success; -} + } -bool platformio_transport_close(struct uxrCustomTransport * transport) -{ + bool platformio_transport_close(struct uxrCustomTransport *transport) { transport_args = nullptr; udp_client.stop(); return true; -} + } -size_t platformio_transport_write(struct uxrCustomTransport * transport, const uint8_t *buf, size_t len, uint8_t *errcode) -{ + size_t platformio_transport_write(struct uxrCustomTransport *transport, const uint8_t *buf, size_t len, uint8_t *errcode) { struct micro_ros_agent_locator *locator = (struct micro_ros_agent_locator *)transport->args; size_t sent = 0; - if(true == udp_client.beginPacket(locator->address, locator->port)) { - sent = udp_client.write(buf, len); - if(true == udp_client.endPacket()) { - udp_client.flush(); - if(sent < len) { - *errcode = 1; // Incomplete write - return sent; - } - return sent; + if (true == udp_client.beginPacket(locator->address, locator->port)) { + sent = udp_client.write(buf, len); + if (true == udp_client.endPacket()) { + udp_client.flush(); + if (sent < len) { + *errcode = 1; // Incomplete write + return sent; } + return sent; + } } - - *errcode = 2; // Write failed + + *errcode = 2; // Write failed return 0; -} + } -size_t platformio_transport_read(struct uxrCustomTransport * transport, uint8_t *buf, size_t len, int timeout, uint8_t *errcode) -{ + size_t platformio_transport_read(struct uxrCustomTransport *transport, uint8_t *buf, size_t len, int timeout, uint8_t *errcode) { int64_t start_time = uxr_millis(); // Wait for packet or timeout while ((uxr_millis() - start_time) < ((int64_t)timeout)) { - int packet_size = udp_client.parsePacket(); - if(packet_size > 0) { - size_t available = udp_client.read(buf, len); - if(available < len) { - *errcode = 1; // Incomplete read - return available; - } - return available; + int packet_size = udp_client.parsePacket(); + if (packet_size > 0) { + size_t available = udp_client.read(buf, len); + if (available < len) { + *errcode = 1; // Incomplete read + return available; } - delay(1); + return available; + } + delay(1); } - *errcode = 2; // Timeout + *errcode = 2; // Timeout return 0; + } } - -} \ No newline at end of file diff --git a/platform_code/arduino/ethernet/micro_ros_transport.h b/platform_code/arduino/ethernet/micro_ros_transport.h index 769bff1b..c03c4f69 100644 --- a/platform_code/arduino/ethernet/micro_ros_transport.h +++ b/platform_code/arduino/ethernet/micro_ros_transport.h @@ -8,88 +8,87 @@ typedef void (*ethernet_event_callback_t)(arduino_event_id_t event, void* event_info); struct micro_ros_agent_locator { - IPAddress address; // Agent IP address - IPAddress gateway; // Gateway IP address - const char* hostname; // Device hostname - int port; // Agent port - - ethernet_event_callback_t on_eth_event = nullptr; + IPAddress address; // Agent IP address + IPAddress gateway; // Gateway IP address + const char* hostname; // Device hostname + int port; // Agent port + + ethernet_event_callback_t on_eth_event = nullptr; }; extern void* transport_args; extern "C" { - bool platformio_transport_open(struct uxrCustomTransport * transport); - bool platformio_transport_close(struct uxrCustomTransport * transport); - size_t platformio_transport_write(struct uxrCustomTransport* transport, const uint8_t * buf, size_t len, uint8_t * err); - size_t platformio_transport_read(struct uxrCustomTransport* transport, uint8_t* buf, size_t len, int timeout, uint8_t* err); + bool platformio_transport_open(struct uxrCustomTransport* transport); + bool platformio_transport_close(struct uxrCustomTransport* transport); + size_t platformio_transport_write(struct uxrCustomTransport* transport, const uint8_t* buf, size_t len, uint8_t* err); + size_t platformio_transport_read(struct uxrCustomTransport* transport, uint8_t* buf, size_t len, int timeout, uint8_t* err); } static void ethernet_event_handler(WiFiEvent_t event) { - auto* locator = (micro_ros_agent_locator*)transport_args; - if (!locator || !locator->on_eth_event) return; - - switch (event) { - case ARDUINO_EVENT_ETH_START: - if (locator->hostname != nullptr) { - ETH.setHostname(locator->hostname); - } - locator->on_eth_event(event, nullptr); - break; - - case ARDUINO_EVENT_ETH_CONNECTED: - locator->on_eth_event(event, nullptr); - break; - - case ARDUINO_EVENT_ETH_GOT_IP: { - IPAddress ip = ETH.localIP(); - locator->on_eth_event(event, &ip); - break; - } - - case ARDUINO_EVENT_ETH_DISCONNECTED: - locator->on_eth_event(event, nullptr); - break; - - case ARDUINO_EVENT_ETH_STOP: - locator->on_eth_event(event, nullptr); - break; - - default: - break; - } + auto* locator = (micro_ros_agent_locator*)transport_args; + if (!locator || !locator->on_eth_event) return; + + switch (event) { + case ARDUINO_EVENT_ETH_START: + if (locator->hostname != nullptr) { + ETH.setHostname(locator->hostname); + } + locator->on_eth_event(event, nullptr); + break; + + case ARDUINO_EVENT_ETH_CONNECTED: + locator->on_eth_event(event, nullptr); + break; + + case ARDUINO_EVENT_ETH_GOT_IP: + { + IPAddress ip = ETH.localIP(); + locator->on_eth_event(event, &ip); + break; + } + + case ARDUINO_EVENT_ETH_DISCONNECTED: + locator->on_eth_event(event, nullptr); + break; + + case ARDUINO_EVENT_ETH_STOP: + locator->on_eth_event(event, nullptr); + break; + + default: + break; + } } static inline void set_microros_ethernet_transports( - IPAddress client_ip, - IPAddress gateway, - IPAddress netmask, - IPAddress agent_ip, - uint16_t agent_port, - const char* hostname = nullptr, - ethernet_event_callback_t event_callback = nullptr -) { - static struct micro_ros_agent_locator locator; - - WiFi.onEvent(ethernet_event_handler); - ETH.begin(); - ETH.config(client_ip, gateway, netmask); - delay(1000); - - locator.address = agent_ip; - locator.gateway = gateway; - locator.hostname = hostname; - locator.port = agent_port; - locator.on_eth_event = event_callback; - - rmw_uros_set_custom_transport( - false, - (void *) &locator, - platformio_transport_open, - platformio_transport_close, - platformio_transport_write, - platformio_transport_read - ); + IPAddress client_ip, + IPAddress gateway, + IPAddress netmask, + IPAddress agent_ip, + uint16_t agent_port, + const char* hostname = nullptr, + ethernet_event_callback_t event_callback = nullptr) { + static struct micro_ros_agent_locator locator; + + WiFi.onEvent(ethernet_event_handler); + ETH.begin(); + ETH.config(client_ip, gateway, netmask); + delay(1000); + + locator.address = agent_ip; + locator.gateway = gateway; + locator.hostname = hostname; + locator.port = agent_port; + locator.on_eth_event = event_callback; + + rmw_uros_set_custom_transport( + false, + (void*)&locator, + platformio_transport_open, + platformio_transport_close, + platformio_transport_write, + platformio_transport_read); } -#endif // MICRO_ROS_TRANSPORT_H_ \ No newline at end of file +#endif // MICRO_ROS_TRANSPORT_H_ diff --git a/platform_code/arduino/native_ethernet/micro_ros_transport.cpp b/platform_code/arduino/native_ethernet/micro_ros_transport.cpp index 2df728cc..62af6fef 100644 --- a/platform_code/arduino/native_ethernet/micro_ros_transport.cpp +++ b/platform_code/arduino/native_ethernet/micro_ros_transport.cpp @@ -9,53 +9,48 @@ extern "C" { -static EthernetUDP udp_client; + static EthernetUDP udp_client; -bool platformio_transport_open(struct uxrCustomTransport * transport) -{ - struct micro_ros_agent_locator *locator = (struct micro_ros_agent_locator *)transport->args; - return true == udp_client.begin(locator->port); -} - -bool platformio_transport_close(struct uxrCustomTransport * transport) -{ - udp_client.stop(); - return true; -} - -size_t platformio_transport_write(struct uxrCustomTransport * transport, const uint8_t *buf, size_t len, uint8_t *errcode) -{ - (void)errcode; - - struct micro_ros_agent_locator *locator = (struct micro_ros_agent_locator *)transport->args; + bool platformio_transport_open(struct uxrCustomTransport *transport) { + struct micro_ros_agent_locator *locator = (struct micro_ros_agent_locator *)transport->args; + return true == udp_client.begin(locator->port); + } - size_t sent = 0; - if(true == udp_client.beginPacket(locator->address, locator->port)){ - sent = udp_client.write(buf, len); - sent = true == udp_client.endPacket() ? sent : 0; + bool platformio_transport_close(struct uxrCustomTransport *transport) { + udp_client.stop(); + return true; } - udp_client.flush(); + size_t platformio_transport_write(struct uxrCustomTransport *transport, const uint8_t *buf, size_t len, uint8_t *errcode) { + (void)errcode; - return sent; -} + struct micro_ros_agent_locator *locator = (struct micro_ros_agent_locator *)transport->args; -size_t platformio_transport_read(struct uxrCustomTransport * transport, uint8_t *buf, size_t len, int timeout, uint8_t *errcode) -{ - (void)errcode; + size_t sent = 0; + if (true == udp_client.beginPacket(locator->address, locator->port)) { + sent = udp_client.write(buf, len); + sent = true == udp_client.endPacket() ? sent : 0; + } - int64_t start_time = uxr_millis(); + udp_client.flush(); - while ((uxr_millis() - start_time) < ((int64_t)timeout) && udp_client.parsePacket() == 0) { - delay(1); + return sent; } - size_t available = 0; - if(udp_client.available()){ - available = udp_client.read(buf, len); - } + size_t platformio_transport_read(struct uxrCustomTransport *transport, uint8_t *buf, size_t len, int timeout, uint8_t *errcode) { + (void)errcode; - return (available > 0) ? available : 0; -} + int64_t start_time = uxr_millis(); + while ((uxr_millis() - start_time) < ((int64_t)timeout) && udp_client.parsePacket() == 0) { + delay(1); + } + + size_t available = 0; + if (udp_client.available()) { + available = udp_client.read(buf, len); + } + + return (available > 0) ? available : 0; + } } diff --git a/platform_code/arduino/native_ethernet/micro_ros_transport.h b/platform_code/arduino/native_ethernet/micro_ros_transport.h index 61a6a44a..ca1d85e5 100644 --- a/platform_code/arduino/native_ethernet/micro_ros_transport.h +++ b/platform_code/arduino/native_ethernet/micro_ros_transport.h @@ -1,26 +1,25 @@ #include struct micro_ros_agent_locator { - IPAddress address; - int port; + IPAddress address; + int port; }; -static inline void set_microros_native_ethernet_transports(byte mac[], IPAddress client_ip, IPAddress agent_ip, uint16_t agent_port){ +static inline void set_microros_native_ethernet_transports(byte mac[], IPAddress client_ip, IPAddress agent_ip, uint16_t agent_port) { - static struct micro_ros_agent_locator locator; + static struct micro_ros_agent_locator locator; - Ethernet.begin(mac, client_ip); - delay(1000); + Ethernet.begin(mac, client_ip); + delay(1000); - locator.address = agent_ip; - locator.port = agent_port; + locator.address = agent_ip; + locator.port = agent_port; - rmw_uros_set_custom_transport( - false, - &locator, - platformio_transport_open, - platformio_transport_close, - platformio_transport_write, - platformio_transport_read - ); + rmw_uros_set_custom_transport( + false, + &locator, + platformio_transport_open, + platformio_transport_close, + platformio_transport_write, + platformio_transport_read); } diff --git a/platform_code/arduino/serial/micro_ros_transport.cpp b/platform_code/arduino/serial/micro_ros_transport.cpp index 87fd10c9..287b9771 100644 --- a/platform_code/arduino/serial/micro_ros_transport.cpp +++ b/platform_code/arduino/serial/micro_ros_transport.cpp @@ -10,32 +10,27 @@ extern "C" { -bool platformio_transport_open(struct uxrCustomTransport * transport) -{ - return true; -} + bool platformio_transport_open(struct uxrCustomTransport *transport) { + return true; + } -bool platformio_transport_close(struct uxrCustomTransport * transport) -{ - return true; -} + bool platformio_transport_close(struct uxrCustomTransport *transport) { + return true; + } -size_t platformio_transport_write(struct uxrCustomTransport * transport, const uint8_t *buf, size_t len, uint8_t *errcode) -{ - (void)errcode; + size_t platformio_transport_write(struct uxrCustomTransport *transport, const uint8_t *buf, size_t len, uint8_t *errcode) { + (void)errcode; - Stream * stream = (Stream *) transport->args; - size_t sent = stream->write(buf, len); - return sent; -} + Stream *stream = (Stream *)transport->args; + size_t sent = stream->write(buf, len); + return sent; + } -size_t platformio_transport_read(struct uxrCustomTransport * transport, uint8_t *buf, size_t len, int timeout, uint8_t *errcode) -{ - (void)errcode; + size_t platformio_transport_read(struct uxrCustomTransport *transport, uint8_t *buf, size_t len, int timeout, uint8_t *errcode) { + (void)errcode; - Stream * stream = (Stream *) transport->args; - stream->setTimeout(timeout); - return stream->readBytes((char *)buf, len); + Stream *stream = (Stream *)transport->args; + stream->setTimeout(timeout); + return stream->readBytes((char *)buf, len); + } } - -} \ No newline at end of file diff --git a/platform_code/arduino/serial/micro_ros_transport.h b/platform_code/arduino/serial/micro_ros_transport.h index c101e50e..ef9e3f3d 100644 --- a/platform_code/arduino/serial/micro_ros_transport.h +++ b/platform_code/arduino/serial/micro_ros_transport.h @@ -1,12 +1,11 @@ #include -static inline void set_microros_serial_transports(Stream & stream){ - rmw_uros_set_custom_transport( - true, - &stream, - platformio_transport_open, - platformio_transport_close, - platformio_transport_write, - platformio_transport_read - ); -} \ No newline at end of file +static inline void set_microros_serial_transports(Stream& stream) { + rmw_uros_set_custom_transport( + true, + &stream, + platformio_transport_open, + platformio_transport_close, + platformio_transport_write, + platformio_transport_read); +} diff --git a/platform_code/arduino/wifi/micro_ros_transport.cpp b/platform_code/arduino/wifi/micro_ros_transport.cpp index c190906a..207bded0 100644 --- a/platform_code/arduino/wifi/micro_ros_transport.cpp +++ b/platform_code/arduino/wifi/micro_ros_transport.cpp @@ -10,53 +10,48 @@ extern "C" { -static WiFiUDP udp_client; + static WiFiUDP udp_client; -bool platformio_transport_open(struct uxrCustomTransport * transport) -{ - struct micro_ros_agent_locator * locator = (struct micro_ros_agent_locator *) transport->args; - return true == udp_client.begin(locator->port); -} + bool platformio_transport_open(struct uxrCustomTransport *transport) { + struct micro_ros_agent_locator *locator = (struct micro_ros_agent_locator *)transport->args; + return true == udp_client.begin(locator->port); + } -bool platformio_transport_close(struct uxrCustomTransport * transport) -{ - udp_client.stop(); - return true; -} + bool platformio_transport_close(struct uxrCustomTransport *transport) { + udp_client.stop(); + return true; + } -size_t platformio_transport_write(struct uxrCustomTransport * transport, const uint8_t *buf, size_t len, uint8_t *errcode) -{ - (void)errcode; + size_t platformio_transport_write(struct uxrCustomTransport *transport, const uint8_t *buf, size_t len, uint8_t *errcode) { + (void)errcode; - struct micro_ros_agent_locator *locator = (struct micro_ros_agent_locator *)transport->args; + struct micro_ros_agent_locator *locator = (struct micro_ros_agent_locator *)transport->args; - size_t sent = 0; - if(true == udp_client.beginPacket(locator->address, locator->port)){ - sent = udp_client.write(buf, len); - sent = true == udp_client.endPacket() ? sent : 0; - } + size_t sent = 0; + if (true == udp_client.beginPacket(locator->address, locator->port)) { + sent = udp_client.write(buf, len); + sent = true == udp_client.endPacket() ? sent : 0; + } - udp_client.flush(); + udp_client.flush(); - return sent; -} + return sent; + } -size_t platformio_transport_read(struct uxrCustomTransport * transport, uint8_t *buf, size_t len, int timeout, uint8_t *errcode) -{ - (void)errcode; + size_t platformio_transport_read(struct uxrCustomTransport *transport, uint8_t *buf, size_t len, int timeout, uint8_t *errcode) { + (void)errcode; - int64_t start_time = uxr_millis(); + int64_t start_time = uxr_millis(); - while ((uxr_millis() - start_time) < ((int64_t)timeout) && udp_client.parsePacket() == 0) { - delay(1); - } + while ((uxr_millis() - start_time) < ((int64_t)timeout) && udp_client.parsePacket() == 0) { + delay(1); + } - size_t available = 0; - if(udp_client.available()){ - available = udp_client.read(buf, len); - } + size_t available = 0; + if (udp_client.available()) { + available = udp_client.read(buf, len); + } - return (available > 0) ? available : 0; + return (available > 0) ? available : 0; + } } - -} \ No newline at end of file diff --git a/platform_code/arduino/wifi/micro_ros_transport.h b/platform_code/arduino/wifi/micro_ros_transport.h index 72b94e06..e79d8bcb 100644 --- a/platform_code/arduino/wifi/micro_ros_transport.h +++ b/platform_code/arduino/wifi/micro_ros_transport.h @@ -2,27 +2,26 @@ #include struct micro_ros_agent_locator { - IPAddress address; - int port; + IPAddress address; + int port; }; -static inline void set_microros_wifi_transports(char * ssid, char * pass, IPAddress agent_ip, uint16_t agent_port){ - WiFi.begin(ssid, pass); +static inline void set_microros_wifi_transports(char *ssid, char *pass, IPAddress agent_ip, uint16_t agent_port) { + WiFi.begin(ssid, pass); - while (WiFi.status() != WL_CONNECTED) { - delay(500); - } + while (WiFi.status() != WL_CONNECTED) { + delay(500); + } - static struct micro_ros_agent_locator locator; - locator.address = agent_ip; - locator.port = agent_port; + static struct micro_ros_agent_locator locator; + locator.address = agent_ip; + locator.port = agent_port; - rmw_uros_set_custom_transport( - false, - (void *) &locator, - platformio_transport_open, - platformio_transport_close, - platformio_transport_write, - platformio_transport_read - ); -} \ No newline at end of file + rmw_uros_set_custom_transport( + false, + (void *)&locator, + platformio_transport_open, + platformio_transport_close, + platformio_transport_write, + platformio_transport_read); +} diff --git a/platform_code/arduino/wifi_nina/micro_ros_transport.cpp b/platform_code/arduino/wifi_nina/micro_ros_transport.cpp index c190906a..207bded0 100644 --- a/platform_code/arduino/wifi_nina/micro_ros_transport.cpp +++ b/platform_code/arduino/wifi_nina/micro_ros_transport.cpp @@ -10,53 +10,48 @@ extern "C" { -static WiFiUDP udp_client; + static WiFiUDP udp_client; -bool platformio_transport_open(struct uxrCustomTransport * transport) -{ - struct micro_ros_agent_locator * locator = (struct micro_ros_agent_locator *) transport->args; - return true == udp_client.begin(locator->port); -} + bool platformio_transport_open(struct uxrCustomTransport *transport) { + struct micro_ros_agent_locator *locator = (struct micro_ros_agent_locator *)transport->args; + return true == udp_client.begin(locator->port); + } -bool platformio_transport_close(struct uxrCustomTransport * transport) -{ - udp_client.stop(); - return true; -} + bool platformio_transport_close(struct uxrCustomTransport *transport) { + udp_client.stop(); + return true; + } -size_t platformio_transport_write(struct uxrCustomTransport * transport, const uint8_t *buf, size_t len, uint8_t *errcode) -{ - (void)errcode; + size_t platformio_transport_write(struct uxrCustomTransport *transport, const uint8_t *buf, size_t len, uint8_t *errcode) { + (void)errcode; - struct micro_ros_agent_locator *locator = (struct micro_ros_agent_locator *)transport->args; + struct micro_ros_agent_locator *locator = (struct micro_ros_agent_locator *)transport->args; - size_t sent = 0; - if(true == udp_client.beginPacket(locator->address, locator->port)){ - sent = udp_client.write(buf, len); - sent = true == udp_client.endPacket() ? sent : 0; - } + size_t sent = 0; + if (true == udp_client.beginPacket(locator->address, locator->port)) { + sent = udp_client.write(buf, len); + sent = true == udp_client.endPacket() ? sent : 0; + } - udp_client.flush(); + udp_client.flush(); - return sent; -} + return sent; + } -size_t platformio_transport_read(struct uxrCustomTransport * transport, uint8_t *buf, size_t len, int timeout, uint8_t *errcode) -{ - (void)errcode; + size_t platformio_transport_read(struct uxrCustomTransport *transport, uint8_t *buf, size_t len, int timeout, uint8_t *errcode) { + (void)errcode; - int64_t start_time = uxr_millis(); + int64_t start_time = uxr_millis(); - while ((uxr_millis() - start_time) < ((int64_t)timeout) && udp_client.parsePacket() == 0) { - delay(1); - } + while ((uxr_millis() - start_time) < ((int64_t)timeout) && udp_client.parsePacket() == 0) { + delay(1); + } - size_t available = 0; - if(udp_client.available()){ - available = udp_client.read(buf, len); - } + size_t available = 0; + if (udp_client.available()) { + available = udp_client.read(buf, len); + } - return (available > 0) ? available : 0; + return (available > 0) ? available : 0; + } } - -} \ No newline at end of file diff --git a/platform_code/arduino/wifi_nina/micro_ros_transport.h b/platform_code/arduino/wifi_nina/micro_ros_transport.h index 03ae65aa..db78d9a8 100644 --- a/platform_code/arduino/wifi_nina/micro_ros_transport.h +++ b/platform_code/arduino/wifi_nina/micro_ros_transport.h @@ -3,25 +3,24 @@ #include struct micro_ros_agent_locator { - IPAddress address; - int port; + IPAddress address; + int port; }; -static inline void set_microros_wifi_transports(char * ssid, char * pass, IPAddress agent_ip, uint16_t agent_port){ - while (WiFi.begin(ssid, pass) != WL_CONNECTED) { - delay(500); - } +static inline void set_microros_wifi_transports(char *ssid, char *pass, IPAddress agent_ip, uint16_t agent_port) { + while (WiFi.begin(ssid, pass) != WL_CONNECTED) { + delay(500); + } - static struct micro_ros_agent_locator locator; - locator.address = agent_ip; - locator.port = agent_port; + static struct micro_ros_agent_locator locator; + locator.address = agent_ip; + locator.port = agent_port; - rmw_uros_set_custom_transport( - false, - (void *) &locator, - platformio_transport_open, - platformio_transport_close, - platformio_transport_write, - platformio_transport_read - ); -} \ No newline at end of file + rmw_uros_set_custom_transport( + false, + (void *)&locator, + platformio_transport_open, + platformio_transport_close, + platformio_transport_write, + platformio_transport_read); +} diff --git a/platform_code/micro_ros_platformio.h b/platform_code/micro_ros_platformio.h index 5773759a..62f5696b 100644 --- a/platform_code/micro_ros_platformio.h +++ b/platform_code/micro_ros_platformio.h @@ -18,14 +18,13 @@ #endif #ifdef __cplusplus -extern "C" -{ +extern "C" { #endif -bool platformio_transport_open(struct uxrCustomTransport * transport); -bool platformio_transport_close(struct uxrCustomTransport * transport); -size_t platformio_transport_write(struct uxrCustomTransport* transport, const uint8_t * buf, size_t len, uint8_t * err); -size_t platformio_transport_read(struct uxrCustomTransport* transport, uint8_t* buf, size_t len, int timeout, uint8_t* err); + bool platformio_transport_open(struct uxrCustomTransport* transport); + bool platformio_transport_close(struct uxrCustomTransport* transport); + size_t platformio_transport_write(struct uxrCustomTransport* transport, const uint8_t* buf, size_t len, uint8_t* err); + size_t platformio_transport_read(struct uxrCustomTransport* transport, uint8_t* buf, size_t len, int timeout, uint8_t* err); #ifdef __cplusplus }