Merge branch 'jazzy' of https://github.com/catweasel999/micro_ros_ras…

…pberrypi_pico_sdk into jazzy
catweasel999 · Jul 2, 2024 · 6370508 · 6370508
2 parents bb8d757 + e30a430
commit 6370508
Show file tree

Hide file tree

Showing 6 changed files with 129 additions and 59 deletions.
diff --git a/.vscode/launch-linux.json b/.vscode/launch-linux.json
@@ -0,0 +1,54 @@
+{
+    "version": "0.2.0",
+    "configurations": [
+        {
+            "name": "Pico Debug",
+            "cwd": "${workspaceRoot}",
+            "executable": "${command:cmake.launchTargetPath}",
+            "request": "launch",
+            "type": "cortex-debug",
+            "servertype": "openocd",
+            // This may need to be "arm-none-eabi-gdb" for some previous builds
+            "gdbPath": "gdb-multiarch",
+            "device": "RP2040",
+            "configFiles": [
+                // This may need to be "interface/picoprobe.cfg" for some previous builds
+                "interface/cmsis-dap.cfg",
+                "target/rp2040.cfg"
+            ],
+            "svdFile": "${env:PICO_SDK_PATH}/src/rp2040/hardware_regs/rp2040.svd",
+            "runToEntryPoint": "main",
+            // Work around for stopping at main on restart
+            "postRestartCommands": [
+                "break main",
+                "continue"
+            ],
+            "openOCDLaunchCommands": [
+                "adapter speed 5000"
+            ]
+        },
+        {
+            "name": "minicom 0",
+            "type": "node-terminal",
+            "request": "launch",
+            "command": "minicom -b 115200 -o -D /dev/ttyACM0",
+        },
+        {
+            "name": "minicom 1",
+            "type": "node-terminal",
+            "request": "launch",
+            "command": "minicom -b 115200 -o -D /dev/ttyACM1",
+        }
+    ],
+    "compounds": [
+        {
+            "name": "Pico with minicom",
+            "stopAll": true,
+            "configurations": [
+                "minicom 0",
+                "minicom 1",
+                "Pico Debug"
+            ]
+        }
+    ]
+}
diff --git a/.vscode/launch.json b/.vscode/launch.json
@@ -1,54 +1,58 @@
 {
-    "version": "0.2.0",
-    "configurations": [
-        {
-            "name": "Pico Debug",
-            "cwd": "${workspaceRoot}",
-            "executable": "${command:cmake.launchTargetPath}",
-            "request": "launch",
-            "type": "cortex-debug",
-            "servertype": "openocd",
-            // This may need to be "arm-none-eabi-gdb" for some previous builds
-            "gdbPath": "gdb-multiarch",
-            "device": "RP2040",
-            "configFiles": [
-                // This may need to be "interface/picoprobe.cfg" for some previous builds
-                "interface/cmsis-dap.cfg",
-                "target/rp2040.cfg"
-            ],
-            "svdFile": "${env:PICO_SDK_PATH}/src/rp2040/hardware_regs/rp2040.svd",
-            "runToEntryPoint": "main",
-            // Work around for stopping at main on restart
-            "postRestartCommands": [
-                "break main",
-                "continue"
-            ],
-            "openOCDLaunchCommands": [
-                "adapter speed 5000"
-            ]
-        },
-        {
-            "name": "minicom 0",
-            "type": "node-terminal",
-            "request": "launch",
-            "command": "minicom -b 115200 -o -D /dev/ttyACM0",
-        },
-        {
-            "name": "minicom 1",
-            "type": "node-terminal",
-            "request": "launch",
-            "command": "minicom -b 115200 -o -D /dev/ttyACM1",
-        }
-    ],
-    "compounds": [
-        {
-            "name": "Pico with minicom",
-            "stopAll": true,
-            "configurations": [
-                "minicom 0",
-                "minicom 1",
-                "Pico Debug"
-            ]
-        }
-    ]
-}
+  "version": "0.2.0",
+  "configurations": [
+    {
+      "name": "Pico Debug (Cortex-Debug)",
+      "cwd": "${workspaceFolder}",
+      "executable": "${command:cmake.launchTargetPath}",
+      "request": "launch",
+      "type": "cortex-debug",
+      "servertype": "openocd",
+      "gdbPath": "arm-none-eabi-gdb",
+      "device": "RP2040",
+      "configFiles": [
+        "interface/cmsis-dap.cfg",
+        "target/rp2040.cfg"
+      ],
+      "svdFile": "${env:PICO_SDK_PATH}/src/rp2040/hardware_regs/rp2040.svd",
+      "runToEntryPoint": "main",
+      "openOCDLaunchCommands": [
+        "adapter speed 5000"
+      ]
+    },
+    {
+      "name": "Pico Debug (Cortex-Debug with external OpenOCD)",
+      "cwd": "${workspaceFolder}",
+      "executable": "${command:cmake.launchTargetPath}",
+      "request": "launch",
+      "type": "cortex-debug",
+      "servertype": "external",
+      "gdbTarget": "localhost:3333",
+      "gdbPath": "arm-none-eabi-gdb",
+      "device": "RP2040",
+      "svdFile": "${env:PICO_SDK_PATH}/src/rp2040/hardware_regs/rp2040.svd",
+      "runToEntryPoint": "main"
+    },
+    {
+      "name": "Pico Debug (C++ Debugger)",
+      "type": "cppdbg",
+      "request": "launch",
+      "cwd": "${workspaceFolder}",
+      "program": "${command:cmake.launchTargetPath}",
+      "MIMode": "gdb",
+      "miDebuggerPath": "arm-none-eabi-gdb",
+      "miDebuggerServerAddress": "localhost:3333",
+      "debugServerPath": "openocd",
+      "debugServerArgs": "-f interface/cmsis-dap.cfg -f target/rp2040.cfg -c \"adapter speed 5000\"",
+      "serverStarted": "Listening on port .* for gdb connections",
+      "filterStderr": true,
+      "stopAtEntry": true,
+      "hardwareBreakpoints": {
+        "require": true,
+        "limit": 4
+      },
+      "preLaunchTask": "Flash",
+      "svdPath": "${env:PICO_SDK_PATH}/src/rp2040/hardware_regs/rp2040.svd"
+    }
+  ]
+}
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -41,9 +41,15 @@ target_include_directories(pico_micro_ros_udp_example PUBLIC
 )
 message(STATUS "PICO_BOARD: ${PICO_BOARD}")
 message(STATUS "WIFI_SSID: ${WIFI_SSID}")
