Skip to content

Commit 23f3141

Browse files
aentingeraentinger
authored
Merge pull request #7 from arduino-libraries/alvik-ros2
Arduino firmware to allow access to Alvik's sensors and actuators via ROS 2 topics and Micro-ROS
2 parents cef645a + 8031ef1 commit 23f3141

File tree

6 files changed

+273
-0
lines changed

6 files changed

+273
-0
lines changed

‎.gitignore

+3
Original file line numberDiff line numberDiff line change
@@ -24,3 +24,6 @@ Icon
2424
Network Trash Folder
2525
Temporary Items
2626
.apdisk
27+
28+
# CLion IDE files
29+
.idea/

‎examples/alvik_ros2_firmware/.gitignore

+2
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,2 @@
1+
wifi_secrets.h
2+
micro_ros_config.h

‎examples/alvik_ros2_firmware/README.md

+66
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,66 @@
1+
:floppy_disk: `alvik_ros2_firmware`
2+
===================================
3+
4+
By compiling and uploading the firmware contained in this repository all Arduino [Alvik](https://store.arduino.cc/products/alvik) internal sensors and actuators can be accessed via default ROS 2 topics.
5+
6+
### How-to-configure
7+
Edit [`wifi_secrets.h`](wifi_secrets.h) to specify your WiFi configuration and edit [`micro_ros_config.h`](micro_ros_config.h) to specify the Micro-ROS agent's IP and port information.
8+
9+
### How-to-build micro-ROS library for ESP32-S3
10+
```bash
11+
git clone https://github.com/micro-ROS/micro_ros_arduino && cd micro_ros_arduino
12+
```
13+
* ROS 2 **Jazzy**
14+
```bash
15+
git checkout jazzy
16+
docker run -it --rm -v $(pwd):/project --env MICROROS_LIBRARY_FOLDER=extras microros/micro_ros_static_library_builder:jazzy -p esp32s3
17+
```
18+
* ROS 2 **Humble**
19+
```bash
20+
git checkout humble
21+
docker run -it --rm -v $(pwd):/project --env MICROROS_LIBRARY_FOLDER=extras microros/micro_ros_static_library_builder:humble -p esp32s3
22+
```
23+
* ROS 2 **Iron**
24+
```bash
25+
git checkout iron
26+
docker run -it --rm -v $(pwd):/project --env MICROROS_LIBRARY_FOLDER=extras microros/micro_ros_static_library_builder:iron -p esp32s3
27+
```
28+
29+
### How-to-build/upload
30+
```bash
31+
cd ~/Arduino/libraries
32+
git clone https://github.com/arduino-libraries/Arduino_Alvik && cd Arduino_Alvik
33+
arduino-cli core install arduino:esp32
34+
arduino-cli compile --fqbn arduino:esp32:nano_nora -u -p /dev/ttyACM0
35+
```
36+
37+
### How-to-run Micro-ROS local agent
38+
* ROS 2 **Jazzy**
39+
```bash
40+
docker run -it --rm --net=host microros/micro-ros-agent:jazzy udp4 --port 8888 -v6
41+
```
42+
* ROS 2 **Humble**
43+
```bash
44+
docker run -it --rm --net=host microros/micro-ros-agent:humble udp4 --port 8888 -v6
45+
```
46+
* ROS 2 **Iron**
47+
```bash
48+
docker run -it --rm --net=host microros/micro-ros-agent:iron udp4 --port 8888 -v6
49+
```
50+
51+
### How-to-teleoperate using keyboard
52+
```bash
53+
source /opt/ros/humble/setup.bash
54+
ros2 run teleop_twist_keyboard teleop_twist_keyboard
55+
```
56+
57+
#### Interface Documentation
58+
##### Subscribed Topics
59+
| Default name | Type |
60+
|:------------:|:-:|
61+
| `/cmd_vel` | [`geometry_msgs/Twist`](https://docs.ros2.org/galactic/api/geometry_msgs/msg/Twist.html) |
62+
63+
##### Published Topics
64+
| Default name | Type |
65+
|:------------:|:---------------------------------------------------------------------------------:|
66+
| `/odom` | [`nav_msgs/Odometry`](https://docs.ros2.org/galactic/api/nav_msgs/msg/Odometry.html) |

‎examples/alvik_ros2_firmware/alvik_ros2_firmware.ino

+180
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,180 @@
1+
/*
2+
* Copyright (c) 2024 Arduino
3+
*
4+
* SPDX-License-Identifier: MPL-2.0
5+
* This Source Code Form is subject to the terms of the Mozilla Public
6+
* License, v. 2.0. If a copy of the MPL was not distributed with this
7+
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
8+
*/
9+
10+
/**************************************************************************************
11+
* INCLUDE
12+
**************************************************************************************/
13+
14+
#include <Arduino_Alvik.h>
15+
16+
#include <micro_ros_arduino.h>
17+
18+
#include <stdio.h>
19+
#include <rcl/rcl.h>
20+
#include <rcl/error_handling.h>
21+
#include <rclc/rclc.h>
22+
#include <rclc/executor.h>
23+
24+
#include <std_msgs/msg/int32.h>
25+
26+
#include <nav_msgs/msg/odometry.h>
27+
#include <geometry_msgs/msg/twist.h>
28+
29+
#include <micro_ros_utilities/string_utilities.h>
30+
31+
#include "wifi_secrets.h"
32+
#include "micro_ros_config.h"
33+
34+
/**************************************************************************************
35+
* GLOBAL VARIABLES
36+
**************************************************************************************/
37+
38+
static rcl_subscription_t cmd_vel_sub;
39+
static geometry_msgs__msg__Twist cmd_vel_msg;
40+
41+
static rcl_timer_t odom_timer;
42+
static rcl_publisher_t odom_pub;
43+
static nav_msgs__msg__Odometry odom_msg;
44+
45+
static rclc_support_t support;
46+
static rcl_allocator_t allocator = rcl_get_default_allocator();
47+
static rclc_executor_t executor = rclc_executor_get_zero_initialized_executor();
48+
static rcl_node_t node = rcl_get_zero_initialized_node();
49+
50+
static Arduino_Alvik alvik;
51+
52+
/**************************************************************************************
53+
* LOCAL FUNCTIONS
54+
**************************************************************************************/
55+
56+
void error_loop(char const * fmt, ...)
57+
{
58+
va_list args;
59+
va_start(args, fmt);
60+
61+
char msg[64] = {0};
62+
vsnprintf(msg, sizeof(msg), fmt, args);
63+
Serial.println(msg);
64+
65+
va_end(args);
66+
67+
for(;;)
68+
{
69+
digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));
70+
delay(100);
71+
}
72+
}
73+
74+
void euler_to_quat(float const x, float const y, float const z, double * q)
75+
{
76+
float c1 = cos((y*3.14f/180.f)/2.f);
77+
float c2 = cos((z*3.14f/180.f)/2.f);
78+
float c3 = cos((x*3.14f/180.f)/2.f);
79+
80+
float s1 = sin((y*3.14f/180.f)/2.f);
81+
float s2 = sin((z*3.14f/180.f)/2.f);
82+
float s3 = sin((x*3.14f/180.f)/2.f);
83+
84+
q[0] = c1 * c2 * c3 - s1 * s2 * s3;
85+
q[1] = s1 * s2 * c3 + c1 * c2 * s3;
86+
q[2] = s1 * c2 * c3 + c1 * s2 * s3;
87+
q[3] = c1 * s2 * c3 - s1 * c2 * s3;
88+
}
89+
90+
void cmd_vel_callback(const void *msgin)
91+
{
92+
const geometry_msgs__msg__Twist * msg = (const geometry_msgs__msg__Twist *)msgin;
93+
alvik.drive(msg->linear.x, msg->angular.z, M_S, RAD_S);
94+
}
95+
96+
void odom_timer_callback(rcl_timer_t * timer, int64_t last_call_time)
97+
{
98+
RCLC_UNUSED(last_call_time);
99+
if (timer != NULL)
100+
{
101+
struct timespec ts;
102+
clock_gettime(CLOCK_REALTIME, &ts);
103+
odom_msg.header.stamp.sec = ts.tv_sec;
104+
odom_msg.header.stamp.nanosec = ts.tv_nsec;
105+
106+
odom_msg.header.frame_id = micro_ros_string_utilities_init("odom");
107+
108+
float x = 0.f, y = 0.f, theta = 0.f;
109+
alvik.get_pose(x, y, theta, M, RAD);
110+
111+
odom_msg.pose.pose.position.x = x;
112+
odom_msg.pose.pose.position.y = y;
113+
odom_msg.pose.pose.position.z = 0.f;
114+
115+
double q[4] = {0.};
116+
euler_to_quat(0., 0., theta, q);
117+
odom_msg.pose.pose.orientation.x = q[1];
118+
odom_msg.pose.pose.orientation.y = q[2];
119+
odom_msg.pose.pose.orientation.z = q[3];
120+
odom_msg.pose.pose.orientation.w = q[0];
121+
122+
float linear = 0.f, angular = 0.f;
123+
alvik.get_drive_speed(linear, angular, M_S, RAD_S);
124+
125+
odom_msg.twist.twist.linear.x = linear;
126+
odom_msg.twist.twist.linear.y = 0.f;
127+
odom_msg.twist.twist.angular.z = angular;
128+
129+
if (rcl_ret_t const rc = rcl_publish(&odom_pub, &odom_msg, NULL); rc != RCL_RET_OK)
130+
error_loop("odom_timer_callback::rcl_publish failed with %d", rc);
131+
}
132+
}
133+
134+
/**************************************************************************************
135+
* SETUP/LOOP
136+
**************************************************************************************/
137+
138+
void setup()
139+
{
140+
Serial.begin();
141+
for (auto const start = millis(); !Serial && (millis() - start) < 1000; ) { }
142+
143+
set_microros_wifi_transports(SECRET_WIFI_SSID, SECRET_WIFI_PASS, MICRO_ROS_AGENT_IP, MICRO_ROS_AGENT_PORT);
144+
145+
pinMode(LED_BUILTIN, OUTPUT);
146+
digitalWrite(LED_BUILTIN, HIGH);
147+
148+
alvik.begin();
149+
150+
if (rcl_ret_t const rc = rclc_support_init(&support, 0, NULL, &allocator); rc != RCL_RET_OK)
151+
error_loop("rclc_support_init failed with %d", rc);
152+
153+
if (rcl_ret_t const rc = rclc_node_init_default(&node, "alvik", "", &support); rc != RCL_RET_OK)
154+
error_loop("rclc_node_init_default failed with %d", rc);
155+
156+
if (rcl_ret_t const rc = rclc_subscription_init_default(&cmd_vel_sub, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist), "/cmd_vel"); rc != RCL_RET_OK)
157+
error_loop("rclc_subscription_init_default failed with %d", rc);
158+
159+
if (rcl_ret_t const rc = rclc_timer_init_default(&odom_timer, &support, RCL_MS_TO_NS(50), odom_timer_callback); rc != RCL_RET_OK)
160+
error_loop("rclc_timer_init_default(odom_timer, ...) failed with %d", rc);
161+
162+
if (rcl_ret_t const rc = rclc_publisher_init_default(&odom_pub, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(nav_msgs, msg, Odometry), "/odom"); rc != RCL_RET_OK)
163+
error_loop("rclc_publisher_init_default(odom_pub, ...) failed with %d", rc);
164+
165+
if (rcl_ret_t const rc = rclc_executor_init(&executor, &support.context, 2, &allocator); rc != RCL_RET_OK)
166+
error_loop("rclc_executor_init failed with %d", rc);
167+
168+
if (rcl_ret_t const rc = rclc_executor_add_subscription(&executor, &cmd_vel_sub, &cmd_vel_msg, &cmd_vel_callback, ON_NEW_DATA); rc != RCL_RET_OK)
169+
error_loop("rclc_executor_add_subscription(..., cmd_vel_sub, ...) failed with %d", rc);
170+
171+
if (rcl_ret_t const rc = rclc_executor_add_timer(&executor, &odom_timer); rc != RCL_RET_OK)
172+
error_loop("rclc_executor_add_timer(..., odom_timer, ...) failed with %d", rc);
173+
174+
Serial.println("alvik_ros2_firmware setup complete.");
175+
}
176+
177+
void loop()
178+
{
179+
rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
180+
}

