Sofa framework study notes (4)

This article will learn how to use SOFA in ROS.
There are cases of using ROS2.0 in the official document, but my computer is installed with ROS, so before exploring the use of this example, I need to install ROS2.0 The code is transcribed to ROS. (worried person)

1. Official example

Example location: Sofa_python3.8/plugins/SoftRobots/docs/sofapython3/examples/sofaros
Open readme.html:

The above content:
(See the following section for the actual operation. I opened the software directly from the beginning, so some environments do not After configuration, we will talk about how to configure it in "1.1 Preliminary Preparation" below) The
required environment is:

1. installed python3
2. sofa with python3 version plug-in
3. And ROS2.0; (the system of this notebook is ubuntu20.04, it is extremely not recommended to use Windows, because it is probably not easy to set up the environment)

Steps to use:
4. Then open two terminals, both execute "source ros2",
5. The first terminal executes "python recv.py", the function of this file should be used as an information transfer station to forward subscription information.
6. Execute "runSofa test_sofaros.py" in the second terminal, the example is a simple scene composed of two MechanicalObjects, one of which can be interactively operated with the mouse. The data position of this MechanicalObject is published as ros topic "/simulation/sender/position", using a dedicated RosSender (python script controller).
7. recv is subscribed to the published ros topic "/simulation/sender/position" run

1.1 Early preparations

1.1.1 Sofa related configuration

First of all, if you want to run sofa in the ROS environment, you hope to be able to open sofa in the shell. (In the official basic tutorial, there is a method to open sofa with a command in the terminal, but I was lazy before, and I didn’t use the terminal after creating an icon for sofa...) Here I will introduce my related configuration process.
Reference article:
Under win11, SOFA v19 (python2) and v21 (python3) dual version coexistence related configuration
official configuration tutorial

The first step is to add the path where the startup file runSofa is located to the system path, so that you don't need to locate the folder where it is located every time you use runSofa in the terminal. Operation process:

• Enter in the terminal: sudo gedit ~/.bashrcopen the configuration file of the current user, (if you want to configure for all users, open the .profile file) , (if you are not used to gedit, you can use vim, nano, etc. to edit the file, I am lazy, use gedit ) .
• Add at the end of the opened .bashrc file: export PATH=$PATH:/home/zf/Software/Sofa_0328/bin, and then save the file (don’t close the file yet, the next step is to add the path)
• Execute the following command in the terminal to make the changes you just made take effect:source ~/.bashrc
• Finished, open a new terminal after the configuration is complete, you can use runSofa anywhere

The second step is to configure environment variables for Sofa, including two parts: SOFA_ROOT, PYTHONPATH:

• Add at the end of the .bashrc file:

# <<< 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

• Here /home/zf/Software/Sofa_0328 is my sofa installation path, which can be changed according to the actual situation (the installation path here is different from the previous ones, because I damaged some files in the installation package, and re-downloaded a version);
• Added after PYTHONPATH is the location of python3/site-packages of my SofaPython3 and SoftRobots plugins
• Execute the following command in the terminal to make the changes you just made take effect:source ~/.bashrc
• complete configuration

The configuration to be done on the SOFA side is over. You can use the following command to open the .py simulation scene file written in the example or before, for example: first locate the directory where the .py file is located (the path behind the cd here should be changed to the actual path), and then use runSofa to run the py file:

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

1.1.2 python related configuration

Probably just install python3, I haven't made any changes to the previous python environment

1.1.3 ROS related configuration

The installation of ROS will not be repeated here, and many reliable installation tutorials can be found on CSDN.

If you have never built a ROS workspace, you must create a workspace first. Let me briefly talk about the creation process, and Baidu will solve the bugs.

1. Path to create workspace:

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

2. Compile workspace:

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

3. Setting environment variables:
   The environment variables set using the source command in the terminal can only take effect in the current terminal. To take effect in all terminals, you need to add the environment variable settings in the terminal configuration file:

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

4. Check environment variables:

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

Then, after creating the workspace, create a function package in the workspace:

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

eg (Under the same workspace, the function package with the same name is not allowed to exist, but under different workspaces, it is allowed to exist)

After the function package is created, it needs to be compiled under the workspace path.
Example:

# 创建功能包
$ 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 Change the official ROS2.0 code to ROS

After a brief look at the following, the code used in the official example can be implemented with ROS1.0, so I want to try to rewrite it. If it doesn’t work, I can only install ROS2.0.
Two days later…
it can be achieved! move!
Because I am not familiar with ROS itself, so I took some more detours, but fortunately it worked out. Here is the process:

1.2.1 Create a function package in the Ros workspace

First of all, make sure that the ROS workspace has been built. See above for the creation process.
Open a new terminal and execute the following commands in sequence:

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

The first line should be adjusted according to your own ROS workspace. My actual location is not this, mine is ~/Code/catkin_ws/src
After creating the function package, create a new folder scripts in the sofaros_test folder, and then add Create three new python files and give them executable permissions. The specific operation method is:

$ 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	# 给这三个文件赋予可执行权限

Then you can write code in these three files. The three files correspond to the three files with the same name in the official example.

1.2.2 Rewrite the forwarding file recv.py

First rewrite recv.py, there are relatively few changes that need to be changed, and they are all relatively basic codes, so no comments will be written.
Using the original file of 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()

After rewriting to 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 Rewrite the simulation file

a.test_sofaros.py

Using the original file of 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

After rewriting to 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

Using the original file of 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

After rewriting to 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 Running Results

The running process after writing:
first open three terminals and execute the following commands:

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

Then execute in the first terminal:

$ roscore

Execute in the second terminal:

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

Execute in the third terminal:

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

After the graphical interface of sofa appears, click Animate, and you can see that the background is already running. In sofa, hold shift and drag the ball, you will find that the two balls are moving together. Double-click animation/receiver and simulation/sender in the tree structure, and then click the first states1/2 label, you can see that the positions of the two are changing synchronously. As shown in the figure:

At this time, if the recv.py running in the background is closed (press ctrl+c in the terminal), the ball will no longer move synchronously (from the negative side, it shows that the communication in ROS just now is successful)