ROS2講座10 更新を検知してsqliteDB情報をROSに送る

環境

この記事は以下の環境で動いています。

項目	値
CPU	Core i5-8250U
Ubuntu	22.04
ROS2	Humble
SQLite	3.37.2

概要

sqliteデータベースの情報を利用するために、DBの情報を読んでROSメッセージでpublishします。またsqlitebrowserでDBの情報を更新したときに自動で更新するようにします。

複数のプロセスからSQLiteDBへのアクセス

SQLiteに限らずデータベースは複数のプロセスからデータのアクセスをするユースケースに対応しています。しかしSQLiteの仕組みの中では、ほかのプロセスがDBが更新されたことを通知する仕組みはありません。SQLiteのDBの実態はファイルなのでファイルの更新を監視する必要があります。

（ちなみに複数のプロセスからDBへの操作を無秩序許すと、データが破壊される恐れがあり、アクセスを制御する仕組みが必要です。SQLiteではファイルをロックすることで書き込めるプロセスを1つに絞ります）

inotify

inotifyというファイルへのアクセスを監視して通知する仕組みがLinuxにはあります。これを使うことでファイルへの種々のアクセスがあったことを検知できます。

ソースコード

ファイル変更監視ライブラリ

sqlite_lecture/include/sqlite_lecture/file_watcher.h

#pragma once

#include <stdio.h>
#include <stdlib.h>
#include <sys/inotify.h>
#include <unistd.h>
#include <termios.h>
#include <fcntl.h>
#include <string>

#define EVENT_SIZE (sizeof(struct inotify_event))
#define BUF_LEN (1024 * (EVENT_SIZE + 16))

class FileWatcher
{
public:
  FileWatcher(void)
  {
  }

  ~FileWatcher()
  {
    if (0 <= fd_ && 0 <= wd_)
    {
      inotify_rm_watch(fd_, wd_);
      close(fd_);
    }
  }

  bool setFilePath(const std::string &file_path)
  {
    fd_ = inotify_init();
    if (fd_ == -1)
    {
      perror("inotify_init");
      return false;
    }
    fcntl(fd_, F_SETFL, fcntl(fd_, F_GETFL) | O_NONBLOCK);

    wd_ = inotify_add_watch(fd_, file_path.c_str(), IN_MODIFY | IN_CREATE | IN_DELETE);
    return true;
  }

  bool checkIfFileModified(void)
  {
    int count = 0;
    int i = 0;
    char buffer[BUF_LEN];
    int length = read(fd_, buffer, BUF_LEN);

    while (i < length)
    {
      struct inotify_event *event = (struct inotify_event *)&buffer[i];
      if (event->mask & IN_MODIFY)
      {
        count++;
      }
      i += EVENT_SIZE + event->len;
    }
    return 0 < count;
  }

private:
  int fd_{-1};
  int wd_{-1};
};

• setFilePath()でinotifyの初期設定をしています。
  • inotify_add_watch()を使って監視を行うファイル名を指定します。絶対パス指定でも相対パス指定でも可能です。第3引数は監視対象とするイベントの一覧です。
• checkIfFileModified()では該当のファイルに変更があったかを確認します。
  • read()で情報を読み取ります。event->maskではイベントに対応したフラグが立っているので、これを読み取ります。

本体

qlite_lecture/src/sample4/main.cpp

#include <sqlite_lecture/relation_data.h>
#include <sqlite_lecture/file_watcher.h>

#include <sqlite3.h>
#include <unistd.h>
#include <chrono>

#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/pose_array.hpp>

using namespace std::chrono_literals;

class SqliteStore : public rclcpp::Node
{
public:
  SqliteStore() : Node("sqlite_store")
  {
    initializeRos();
    initializeDB("data.db");

    printf("start\n");
    updatePoses();
  }

