Fixed bug in fix 59cf8c1d

The fix introduced a new bug... Hello Ana ;-)
Minor ROS fixes