Sofa framework 学习笔记（四）

本篇将学一学在ROS中使用SOFA的方法
官方文档中有使用ROS2.0的案例，但是我的电脑装的是ROS，所以在探索这个示例的使用方法之前还要先把ROS2.0的代码转写到ROS。（愁人）

一、官方示例

示例位置：Sofa_python3.8/plugins/SoftRobots/docs/sofapython3/examples/sofaros
打开readme.html:

上文内容：
（实际操作看下面 一节，我从一开始就是直接打开软件用的，所以有些环境没配置好，将在下面的“1.1 前期准备”中讲一下怎么配置）
需要的环境为：

1. 安装了python3
2. 有python3版本插件的sofa
3. 以及ROS2.0；（本笔记的系统为ubuntu20.04，极其不建议使用Windows，因为多半不好设环境）

使用步骤：
4. 然后开俩终端，都执行 “source ros2”,
5. 第一个终端执行 “python recv.py”, 该文件的作用应该是作为信息中转站，转发订阅信息。
6. 在第二个终端中执行 “runSofa test_sofaros.py”, 该示例是由两个MechanicalObject组成的简单场景，其中一个可以用鼠标交互操作。这个MechanicalObject的数据位置被发布为ros主题“/simulation/sender/position”，使用专用的RosSender (python脚本控制器)。
7. recv被订阅到这个发布的ros主题“/simulation/sender/position”run

1.1 前期准备

1.1.1 Sofa相关配置

首先，想在ROS环境中运行sofa，就希望能够在shell里打开sofa。（在官方基础教程里有在终端里用命令打开sofa的方法，但我之前懒，给sofa创建了一个图标之后就没有使用终端了。。。）这里介绍一下我的相关配置过程。
参考文章：
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 了

第二步，为Sofa 配置环境变量，包括两部分：SOFA_ROOT， PYTHONPATH：

• 在 .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 是我的 sofa 安装路径，可根据实际情况进行更改（这里安装路径已经和前几篇不一样了，因为我把安装包里某些文件搞坏了，又重新下载了一版）；
• 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工作空间要先创建工作空间，简单说一下创建过程，遇到bug百度解决。

1. 创建工作空间的路径：

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

2. 编译工作空间：

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

3. 设置环境变量：
   在终端中使用source命令设置的环境变量只能在当前终端生效，若要在所有终端生效，则需要在终端配置文件中加入环境变量的设置：

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

4. 检查环境变量：

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

然后，创建好工作空间后，在工作空间中创建功能包：

扫描二维码关注公众号，回复： 15496663 查看本文章

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

e.g. (同一个工作空间下，不允许存在同名功能包，不同工作空间下，允许存在)

功能包创建好后还要到工作空间路径下进行编译。
示例：

# 创建功能包
$ 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了。
Two days later…
可以实现！感动！
因为我对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文件夹内新建文件夹 scripts， 然后在其中新建三个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	# 给这三个文件赋予可执行权限

随后可以在这个三个文件里写代码了。三个文件对应官方示例的三个同名文件。

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. sofaros.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 运行结果

写好之后的运行流程：
首先打开三个终端，都执行以下命令：

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

然后在第一个终端执行：

$ roscore

在第二个终端执行：

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

在第三个终端执行：

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

sofa的图形界面出现后，点击 Animate，就可以看到后台已经在运行了 。在sofa中，按住shift, 拖动小球，会发现两个小球一起运动了。双击树形结构里的 animation/receiver 和 simulation/sender， 然后都点第一个 states1/2 标签，可以看到二者的position在同步变化。如图所示：

此时，如果把后台运行的 recv.py 关了（在终端里按 ctrl+c）, 则小球不再同步运动（从反面说明了刚才在ROS里的通讯是成功的）