Publish the previous pose in the case of error
Merge branch 'hydro-devel' into hydro