ROS2 Driver for Braccio++

.github/workflows/compile-examples.yml

@@ -52,6 +52,8 @@ 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
           enable-deltas-report: true

examples/ROS2_Braccio_Driver/README.md

@@ -0,0 +1,46 @@
+<img src="https://raw.githubusercontent.com/ros-infrastructure/artwork/master/distributions/galactic.png" height="100" align="right" />
+
+: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)
+
+#### 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: []
+...
+```

examples/ROS2_Braccio_Driver/ROS2_Braccio_Driver.ino

@@ -0,0 +1,158 @@
+/**
+ * ROS driver for controlling the Arduino Braccio++ with ROS2.
+ */
+
+/**************************************************************************************
+ * INCLUDE
+ **************************************************************************************/
+
+#include <Braccio++.h>
+
+#include <micro_ros_arduino.h>
+
+#include <stdio.h>
+#include <rcl/rcl.h>
+#include <rcl/error_handling.h>
+#include <rclc/rclc.h>
+#include <rclc/executor.h>
+
+#include <sensor_msgs/msg/joint_state.h>
+
+#include <rosidl_runtime_c/string_functions.h>
+
+#include <vector>
+#include <algorithm>
+
+/**************************************************************************************
+ * GLOBAL VARIABLES
+ **************************************************************************************/
+
+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;
+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 setup()
+{
+  set_microros_transports();
+
+  pinMode(LED_PIN, OUTPUT);
+  digitalWrite(LED_PIN, HIGH);  
+
+  if (!Braccio.begin()) {
+    error_loop();
+  }
+  Braccio.disengage();
+
+  allocator = rcl_get_default_allocator();
+
+  /* Create init_options. */
+  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
+
+  /* Create node. */
+  RCCHECK(rclc_node_init_default(&node, "braccio_plusplus_node", "", &support));
+
+  /* Create JointState publisher. */
+  RCCHECK(rclc_publisher_init_default(
+    &joint_state_publisher,
+    &node,
+    ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, JointState),
+    "/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, "base");
+
+  {
+    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<std::string> 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(
+    &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));
+}
+
+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)
+  {
+    /* 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));
+  }
+}