【AI・ロボット】シンクロ率100％を目指して【Lv.0.1】

シンギュラリティ(Lv.100カンスト超え)を主張する人はたくさんいる。
残念ながら、2020年 現在の技術ではまだその段階にはいけない。

しかし、Lv.45までなら可能である！！！
(現在存在する職業を1％置き換えるごとにレベルが1上がります)

というわけで、

とりあえず、Gazebo IgnitionのJointとモータをシンクロさせてみよう。

Arduino Megaのシリアル通信ではやはり通信速度が遅かったので、ESP32を投入！！
これでWi-Fi通信(1Gbps)が可能だ！！

環境

Ubuntu18.04
Ignition Gazebo Citadel
ROS2 Dashing

ros_ign_bridge:ros_ign/ros_ign_bridge at dashing · ignitionrobotics/ros_ign · GitHub
DDS:MicroXRCEAgent
ros2arduino:GitHub - ROBOTIS-GIT/ros2arduino: This library helps the Arduino board communicate with the ROS2 using XRCE-DDS.

構成

[Ignition Gazebo]⇒[ros_ign_bridge]⇒[DDS]⇒[Arduino]

結線

DIRとSTEPのピン以外はArduinoとほぼ同じです。 nullpo24.hatenablog.com

Subscriber作成(Arduino)

ros2arduinoをインクルードします。
nullpo24.hatenablog.com

subscriber_wifi.ino

Sketch

#include <ros2arduino.h>

#include <WiFi.h>
#include <WiFiUdp.h>
#define SSID       "ssid"
#define SSID_PW    "password"
#define AGENT_IP   "192.168.0.2"
#define AGENT_PORT 2018 //AGENT port number

#define PUBLISH_FREQUENCY 500 //hz

// configure the pins connected
#define DIR 18
#define STEP 19
double current = 0;
const double one_step = 1.8 * M_PI / 180.0;

void publishString(std_msgs::String* msg, void* arg)
{
//  Serial.println("publishString");
  (void)(arg);

  static int cnt = 0;
  sprintf(msg->data, "Hello ros2arduino %d", cnt++);
//  Serial.println("Sending: ");
}

void subscribeString(std_msgs::String* msg, void* arg)
{
  (void)(arg);

  Serial.println("Received: " + String(msg->data));
}

void startMotor(const sensor_msgs::JointState& joint_state)
{
    double delta = joint_state.position[0] - current;
    double next_step = delta / one_step;
    if ( next_step >= 0){
      digitalWrite(DIR, LOW);

      for (int i=0; i < (int)next_step; i++){
        digitalWrite(STEP, HIGH);
        delayMicroseconds(2000);
        digitalWrite(STEP, LOW);
        delayMicroseconds(2000);
        current += one_step;
      }
    }else{
      digitalWrite(DIR, HIGH);

      for (int i=0; i >= (int)next_step; i--){
        digitalWrite(STEP, HIGH);
        delayMicroseconds(2000);
        digitalWrite(STEP, LOW);
        delayMicroseconds(2000);
        current -= one_step;
      }
    }

}

class StringPub : public ros2::Node
{
public:
  StringPub()
  : Node()
  {
    ros2::Publisher<std_msgs::String>* publisher_ = this->createPublisher<std_msgs::String>("arduino_chatter");
    this->createWallFreq(PUBLISH_FREQUENCY, (ros2::CallbackFunc)publishString, nullptr, publisher_);
    this->createSubscriber<std_msgs::String>("arduino_listener", (ros2::CallbackFunc)subscribeString, nullptr);
    this->createSubscriber<sensor_msgs::JointState>("world/lift_drag/model/lift_drag_demo_model/joint_state", (ros2::CallbackFunc)startMotor, nullptr);
  }
};

WiFiUDP udp;

void setup() 
{
  Serial.begin(115200);
  Serial.println("Setup Started!");
  WiFi.begin(SSID, SSID_PW);
  Serial.println("Wifi init!");
  while(WiFi.status() != WL_CONNECTED);
  Serial.println("Wifi connected!");

  ros2::init(&udp, AGENT_IP, AGENT_PORT);
  Serial.println("Setup completed!");

  pinMode(DIR, OUTPUT);
  pinMode(STEP, OUTPUT);
  digitalWrite(DIR, LOW);
  //digitalWrite(STEP, LOW);
  digitalWrite(STEP, HIGH);
}

void loop() 
{
  static StringPub StringNode;

  ros2::spin(&StringNode);
}

ign_ros_bridge for ROS2 Dashing

nullpo24.hatenablog.com

実行

端末	プロセス名称	実行コマンド
ShellA	Publisher	$ ign gazebo /usr/share/ignition/ignition-gazebo3/worlds/lift_drag.sdf
ShellB	ign_ros_bridge	$ . ~/bridge_ws/install/setup.sh $ ros2 run ros_ign_bridge parameter_bridge /world/lift_drag/model/lift_drag_demo_model/joint_state@sensor_msgs/msg/JointState@ignition.msgs.Model
ShellC	DDS	$ MicroXRCEAgent udp4 -p 2018
Arduino	subscriber_wifi	(事前に書き込みしておく)

実行順序

[ign_ros_bridge]⇒[DDS]⇒[Arduino]⇒[Ignition Gazebo]
(※DDSを起動してからArduinoをRunさせる)

結果

youtu.be

考察

Ignition Gazeboはデフォルトで1000Hz。
ros2arduinoは多分500Hzかそれを下回ります。
その差がぎこちなさに影響しているかは不明です。

やっぱり1.8°ずつなのでカクカクしています。

Tips

ESP32のピン

DIR 18 STEP 19。 すなわち、18pinを回転 19pinをステップとして使用しています 。 ESP32はそれ以外のピンを使用するとうまく動きませんでした。

ライブラリ

#include "A4988.h"
はESP32では使用できません。

Lチカ

そのコードが走ったかどうかをLチカで確認した場合は、以下のようにちょっと間隔を空けます。
また、ESP32のビルトインLEDを光らすには pin 2をHIGHにします。

# define LED 2
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
void setup()
{
  pinMode(LED, OUTPUT);
}
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
  digitalWrite(LED, HIGH);
  delayMicroseconds(10*1000);// 10ms以上待たないと視認性が悪い。
  digitalWrite(LED, LOW);
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

デバッグ

「ツール」→「シリアルモニタ」
int型double型問わず、以下のようにして、printデバッグします。

    Serial.print(joint_state.position[0]);
    Serial.println(""); //改行

無線環境

無線Lanルータが離れているとSubscribeしてくれないことがあります。

複数のライブラリが見つかりコンパイルエラー

「WiFi.h」に対して複数のライブラリが見つかりました
使用済：/home/user/Arduino/hardware/espressif/esp32/libraries/WiFi 未使用：/home/user/arduino-1.8.12/libraries/WiFi

同じ階層に同一内容の.inoファイルを置かないようにする！！

参考

ESP32をROS2ノードにする(ros2arduino) - Qiita

今後参考になりそうな情報

ros_controls/ros_controllers の制御の仕組み (position/effort/velocity_controllers の基礎) - Qiita