ros_control failed to contact

[andrei91ro]

Hello,

First of all thumbs UP for open-sourcing Webots and in particular for the ROS integration! :1st_place_medal:

My problem is that I cannot enable the ros_control integration of Webots to work with any robot other than the Pioneer sample). I am trying to add cmd_vel support through the use of diff_drive_controller but the ROS side fails to contact the Webots side only for the diff_drive_controller node. Everything else (topics, services) work as expected.

I get errors such as this when running my launch file which is adapted from the Pioneer one and using ros controller for the world used in Tutorial 6: 4-Wheeled Robot

[INFO] [1630803265.662890]: Controller Spawner: Waiting for service controller_manager/switch_controller                     
[INFO] [1630803265.665831]: Controller Spawner: Waiting for service controller_manager/unload_controller                     
[INFO] [1630803265.670367]: Loading controller: robot_diff_drive_controller                                                  
Traceback (most recent call last):                                                                                           
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/impl/tcpros_base.py", line 562, in connect                           
    self.read_header()                                                                                                       
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/impl/tcpros_base.py", line 657, in read_header                       
    self._validate_header(read_ros_handshake_header(sock, self.read_buff, self.protocol.buff_size))                          
  File "/opt/ros/noetic/lib/python3/dist-packages/rosgraph/network.py", line 357, in read_ros_handshake_header               
    d = sock.recv(buff_size)                                                                                                 
ConnectionResetError: [Errno 104] Connection reset by peer                                                                   
                                                                                                                             
During handling of the above exception, another exception occurred:                                                          
                                                                                                                             
Traceback (most recent call last):                                                                                           
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/impl/tcpros_service.py", line 509, in call                           
    transport.connect(dest_addr, dest_port, service_uri)                                                                     
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/impl/tcpros_base.py", line 589, in connect                           
    raise TransportInitError(str(e)) #re-raise i/o error                                                                     
rospy.exceptions.TransportInitError: [Errno 104] Connection reset by peer                                                    
                                                                                                                             
During handling of the above exception, another exception occurred:                                                          
                                                                                                                             
Traceback (most recent call last):                                                                                           
  File "/opt/ros/noetic/lib/controller_manager/spawner", line 212, in <module>                                               
    if __name__ == '__main__': main()                                                                                        
  File "/opt/ros/noetic/lib/controller_manager/spawner", line 190, in main                                                   
    resp = load_controller(name)                                                                                             
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/impl/tcpros_service.py", line 442, in __call__                       
    return self.call(*args, **kwds)                                                                                          
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/impl/tcpros_service.py", line 512, in call                           
    raise ServiceException("unable to connect to service: %s"%e)                                                             
rospy.service.ServiceException: unable to connect to service: [Errno 104] Connection reset by peer                           
[INFO] [1630803265.967798]: Shutting down spawner. Stopping and unloading controllers...                                     
[INFO] [1630803265.969545]: Stopping all controllers...                                                                      
[WARN] [1630803265.972946]: Controller Spawner error while taking down controllers: unable to connect to service: [Errno 111]
 Connection refused                                                                                                          
[controller_spawner-2] process has died [pid 1348, exit code 1, cmd /opt/ros/noetic/lib/controller_manager/spawner robot_diff
_drive_controller __name:=controller_spawner __log:=/home/user/.ros/log/ce532610-0de3-11ec-b206-0242ac110002/controller_spawn
er-2.log].                                                                                                                   
log file: /home/user/.ros/log/ce532610-0de3-11ec-b206-0242ac110002/controller_spawner-2*.log   

Also, if I try to call a controller related service such as list_controllers I also get a communication error:

$  rosservice call /controller_manager/list_controllers
ERROR: Unable to communicate with service [/controller_manager/list_controllers], address [rosrpc://webots_ros:48629]

I have tried adding a respawn=True to the controller part but there is no change. On the Webots side the controller seems to be working fine

[ INFO] [1630803265.390873175]: Robot's unique name is robot.
[ INFO] [1630803265.407668736]: The controller is now connected to the ROS master.

I have attached my launch script and associated Webots world (the sample world where I modified the controller to ros and set the specific controller_args identical to those in the Pioneer sample)

diff_drive_robot_launch.txt tutorial_wbt.txt

I have also attached the node, topic, param and service lists if they are of any use. node.txt topic.txt service.txt param.txt