Hello! I’m somewhat new to ros2 control and I’m working on a repo for the robotiq 3f gripper:
I would love to get some feedback from others with more experience, and I have an initial question: How to deal with “modes” in ros2 control?
The gripper can be commanded in either “simple” or “individual” control modes, and each mode has different inputs. There simple mode needs a single position, speed, and force, as well as a discrete “grasp mode” input. The individual mode needs 4 sets of position/speed/force.
So I’m not sure how that should be exposed by the hardware interface?
Also I’m not sure if I should have one controller for each mode, and if that means I need to write custom controllers, or …?
I think one of my main thoughts is – It seems I can only bind state or command interfaces to “doubles”, as opposed to any custom data type (or even something simple like bool, uin8_t, etc…) so I feel like I would have to store things as doubles and have conversion functions (like PickNik did with their driver here). Is this really the recommended way?
This is a topic I spent a lot of time on myself, and this is my answer. All joint or GPIO interfaces are float values, and there is very little difference between the two, just the meaning.
If you want to represent a rotation of a joint, then obviously use a joint interface.
If you want to represent a binary, you may want to use a GPIO interface, although it is still a float value, and you need to deal with the conversion between bool and float. There is no standard way to convert between the two; be careful.
You probably still want to use GPIO interfaces for enums, but the conversion between enums and float may start to be a bit on the edge. e.g.
0.0 <= x < 1.0 = enum 1
1.0 <= x < 2.0 = enum 2
etc.
For everything more complex than that, you are better off adding a ROS service, publisher, subscriber, or action directly into the driver. The controller and the driver can then talk with each other using ROS messages, or your ROS2 client can avoid the controller altogether and talk directly to the driver.
Good to have confirmation that this is indeed a problem. I went down the route of using doubles and doing conversions, but it seems to get a sensible ROS API at the end I still have to write a custom controller & broadcaster – is that right?
Another question is what is the point of desribing my hardware with the ros2 control XML if I have to write the HW iface, controllers, and broadcasters? It feels like I was just duplicating information. I can’t see how to implement my hardware interface without hard-coding the state/command interfaces, at which point I found I don’t even need any of the ros2 control XML. You can see my current XML here (basically empty): ros2_robotiq_3f_gripper/robotiq_3f_description/urdf/robotiq_3f_driver_ros2_control.xacro at robotiq_3f · PeterMitrano/ros2_robotiq_3f_gripper · GitHub
Adding them to the XML seems redundant and doesn’t add functionality, but it’s likely I’m just misunderstanding their purpose.
Some food for thought - When I had to work with the Robotiq 3F in the past, I found that I didn’t actually need to bother myself with the low level control of the fingers - the gripper has its own firmware doing that already and I was more interested in more discrete/asynchronous control (e.g. open the gripper, close the gripper in this way, change to this mode, etc). Instead of using ros2_control, I created a node that offered services for all the functions of the gripper to be executed discretely when needed. I unfortunately can’t share the code, but I think that is probably the easiest path forward when the additional ros2_control boilerplate isn’t warranted (e.g. if you have no intention of reusing different controllers, needing ‘synchronous’ real-time control at the ROS level, etc).
If you do feel like you need closed loop control and want to use standard ros_control interfaces, then my suggestion might be to have a single hardware interface for the gripper that controls the fingers in “individual” mode, then have multiple controllers that can control the gripper in the modes you are interested in (e.g. a force control mode, or an individual finger position control mode). Each controller can then subscribe to a topic with a message type that only contains the information needed for that mode of control (e.g. a single float setpoint for force control, or an array of floats for individual finger control). ros2_control will let you load/unload controllers and let controllers “claim” the hardware interface as needed.
Try to add your service into the hardware interface instead of converting the driver into a ROS node. The driver in my opinion should not contain ROS stuff.
If you have to send or receive complex messages to and from the hardware interface, I would add the corresponding service, etc, inside the hardware interface directly. I wouldn’t bother creating a controller unless you want to support different hardware interfaces. I haven’t tried, but this is the first thing I would do.
The xacro is required to be able to load your hardware interface inside the robot URDF. It is not necessarily redundant, you can only list the interfaces that you want to own.
I see that a HW iface inherits from LifecycleNodeInterface, but not Node, so I’m still not sure if I can create publishers or subscribers inside a HW Iface. In fact, I’m pretty sure doing so is discouraged. ros2 - Access ROS node in hardware_interface - Robotics Stack Exchange
Do you have an example of how to do this?
As for the XML, it’s true I need need to list the HardwareInterface like so
What I understand is that you have multiple control modes where you can set some states of the gripper, but those cannot be mixed. This is usually solved with multiple interfaces on your hardware. This means that each control mode has its set of interfaces and when you start a controller that needs a certain mode, those interface will be requested and you on the hardware side know if you can switch or not, depending on the system state and combination of requested interfaces.
One good real-world example for this is UR driver - there are position and velocity mode that can not be used at the same time. Check for prepare_command_mode_switch and perform_command_mode_switch methods.
In most cases, you don’t have to write custom controllers since the standard controllers are configurable, and you can be just initialize multiple those controllers with differnet configurations. The simplest use case for you would be “GripperController” for the mode where you can send target joint states of your gripper for “ForwardCommandController” where you want to set the joint state of each joint (this should be individual mode if I got it correctly).
Finally, the JointStateBroadcaster exposes also all the states in the system, so at first initially if you don’t want to have custom messages, you don’t need a custom broacaster.
In practice this is much less fuss as it sounds. Yes, this is your only option as of now. We are currently working on extending to other C++ types and that functionality should be available from Jazzy.
This might be one way to solve some of your problems and invite a larger number of other problems into your system. If this is the only solution, then probably you don’t want to use ros2_control at all, since it makes this approach just much more complex. You have to take care of all thread synchronizations on your own, which is not fun. I never saw in practice that this approach is necessary and, in my honest opinion, makes any sense. If you are at this point, writing your small control framework would be a better idea.
Regarding URDF extension. You don’t need it for now, but increases clarity. There is a PR just now that should reduce much of the code in HW interface if you have this extension. Besides that, using it enables you to have adjustable joint names, for example when you want to have multiple of your grippers in ROS system - than, if done properly, you don’t need to change any code but just update URDF and controllers YAML and have a lot of fun.
If needed, feel free to instantiate a node in your HW interface. I am usually discouraging this because it is easy to break your real-time loop if you are not careful. So if you do this, check controllers’ code on how real-time safe publishing is done.
