Custom ROS2 driver can't run properly

[swjqq]

When trying to build a custom ROS2 driver according to Tutorial Write ROS2 Driver, it seems that the driver node can't run properly.

System: Webots Version: R2022a ROS Version: foxy Operating System: Ubuntu 20.04

test_ws.zip

[BenjaminHug8]

Hi, it seems that you are following a deprecated tutorial with webots_ros2_core.

This package has been replaced by webots_ros2_driver, so you might want to check this tutorial instead.

[swjqq]

Hi, it seems that you are following a deprecated tutorial with webots_ros2_core.

This package has been replaced by webots_ros2_driver, so you might want to check this tutorial instead.

Got it.

[BenjaminHug8]

Let me know if we can close this issue when you have tried the other tutorial!

[swjqq]

Let me know if we can close this issue when you have tried the other tutorial!

Well, I have some questions when trying new tutorial:

1. I want to control the LinearMotor with a topic and place the following line under the <webots> tag. However, when starting the simulation with launch file, there is no topic called /lift_motor (no error messages). I wonder if I'm using the <device> tag correctly.

   <device reference="lift motor" type="LinearMotor">
   <ros>
       <topicName>/lift_motor</topicName>
   </ros>
   </device>

2. Then I checked the Tutorial Creating a Custom Python Plugin. My package(webots_ros2_my_robot) is modified from webots_ros2_turtlebot, so it already has a sample plugin(plugin_example.py, It works correctly). I created a new plugin(plugin_my_driver.py). It was modified from mavic_driver.py and implemented as follows:

   
   import math
   import rclpy
   from geometry_msgs.msg import Twist
   from std_msgs.msg import Float32

class PluginMyDriver: def init(self, webots_node, properties): self.__robot = webots_node.robot self.timestep = int(self.robot.getBasicTimeStep())

    # Sensors
    self.__gps = self.__robot.getDevice('gps')
    self.__gyro = self.__robot.getDevice('gyro')
    self.__imu = self.__robot.getDevice('inertial unit')

    # Motor
    self.__motor_lift = self.__robot.getDevice('lift motor')
    self.__motor_left_wheel = self.__robot.getDevice('left wheel motor')
    self.__motor_right_wheel  = self.__robot.getDevice('right wheel motor')

    self.__motor_lift.setPosition(0)

    self.__motor_left_wheel.setPosition(float('inf'))
    self.__motor_left_wheel.setVelocity(0)

    self.__motor_right_wheel.setPosition(float('inf'))
    self.__motor_right_wheel.setVelocity(0)

    # State
    self.__lift__position = Float32()

    # ROS interface
    rclpy.init(args=None)
    self.__node = rclpy.create_node('my_driver')
    self.__node.create_subscription(Float32, 'lift_position', self.__lift_position_callback, 1)

def __lift_position_callback(self, position):
    self.__lift__position = position

def step(self):
    rclpy.spin_once(self.__node, timeout_sec=0)

    # Apply control
    self.__motor_lift.setPosition(self.__lift__position.data)

Then I included the plugin in the robot description file as following:

