Joints randomly stuck in zero position when used with ROS controllers

[pallgeuer]

I am running a Gazebo simulation of a robot with 20 joints, each of which is being controlled by an effort_controllers::JointPositionController. Under ROS Indigo (Gazebo 2.x) everything works fine and I've been using it for a while. Under ROS Kinetic (Gazebo 7.x) however, very often when I launch the simulation, one or more joints permanently have a setpoint of zero, and those joints therefore don't move, even though valid commands are being published.

So for example, if on a particular run of the simulation the left_hip_yaw joint is the problem, that means that my robot control node is publishing on:

/my_robot/left_hip_yaw_position_controller/command

And I can confirm this with rostopic echo, that the data is really being published, and is non-zero. When I do however:

rostopic echo /nimbro_op/left_hip_yaw_position_controller/state

...I permanently see set_point: 0.0. I checked, and was able to definitively see that the reason for this is because despite this line:

sub_command_ = n.subscribe<std_msgs::Float64>("command", 1, &JointPositionController::setCommandCB, this);

in JointPositionController::init(), the callback JointPositionController::setCommandCB is never called. Using rosnode info and rostopic info says that the node is correctly subscribed though, and the ROS_DEBUG messages also say:

[DEBUG][/gazebo->Subscription::pendingConnectionDone]: Connecting via tcpros to topic [/nimbro_op/left_hip_yaw_position_controller/command] at host [nelson.local:58643]
[DEBUG][/gazebo->TransportTCP::connect]: Resolved publisher host [nelson.local] to [127.0.1.1] for socket [72]
[DEBUG][/gazebo->TransportTCP::initializeSocket]: Adding tcp socket [72] to pollset
[DEBUG][/gazebo->TransportTCP::connect]: Async connect() in progress to [nelson.local:58643] on socket [72]
[DEBUG][/gazebo->Subscription::pendingConnectionDone]: Connected to publisher of topic [/nimbro_op/left_hip_yaw_position_controller/command] at [nelson.local:58643]

The only indication, other than that the callback isn't being called, that something is wrong is the ROS_DEBUG message (once only):

[DEBUG][/gazebo->SubscriptionQueue::push]: Incoming queue was full for topic "/nimbro_op/left_hip_yaw_position_controller/command". Discarded oldest message (current queue size [0])

which indicates that the messages are never being processed at all, because otherwise the second time the buffer of size 1 is full after being emptied again once, the ROS_DEBUG message would appear again.

But on every launch it's other joint(s) having the problem, and even if one is not working, the other 19 JointPositionControllers are working. Also, if a JointPositionController is working after being launched, then it will continue to do so for as long as Gazebo is running, and if it is not, then it never suddenly starts working later.

Something is not working right, but what? I suspect it may have something to do with how ROS is initialised and brought up by the plugin.

[j-rivero]

Could it be related to issue #479? Can you try the workaround/solution described in there?

Thanks.

[pallgeuer]

Unfortunately, I don't think it's related. #479 is about PositionJointInterface's and I'm using EffortJointInterface's (effort_controllers::JointPositionController), so I can't even try the workaround because POSITION isn't used in the first place, EFFORT is, in a different branch of the if statement.

[pallgeuer]

Just as a reference, in case it's important, this is the simulation I am experiencing the problems with:

https://github.com/AIS-Bonn/humanoid_op_ros/tree/master/src/nimbro/hardware/nimbro_op_gazebo
https://github.com/AIS-Bonn/humanoid_op_ros/tree/master/src/nimbro/hardware/nimbro_op_model

I launch nimbro_op.launch in nimbro_op_gazebo. You need to set an environment variable like:

export NIMBRO_ROBOT_VARIANT=nimbro_op_hull

The repository is released for indigo (where everything works without a problem), but internally I am currently trying to move to kinetic, and that's where I am getting problems. To get the whole robot control node (the node that publishes the joint commands) running would take more effort than it's worth, but some basic test node that just publishes (non-zero) joint commands for the robot's 20 joints would suffice to see the problem.

Thanks