diff --git a/.gitignore b/.gitignore index e7bcaed..b702641 100644 --- a/.gitignore +++ b/.gitignore @@ -24,3 +24,6 @@ Icon Network Trash Folder Temporary Items .apdisk + +# CLion IDE files +.idea/ diff --git a/examples/alvik_ros2_firmware/.gitignore b/examples/alvik_ros2_firmware/.gitignore new file mode 100644 index 0000000..4980fa7 --- /dev/null +++ b/examples/alvik_ros2_firmware/.gitignore @@ -0,0 +1,2 @@ +wifi_secrets.h +micro_ros_config.h diff --git a/examples/alvik_ros2_firmware/README.md b/examples/alvik_ros2_firmware/README.md new file mode 100644 index 0000000..1928d86 --- /dev/null +++ b/examples/alvik_ros2_firmware/README.md @@ -0,0 +1,66 @@ +:floppy_disk: `alvik_ros2_firmware` +=================================== + +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. + +### How-to-configure +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. + +### How-to-build micro-ROS library for ESP32-S3 +```bash +git clone https://github.com/micro-ROS/micro_ros_arduino && cd micro_ros_arduino +``` +* ROS 2 **Jazzy** +```bash +git checkout jazzy +docker run -it --rm -v $(pwd):/project --env MICROROS_LIBRARY_FOLDER=extras microros/micro_ros_static_library_builder:jazzy -p esp32s3 +``` +* ROS 2 **Humble** +```bash +git checkout humble +docker run -it --rm -v $(pwd):/project --env MICROROS_LIBRARY_FOLDER=extras microros/micro_ros_static_library_builder:humble -p esp32s3 +``` +* ROS 2 **Iron** +```bash +git checkout iron +docker run -it --rm -v $(pwd):/project --env MICROROS_LIBRARY_FOLDER=extras microros/micro_ros_static_library_builder:iron -p esp32s3 +``` + +### How-to-build/upload +```bash +cd ~/Arduino/libraries +git clone https://github.com/arduino-libraries/Arduino_Alvik && cd Arduino_Alvik +arduino-cli core install arduino:esp32 +arduino-cli compile --fqbn arduino:esp32:nano_nora -u -p /dev/ttyACM0 +``` + +### How-to-run Micro-ROS local agent +* ROS 2 **Jazzy** +```bash +docker run -it --rm --net=host microros/micro-ros-agent:jazzy udp4 --port 8888 -v6 +``` +* ROS 2 **Humble** +```bash +docker run -it --rm --net=host microros/micro-ros-agent:humble udp4 --port 8888 -v6 +``` +* ROS 2 **Iron** +```bash +docker run -it --rm --net=host microros/micro-ros-agent:iron udp4 --port 8888 -v6 +``` + +### How-to-teleoperate using keyboard +```bash +source /opt/ros/humble/setup.bash +ros2 run teleop_twist_keyboard teleop_twist_keyboard +``` + +#### Interface Documentation +##### Subscribed Topics +| Default name | Type | +|:------------:|:-:| +| `/cmd_vel` | [`geometry_msgs/Twist`](https://docs.ros2.org/galactic/api/geometry_msgs/msg/Twist.html) | + +##### Published Topics +| Default name | Type | +|:------------:|:---------------------------------------------------------------------------------:| +| `/odom` | [`nav_msgs/Odometry`](https://docs.ros2.org/galactic/api/nav_msgs/msg/Odometry.html) | diff --git a/examples/alvik_ros2_firmware/alvik_ros2_firmware.ino b/examples/alvik_ros2_firmware/alvik_ros2_firmware.ino new file mode 100644 index 0000000..cff7d13 --- /dev/null +++ b/examples/alvik_ros2_firmware/alvik_ros2_firmware.ino @@ -0,0 +1,180 @@ +/* + * Copyright (c) 2024 Arduino + * + * SPDX-License-Identifier: MPL-2.0 + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at http://mozilla.org/MPL/2.0/. + */ + +/************************************************************************************** + * INCLUDE + **************************************************************************************/ + +#include + +#include + +#include +#include +#include +#include +#include + +#include + +#include +#include + +#include + +#include "wifi_secrets.h" +#include "micro_ros_config.h" + +/************************************************************************************** + * GLOBAL VARIABLES + **************************************************************************************/ + +static rcl_subscription_t cmd_vel_sub; +static geometry_msgs__msg__Twist cmd_vel_msg; + +static rcl_timer_t odom_timer; +static rcl_publisher_t odom_pub; +static nav_msgs__msg__Odometry odom_msg; + +static rclc_support_t support; +static rcl_allocator_t allocator = rcl_get_default_allocator(); +static rclc_executor_t executor = rclc_executor_get_zero_initialized_executor(); +static rcl_node_t node = rcl_get_zero_initialized_node(); + +static Arduino_Alvik alvik; + +/************************************************************************************** + * LOCAL FUNCTIONS + **************************************************************************************/ + +void error_loop(char const * fmt, ...) +{ + va_list args; + va_start(args, fmt); + + char msg[64] = {0}; + vsnprintf(msg, sizeof(msg), fmt, args); + Serial.println(msg); + + va_end(args); + + for(;;) + { + digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN)); + delay(100); + } +} + +void euler_to_quat(float const x, float const y, float const z, double * q) +{ + float c1 = cos((y*3.14f/180.f)/2.f); + float c2 = cos((z*3.14f/180.f)/2.f); + float c3 = cos((x*3.14f/180.f)/2.f); + + float s1 = sin((y*3.14f/180.f)/2.f); + float s2 = sin((z*3.14f/180.f)/2.f); + float s3 = sin((x*3.14f/180.f)/2.f); + + q[0] = c1 * c2 * c3 - s1 * s2 * s3; + q[1] = s1 * s2 * c3 + c1 * c2 * s3; + q[2] = s1 * c2 * c3 + c1 * s2 * s3; + q[3] = c1 * s2 * c3 - s1 * c2 * s3; +} + +void cmd_vel_callback(const void *msgin) +{ + const geometry_msgs__msg__Twist * msg = (const geometry_msgs__msg__Twist *)msgin; + alvik.drive(msg->linear.x, msg->angular.z, M_S, RAD_S); +} + +void odom_timer_callback(rcl_timer_t * timer, int64_t last_call_time) +{ + RCLC_UNUSED(last_call_time); + if (timer != NULL) + { + struct timespec ts; + clock_gettime(CLOCK_REALTIME, &ts); + odom_msg.header.stamp.sec = ts.tv_sec; + odom_msg.header.stamp.nanosec = ts.tv_nsec; + + odom_msg.header.frame_id = micro_ros_string_utilities_init("odom"); + + float x = 0.f, y = 0.f, theta = 0.f; + alvik.get_pose(x, y, theta, M, RAD); + + odom_msg.pose.pose.position.x = x; + odom_msg.pose.pose.position.y = y; + odom_msg.pose.pose.position.z = 0.f; + + double q[4] = {0.}; + euler_to_quat(0., 0., theta, q); + odom_msg.pose.pose.orientation.x = q[1]; + odom_msg.pose.pose.orientation.y = q[2]; + odom_msg.pose.pose.orientation.z = q[3]; + odom_msg.pose.pose.orientation.w = q[0]; + + float linear = 0.f, angular = 0.f; + alvik.get_drive_speed(linear, angular, M_S, RAD_S); + + odom_msg.twist.twist.linear.x = linear; + odom_msg.twist.twist.linear.y = 0.f; + odom_msg.twist.twist.angular.z = angular; + + if (rcl_ret_t const rc = rcl_publish(&odom_pub, &odom_msg, NULL); rc != RCL_RET_OK) + error_loop("odom_timer_callback::rcl_publish failed with %d", rc); + } +} + +/************************************************************************************** + * SETUP/LOOP + **************************************************************************************/ + +void setup() +{ + Serial.begin(); + for (auto const start = millis(); !Serial && (millis() - start) < 1000; ) { } + + set_microros_wifi_transports(SECRET_WIFI_SSID, SECRET_WIFI_PASS, MICRO_ROS_AGENT_IP, MICRO_ROS_AGENT_PORT); + + pinMode(LED_BUILTIN, OUTPUT); + digitalWrite(LED_BUILTIN, HIGH); + + alvik.begin(); + + if (rcl_ret_t const rc = rclc_support_init(&support, 0, NULL, &allocator); rc != RCL_RET_OK) + error_loop("rclc_support_init failed with %d", rc); + + if (rcl_ret_t const rc = rclc_node_init_default(&node, "alvik", "", &support); rc != RCL_RET_OK) + error_loop("rclc_node_init_default failed with %d", rc); + + 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) + error_loop("rclc_subscription_init_default failed with %d", rc); + + 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) + error_loop("rclc_timer_init_default(odom_timer, ...) failed with %d", rc); + + 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) + error_loop("rclc_publisher_init_default(odom_pub, ...) failed with %d", rc); + + if (rcl_ret_t const rc = rclc_executor_init(&executor, &support.context, 2, &allocator); rc != RCL_RET_OK) + error_loop("rclc_executor_init failed with %d", rc); + + 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) + error_loop("rclc_executor_add_subscription(..., cmd_vel_sub, ...) failed with %d", rc); + + if (rcl_ret_t const rc = rclc_executor_add_timer(&executor, &odom_timer); rc != RCL_RET_OK) + error_loop("rclc_executor_add_timer(..., odom_timer, ...) failed with %d", rc); + + Serial.println("alvik_ros2_firmware setup complete."); +} + +void loop() +{ + rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)); +} diff --git a/examples/alvik_ros2_firmware/micro_ros_config.h b/examples/alvik_ros2_firmware/micro_ros_config.h new file mode 100644 index 0000000..ceebdaf --- /dev/null +++ b/examples/alvik_ros2_firmware/micro_ros_config.h @@ -0,0 +1,11 @@ +/* + * Copyright (c) 2024 Arduino + * + * SPDX-License-Identifier: MPL-2.0 + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at http://mozilla.org/MPL/2.0/. + */ + +#define MICRO_ROS_AGENT_IP "YOUR_MICRO_ROS_AGENT_IP" +#define MICRO_ROS_AGENT_PORT 8888 diff --git a/examples/alvik_ros2_firmware/wifi_secrets.h b/examples/alvik_ros2_firmware/wifi_secrets.h new file mode 100644 index 0000000..9cb9076 --- /dev/null +++ b/examples/alvik_ros2_firmware/wifi_secrets.h @@ -0,0 +1,11 @@ +/* + * Copyright (c) 2024 Arduino + * + * SPDX-License-Identifier: MPL-2.0 + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this + * file, You can obtain one at http://mozilla.org/MPL/2.0/. + */ + +#define SECRET_WIFI_SSID "YOUR_WIFI_NETWORK_NAME" +#define SECRET_WIFI_PASS "YOUR_WIFI_PASSWORD"