‎examples/alvik_ros2_firmware/micro_ros_config.h

+11
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,11 @@
1+
/*
2+
* Copyright (c) 2024 Arduino
3+
*
4+
* SPDX-License-Identifier: MPL-2.0
5+
* This Source Code Form is subject to the terms of the Mozilla Public
6+
* License, v. 2.0. If a copy of the MPL was not distributed with this
7+
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
8+
*/
9+
10+
#define MICRO_ROS_AGENT_IP "YOUR_MICRO_ROS_AGENT_IP"
11+
#define MICRO_ROS_AGENT_PORT 8888

‎examples/alvik_ros2_firmware/wifi_secrets.h

+11
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,11 @@
1+
/*
2+
* Copyright (c) 2024 Arduino
3+
*
4+
* SPDX-License-Identifier: MPL-2.0
5+
* This Source Code Form is subject to the terms of the Mozilla Public
6+
* License, v. 2.0. If a copy of the MPL was not distributed with this
7+
* file, You can obtain one at http://mozilla.org/MPL/2.0/.
8+
*/
9+
10+
#define SECRET_WIFI_SSID "YOUR_WIFI_NETWORK_NAME"
11+
#define SECRET_WIFI_PASS "YOUR_WIFI_PASSWORD"

0 commit comments

Comments
 (0)