add check-state-validity service and fix minor bug

add :check-state-validity and :update-planning-robot
fix minor bug of collision-object-publisher.l