Fix lazy rtps problem