  void onTimer()
  {
    if (file_watcger_.checkIfFileModified())
    {
      printf("update\n");
      updatePoses();
    }
  }

private:
  bool initializeRos(void)
  {
    pose_array_pub_ = this->create_publisher<geometry_msgs::msg::PoseArray>("stored_pose_list", rclcpp::QoS(1).transient_local());
    timer_ = this->create_wall_timer(500ms, std::bind(&SqliteStore::onTimer, this));
    return true;
  }

  bool initializeDB(const std::string filename)
  {
    // open DB
    if (sqlite3_open(filename.c_str(), &db_) != SQLITE_OK)
    {
      printf("%s[%s]\n", __func__, sqlite3_errmsg(db_));
      return false;
    }
    // create Table
    if (sqlite3_exec(db_, "CREATE TABLE IF NOT EXISTS PositionList (name PRIMARY KEY, x, y);", NULL, NULL, NULL) != SQLITE_OK)
    {
      printf("%s[%s]\n", __func__, sqlite3_errmsg(db_));
      return false;
    }
    // initialize inotify
    if (!file_watcger_.setFilePath(filename))
    {
      printf("inotify open error\n");
      return false;
    }
    return true;
  }

  void updatePoses(void)
  {
    // get & view record
    RelationData relation_data;
    if (sqlite3_exec(db_, "SELECT * FROM PositionList", SqliteStore::sqlExecuteCallback, (void *)&relation_data, NULL))
    {
      printf("%s[%s]\n", __func__, sqlite3_errmsg(db_));
      return;
    }
    relation_data.print();

    // convert
    std::vector<geometry_msgs::msg::Pose> poses;
    for (size_t i = 0; i < relation_data.size(); i++)
    {
      geometry_msgs::msg::Pose pose;
      pose.position.x = std::stod(relation_data.get("x", i));
      pose.position.y = std::stod(relation_data.get("y", i));
      pose.position.z = 0.0f;
      pose.orientation.w = 1.0f;
      poses.push_back(pose);
    }

    // publish Msg
    geometry_msgs::msg::PoseArray msg;
    msg.header.stamp = now();
    msg.header.frame_id = "map";
    msg.poses = poses;
    pose_array_pub_->publish(msg);
  }

  static int sqlExecuteCallback(void *data, int argc, char **argv, char **azColName)
  {
    RelationData *relation_data = (RelationData *)data;
    relation_data->increment();
    for (int i = 0; i < argc; i++)
    {
      relation_data->setAtBack(azColName[i], argv[i]);
    }
    return 0;
  };

  rclcpp::TimerBase::SharedPtr timer_{nullptr};
  rclcpp::Publisher<geometry_msgs::msg::PoseArray>::SharedPtr pose_array_pub_{nullptr};
  FileWatcher file_watcger_;
  sqlite3 *db_{NULL};
};

int main(int argc, char *argv[])
{
  rclcpp::init(argc, argv);
  auto sqlite_store = std::make_shared<SqliteStore>();
  rclcpp::spin(sqlite_store);
  rclcpp::shutdown();
  return 0;
}

• コンストラクタではROSとSQLiteDBの初期化をしています。またDBから情報を読み取ってROSメッセージとしてpublishします。
• onTimer()は0.5秒ごとに呼ばれる関数です。inotifyでの通知を確認して、更新があったら、DBからの読み込みとROSメッセージのpublishを行います。

ビルド&実行

ビルド

source /opt/ros/humble/setup.bash
cd ros2_ws
colcon build

ターミナル1

ros2 run sql_lecure sample4

ターミナル2

rviz2

"/stored_pose_list"を表示します。

ターミナル3

sqlitebrowser data.db

sqlitebrowserでの更新によるdata.dbへのファイルアクセスがinotyfyで通知され、Rviz2の表示に反映されました。
この値の変更はdata.dbで不揮発に持っているためにこれらのROSノードを立ち上げなおしても保存されます。

参考

sqliteのファイルロック

目次ページへのリンク

ROS2講座の目次へのリンク