+message(STATUS "ROS_AGENT_IP_ADDR: ${ROS_AGENT_IP_ADDR}")
+message(STATUS "ROS_AGENT_UDP_PORT: ${ROS_AGENT_UDP_PORT}")
+message(STATUS "ROS_DOMAIN_ID: ${ROS_DOMAIN_ID}")
 
 add_definitions(-DWIFI_SSID="${WIFI_SSID}")
 add_definitions(-DWIFI_PASSWORD="${WIFI_PASSWORD}")
+add_definitions(-DROS_AGENT_IP_ADDR="${ROS_AGENT_IP_ADDR}")
+add_definitions(-DROS_AGENT_UDP_PORT=${ROS_AGENT_UDP_PORT})
+add_definitions(-DROS_DOMAIN_ID=${ROS_DOMAIN_ID})
 
 SET(CMAKE_C_FLAGS  "${CMAKE_C_FLAGS} -ffunction-sections -fdata-sections")
 SET(CMAKE_CXX_FLAGS  "${CMAKE_CXX_FLAGS} -ffunction-sections -fdata-sections")

diff --git a/include/picow_udp_transports.h b/include/picow_udp_transports.h
@@ -9,9 +9,9 @@
 #include "lwip/pbuf.h"
 #include "lwip/udp.h"
 
-#define ROS_AGENT_UDP_PORT (8888)
-#define ROS_AGENT_IP_ADDR "192.168.178.121" // You need modify IP
-#define ROS_DOMAIN_ID 1
+// #define ROS_AGENT_UDP_PORT (8888)
+// #define ROS_AGENT_IP_ADDR "192.168.178.121" // You need modify IP
+// #define ROS_DOMAIN_ID 1
 #define ROS_NODE_NAME "pico_w_udp_node"
 
 #ifdef __cplusplus

diff --git a/micro_ros_raspberrypi_pico_sdk.code-workspace b/micro_ros_raspberrypi_pico_sdk.code-workspace
@@ -2,6 +2,9 @@
 	"folders": [
 		{
 			"path": "."
+		},
+		{
+			"path": "../../Documents/Pico-v1.5.1"
 		}
 	],
 	"settings": {

diff --git a/pico_micro_ros_udp_example.c b/pico_micro_ros_udp_example.c
@@ -85,7 +85,9 @@ int main()
     printf("Connecting to WiFi, SSID: %s\n ...\n", WIFI_SSID);
 
     printf("SSID: %s\n", WIFI_SSID);
-    printf("password: %s\n", WIFI_PASSWORD);
+    // printf("password: %s\n", WIFI_PASSWORD);
+    printf("ros agent ip:port: %s:%d\n", ROS_AGENT_IP_ADDR, ROS_AGENT_UDP_PORT);
+    printf("ros domain id: %d\n", ROS_DOMAIN_ID);
 
     if (cyw43_arch_init())
     {
@@ -123,12 +125,13 @@ int main()
     // Initialize and modify options (Set DOMAIN ID to 1)
     rcl_init_options_t init_options = rcl_get_zero_initialized_init_options();
     rcl_init_options_init(&init_options, allocator);
-    rcl_init_options_set_domain_id(&init_options, ROS_DOMAIN_ID);
+    // rcl_init_options_set_domain_id(&init_options, ROS_DOMAIN_ID);
+    rcl_init_options_set_domain_id(&init_options, 1);
 
     rclc_support_t support;
     rclc_executor_t executor;
 
-    cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, 1);
+    cyw43_arch_gpio_put(CYW43_WL_GPIO_LED_PIN, ROS_DOMAIN_ID);
 
     // Wait for agent successful ping for 1 minutes.
     const int timeout_ms = 1000;