I noticed that in your ::update method in the cartesian_velocity_control_node you are setting velocity command to zero in the if clauses. However, in the FCI they are discouraging such stopping commands. Why don't you call ::stopping method in those if cases?