Rename to ROS2_Braccio_Driver.

examples/micro-ros_publisher/README.md → 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

examples/micro-ros_publisher/micro-ros_publisher.ino → examples/ROS2_Braccio_Driver/ROS2_Braccio_Driver.ino

@@ -1,3 +1,11 @@
+/**
+ * ROS driver for controlling the Arduino Braccio++ with ROS2.
+ */
+
+/**************************************************************************************
+ * INCLUDE
+ **************************************************************************************/
+
 #include <micro_ros_arduino.h>
 
 #include <stdio.h>
@@ -8,6 +16,10 @@
 
 #include <std_msgs/msg/int32.h>
 
+/**************************************************************************************
+ * 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,35 +52,59 @@ 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,
     &support,
     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++;
+  }
+}