この記事では、ROS で SOFA を使用する方法を学びます。
公式ドキュメントでは ROS2.0 を使用する例がありますが、私のコンピューターには ROS がインストールされているため、この例の使用を検討する前に、ROS2.0 のコードをインストールする必要がありますROSに転写されます。（心配な人）

1. 公式例

例の場所: Sofa_python3.8/plugins/SoftRobots/docs/sofapython3/examples/sofaros
readme.html を開きます:
画像の説明を追加してください
上記の内容:
(実際の操作については次のセクションを参照してください。最初からソフトウェアを直接開いたので、環境によっては設定後は、後述の「1.1 事前準備」で設定方法を説明します）
必要な環境は以下のとおりです。

インストールされたPython3
Python3バージョンプラグインを使用したソファ
ROS2.0; (このノートブックのシステムは ubuntu20.04 ですが、環境をセットアップするのが簡単ではない可能性があるため、Windows を使用することは非常にお勧めできません)

使用手順:
4. 次に 2 つの端末を開き、どちらも「source ros2」を実行します。
5. 最初の端末は「python recv.py」を実行します。このファイルの機能は、サブスクリプション情報を転送するための情報転送ステーションとして使用されます。
6. 2 番目のターミナルで「runSofa test_sofaros.py」を実行します。この例は 2 つの MechanicalObject で構成された単純なシーンで、そのうちの 1 つはマウスで対話的に操作できます。この MechanicalObject のデータ位置は、専用の RosSender (Python スクリプトコントローラー) を使用して、ros トピック "/simulation/sender/position" として公開されます。
7.recv は、公開された ros トピック「/simulation/sender/position」実行にサブスクライブされます。

1.1 早めの準備

1.1.1 ソファ関連の設定

まず、ROS 環境でソファを実行したい場合は、シェルでソファを開くことができるようにする必要があります。（公式の基本チュートリアルにはターミナルのコマンドでソファを開く方法が載っていますが、以前はめんどくさいのでソファのアイコンを作ってからターミナルを使っていませんでした…）ここで私の紹介します。関連する構成プロセス。
参考記事：
win11下、SOFA v19(python2)とv21(python3)のデュアルバージョン共存関連設定
 公式設定チュートリアル

最初のステップは、スタートアップファイル runSofa が配置されているパスをシステムパスに追加することです。これにより、ターミナルで runSofa を使用するたびに、起動ファイルが配置されているフォルダーを見つける必要がなくなります。操作プロセス:

ターミナルに次のように入力します:sudo gedit ~/.bashrc現在のユーザーの構成ファイルを開きます(すべてのユーザーに対して構成する場合は、.profile ファイルを開きます) ( gedit に慣れていない場合は、vim、nano などを使用して、ファイルを編集します。私は怠け者なので、 gedit を使用します)。
開いた .bashrc ファイルの末尾に次の行を追加しexport PATH=$PATH:/home/zf/Software/Sofa_0328/bin、ファイルを保存します(ファイルはまだ閉じないでください。次の手順ではパスを追加します)。
ターミナルで次のコマンドを実行して、行った変更を有効にします。source ~/.bashrc
完了しました。設定が完了したら新しいターミナルを開きます。どこでも runSofa を使用できます。

2 番目のステップは、SOFA_ROOT、PYTHONPATH の 2 つの部分を含む Sofa の環境変数を構成することです。

.bashrc ファイルの末尾に以下を追加します。

# <<< sofa initialize <<<
export SOFA_ROOT=/home/zf/Software/Sofa_0328
export PYTHONPATH=$PYTHONPATH:$SOFA_ROOT/plugins/SofaPython3/lib/python3/site-packages:$SOFA_ROOT/plugins/SoftRobots/lib/python3/site-packages

ここで、/home/zf/Software/Sofa_0328 は私のソファのインストールパスであり、実際の状況に応じて変更できます (ここのインストールパスは以前のものとは異なります。インストールパッケージ内のいくつかのファイルが破損し、再ダウンロードしたためです)バージョン）;
PYTHONPATH の後に追加されたのは、SofaPython3 および SoftRobots プラグインの python3/site-packages の場所です。
ターミナルで次のコマンドを実行して、行った変更を有効にします。source ~/.bashrc
完全な構成

SOFA側で行う設定は以上です。次のコマンドを使用して、この例または以前に記述された .py シミュレーションシーンファイルを開くことができます。例: まず、.py ファイルが配置されているディレクトリを見つけます (ここでの cd の後ろのパスは実際のパスに変更する必要があります)。次に、runSofa を使用して py ファイルを実行します。

$ cd ~/Sofa_0328/plugins/SoftRobots/docs/sofapython3/tutorials/CableGripper/details
$ runSofa cablegripper.py

1.1.2 Python関連の設定

おそらく python3 をインストールするだけです。以前の Python 環境には何も変更を加えていません。

1.1.3 ROS関連の設定

ROS のインストールについてはここでは繰り返しません。多くの信頼できるインストールチュートリアルが CSDN で見つかります。

ROS ワークスペースを構築したことがない場合は、まずワークスペースを作成する必要があります。作成プロセスについて簡単に説明します。バグは Baidu が解決します。

ワークスペースを作成するパス:

$ mkdir -p ~/catkin_ws/src    # 创路径，catkin_ws可自定名称，但src一定要有
$ cd ~/catkin_ws/src    # 进入路径
$ catkin_ini_workspace    # 工作空间初始化，把该文件夹定义为工作空间的属性

コンパイルワークスペース:

$ cd ~/catkin_ws/    # 换到根路径
$ catkin_make     # 对该目录进行编译，编译之后要设置环境变量
$ catkin_make install    # 编译出install文件夹

環境変数の設定:
ターミナルでsourceコマンドを使用して設定された環境変数は、現在のターミナルでのみ有効になります。すべてのターミナルで有効にするには、ターミナル構成ファイルに環境変数設定を追加する必要があります。

$ echo "source /WORKSPACE/devel/setup.bash">>~/.bashrc$ source devel/setup.bash

環境変数を確認します。

$ echo $ROS_PACKAGE_PATH    #如果打印的路径中已经包含当前工作空间的路径，则说明环境变量设置成功

次に、ワークスペースを作成した後、ワークスペースに関数パッケージを作成します。

$ catkin_create_pkg <pkg_name> [depend1] [depend2] [depend3]

例 (同じワークスペースでは、同じ名前の関数パッケージは存在できませんが、異なるワークスペースでは存在できます)

関数パッケージを作成した後、ワークスペースパスでコンパイルする必要があります。
例：

# 创建功能包
$ cd ~/catkin_ws/src    # 功能包在代码空间内创建
$ catkin_create_pkg test_pkg std_msgs rospy roscpp 
#功能包必须放到src里，后面是该功能包要用到哪些依赖
# std_msgs是ROS定义的标准的数据结构

# 编译功能包
$ cd ~/catkin_ws
$ catkin_make
$ source ~/catkin_ws/devel/setup.bash

1.2 公式の ROS2.0 コードを ROS に変更する

以下をざっと見てみると、公式サンプルで使っているコードはROS1.0でも実装できるので書き換えてみたいと思いますが、ダメならROS2.0をインストールするしかありません。
2日後…
達成できます！動く！
ROS自体に詳しくないので、さらに回り道をしてしまいましたが、幸いにもうまくいきました。プロセスは次のとおりです。

1.2.1 Ros ワークスペースで関数パッケージを作成する

まず、ROS ワークスペースが構築されていることを確認します (作成プロセスについては上記を参照)。
新しいターミナルを開き、次のコマンドを順番に実行します。

$ cd ~/catkin_ws/src
$ catkin_creat_pkg sofaros_test rospy std_msgs
$ cd ../
$ catkin_make
$ source devel/setup.bash

最初の行は、独自の ROS ワークスペースに応じて調整する必要があります。実際の場所はこれではなく、~/Code/catkin_ws/src です。
関数パッケージを作成した後、sofaros_test フォルダーに新しいフォルダー script を作成し、次の 3 つを追加します。新しい Python ファイルを作成し、実行可能権限を与えます。具体的な操作方法は以下の通りです。

$ cd ~/catkin_ws/src/sofaros_test/
$ mkdir scripts	# 在ros工作空间里，为了与放C++代码的文件夹src进行区分，一般会新建一个文件夹scripts
$ cd scripts	# 进入该文件夹
$ touch recv.py sofaros.py test_sofaros.py	# 新建三个python文件
$ chmod a+x recv.py sofaros.py test_sofaros.py	# 给这三个文件赋予可执行权限

その後、これら 3 つのファイルにコードを記述できます。3 つのファイルは、公式の例の同じ名前の 3 つのファイルに対応します。

1.2.2 転送ファイルrecv.pyを書き換える

まずrecv.pyを書き換えます。変更箇所は比較的少なく、比較的基本的なコードばかりなのでコメントは書きません。ROS2.0
のオリジナルファイルを使用:

#!/usr/bin/env python
import rclpy
from std_msgs.msg import Float32MultiArray
import sys

pub = None


def callback(data):
    global pub
    pub.publish(data)
    print(data)


if __name__ == '__main__':
    rclpy.init(args=sys.argv)
    node = rclpy.create_node('listener')
    node.get_logger().info('Created node')

    sub = node.create_subscription(Float32MultiArray, "/simulation/sender/position", callback, 10)
    pub = node.create_publisher(Float32MultiArray, "/animation/receiver/position", 10)

    rclpy.spin(node)

    node.destroy_node()
    rclpy.shutdown()

ROS1に書き換えた後:

#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
from std_msgs.msg import Float32MultiArray
import sys

pub = None


def callback(data):
    global pub
    pub.publish(data)
    print(data)


if __name__ == '__main__':
    rospy.init_node("listener")
    sub = rospy.Subscriber("/simulation/sender/position", Float32MultiArray, callback, queue_size=10)
    pub = rospy.Publisher("/animation/receiver/position", Float32MultiArray, queue_size=10)

    rospy.spin()
    rospy.shutdown()

1.2.3 シミュレーションファイルを書き換える

a.test_sofaros.py

ROS2.0のオリジナルファイルを使用:

# coding: utf8
import sofaros
from std_msgs.msg import Float32MultiArray  # wrapper for ROS primitive types, see : https://github.com/ros2/common_interfaces/tree/master/std_msgs


def send(data):
    msg = Float32MultiArray()
    msg.data = list(data.value[0])
    return msg


def recv(data, datafield):
    t = data.tolist()
    datafield.value = [[t[0] + 1.0, t[1], t[2]]]


def createScene(rootNode):
    rootNode.addObject('RequiredPlugin',
                       pluginName=["Sofa.Component.ODESolver.Backward", "Sofa.Component.SceneUtility"])
    rootNode.addObject("EulerImplicitSolver")
    rootNode.addObject("CGLinearSolver", iterations=25, threshold=1e-5, tolerance=1e-5)
    rootNode.addObject('DefaultAnimationLoop')
    rootNode.addObject('DefaultVisualManagerLoop')

    s = rootNode.addChild("simulated")
    s.addObject("MechanicalObject", name="sender", position=[0.0, 0.0, 0.0])
    s.sender.showObject = True
    s.sender.showObjectScale = 1.0
    s.sender.drawMode = 1

    a = rootNode.addChild("animated")
    a.addObject("MechanicalObject", name="receiver", position=[0.0, 0.0, 0.0])
    a.receiver.showObject = True
    a.receiver.showObjectScale = 1.0
    a.receiver.drawMode = 1
    a.receiver.showColor = [0,1,0,1]

    rosNode = sofaros.init("SofaNode")
    rootNode.addObject(sofaros.RosReceiver(rosNode, "/animation/receiver/position",
                                           a.receiver.findData("position"), Float32MultiArray, recv))

    rootNode.addObject(sofaros.RosSender(rosNode, "/simulation/sender/position",
                                         s.sender.findData("position"), Float32MultiArray, send))

    return rootNode

ROS1に書き換えた後:

# coding: utf8
import sofaros
from std_msgs.msg import Float32MultiArray


def send(data):
    msg = Float32MultiArray()
    msg.data = list(data.value[0])
    return msg


def recv(data, datafield):
    t = list(data)
    datafield.value = [[t[0] + 1.0, t[1], t[2]]]


def createScene(rootNode):
    rootNode.addObject('RequiredPlugin',
                       pluginName=["Sofa.Component.ODESolver.Backward", "Sofa.Component.SceneUtility"])
    rootNode.addObject("EulerImplicitSolver")
    rootNode.addObject("CGLinearSolver", iterations=25, threshold=1e-5, tolerance=1e-5)
    rootNode.addObject('DefaultAnimationLoop')
    rootNode.addObject('DefaultVisualManagerLoop')

    s = rootNode.addChild("simulated")
    s.addObject("MechanicalObject", name="sender", position=[0.0, 0.0, 0.0])
    s.sender.showObject = True
    s.sender.showObjectScale = 1.0
    s.sender.drawMode = 1

    a = rootNode.addChild("animated")
    a.addObject("MechanicalObject", name="receiver", position=[0.0, 0.0, 0.0])
    a.receiver.showObject = True
    a.receiver.showObjectScale = 1.0
    a.receiver.drawMode = 1
    a.receiver.showColor = [0, 1, 0, 1]

    # 下面的内容涉及到ROS的使用了
    sofaros.init("ReceiverNode")
    rootNode.addObject(sofaros.RosReceiver("/animation/receiver/position",
                                                    a.receiver.findData("position"), Float32MultiArray, recv))

    rootNode.addObject(sofaros.RosSender("/simulation/sender/position",
                                                s.sender.findData("position"), Float32MultiArray, send))

    return rootNode

b. ソファロス.py

ROS2.0のオリジナルファイルを使用:

# coding: utf8
import Sofa.Core
import rclpy


class RosSender(Sofa.Core.Controller):

    def __init__(self, *args, **kwargs):
        # to reduce the latency in TCP, we can disable Nagle's algo with tcp_nodelay=False in ROS1
        # (https://en.wikipedia.org/wiki/Nagle%27s_algorithm)
        # Todo: find the equivalent in ROS2
        Sofa.Core.Controller.__init__(self, *args, **kwargs)
        self.name = "RosSender"

        # Args
        self.node = args[0]
        rosname = args[1]
        self.datafield = args[2]
        msgtype = args[3]
        self.sendingcb = args[4]

        # Create or connect to the topic rosname as a publisher
        self.pub = self.node.create_publisher(msgtype, rosname, 10)

    def onAnimateEndEvent(self, event):
        data = self.sendingcb(self.datafield)
        self.pub.publish(data)


class RosReceiver(Sofa.Core.Controller):

    def __init__(self, *args, **kwargs):
        Sofa.Core.Controller.__init__(self, *args, **kwargs)
        self.name = "RosReceiver"

        self.node = args[0]
        rosname = args[1]
        self.datafield = args[2]
        msgtype = args[3]
        self.recvcb = args[4]

        # Create or connect to the topic rosname as a subscription
        self.sub = self.node.create_subscription(msgtype, rosname, self.callback, 10)

        self.data = None

    def callback(self, data):
        self.data = data.data

    def onAnimateBeginEvent(self, event):
        rclpy.spin_once(self.node, timeout_sec=0.001)  # Something must be hidden in this spin_once(), without it the callback() is never called
        if self.data is not None:
            self.recvcb(self.data, self.datafield)
            self.data = None


def init(nodeName="Sofa"):
    rclpy.init()
    node = rclpy.create_node(nodeName)
    node.get_logger().info('Created node')
    return node

ROS1に書き換えた後:

# coding: utf8
import Sofa.Core
import rospy
from std_msgs.msg import Float32MultiArray


class RosReceiver(Sofa.Core.Controller):

    def __init__(self, *args, **kwargs):
        Sofa.Core.Controller.__init__(self, *args, **kwargs)
        self.name = "RosReceiver"

        rosname = args[0]
        self.datafield = args[1]
        msgtype = args[2]
        self.recvcb = args[3]

        # Create or connect to the topic rosname as a subscription
        self.sub = rospy.Subscriber(rosname, msgtype, self.callback, queue_size=10)
        self.data = None

    def callback(self, data):
        self.data = data.data

    def onAnimateBeginEvent(self, event):
        if self.data is not None:
            self.recvcb(self.data, self.datafield)
            self.data = None


class RosSender(Sofa.Core.Controller):
    def __init__(self, *args, **kwargs):
        # Todo: find the equivalent in ROS2
        Sofa.Core.Controller.__init__(self, *args, **kwargs)
        self.name = "RosSender"

        # Args
        rosname = args[0]
        self.datafield = args[1]
        msgtype = args[2]
        self.sendingcb = args[3]

        # Create or connect to the topic rosname as a publisher
        self.pub = rospy.Publisher(rosname, msgtype, queue_size=10)

    def onAnimateEndEvent(self, event):
        data = self.sendingcb(self.datafield)
        self.pub.publish(data)


def init(nodeName="Sofa"):
    rospy.init_node(nodeName)
    rospy.loginfo('Created Receiver node')

1.3 実行結果

書き込み後の実行プロセス:
まず 3 つのターミナルを開き、次のコマンドを実行します。

$ cd ~/catkin_ws
$ source devel/setup.bash

次に、最初のターミナルで次を実行します。

$ roscore

2 番目のターミナルで実行します。

$ cd ~/catkin_ws/src/sofaros_test/scripts
$ rosrun sofaros_test recv.py

3 番目のターミナルで実行します。

$ cd ~/catkin_ws/src/sofaros_test/scripts
$ runSofa test_sofaros.py

ソファのグラフィカルインターフェイスが表示されたら、[アニメーション] をクリックすると、背景がすでに実行されていることがわかります。ソファで、Shift キーを押しながらボールをドラッグすると、2 つのボールが一緒に動いていることがわかります。ツリー構造のアニメーション/レシーバーとシミュレーション/センダーをダブルクリックし、最初のstates1/2ラベルをクリックすると、2つの位置が同期して変化していることがわかります。図に示すように:
ここに画像の説明を挿入
このとき、バックグラウンドで実行されているrecv.pyを閉じると(ターミナルでctrl+cを押す)、ボールは同期して動かなくなります(マイナス側から見ると、先ほどの ROS は成功しました)

ソファフレームワーク研究記(4)

1. 公式例

1.1 早めの準備

1.1.1 ソファ関連の設定

1.1.2 Python関連の設定

1.1.3 ROS関連の設定

1.2 公式の ROS2.0 コードを ROS に変更する

1.2.1 Ros ワークスペースで関数パッケージを作成する

1.2.2 転送ファイルrecv.pyを書き換える

1.2.3 シミュレーションファイルを書き換える

a.test_sofaros.py

b. ソファロス.py

1.3 実行結果

おすすめ