Added MotorCommand publishing in the PID node