Joint state controller takes a long time to reset

edit

I'm building my cartpole based robot

OpenAI_ROS + Gazebo. When the simulator is reset, the controllers are reset, while the joint state controller does not publish anything for about a minute, and so on each training episode. In this case, the delay time increases with each episode.

https://github.com/Djoade/TL1 roslaunch tl1 simulation_main.launch rosrun tl1 tl1_mbalunovic_algorithm

If I disable the controller reset, nothing changes.

The problem is these lines

        self.gazebo.unpauseSim()
        self._check_all_systems_ready()
        self._set_init_pose()
        self.gazebo.pauseSim()
        self.gazebo.resetSim()
        self.gazebo.unpauseSim()
        self._check_all_systems_ready()
        self.gazebo.pauseSim()

Roslog output:

[DEBUG] [1613031553.597226, 27.748000]: Reseting RobotGazeboEnvironment
[DEBUG] [1613031553.598571, 27.748000]: RESET SIM START
[DEBUG] [1613031553.600452, 27.748000]: DONT RESET CONTROLLERS
[DEBUG] [1613031553.601775, 27.748000]: UNPAUSING START
[DEBUG] [1613031553.604799, 27.748000]: connecting to ('dkozlov-ubuntu', 46939)
[DEBUG] [1613031553.606330, 27.748000]: UNPAUSING service found...
[DEBUG] [1613031553.607703, 27.748000]: UNPAUSING service calling...
[DEBUG] [1613031553.610439, 27.748000]: connecting to dkozlov-ubuntu 46939
[DEBUG] [1613031553.612605, 27.748000]: UNPAUSING service calling...DONE
[DEBUG] [1613031553.614523, 27.750000]: UNPAUSING FiNISH
[DEBUG] [1613031553.647340, 27.779000]: ALL SYSTEMS READY
[DEBUG] [1613031553.650769, 27.783000]: PAUSING START
[DEBUG] [1613031553.653832, 27.786000]: connecting to ('dkozlov-ubuntu', 46939)
[DEBUG] [1613031553.655772, 27.788000]: PAUSING service found...
[DEBUG] [1613031553.657606, 27.790000]: PAUSING service calling...
[DEBUG] [1613031553.661158, 27.794000]: connecting to dkozlov-ubuntu 46939
[DEBUG] [1613031553.663986, 27.796000]: PAUSING service calling...DONE
[DEBUG] [1613031553.665636, 27.797000]: PAUSING FINISH
[DEBUG] [1613031553.666838, 27.797000]: SIMULATION RESET
[DEBUG] [1613031553.668789, 27.797000]: connecting to ('dkozlov-ubuntu', 46939)
[DEBUG] [1613031553.671120, 27.797000]: connecting to dkozlov-ubuntu 46939
[DEBUG] [1613031553.672869, 27.797000]: UNPAUSING START
[DEBUG] [1613031553.675667, 27.797000]: connecting to ('dkozlov-ubuntu', 46939)
[DEBUG] [1613031553.677711, 27.797000]: UNPAUSING service found...
[DEBUG] [1613031553.679233, 27.797000]: UNPAUSING service calling...
[DEBUG] [1613031553.682158, 27.797000]: connecting to dkozlov-ubuntu 46939
[DEBUG] [1613031553.684629, 27.797000]: UNPAUSING service calling...DONE
[DEBUG] [1613031553.687197, 0.003000]: UNPAUSING FiNISH
[ERROR] [1613031554.692582, 1.006000]: Current cartpole_v0/joint_states not ready yet, retrying for getting joint_states
[ERROR] [1613031555.700280, 2.004000]: Current cartpole_v0/joint_states not ready yet, retrying for getting joint_states
[ERROR] [1613031556.705719, 3.009000]: Current cartpole_v0/joint_states not ready yet, retrying for getting joint_states
[ERROR] [1613031557.713263, 4.010000]: Current cartpole_v0/joint_states not ready yet, retrying for getting joint_states