![image](https://user-images.githubusercontent.com/59327429/156864021-92f29969-5e8c-41ed-88d0-fa84d001c087.png)

When starting the simulation with launch file, it throws the following error:

![Snipaste_2022-03-05_10-24-49](https://user-images.githubusercontent.com/59327429/156864307-836028eb-7e77-461a-98b2-7331f5b3d01c.png)

Howerer, if I comment  the inclusion of plugin_example in `urdf` file, my plugin will work correctly. I also created another simple plugin(plugin_test.py) to do an experiment. I found that when two custom plugins was included in  `urdf` file, only one of them could works correctly. And it seems that which one can work normally depends on which one is included in the `urdf` file first. **I wonder if there is anything I need to be aware of when using two custom plugins.**

![Snipaste_2022-03-05_11-42-09](https://user-images.githubusercontent.com/59327429/156866371-36f98063-45f2-433e-9969-42d849bb2878.png)
![Snipaste_2022-03-05_11-39-54](https://user-images.githubusercontent.com/59327429/156866380-d9be3572-0bad-4720-9565-6033b918412c.png)

plugin_test.py:

import rclpy from geometry_msgs.msg import Twist

def clamp(value, value_min, value_max): return min(max(value, value_min), value_max)

class PluginTest: def init(self, webots_node, properties): self.__robot = webots_node.robot self.timestep = int(self.robot.getBasicTimeStep())

    rclpy.init(args=None)
    self.__node = rclpy.create_node('plugin_test')
    self.__node.create_subscription(Twist, 'plugin_test', self.__plugin_test_callback, 1)

def __plugin_test_callback(self, twist):
    self.__target_twist = twist

def step(self):
    rclpy.spin_once(self.__node, timeout_sec=0)

[swjqq]

Oh, I find that comment the following line in one of the plugin can make both of them work normally.

[BenjaminHug8]

I want to control the LinearMotor with a topic and place the following line under the <webots> tag. However, when starting the simulation with launch file, there is no topic called /lift_motor (no error messages). I wonder if I'm using the <device> tag correctly.

<device reference="lift motor" type="LinearMotor">
    <ros>
        <topicName>/lift_motor</topicName>
    </ros>
</device>

It seems ok to me, the reference has to match the name of your device in your robot and the type is the type of your device, so double check that you have a LinearMotor named lift motor in your robot.

Oh, I find that comment the following line in one of the plugin can make both of them work normally.

A plugin has to create a new node in order to function, so you will always have to write in the init part for each pluggin, as described in this python plugin tutorial :

rclpy.init(args=None)
self.__node = rclpy.create_node('mavic_driver')

---

Overall, if I take the example of the Mavic, to use a plugin you need to:

• Obviously create the plugin: https://github.com/cyberbotics/webots_ros2/blob/ebf6c40e890eb295a8cb15aac3b79b4f61e5f472/webots_ros2_mavic/webots_ros2_mavic/mavic_driver.py

• Register the plugin in the URDF file so it can be found by the webots_ros2_driver node:https://github.com/cyberbotics/webots_ros2/blob/ebf6c40e890eb295a8cb15aac3b79b4f61e5f472/webots_ros2_mavic/resource/mavic_webots.urdf#L20

• Register this plugin in the setup.py file so it can be launched by the webots_ros2_driver node: https://github.com/cyberbotics/webots_ros2/blob/ebf6c40e890eb295a8cb15aac3b79b4f61e5f472/webots_ros2_mavic/setup.py#L42

[BenjaminHug8]

Oh, I find that comment the following line in one of the plugin can make both of them work normally.

I just realized that maybe you are trying to use 2 python plugins for the same robot?

If it is the case you should only have one plugin for your robot, but thereafter you can have other nodes and use topics/services/actions to communicate with your plugin. The plugin is here only to extend the interface.

[swjqq]

It seems ok to me, the reference has to match the name of your device in your robot and the type is the type of your device, so double check that you have a LinearMotor named lift motor in your robot.

I have checked many times. I'm sure that I have a LinearMotor named lift motor in my robot. Please look at the picture below.

[swjqq]

A plugin has to create a new node in order to function, so you will always have to write in the init part for each pluggin, as described in this python plugin tutorial :

Writing in the init part for each pluggin will throw an error. Just place rclpy.init(args=None) in one pluggin can make 2 python plugins work normally.

I just realized that maybe you are trying to use 2 python plugins for the same robot?

Yes.

If it is the case you should only have one plugin for your robot, but thereafter you can have other nodes and use topics/services/actions to communicate with your plugin. The plugin is here only to extend the interface.

Ok.

[BenjaminHug8]

I have checked many times. I'm sure that I have a LinearMotor named lift motor in my robot.

You should see a topic /lift_position and be able to control your motor is you use this plugin you mentioned and only one plugin for your robot, no?:

import math
import rclpy
from geometry_msgs.msg import Twist
from std_msgs.msg import Float32

class PluginMyDriver:
    def init(self, webots_node, properties):
        self.__robot = webots_node.robot
        self.__timestep = int(self.__robot.getBasicTimeStep())

        # Sensors
        self.__gps = self.__robot.getDevice('gps')
        self.__gyro = self.__robot.getDevice('gyro')
        self.__imu = self.__robot.getDevice('inertial unit')

        # Motor
        self.__motor_lift = self.__robot.getDevice('lift motor')
        self.__motor_left_wheel = self.__robot.getDevice('left wheel motor')
        self.__motor_right_wheel  = self.__robot.getDevice('right wheel motor')

        self.__motor_lift.setPosition(0)

        self.__motor_left_wheel.setPosition(float('inf'))
        self.__motor_left_wheel.setVelocity(0)

        self.__motor_right_wheel.setPosition(float('inf'))
        self.__motor_right_wheel.setVelocity(0)

        # State
        self.__lift__position = Float32()

        # ROS interface
        rclpy.init(args=None)
        self.__node = rclpy.create_node('my_driver')
        self.__node.create_subscription(Float32, 'lift_position', self.__lift_position_callback, 1)

    def __lift_position_callback(self, position):
        self.__lift__position = position

    def step(self):
        rclpy.spin_once(self.__node, timeout_sec=0)

        # Apply control
        self.__motor_lift.setPosition(self.__lift__position.data)

[swjqq]

Yes, I can see a topic /lift_position and be able to control the LinearMotor already when using that plugin alone. I actually wonder if it is possible to create a topic that controls the LinearMotor by using the <device> tag in the URDF file without creating a plugin.

[BenjaminHug8]

Unfortunately the Motor device has not yet a complete interface in webots_ros2, therefore the <device> tag has no effect on it.

[swjqq]

Okay, I got it. Thanks.

[lukicdarkoo]

Oh, I find that comment the following line in one of the plugin can make both of them work normally.

You can initialize the rclpy context in each plugin like the following:

try:
    rclpy.init(args=None)
except Exception:  # noqa: E501
    pass  # noqa: E501

Then you don't need to worry about which plugin does the rclpy initialization.

I actually wonder if it is possible to create a topic that controls the LinearMotor by using the <device> tag in the URDF file without creating a plugin.

I don't think ROS provides standard message types for motor control. If you think about then it makes sense, on a physical robot you want the motor control to be reliable and real-time. Therefore, you should implement a controller (e.g. diffdrive controller, mecanum controller, swerve controller...) directly on a microcontroller or use ros2_control (supported by webots_ros2). With any other approach, you would likely have issues with the sim2real transfer.