From 14d31bf4c862a9b8bedb0260e3b94d66e5bc5016 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Thu, 12 May 2022 09:23:18 +0200 Subject: [PATCH 1/8] Initial import of publishing example from micro_ros_arduino. --- examples/micro-ros_publisher/README.md | 12 +++ .../micro-ros_publisher.ino | 82 +++++++++++++++++++ 2 files changed, 94 insertions(+) create mode 100644 examples/micro-ros_publisher/README.md create mode 100755 examples/micro-ros_publisher/micro-ros_publisher.ino diff --git a/examples/micro-ros_publisher/README.md b/examples/micro-ros_publisher/README.md new file mode 100644 index 0000000..8a7d2f7 --- /dev/null +++ b/examples/micro-ros_publisher/README.md @@ -0,0 +1,12 @@ +### Install +#### ROS2 Galactic +https://docs.ros.org/en/galactic/Installation/Ubuntu-Install-Debians.html +#### Install colcon +https://colcon.readthedocs.io/en/released/user/installation.html +#### Micro-ROS Tooling +https://micro.ros.org/docs/tutorials/core/teensy_with_arduino/ + + +the final command to start the partner tool + +ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyACM0 diff --git a/examples/micro-ros_publisher/micro-ros_publisher.ino b/examples/micro-ros_publisher/micro-ros_publisher.ino new file mode 100755 index 0000000..2f692c4 --- /dev/null +++ b/examples/micro-ros_publisher/micro-ros_publisher.ino @@ -0,0 +1,82 @@ +#include + +#include +#include +#include +#include +#include + +#include + +rcl_publisher_t publisher; +std_msgs__msg__Int32 msg; +rclc_executor_t executor; +rclc_support_t support; +rcl_allocator_t allocator; +rcl_node_t node; +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)){}} + + +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) +{ + RCLC_UNUSED(last_call_time); + if (timer != NULL) { + RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL)); + msg.data++; + } +} + +void setup() { + set_microros_transports(); + + pinMode(LED_PIN, OUTPUT); + digitalWrite(LED_PIN, HIGH); + + delay(2000); + + allocator = rcl_get_default_allocator(); + + //create init_options + RCCHECK(rclc_support_init(&support, 0, NULL, &allocator)); + + // create node + RCCHECK(rclc_node_init_default(&node, "micro_ros_arduino_node", "", &support)); + + // create publisher + RCCHECK(rclc_publisher_init_default( + &publisher, + &node, + ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), + "micro_ros_arduino_node_publisher")); + + // create timer, + const unsigned int timer_timeout = 1000; + RCCHECK(rclc_timer_init_default( + &timer, + &support, + RCL_MS_TO_NS(timer_timeout), + timer_callback)); + + // create executor + RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator)); + RCCHECK(rclc_executor_add_timer(&executor, &timer)); + + msg.data = 0; +} + +void loop() { + delay(100); + RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100))); +} From e4a5bc6e2ad540fb1b8f78289849b0493e6d8496 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Thu, 12 May 2022 09:33:07 +0200 Subject: [PATCH 2/8] Rename to ROS2_Braccio_Driver. --- .../README.md | 4 + .../ROS2_Braccio_Driver.ino} | 78 +++++++++++++------ 2 files changed, 57 insertions(+), 25 deletions(-) rename examples/{micro-ros_publisher => ROS2_Braccio_Driver}/README.md (84%) rename examples/{micro-ros_publisher/micro-ros_publisher.ino => ROS2_Braccio_Driver/ROS2_Braccio_Driver.ino} (52%) diff --git a/examples/micro-ros_publisher/README.md b/examples/ROS2_Braccio_Driver/README.md similarity index 84% rename from examples/micro-ros_publisher/README.md rename to examples/ROS2_Braccio_Driver/README.md index 8a7d2f7..a9c6ab1 100644 --- a/examples/micro-ros_publisher/README.md +++ b/examples/ROS2_Braccio_Driver/README.md @@ -1,3 +1,7 @@ +:floppydisk: `ROS2_Braccio_Driver` +================================== + + ### Install #### ROS2 Galactic https://docs.ros.org/en/galactic/Installation/Ubuntu-Install-Debians.html diff --git a/examples/micro-ros_publisher/micro-ros_publisher.ino b/examples/ROS2_Braccio_Driver/ROS2_Braccio_Driver.ino similarity index 52% rename from examples/micro-ros_publisher/micro-ros_publisher.ino rename to examples/ROS2_Braccio_Driver/ROS2_Braccio_Driver.ino index 2f692c4..9a7d369 100755 --- a/examples/micro-ros_publisher/micro-ros_publisher.ino +++ b/examples/ROS2_Braccio_Driver/ROS2_Braccio_Driver.ino @@ -1,3 +1,11 @@ +/** + * ROS driver for controlling the Arduino Braccio++ with ROS2. + */ + +/************************************************************************************** + * INCLUDE + **************************************************************************************/ + #include #include @@ -8,6 +16,10 @@ #include +/************************************************************************************** + * GLOBAL VARIABLES + **************************************************************************************/ + rcl_publisher_t publisher; std_msgs__msg__Int32 msg; rclc_executor_t executor; @@ -16,29 +28,21 @@ rcl_allocator_t allocator; rcl_node_t node; rcl_timer_t timer; +/************************************************************************************** + * DEFINES + **************************************************************************************/ + #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)){}} +/************************************************************************************** + * SETUP/LOOP + **************************************************************************************/ -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) -{ - RCLC_UNUSED(last_call_time); - if (timer != NULL) { - RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL)); - msg.data++; - } -} - -void setup() { +void setup() +{ set_microros_transports(); pinMode(LED_PIN, OUTPUT); @@ -48,20 +52,20 @@ void setup() { allocator = rcl_get_default_allocator(); - //create init_options + /* Create init_options. */ RCCHECK(rclc_support_init(&support, 0, NULL, &allocator)); - // create node - RCCHECK(rclc_node_init_default(&node, "micro_ros_arduino_node", "", &support)); + /* Create node. */ + RCCHECK(rclc_node_init_default(&node, "braccio_plusplus_node", "", &support)); - // create publisher + /* Create JointState publisher. */ RCCHECK(rclc_publisher_init_default( &publisher, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), - "micro_ros_arduino_node_publisher")); + "braccio_plusplus_node_joint_state_publisher")); - // create timer, + /* Create Timer. */ const unsigned int timer_timeout = 1000; RCCHECK(rclc_timer_init_default( &timer, @@ -69,14 +73,38 @@ void setup() { RCL_MS_TO_NS(timer_timeout), timer_callback)); - // create executor + /* Create executor. */ RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator)); RCCHECK(rclc_executor_add_timer(&executor, &timer)); msg.data = 0; } -void loop() { +void loop() +{ delay(100); RCSOFTCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100))); } + +/************************************************************************************** + * FUNCTIONS + **************************************************************************************/ + +void error_loop() +{ + for (;;) + { + digitalWrite(LED_PIN, !digitalRead(LED_PIN)); + delay(100); + } +} + +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)); + msg.data++; + } +} From f910a0ac735675afed5872988dc904961f75b110 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Thu, 12 May 2022 09:40:24 +0200 Subject: [PATCH 3/8] Publish a (empty JointState) message. --- .../ROS2_Braccio_Driver/ROS2_Braccio_Driver.ino | 17 +++++++---------- 1 file changed, 7 insertions(+), 10 deletions(-) diff --git a/examples/ROS2_Braccio_Driver/ROS2_Braccio_Driver.ino b/examples/ROS2_Braccio_Driver/ROS2_Braccio_Driver.ino index 9a7d369..1edd0b4 100755 --- a/examples/ROS2_Braccio_Driver/ROS2_Braccio_Driver.ino +++ b/examples/ROS2_Braccio_Driver/ROS2_Braccio_Driver.ino @@ -14,14 +14,14 @@ #include #include -#include +#include /************************************************************************************** * GLOBAL VARIABLES **************************************************************************************/ -rcl_publisher_t publisher; -std_msgs__msg__Int32 msg; +rcl_publisher_t joint_state_publisher; +sensor_msgs__msg__JointState joint_state_msg; rclc_executor_t executor; rclc_support_t support; rcl_allocator_t allocator; @@ -60,13 +60,13 @@ void setup() /* Create JointState publisher. */ RCCHECK(rclc_publisher_init_default( - &publisher, + &joint_state_publisher, &node, - ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32), + ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, JointState), "braccio_plusplus_node_joint_state_publisher")); /* Create Timer. */ - const unsigned int timer_timeout = 1000; + const unsigned int timer_timeout = 100; RCCHECK(rclc_timer_init_default( &timer, &support, @@ -76,8 +76,6 @@ void setup() /* Create executor. */ RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator)); RCCHECK(rclc_executor_add_timer(&executor, &timer)); - - msg.data = 0; } void loop() @@ -104,7 +102,6 @@ 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)); - msg.data++; + RCSOFTCHECK(rcl_publish(&joint_state_publisher, &joint_state_msg, NULL)); } } From 3855ed72d479ca11d840a862a52b78cfa2ba3760 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Thu, 12 May 2022 10:28:49 +0200 Subject: [PATCH 4/8] Publish current joint angles in degree every 100 ms. --- .../ROS2_Braccio_Driver.ino | 55 ++++++++++++++++++- 1 file changed, 53 insertions(+), 2 deletions(-) diff --git a/examples/ROS2_Braccio_Driver/ROS2_Braccio_Driver.ino b/examples/ROS2_Braccio_Driver/ROS2_Braccio_Driver.ino index 1edd0b4..fa61ae2 100755 --- a/examples/ROS2_Braccio_Driver/ROS2_Braccio_Driver.ino +++ b/examples/ROS2_Braccio_Driver/ROS2_Braccio_Driver.ino @@ -6,6 +6,8 @@ * INCLUDE **************************************************************************************/ +#include + #include #include @@ -16,6 +18,11 @@ #include +#include + +#include +#include + /************************************************************************************** * GLOBAL VARIABLES **************************************************************************************/ @@ -47,8 +54,11 @@ void setup() pinMode(LED_PIN, OUTPUT); digitalWrite(LED_PIN, HIGH); - - delay(2000); + + if (!Braccio.begin()) { + error_loop(); + } + Braccio.disengage(); allocator = rcl_get_default_allocator(); @@ -65,6 +75,32 @@ void setup() ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, JointState), "braccio_plusplus_node_joint_state_publisher")); + /* Initialize JointState message. */ + rosidl_runtime_c__String__init (&joint_state_msg.header.frame_id); + rosidl_runtime_c__String__assign(&joint_state_msg.header.frame_id, "/braccio_plusplus_joint_state"); + + { + static rosidl_runtime_c__String joint_state_msg_name[SmartServoClass::NUM_MOTORS]; + joint_state_msg.name.data = joint_state_msg_name; + joint_state_msg.name.size = SmartServoClass::NUM_MOTORS; + joint_state_msg.name.capacity = SmartServoClass::NUM_MOTORS; + std::vector const JOINT_NAMES = {"base", "shoulder", "elbow", "wrist_roll", "wrist_pitch", "pinch"}; + int i = 0; + std::for_each(std::cbegin(JOINT_NAMES), + std::cend (JOINT_NAMES), + [&joint_state_msg, &i](std::string const & joint_name) + { + rosidl_runtime_c__String__init (&joint_state_msg.name.data[i]); + rosidl_runtime_c__String__assign(&joint_state_msg.name.data[i], joint_name.c_str()); + i++; + }); + } + + static double joint_state_msg_position[SmartServoClass::NUM_MOTORS] = {0}; + joint_state_msg.position.data = joint_state_msg_position; + joint_state_msg.position.size = SmartServoClass::NUM_MOTORS; + joint_state_msg.position.capacity = SmartServoClass::NUM_MOTORS; + /* Create Timer. */ const unsigned int timer_timeout = 100; RCCHECK(rclc_timer_init_default( @@ -102,6 +138,21 @@ void timer_callback(rcl_timer_t * timer, int64_t last_call_time) RCLC_UNUSED(last_call_time); if (timer != NULL) { + /* Retrieve current servo positions. */ + float current_servo_pos[SmartServoClass::NUM_MOTORS] = {0}; + Braccio.positions(current_servo_pos); + /* Revert the array to fit with the names within the joint state message. */ + std::reverse(current_servo_pos, current_servo_pos + SmartServoClass::NUM_MOTORS); + + /* Populate header. */ + joint_state_msg.header.stamp.sec = millis() / 1000; + joint_state_msg.header.stamp.nanosec = micros() * 1000; + + /* Populate data. */ + for (int i = 0; i < SmartServoClass::NUM_MOTORS; i++) + joint_state_msg.position.data[i] = current_servo_pos[i]; + + /* Publish message. */ RCSOFTCHECK(rcl_publish(&joint_state_publisher, &joint_state_msg, NULL)); } } From 8fb5a6d3d02d1b0d98ddfe24eeda472b3986924d Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Thu, 12 May 2022 15:31:15 +0200 Subject: [PATCH 5/8] Documentation. --- examples/ROS2_Braccio_Driver/README.md | 51 ++++++++++++++----- .../ROS2_Braccio_Driver.ino | 4 +- 2 files changed, 41 insertions(+), 14 deletions(-) diff --git a/examples/ROS2_Braccio_Driver/README.md b/examples/ROS2_Braccio_Driver/README.md index a9c6ab1..198beb7 100644 --- a/examples/ROS2_Braccio_Driver/README.md +++ b/examples/ROS2_Braccio_Driver/README.md @@ -1,16 +1,43 @@ :floppydisk: `ROS2_Braccio_Driver` ================================== +#### How-to-install +* Install [ROS2 Galactic](https://docs.ros.org/en/galactic/Installation/Ubuntu-Install-Debians.html) +* Install [colcon](https://colcon.readthedocs.io/en/released/user/installation.html) +* Install/Setup [Micro-ROS](https://micro.ros.org/docs/tutorials/core/teensy_with_arduino) - -### Install -#### ROS2 Galactic -https://docs.ros.org/en/galactic/Installation/Ubuntu-Install-Debians.html -#### Install colcon -https://colcon.readthedocs.io/en/released/user/installation.html -#### Micro-ROS Tooling -https://micro.ros.org/docs/tutorials/core/teensy_with_arduino/ - - -the final command to start the partner tool - +#### Compile and Upload +```bash +arduino-cli compile -b arduino:mbed_nano:nanorp2040connect -v examples/ROS2_Braccio_Driver +arduino-cli upload -b arduino:mbed_nano:nanorp2040connect -v examples/ROS2_Braccio_Driver -p /dev/ttyACM0 +``` +#### Run Micro-ROS Agent +```bash +cd microros_ws +source /opt/ros/galactic/setup.bash +source install/local_setup.bash ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyACM0 +``` +#### View available ROS topics +```bash +source /opt/ros/galactic/setup.bash +ros2 topic list + /braccio_joint_state_actual + /parameter_events + /rosout +``` +#### View published JointStates +```bash +source /opt/ros/galactic/setup.bash +ros2 topic echo /braccio_joint_state_actual +... + header: + stamp: + sec: 44 + nanosec: 1567475040 + frame_id: base + name: [base, shoulder, elbow, wrist_roll, wrist_pitch, pinch] + position: [90.32624816894531, 156.71249389648438, 156.3975067138672, 157.34249877929688, 24.885000228881836, 153.7987518310547] + velocity: [] + effort: [] +... +``` diff --git a/examples/ROS2_Braccio_Driver/ROS2_Braccio_Driver.ino b/examples/ROS2_Braccio_Driver/ROS2_Braccio_Driver.ino index fa61ae2..612c462 100755 --- a/examples/ROS2_Braccio_Driver/ROS2_Braccio_Driver.ino +++ b/examples/ROS2_Braccio_Driver/ROS2_Braccio_Driver.ino @@ -73,11 +73,11 @@ void setup() &joint_state_publisher, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, JointState), - "braccio_plusplus_node_joint_state_publisher")); + "/braccio_joint_state_actual")); /* Initialize JointState message. */ rosidl_runtime_c__String__init (&joint_state_msg.header.frame_id); - rosidl_runtime_c__String__assign(&joint_state_msg.header.frame_id, "/braccio_plusplus_joint_state"); + rosidl_runtime_c__String__assign(&joint_state_msg.header.frame_id, "base"); { static rosidl_runtime_c__String joint_state_msg_name[SmartServoClass::NUM_MOTORS]; From bd3d25366ff7ce7eef2bfc65ca9138a467f969b9 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Tue, 17 May 2022 10:51:56 +0200 Subject: [PATCH 6/8] Adding ROS Galactic logo. --- examples/ROS2_Braccio_Driver/README.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/examples/ROS2_Braccio_Driver/README.md b/examples/ROS2_Braccio_Driver/README.md index 198beb7..e427470 100644 --- a/examples/ROS2_Braccio_Driver/README.md +++ b/examples/ROS2_Braccio_Driver/README.md @@ -1,5 +1,8 @@ + + :floppydisk: `ROS2_Braccio_Driver` ================================== + #### How-to-install * Install [ROS2 Galactic](https://docs.ros.org/en/galactic/Installation/Ubuntu-Install-Debians.html) * Install [colcon](https://colcon.readthedocs.io/en/released/user/installation.html) From 073d8e4761a3370ef0a8715336d7ee375a86f0fb Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Tue, 17 May 2022 10:59:01 +0200 Subject: [PATCH 7/8] Adding micro-ROS:v2.0.4-galactic as CI build depdendency. --- .github/workflows/compile-examples.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/compile-examples.yml b/.github/workflows/compile-examples.yml index f26b2e3..a09dec4 100644 --- a/.github/workflows/compile-examples.yml +++ b/.github/workflows/compile-examples.yml @@ -52,6 +52,7 @@ jobs: - source-path: ./ # Additional library dependencies can be listed here. # See: https://github.com/arduino/compile-sketches#libraries + - source-url: https://github.com/micro-ROS/micro_ros_arduino/archive/refs/tags/v2.0.4-galactic.zip sketch-paths: | - examples enable-deltas-report: true From 435db226110ad55a83db453c4c56045d4c1a7eb3 Mon Sep 17 00:00:00 2001 From: Alexander Entinger Date: Tue, 17 May 2022 11:58:08 +0200 Subject: [PATCH 8/8] Adding WiFiNINA as CI build dependency, since apparently the micro-ROS pre-built lib has been built with it. --- .github/workflows/compile-examples.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/compile-examples.yml b/.github/workflows/compile-examples.yml index a09dec4..96f244c 100644 --- a/.github/workflows/compile-examples.yml +++ b/.github/workflows/compile-examples.yml @@ -52,6 +52,7 @@ jobs: - source-path: ./ # Additional library dependencies can be listed here. # See: https://github.com/arduino/compile-sketches#libraries + - name: WiFiNINA - source-url: https://github.com/micro-ROS/micro_ros_arduino/archive/refs/tags/v2.0.4-galactic.zip sketch-paths: | - examples